## How to use simx{Get|Set}ObjectPosition() to record/playback

Typically: "How do I... ", "How can I... " questions
nettercm
Posts: 11
Joined: 22 Sep 2014, 15:04

### How to use simx{Get|Set}ObjectPosition() to record/playback

When I call simxSetObjectPosition() while the simulation is running, everything moves as expected, except for the wheels. Happens to my own model as well as to the ePuck model.

* Using a choice of parameters for things like PID loops etc, I'm running through a simulation start to finish to see how well my controller (implemented as a remote client in synchronous mode) does.

* Now suppose that over the course of the simulation, my robot collides with the wall 3 times. This is of course not expected.

* To find out why my controller misbehaved, I want to "re-run" the simulation with the exact same conditions, but with a break-point in my remote client code in the right place, so that the break point is hit a few seconds prior to the first collission. This will allow me to examine the state in detail and single-step until the point of collission.

* Since I'm not sure if a dynamic simulation is 100% repeatable (see related forum post) or deliberately includes some element of "noise" (and since my own code deliberately adds some sensor noise to the readings provided by the V-REP sensors), I have come to the conclusion that it would be best if I simply "record" the relevant state information (current time, all inputs, all outputs, robot position) at each simulation step, so that I can later feed the exact same sequence of inputs into my controller during the debugging pass.

* To help me understand what is happening, I still want to visually see how the robot is moving and I want to see which point in the scene the infrared and other sensors are sensing. I also would like the graphs and vision sensor to show correctly.

* To achieve this visualization, I figured I could simply start the simulation and then use the simxSetObjectPosition() to keep moving the robot along in accordance with the previously recorded state. This API fine when the simiulation is stopped. However, when the simulation is stopped, graphs, vision sensors and proximity sensor detection rays are not updating and so I'm missing out on much of the visual feedback. When the simulation is running, simxSetObjectPosition() succeeds in moving most of the robot, but the wheels stay behind. In addition, the prximity sensors don't seem to do the right thing either - the blinking yellow/black rays do not correspond to the distance of the neares wall. Its as if they are seeing some ghost object that is much closer.

Any suggestions of how I can achieve something like this, i.e. debugging and visualization of a recorded simulation run?

Thanks

nettercm
Posts: 11
Joined: 22 Sep 2014, 15:04

### Re: How to use simx{Get|Set}ObjectPosition() to record/playb

