I'm currently working on a project where I to repeat an experiment multiple times.
I'm controlling my simulated robot through ROS, so I thought that the smartest thing would be to just reset to robot back to its initial position and orientation (that way I do not have to stop and start the simulation - and reconnect the ROS nodes).
However, after a few reset's some parts tend to "float"/move away from their desired attachment points (see picture).
How it should be:

How it is after few resets (in this case the force-sensor and angle joint moves out of place):

My resetting code is as follows and is placed in the actuator section of the child script:
Code: Select all
if(testParameters[2] > 45) then
--moving the dynamic object
t={robotHandle}
while (#t~=0) do
h=t[1]
simResetDynamicObject(h)
table.remove(t,1)
ind=0
child=simGetObjectChild(h,ind)
while (child~=-1) do
table.insert(t,child)
ind=ind+1
child=simGetObjectChild(h,ind)
end
end
--moving the robot to the initial position and orientation (i.e. where the simulation started)
sim.setObjectPosition(robotHandle,-1,initialPosition)
sim.setObjectOrientation(robotHandle,-1,initialOrientation)
sim.resetDynamicObject(robotHandle)
end
Best Regards & Thanks in advance,
Mathias Thor