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

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

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

Post by nettercm » 22 Oct 2014, 21:32

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.

Here is what I'm trying to do. Maybe I'm going about this completely wrong:

* 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

Post by nettercm » 22 Oct 2014, 22:09

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
Site Admin
Posts: 7396
Joined: 14 Dec 2012, 00:25

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

Post by coppelia » 23 Oct 2014, 00:03

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)
		simSetThreadAutomaticSwitch(false)
		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
		simSetThreadAutomaticSwitch(true)

		-- Second method:
		simWait(2) -- not needed
		allModelObjects=simGetObjectsInTree(h) -- get all objects in the model
		simSetThreadAutomaticSwitch(false)
		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
		simSetThreadAutomaticSwitch(true)
	end
end

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

Cheers
simSetThreadSwitchTiming(2) -- Default timing for automatic thread switching
h=simGetObjectHandle('Asti')

res,err=pcall(threadFunction)
if not res then
	simAddStatusbarMessage('Lua runtime error: '..err)
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

Post by bartville » 29 Jul 2015, 11:09

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".
Image

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
Site Admin
Posts: 7396
Joined: 14 Dec 2012, 00:25

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

Post by coppelia » 29 Jul 2015, 14:33

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

Post by bartville » 30 Jul 2015, 14:16

Thanks for the answer,

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
Image

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

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

Post by coppelia » 30 Jul 2015, 18:07

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

Post by Felix » 02 Dec 2015, 14:00

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?

Thank you in advance! Best,
Felix

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

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

Post by coppelia » 03 Dec 2015, 00:26

Felix,

the best would be to send a signal to V-REP, telling it to reposition and reset all objects in the robot. So your ROS node could simply set a string signal, that could then be intercepted and interpreted by a child script, that would do the heavy lifting instead of the ROS node.

Cheers

Post Reply