Thanks for detail answer! Finally, I choose the second method to rewrite a new function similar to sim.moveToPose. Here is the simplified code:
Code: Select all
----------------------prepare parameters-----------------------
function parafornewMove(Object1changeP,Objec2changeP,Object3changeP)
para1={Object1initialP[1],Object1initialP[2],Object1initialP[3],0,0,0,0,0,0}
para2={Object2initialP[1],Object2initialP[2],Object2initialP[3],0,0,0,0,0,0}
para3={Object3initialP[1],Object3initialP[2],Object3initialP[3],0,0,0,0,0,0}
para4={para1[1]+Object1changeP[1],para1[2]+Object1changeP[2],para1[3]+Object1changeP[3],0,0,0}
para5={para2[1]+Object2changeP[1],para2[2]+Object2changeP[2],para2[3]+Object2changeP[3],0,0,0}
para6={para3[1]+Object3changeP[1],para3[2]+Object3changeP[2],para3[3]+Object3changeP[3],0,0,0}
para7=ObjectHandle1
para8=ObjectHandle2
para9=ObjectHandle3
return para1,para2,para3,para4,para5,para6,para7,para8,para9
end
------------------------new moveToPose function--------------------------------
function moveToPose2(currentPosVelAccel1,currentPosVelAccel2,currentPosVelAccel3,targetPosVel1,targetPosVel2,targetPosVel3,h1,h2,h3)--Not UI function----For GoLeft/GoRight move.(moveToPose2)
local maxVelAccelJerk={vel_X,vel_Y,vel_Z,accel_X,accel_Y,accel_Z,0,0,0}
-- vx,vy,vz in m/s, ax,ay,az in m/s^2, jx,jy,jz is ignored (i.e. infinite) with RML type 2
--in this function, maxVelAccelJerk was set to the same value in three Objects
local rmlObject1=sim.rmlPos(3,0.0001,-1,currentPosVelAccel1,maxVelAccelJerk,{1,1,1},targetPosVel1)
local rmlObject2=sim.rmlPos(3,0.0001,-1,currentPosVelAccel2,maxVelAccelJerk,{1,1,1},targetPosVel2)
local rmlObject3=sim.rmlPos(3,0.0001,-1,currentPosVelAccel3,maxVelAccelJerk,{1,1,1},targetPosVel3)
local lb=sim.setThreadAutomaticSwitch(false)
result=0
while result==0 do
result,newPosVelAccel1=sim.rmlStep(rmlObject1,sim.getSimulationTimeStep())
result,newPosVelAccel2=sim.rmlStep(rmlObject2,sim.getSimulationTimeStep())
result,newPosVelAccel3=sim.rmlStep(rmlObject3,sim.getSimulationTimeStep())
if result~=-1 then
sim.setObjectPosition(h1,sim.handle_parent,newPosVelAccel1) ----the movement reference in this function was set to sim.handle_parent
sim.setObjectPosition(h2,sim.handle_parent,newPosVelAccel2)
sim.setObjectPosition(h3,sim.handle_parent,newPosVelAccel3)
end
sim.switchThread()
end
sim.setThreadAutomaticSwitch(lb)
sim.rmlRemove(rmlObject1)
sim.rmlRemove(rmlObject2)
sim.rmlRemove(rmlObject3)
end
--------------------------callback the new function to move three object in a thread------------------------
para1,para2,para3,para4,para5,para6,para7,para8,para9=parafornewMove1(object1changeP,objec2changeP,object3changeP)
moveToPose2(para1,para2,para3,para4,para5,para6,para7,para8,para9)