After trying a few more things, I have found that by setting the base of my robot to "Static", all parts of the robot move as expected in response to the simxSetObjectPosition() API, even when the simulation is running. In addition, the proximity and vision sensors seem to behave correctly, i.e. after programatically moving the robot to a new position, they visually show the correct behavior (I have not looked at the actual sensor readings, since I don't care about them while playing back previously recorded state information)

Nonetheless, I would still like have some feedback regarding my proposed debugging workflow. If I'm going about this completely wrong, do let me know.

Thanks.

coppelia
Posts: 7396
Joined: 14 Dec 2012, 00:25

### Re: How to use simx{Get|Set}ObjectPosition() to record/playb

Hello,

yes, when you simply change the position/orientation of a dynamic object, you actually do something that is not possible in reality (i.e. move an object instantaneously). So internally, the call to simSetObjectPosition (and similar) will reset the object dynamically (i.e. remove it from the physics representation, then add it again. Like when calling simResetDynamicObject). But if you just do it with one object of a model (e.g. the base of the model), the other dynamic objects will not follow, since they would also need to be dynamically reset.

You have two ways of dynamically resetting all objects in a model and changing the model's position/orientation (here as an example inside of a threaded child script):

Code: Select all

threadFunction=function()
while true do
-- First method:
simWait(2) -- not needed
p=simGetModelProperty(h)
p=simBoolOr16(p,sim_modelproperty_not_dynamic+sim_modelproperty_not_respondable)
simSetModelProperty(h,p) -- make the model static and non-respondable (will be invisible to the physics engine)
pos=simGetObjectPosition(h,-1)
pos[2]=pos[2]-0.2
simSetObjectPosition(h,-1,pos) -- change the position of the model
simSwitchThread() -- make sure that the main script calls simHandleDynamics once, otherwise the model is not reset
simSetModelProperty(h,p-sim_modelproperty_not_dynamic-sim_modelproperty_not_respondable) -- make the model dynamic and respondable again

-- Second method:
simWait(2) -- not needed
allModelObjects=simGetObjectsInTree(h) -- get all objects in the model
for i=1,#allModelObjects,1 do
simResetDynamicObject(allModelObjects[i]) -- reset all objects in the model
end
pos=simGetObjectPosition(h,-1)
pos[2]=pos[2]-0.2
simSetObjectPosition(h,-1,pos) -- change the position of the model
end
end

Inside of a non-threaded child script you would use the second method (slightly adapted, since thread functions are not needed).

Cheers
h=simGetObjectHandle('Asti')

if not res then
end
Inside of a non-threaded child script you would use the second method (slightly adapted, since thread functions are not needed).

Cheers

bartville
Posts: 13
Joined: 19 Nov 2013, 13:54

### Re: How to use simx{Get|Set}ObjectPosition() to record/playb

Hi coppelia,

in order to do the same in a C++ plugin, i.e. change the position of a robot (in my case a pioneer), I'm obtaining the "classical result".

Here's my code

Code: Select all

				simResetDynamicObject(pioneer.handle);
simSetObjectPosition(pioneer.handle, -1, origin);
I've used simResetDynamicObject as I've red in many posts, but for sure I'm wrong.

Can you help me?

Thank you,
Bart

coppelia
Posts: 7396
Joined: 14 Dec 2012, 00:25

### Re: How to use simx{Get|Set}ObjectPosition() to record/playb

You will have to reset all objects in your model, not just the base.

You could use simGetObjectsInTree to retrieve all objects in your model tree.

Cheers

bartville
Posts: 13
Joined: 19 Nov 2013, 13:54

### Re: How to use simx{Get|Set}ObjectPosition() to record/playb

I've reset all the object and it works properly, but I'm surprised to see that after about 50 reset&setPosition(I'm performing some learning experiments) the model starts to "vibrate". I'm working with the model of the pioneer, and it simply starts to vibrate, as if the caster wheel of the robot slowly changes its position wrt to robot.
Maybe I will try with save&load model instead of resetDyn & setPosition, but I just want you to inform about that =)

Bart

[EDIT]
In fact, the caster starts to collide with the base

coppelia
Posts: 7396
Joined: 14 Dec 2012, 00:25

### Re: How to use simx{Get|Set}ObjectPosition() to record/playb

Bart, you are right. This is normal, since most physics engines will work with a very small error in the joints (i.e. the joint axes are not perfectly aligned because of the resolution algorithm). You can notice this when you have a large mass attached to a joint that is rotating quite fast: the joint will appear to be wobbly.

If you don't stop/restart your scene for each run, and if you often reset your dynamic shapes, then errors from the joints will add-up (i.e. the joint will be perfectly reset, but the attached shapes will remain in position).

To avoid this, you can reposition/reorient the shapes attached to the joints that have been reset.

Have a look at this scene, where this is being done. If you comment out line 18, you will see that the shapes are slowly shifting. So you could also use the function simSetConfigurationTree for your robot, but you cannot set it for the robot base, otherwise it will be reset in its initial position/orientation too (or maybe you want that?).

Cheers

Felix
Posts: 10
Joined: 30 Nov 2015, 09:22

### Re: How to use simx{Get|Set}ObjectPosition() to record/playb

Hi Bart,

i am having the same problem with my robot. I am trying to set an initial pose for my mobile robot using simRosSetObjectPose.
Could you please provide your C++ code snippet for performing this repositioning correctly?

Felix

coppelia