use 'sim.moveToPose' to control two object move with different position

Typically: "How do I... ", "How can I... " questions
Post Reply
MaJiamu
Posts: 14
Joined: 19 Jan 2021, 03:52

use 'sim.moveToPose' to control two object move with different position

Post by MaJiamu »

Hello, I want to control two or more dummies to move along different direction and different distance at the same time. I read the instruction about the API function 'sim.moveToPose' , but it seems that this API function can not do this. Maybe Prismatic joints can do this, but I am preferring the 'sim.moveToPose'. Here is the simplified code:

Code: Select all

function callback(m,vel,accel,auxData)
    sim.setObjectMatrix(auxData[1],auxData[2],m)
end

function moveToPose(obj,relObj,pos,euler,vel,accel)
    local auxData={obj,relObj}
    local mStart=sim.getObjectMatrix(obj,relObj)
    local mGoal=sim.buildMatrix(pos,euler)
    sim.moveToPose(-1,mStart,{vel},{accel},{0.1},mGoal,callback,auxData,{1,1,1,0.1})
end
--------------
    initialP[3]=initialP[3]+0.03*sizeFactor_Z
    moveToPose(dummyHandle1,antBase,initialP,initialO,vel_Z,accel)
    moveToPose(dummyHandle2,antBase,initialP,initialO,vel_Z,accel)
    moveToPose(dummyHandle3,antBase,initialP,initialO,vel_Z,accel)----three dummies can only move one by one rather than simultaneous, this is not what I want.
Thanks for anyone who can help me.

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

Re: use 'sim.moveToPose' to control two object move with different position

Post by coppelia »

Hello,

you have at least 3 possibilities:
  • use a threaded child script for each object to move
  • write an alternative function similar to sim.moveToPose: that function merely uses sim.rmlPos, sim.rmlStep and sim.rmlRemove, and you can then write movement functions for n DoFs, or for n objects
  • However the simplest solution would be to have n individual threads running in your child script, similar to:

Code: Select all

function sysCall_init()
    local coroutineFuncs={coroutine1Main,coroutine2Main,coroutine3Main}
    coroutines={}
    for i=1,#coroutineFuncs,1 do
        coroutines[i]=coroutine.create(coroutineFuncs[i])
    end
end

function sysCall_actuation()
    for i=1,#coroutines,1 do
        if coroutine.status(coroutines[i])~='dead' then
            local ok,errorMsg=coroutine.resume(coroutines[i])
            if errorMsg then
                error(debug.traceback(coroutines[i],errorMsg),2)
            end
        end
    end
end

function cb(pose,vel,accel,handle)
    sim.setObjectPose(handle,-1,pose)
end

function coroutine1Main()
    local h=sim.getObjectHandle('Cylinder1')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ=sim.getObjectPose(h,-1)
    local goalQ=sim.copyTable(startQ)
    goalQ[3]=goalQ[3]+0.5
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
end

function coroutine2Main()
    local h=sim.getObjectHandle('Cylinder2')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ=sim.getObjectPose(h,-1)
    local goalQ=sim.copyTable(startQ)
    goalQ[3]=goalQ[3]+0.5
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
end

function coroutine3Main()
    local h=sim.getObjectHandle('Cylinder3')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ=sim.getObjectPose(h,-1)
    local goalQ=sim.copyTable(startQ)
    goalQ[3]=goalQ[3]+0.5
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
end
For above to work, you'll have to first fix a small bug in sim.moveToPose, which currently is not multi-thread safe. Simply edit lua/sim.lua, around line 584, in function sim.moveToPose:

