Models get dismantled when moved during simulation

[Billie1123]

Hello,

I want to move a robot during the simulation to a specific location pointed out by RViz using the "2D Pose Estimate" button, which is usually used to set the robot's initial pose, which is what I want. The problem is that when I do that, the robot gets dismantled. What I observe is that the joints of the model do get translated, but the child shapes attached to them, don't.

You can check this behaviour by importing, for instance, the robots/mobile/Asti.ttm model and declaring the subscriber and the callback:

Code: Select all

function cb_initial_pose(msg)
    local p = msg.pose.pose.position
    local o = msg.pose.pose.orientation
    simSetObjectPosition(asti,-1,{p.x,p.y,0.85})
    simSetObjectQuaternion(asti,-1,{o.x,o.y,o.z,o.w})
end

if (sim_call_type==sim_childscriptcall_initialization) then 
   --[...]
    _subInitialPose =simExtRosInterface_subscribe('/initialpose', 'geometry_msgs/PoseWithCovarianceStamped', 'cb_initial_pose')
end 

Then start the simulation and open RViz and use the "2D Pose Estimate" to send the initial pose.

I guess I could try translating the joints' child objects relative to their parent joint's new position, but this is what is expected V-REP to do when moving a model base object, right? Please let me know if it is a bug or if it should be done otherwise.

Regards

[coppelia]

Hello,

before moving a dynamic model around, you should first dynamically reset all objects composing it: when you move a dynamic object instantly, the attached dynamic objects will have difficulties to follow (e.g. like in real life: you cannot instantaneously move an object, and if you do (or something close to instantaneous), it will break because of the infinite acceleration).

So you have two options:

• move the objects slowly, or by applying forces (e.g. simAddForce)
• remove the object from the dynamics engine, move it, then add it again. You would need the function simResetDynamicObject. If you have several objects built on a tree, you will have to call simResetDynamicObject for all the objects in the tree, for example to iterate over all objects in a model:

Code: Select all

local robotObjects=simGetObjectsInTree(robotBaseHandle,sim_handle_all,0)
for i=1,#robotObjects,1 do
   simResetDynamicObject(robotObjects[i])
end

-- Now you can move the robot to a new position
simSetObjectPosition(robotBaseHandle,-1,newRobotPosition)

If you have to iterate over several objects to reset, make sure you are running in a non-threaded child script, or if you are running in a threaded child script, make sure you won't get interrupted for the full procedure of reset and reposition: use simSetThreadAutomaticSwitch(false) before and simSetThreadAutomaticSwitch(true) after, in that case.

Additionally, if you have to reset your robot many times, you will notice that small errors accumulate: indeed, dynamically enabled objects (e.g. frame --> joint --> wheel) will always have small positional and orientational errors (that's how most physics engine work). This means that the physics engine will not keep the wheel perfectly centered on the joint during dynamic simulation. If you reset your frame-joint-wheel, then that small error will be kept. In order to avoid that, you should additionally also memorize the relative positions/orientations of all your objects in the robot, and just after reset, apply them. You can use simGetConfigurationTree for that:

Code: Select all

--init phase:
robotObjects=simGetObjectsInTree(robotBaseHandle,sim_handle_all,0)
robotInitialConfig=simGetConfigurationTree(robotBaseHandle)

-- When you want to reset your robot to a new position:
simSetThreadAutomaticSwitch(false) -- only if you run in a threaded script
for i=1,#robotObjects,1 do
   simResetDynamicObject(robotObjects[i])
end
sim.setConfigurationTree(robotInitialConfig)
simSetObjectPosition(robotBaseHandle,-1,newRobotPosition)
simSetThreadAutomaticSwitch(true) -- only if you run in a threaded script

Cheers