Strange RML behaviour after using simSetJointPosition

[jacobi]

Dear coppelia,

I want to modify the robot joint angles before a simulation is started, and the modified joint angles will be taken as the home position for the robot's threaded script (with a series of simRMLMoveToJointPositions).

Currently, I found that by unchecking the "At simulation end, Reset scene to initial state", I get some desirable results. The followed simulation will use the last robot joints as the starting point, and the RML motion is quite smooth.

However, this is not enough, I cannot adjust the joint angles as I wish.
So I did some experiments. I use the UR5 robot scene, and I created an add-on script,

Code: Select all

d2r=math.pi/180
targetPos1={90*d2r,90*d2r,-90*d2r,90*d2r,90*d2r,90*d2r}
jointHandles={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
	jointHandles[i]=simGetObjectHandle('UR5_joint'..i)
	result = simSetJointPosition(jointHandles[i],targetPos1[i])
end
       

The targetPos1 is the same position as the one in the threaded child script.

The RML script will show some shaking behavior. The robot seems try to move towards the initial state {0,0,0,0,0,0}, then after some distance it switch back to targetPos1.

If we remove the simRMLMoveToJointPositions(.., targetPos1) in the threaded child script, the motion is also smooth. So it seems the first RML target pos should not be identical to the robot pos set in the add-on script.
But we can put several identical lines of simRMLMoveToJointPositions(.., targetPos1) in the child script without showing any shaking. So there must be some other reasons.

I tested the following steps and it works for me:
1. run the above Add-on script the modify the joint angles.
2. manually uncheck "At simulation end, Reset scene to initial state".
3. run another Add-on script with two lines,
simStartSimulation()
simStopSimulation()
4. manually check "At simulation end, Reset scene to initial state".
5. Save the scene

But I want to find a way to check and uncheck the option using script.
This approach has some drawbacks, as Start and Stop could cause some small movements.
I wish there will be a new command like simSetCurrentStateAsInitialState().

[coppelia]

Hello,

this is not a bug, but a normal behaviour. Try to put following code in your add-on script instead:

Code: Select all

d2r=math.pi/180
targetPos1={90*d2r,90*d2r,-90*d2r,90*d2r,90*d2r,90*d2r}
jointHandles={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
   jointHandles[i]=simGetObjectHandle('UR5_joint'..i)
   result = simSetJointPosition(jointHandles[i],targetPos1[i])
   simSetJointTargetPosition(jointHandles[i],targetPos1[i])
end

The initial shaking should be gone.
Since your joint is dynamically enabled and the control loop is on, its position is determined by the target position in the joint dynamic properties, that is used by the controller (e.g. PID). And at simulation start, before the threaded child script is able to run the first simRML function, the current joint target position will be driven towards.

So with above add-on script, you don't even need to uncheck the reset scene to initial state item.

Cheers