Code: Select all

    local usingMatrices=(#currentPoseOrMatrix>=12)
    if usingMatrices then
        ...
replace above with:

Code: Select all

    local usingMatrices=(#currentPoseOrMatrix>=12)
    local currentMatrix,targetMatrix
    if usingMatrices then
        ...
Cheers

MaJiamu
Posts: 14
Joined: 19 Jan 2021, 03:52

Re: use 'sim.moveToPose' to control two object move with different position

Post by MaJiamu »

coppelia wrote: 30 Nov 2021, 05:26 Hello,

you have at least 3 possibilities:
  • use a threaded child script for each object to move
  • write an alternative function similar to sim.moveToPose: that function merely uses sim.rmlPos, sim.rmlStep and sim.rmlRemove, and you can then write movement functions for n DoFs, or for n objects
  • However the simplest solution would be to have n individual threads running in your child script, similar to:

Code: Select all

function sysCall_init()
    local coroutineFuncs={coroutine1Main,coroutine2Main,coroutine3Main}
    coroutines={}
    for i=1,#coroutineFuncs,1 do
        coroutines[i]=coroutine.create(coroutineFuncs[i])
    end
end

function sysCall_actuation()
    for i=1,#coroutines,1 do
        if coroutine.status(coroutines[i])~='dead' then
            local ok,errorMsg=coroutine.resume(coroutines[i])
            if errorMsg then
                error(debug.traceback(coroutines[i],errorMsg),2)
            end
        end
    end
end

function cb(pose,vel,accel,handle)
    sim.setObjectPose(handle,-1,pose)
end

function coroutine1Main()
    local h=sim.getObjectHandle('Cylinder1')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ=sim.getObjectPose(h,-1)
    local goalQ=sim.copyTable(startQ)
    goalQ[3]=goalQ[3]+0.5
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
end

function coroutine2Main()
    local h=sim.getObjectHandle('Cylinder2')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ=sim.getObjectPose(h,-1)
    local goalQ=sim.copyTable(startQ)
    goalQ[3]=goalQ[3]+0.5
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
end

function coroutine3Main()
    local h=sim.getObjectHandle('Cylinder3')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ=sim.getObjectPose(h,-1)
    local goalQ=sim.copyTable(startQ)
    goalQ[3]=goalQ[3]+0.5
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
end
For above to work, you'll have to first fix a small bug in sim.moveToPose, which currently is not multi-thread safe. Simply edit lua/sim.lua, around line 584, in function sim.moveToPose:

Code: Select all

    local usingMatrices=(#currentPoseOrMatrix>=12)
    if usingMatrices then
        ...
replace above with:

Code: Select all

    local usingMatrices=(#currentPoseOrMatrix>=12)
    local currentMatrix,targetMatrix
    if usingMatrices then
        ...
Cheers
Thanks for ur answer! I prefer the last one. But, I got a new problem. How to switch to other coroutineMain functions from a coroutineMain function? Here is the simplified code:

Code: Select all

function sysCall_init()
    local coroutineFuncs={coroutine1Main,coroutine2Main,coroutine3Main,coroutine4Main}
    coroutines={}
    for i=1,#coroutineFuncs,1 do
        coroutines[i]=coroutine.create(coroutineFuncs[i])
    end
end

function sysCall_actuation()
    for i=1,#coroutines,1 do
        if coroutine.status(coroutines[i])~='dead' then
            local ok,errorMsg=coroutine.resume(coroutines[i])
            if errorMsg then
                error(debug.traceback(coroutines[i],errorMsg),2)
            end
        end
    end
end

function cb(pose,vel,accel,handle)
    sim.setObjectPose(handle,-1,pose)
end

function Forward()
    goalQ1=goalQ1+0.5
    goalQ2=goalQ2+0.5
    goalQ3=goalQ3+0.5
    flag_coroutine1Main=1
     flag_coroutine2Main=1
      flag_coroutine3Main=1--the function Forward() can obtain the changed position data, and this data will set the goal 
      --position to different objects. Therefore, I give three flag value and I hope that program will go to coroutine1Main, 
      --coroutine2Main, and coroutine3Main at same time while the flag value is equal to 1. So that, the three objects can 
      --move together with different position data. However, the program seems never jump to the other three coroutineMain function. 
end

function coroutine1Main()
	if flag_coroutine1Main==1 then
    local h=sim.getObjectHandle('Cylinder1')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ1=sim.getObjectPose(h,-1)
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ1,cb,h)
    else sim,switchThread()
end

function coroutine2Main()
if flag_coroutine2Main==2 then
    local h=sim.getObjectHandle('Cylinder2')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ2=sim.getObjectPose(h,-1)
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ2,cb,h)
    else sim,switchThread()
end

function coroutine3Main()
if flag_coroutine3Main==1 then
    local h=sim.getObjectHandle('Cylinder3')
    local maxVel={0.01,0.01,0.01,0.01}
    local maxAccel={0.002,0.002,0.002,0.002}
    local maxJerk={0.001,0.001,0.001,0.001}
    local startQ3=sim.getObjectPose(h,-1)
    sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ3,cb,h)
    else sim,switchThread()
end

function coroutine4Main()
    --some initial parameters
    while true do
        if index then
            if index==1 and flag_Loop==0 then
                index=nil
                Forward()
                else if index==1 and flag_Loop==1 then
                Forward()
                end
            end
           ...
        else
            sim.switchThread()
        end
        
        
    end
end

the function Forward() can obtain the changed position data, and this data will set the goal position to different objects. Therefore, I give three flag value and I hope that program will go to coroutine1Main, coroutine2Main, and coroutine3Main at same time while the flag value is equal to 1. So that, the three objects can move together with different position data. However, the program seems never jump to the other three coroutineMain function.

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

Re: use 'sim.moveToPose' to control two object move with different position

Post by coppelia »

You can't call another coroutine/thread from a coroutine/thread. You'll have to work by organizing your coroutines as workers, that can be triggered/activated with specific variables for instance:

Code: Select all

function coroutineMain()
    while true do
        if commandPending then
            -- handle pending command
            commandPending=nil
        else
            sim.switchThread()
        end
    end
end
With my previous code example, that could look like:

Code: Select all

function sysCall_init()
    sim.setThreadAutomaticSwitch(false)
    local coroutineFuncs={coroutine1Main,coroutine2Main,coroutine3Main}
    coroutine1TargetPos=nil
    coroutine2TargetPos=nil
    coroutine3TargetPos=nil
    coroutines={}
    for i=1,#coroutineFuncs,1 do
        coroutines[i]=coroutine.create(coroutineFuncs[i])
    end
end

function sysCall_actuation()
    for i=1,#coroutines,1 do
        if coroutine.status(coroutines[i])~='dead' then
            local ok,errorMsg=coroutine.resume(coroutines[i])
            if errorMsg then
                error(debug.traceback(coroutines[i],errorMsg),2)
            end
        end
    end
    coroutine1TargetPos={0,0,1}
    coroutine2TargetPos={0,0.1,1}
    coroutine3TargetPos={0,0.2,1}
end

function cb(pose,vel,accel,handle)
    sim.setObjectPose(handle,-1,pose)
end

function coroutine1Main()
    while true do
        if coroutine1TargetPos then
            local h=sim.getObjectHandle('Cylinder1')
            local maxVel={0.01,0.01,0.01,0.01}
            local maxAccel={0.002,0.002,0.002,0.002}
            local maxJerk={0.001,0.001,0.001,0.001}
            local startQ=sim.getObjectPose(h,-1)
            local goalQ=sim.copyTable(startQ)
            goalQ[1]=coroutine1TargetPos[1]
            goalQ[2]=coroutine1TargetPos[2]
            goalQ[3]=coroutine1TargetPos[3]
            sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
            coroutine1TargetPos=nil
        else
            sim.switchThread()
        end
    end
end

function coroutine2Main()
    while true do
        if coroutine2TargetPos then
            local h=sim.getObjectHandle('Cylinder2')
            local maxVel={0.01,0.01,0.01,0.01}
            local maxAccel={0.002,0.002,0.002,0.002}
            local maxJerk={0.001,0.001,0.001,0.001}
            local startQ=sim.getObjectPose(h,-1)
            local goalQ=sim.copyTable(startQ)
            goalQ[1]=coroutine2TargetPos[1]
            goalQ[2]=coroutine2TargetPos[2]
            goalQ[3]=coroutine2TargetPos[3]
            sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
            coroutine2TargetPos=nil
        else
            sim.switchThread()
        end
    end
end

function coroutine3Main()
    while true do
        if coroutine3TargetPos then
            local h=sim.getObjectHandle('Cylinder3')
            local maxVel={0.01,0.01,0.01,0.01}
            local maxAccel={0.002,0.002,0.002,0.002}
            local maxJerk={0.001,0.001,0.001,0.001}
            local startQ=sim.getObjectPose(h,-1)
            local goalQ=sim.copyTable(startQ)
            goalQ[1]=coroutine3TargetPos[1]
            goalQ[2]=coroutine3TargetPos[2]
            goalQ[3]=coroutine3TargetPos[3]
            sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
            coroutine3TargetPos=nil
        else
            sim.switchThread()
        end
    end
end
Cheers

MaJiamu
Posts: 14
Joined: 19 Jan 2021, 03:52

Re: use 'sim.moveToPose' to control two object move with different position

Post by MaJiamu »

coppelia wrote: 02 Dec 2021, 10:30 You can't call another coroutine/thread from a coroutine/thread. You'll have to work by organizing your coroutines as workers, that can be triggered/activated with specific variables for instance:

Code: Select all

function coroutineMain()
    while true do
        if commandPending then
            -- handle pending command
            commandPending=nil
        else
            sim.switchThread()
        end
    end
end
With my previous code example, that could look like:

Code: Select all

function sysCall_init()
    sim.setThreadAutomaticSwitch(false)
    local coroutineFuncs={coroutine1Main,coroutine2Main,coroutine3Main}
    coroutine1TargetPos=nil
    coroutine2TargetPos=nil
    coroutine3TargetPos=nil
    coroutines={}
    for i=1,#coroutineFuncs,1 do
        coroutines[i]=coroutine.create(coroutineFuncs[i])
    end
end

function sysCall_actuation()
    for i=1,#coroutines,1 do
        if coroutine.status(coroutines[i])~='dead' then
            local ok,errorMsg=coroutine.resume(coroutines[i])
            if errorMsg then
                error(debug.traceback(coroutines[i],errorMsg),2)
            end
        end
    end
    coroutine1TargetPos={0,0,1}
    coroutine2TargetPos={0,0.1,1}
    coroutine3TargetPos={0,0.2,1}
end

function cb(pose,vel,accel,handle)
    sim.setObjectPose(handle,-1,pose)
end

function coroutine1Main()
    while true do
        if coroutine1TargetPos then
            local h=sim.getObjectHandle('Cylinder1')
            local maxVel={0.01,0.01,0.01,0.01}
            local maxAccel={0.002,0.002,0.002,0.002}
            local maxJerk={0.001,0.001,0.001,0.001}
            local startQ=sim.getObjectPose(h,-1)
            local goalQ=sim.copyTable(startQ)
            goalQ[1]=coroutine1TargetPos[1]
            goalQ[2]=coroutine1TargetPos[2]
            goalQ[3]=coroutine1TargetPos[3]
            sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
            coroutine1TargetPos=nil
        else
            sim.switchThread()
        end
    end
end

function coroutine2Main()
    while true do
        if coroutine2TargetPos then
            local h=sim.getObjectHandle('Cylinder2')
            local maxVel={0.01,0.01,0.01,0.01}
            local maxAccel={0.002,0.002,0.002,0.002}
            local maxJerk={0.001,0.001,0.001,0.001}
            local startQ=sim.getObjectPose(h,-1)
            local goalQ=sim.copyTable(startQ)
            goalQ[1]=coroutine2TargetPos[1]
            goalQ[2]=coroutine2TargetPos[2]
            goalQ[3]=coroutine2TargetPos[3]
            sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
            coroutine2TargetPos=nil
        else
            sim.switchThread()
        end
    end
end

function coroutine3Main()
    while true do
        if coroutine3TargetPos then
            local h=sim.getObjectHandle('Cylinder3')
            local maxVel={0.01,0.01,0.01,0.01}
            local maxAccel={0.002,0.002,0.002,0.002}
            local maxJerk={0.001,0.001,0.001,0.001}
            local startQ=sim.getObjectPose(h,-1)
            local goalQ=sim.copyTable(startQ)
            goalQ[1]=coroutine3TargetPos[1]
            goalQ[2]=coroutine3TargetPos[2]
            goalQ[3]=coroutine3TargetPos[3]
            sim.moveToPose(-1,startQ,maxVel,maxAccel,maxJerk,goalQ,cb,h)
            coroutine3TargetPos=nil
        else
            sim.switchThread()
        end
    end
end
Cheers
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)

Post Reply