Tasks sequence

Typically: "How do I... ", "How can I... " questions
Post Reply
Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Tasks sequence

Post by Leizah »

Hi,
I got a Bill on path model and a UR10 robot and I would like to develop a sequence of tasks that involve both the models.

The Bill model stops when it has followed the 60% of its path, then it sends an integer signal to start the movement of the robot.
I was able to get this using the following code:

Bill's child script:

Code: Select all

if currentPosOnPath>pathL*0.59 and walkingDir>0 then
   sim.setIntegerSignal('Human_TP',1)
   end
UR10's child script

Code: Select all

Robot_start=sim.waitForSignal('Human_TP')
if Robot_Start then
        sim.setJointTargetVelocity(simJoints[1],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[2],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[3],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[4],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[5],maxIkVel[1])
        sim.setJointTargetVelocity(simJoints[6],maxIkVel[1])
        moveToConfig_viaFK(maxConfVel,maxConfAccel,maxConfJerk,initConf,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_1,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_2,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_3,data)
        moveToPose_viaIK(maxIkVel,maxIkAccel,maxIkJerk,Target_4,data)
    end
This works fine, but I'm not able to stop the Bill model until the robot ends his task.
I tried creating a new integer signal into the robot code as follows:

Code: Select all

Tip_pos=sim.getObjectpose(simTip,-1)
if Tip_pos>Target_3 then
  sim.setIntegerSignal('Robot_Finish',2)
  end
But this solution doesn't work.
When Bill reaches the 60% of the path, the 'Human_TP' signal is sent to the robot, which starts its movement, but the Bill model doesn't stop, he continues to move.

In order to be clear, this is the sequence that I would like to get:
[*]1 - Bill walks on path
The robot is not moving
[*]2 - Bill reaches the 60% of the path, sends the signal and stops.
The robot starts moving
[*]3 - Bill stays at the 60% of the path
The robot performs its task
[*]4 - The robot ends its movement and sends a signal to let Bill move
Bill restarts his path

Right now, I was able to achieve the first 2 points, but I didn't figure out how to make Bill stop once he sends its signal.
I cannot set a time-related stop (using a wait command), because I don't know how much time the robot needs to complete its task, so I've tried with a sim.waitForSignal command, but I doesn't work.

Have you any suggestion about that issue?
Cheers

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

Re: Tasks sequence

Post by coppelia »

Hello,

make sure that both items/models use threaded scripts. Then it is as simple as doing:

Code: Select all

-- Model 1:
function sysCall_init()
    corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
end

function coroutineMain()

    -- thread1, do task1

    sim.setInt32Signal("syncSignal",1)
    while sim.getInt32Signal("syncSignal")==1 do
        sim.switchThread()
    end
    
    -- thread1, do task2
    
    local s=sim.getInt32Signal("syncSignal")+1
    sim.setInt32Signal("syncSignal",s)
    while sim.getInt32Signal("syncSignal")==s do
        sim.switchThread()
    end

    -- etc.
end

Code: Select all

-- Model 2:
function sysCall_init()
    corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
end

function coroutineMain()
    while sim.getInt32Signal("syncSignal")~=1 do
        sim.switchThread()
    end
    
    -- thread2, do task1
    
    local s=sim.getInt32Signal("syncSignal")+1
    while sim.getInt32Signal("syncSignal")==s do
        sim.switchThread()
    end

    -- etc.
end
If some part of the code runs non-threaded, you can still interact with it easily, for instance:

Code: Select all

function sysCall_init()
    corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
    
    if canMove then
        local p=sim.getObjectPosition(objectHandle,-1)
        p[1]=p[1]+0.01
        sim.setObjectPosition(objectHandle,-1,p)
    end
end

function coroutineMain()
    while sim.getInt32Signal("syncSignal")~=1 do
        sim.switchThread()
    end

    canMove=true
    sim.wait(5)
    canMove=false
    
    local s=sim.getInt32Signal("syncSignal")+1
    while sim.getInt32Signal("syncSignal")==s do
        sim.switchThread()
    end

    -- etc.
end
In case of the walking human, the best and easiest would probably to rewrite its code to run from a thread.

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Tasks sequence

Post by Leizah »

Thank you for your quick reply.
I tried something similiar using the code from the youBotAndHanoiTower model as a reference, but I was not able to set the path for the walking human, I'll make an other try.
coppelia wrote: 13 Sep 2021, 07:38 In case of the walking human, the best and easiest would probably to rewrite its code to run from a thread.
Ok, there's any tutorial/existing scene that I could use as reference?
Cheers

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

Re: Tasks sequence

Post by coppelia »

If you take Model Bill on path from release V4.2.0, then you can replace its child script with following, then it will run threaded and it will be much easier to introduce modifications:

Code: Select all

function setColor(colorName,color)
    local objectTable=sim.getObjectsInTree(BillHandle,sim.object_shape_type)
    for i=1,#objectTable,1 do
        sim.setShapeColor(objectTable[i],colorName,0,color)
    end
end

function moveBody(dist)
    if dist then
        local t1=math.mod(dist/1,1)
        local t2=math.mod(t1+0.5,1)
      
        sim.setJointPosition(legJointHandles[1],sim.getPathInterpolatedConfig(legWaypoints,timings,t1)[1])
        sim.setJointPosition(kneeJointHandles[1],sim.getPathInterpolatedConfig(kneeWaypoints,timings,t1)[1])
        sim.setJointPosition(ankleJointHandles[1],sim.getPathInterpolatedConfig(ankleWaypoints,timings,t1)[1])
        sim.setJointPosition(shoulderJointHandles[1],sim.getPathInterpolatedConfig(shoulderWaypoints,timings,t1)[1])
        sim.setJointPosition(elbowJointHandles[1],sim.getPathInterpolatedConfig(elbowWaypoints,timings,t1)[1])
        
        sim.setJointPosition(legJointHandles[2],sim.getPathInterpolatedConfig(legWaypoints,timings,t2)[1])
        sim.setJointPosition(kneeJointHandles[2],sim.getPathInterpolatedConfig(kneeWaypoints,timings,t2)[1])
        sim.setJointPosition(ankleJointHandles[2],sim.getPathInterpolatedConfig(ankleWaypoints,timings,t2)[1])
        sim.setJointPosition(shoulderJointHandles[2],sim.getPathInterpolatedConfig(shoulderWaypoints,timings,t2)[1])
        sim.setJointPosition(elbowJointHandles[2],sim.getPathInterpolatedConfig(elbowWaypoints,timings,t2)[1])
    else
        for i=1,2,1 do
            sim.setJointPosition(legJointHandles[i],0)
            sim.setJointPosition(kneeJointHandles[i],0)
            sim.setJointPosition(ankleJointHandles[i],0)
            sim.setJointPosition(shoulderJointHandles[i],0)
            sim.setJointPosition(elbowJointHandles[i],0)
        end
    end
end

function coroutineMain()
    sim.setThreadAutomaticSwitch(false)
    for journey=1,maxJourneys,1 do
        if journey==1 then
            sim.wait(startPosInitialPause)
        else
            sim.wait(startPosPause)
        end
        local dist=0
        local st=sim.getSimulationTime()
        while dist<pathL do
            local dt=sim.getSimulationTime()-st
            dist=velocity*dt
            local p=sim.getPathInterpolatedConfig(pathPositions,pathLengths,dist)
            p=sim.multiplyVector(pathM,p)
            local pp=sim.getObjectPosition(BillHandle,-1)
            local dx=p[1]-pp[1]
            local dy=p[2]-pp[2]
            sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
            sim.setObjectPosition(BillHandle,-1,p)
            moveBody(dist)
            sim.switchThread()
        end
    
        moveBody()
        sim.wait(goalPosPause)
        
        local dist=pathL
        local st=sim.getSimulationTime()
        while dist>0 do
            local dt=sim.getSimulationTime()-st
            dist=pathL-velocity*dt
            local p=sim.getPathInterpolatedConfig(pathPositions,pathLengths,dist)
            p=sim.multiplyVector(pathM,p)
            local pp=sim.getObjectPosition(BillHandle,-1)
            local dx=p[1]-pp[1]
            local dy=p[2]-pp[2]
            sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
            sim.setObjectPosition(BillHandle,-1,p)
            moveBody(dist)
            sim.switchThread()
        end
        moveBody()
    end
end

function sysCall_init() 
    BillHandle=sim.getObjectHandle('Bill_base')
    legJointHandles={sim.getObjectHandle('Bill_leftLegJoint'),sim.getObjectHandle('Bill_rightLegJoint')}
    kneeJointHandles={sim.getObjectHandle('Bill_leftKneeJoint'),sim.getObjectHandle('Bill_rightKneeJoint')}
    ankleJointHandles={sim.getObjectHandle('Bill_leftAnkleJoint'),sim.getObjectHandle('Bill_rightAnkleJoint')}
    shoulderJointHandles={sim.getObjectHandle('Bill_leftShoulderJoint'),sim.getObjectHandle('Bill_rightShoulderJoint')}
    elbowJointHandles={sim.getObjectHandle('Bill_leftElbowJoint'),sim.getObjectHandle('Bill_rightElbowJoint')}
    neckJoint=sim.getObjectHandle('Bill_neck')
    pathHandle=sim.getObjectHandle('Bill_path')
    
    local pathData=sim.unpackDoubleTable(sim.readCustomDataBlock(pathHandle,'PATH'))
    local m=Matrix(#pathData//7,7,pathData)
    pathPositions=m:slice(1,1,m:rows(),3):data()
    pathLengths,pathL=sim.getPathLengths(pathPositions,3)
    pathM=sim.getObjectMatrix(pathHandle,-1)

    
    velocity=1
    setRandomColors=true
    startPosInitialPause=0
    startPosPause=3
    goalPosPause=3
    maxJourneys=9999
   
    -- Now we set random colors:
    HairColors={4,{0.30,0.22,0.14},{0.75,0.75,0.75},{0.075,0.075,0.075},{0.75,0.68,0.23}}
    skinColors={2,{0.91,0.84,0.75},{0.52,0.45,0.35}}
    shirtColors={5,{0.37,0.46,0.74},{0.54,0.27,0.27},{0.31,0.51,0.33},{0.46,0.46,0.46},{0.18,0.18,0.18}}
    trouserColors={2,{0.4,0.34,0.2},{0.12,0.12,0.12}}
    shoeColors={2,{0.12,0.12,0.12},{0.25,0.12,0.045}}
    if setRandomColors then
        math.randomseed(sim.getFloatParam(sim.floatparam_rand)*10000) -- each lua instance should start with a different and 'good' seed
        setColor('HAIR',HairColors[1+math.random(HairColors[1])])
        setColor('SKIN',skinColors[1+math.random(skinColors[1])])
        setColor('SHIRT',shirtColors[1+math.random(shirtColors[1])])
        setColor('TROUSERS',trouserColors[1+math.random(trouserColors[1])])
        setColor('SHOE',shoeColors[1+math.random(shoeColors[1])])
    end

    -- Data for body movement:
    timings={0,1/21,2/21,3/21,4/21,5/21,6/21,7/21,8/21,9/21,10/21,11/21,12/21,13/21,14/21,15/21,16/21,17/21,18/21,19/21,20/21,21/21}
    legWaypoints={0.237,0.228,0.175,-0.014,-0.133,-0.248,-0.323,-0.450,-0.450,-0.442,-0.407,-0.410,-0.377,-0.303,-0.178,-0.111,-0.010,0.046,0.104,0.145,0.188,0.237}
    kneeWaypoints={0.282,0.403,0.577,0.929,1.026,1.047,0.939,0.664,0.440,0.243,0.230,0.320,0.366,0.332,0.269,0.222,0.133,0.089,0.065,0.073,0.092,0.282}
    ankleWaypoints={-0.133,0.041,0.244,0.382,0.304,0.232,0.266,0.061,-0.090,-0.145,-0.043,0.041,0.001,0.011,-0.099,-0.127,-0.121,-0.120,-0.107,-0.100,-0.090,-0.133}
    shoulderWaypoints={0.028,0.043,0.064,0.078,0.091,0.102,0.170,0.245,0.317,0.337,0.402,0.375,0.331,0.262,0.188,0.102,0.094,0.086,0.080,0.051,0.058,0.028}
    elbowWaypoints={-1.148,-1.080,-1.047,-0.654,-0.517,-0.366,-0.242,-0.117,-0.078,-0.058,-0.031,-0.001,-0.009,0.008,-0.108,-0.131,-0.256,-0.547,-0.709,-0.813,-1.014,-1.148}
    
    corout=coroutine.create(coroutineMain)
end
 
function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
end 

function sysCall_cleanup() 
    -- Restore to initial colors:
    setColor('HAIR',HairColors[2])
    setColor('SKIN',skinColors[2])
    setColor('SHIRT',shirtColors[2])
    setColor('TROUSERS',trouserColors[2])
    setColor('SHOE',shoeColors[2])
end

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Tasks sequence

Post by Leizah »

Thank you so much!
I've an issue with that code, now Bill stops when he has to and waits for the signal from the robot, but when the robot ends its task Bill just "pops" into the final position.
He doesn't move from when he stopped, he "desappears" from that position and reappear in an other position of the path.
How could I fix this?

Again, thank you for you support
Cheers

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

Re: Tasks sequence

Post by coppelia »

If you have a wait in there somewhere, you should adjust the st variable, i.e. distance on the path is calculated with:

Code: Select all

local dt=sim.getSimulationTime()-st
dist=velocity*dt
where st is the start time. If you wait somewhere, then you'll have to correct the start time with the waiting time, e.g.:

Code: Select all

    ...
    local st=sim.getSimulationTime()
    while dist<pathL do
        st=st+waitFunction()
        local dt=sim.getSimulationTime()-st
        dist=velocity*dt
        local p=sim.getPathInterpolatedConfig(pathPositions,pathLengths,dist)
        p=sim.multiplyVector(pathM,p)
        local pp=sim.getObjectPosition(BillHandle,-1)
        local dx=p[1]-pp[1]
        local dy=p[2]-pp[2]
        sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
        sim.setObjectPosition(BillHandle,-1,p)
        moveBody(dist)
        sim.switchThread()
    end
    ...

function waitFunction
    local st=sim.getSimulationTime()
    sim.waitForSignal('mySignal')
    return sim.getSimulationTime()-st
end
Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Tasks sequence

Post by Leizah »

Thank you, it works just as I wanted!

Even if the original question is now solved, I would like to know if it's possible, using the threated script, to evaluate the position into the path of a point with a command.
In other words, the value of 60% that I've set has been found after many tries, and if I move one of the contol point Bill will not stop where I need.
I would like to get something like "When you're on the point of the path closest to the object A, stop and turn to that object".
I tried with the non-threated script using the sim.getClosestPosOnPath function, but I was not able to find out how to achieve this result (The function returned always a value of 0).

However, thank you again for you help with the task sequence issue.
Cheers

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

Re: Tasks sequence

Post by coppelia »

In next release (i.e. V4.3.0), the Bill on path model will be able to do that. You can already get it here.

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Tasks sequence

Post by Leizah »

Thank you, it works just as I wanted!

I'm just struggling on how to merge this model with the task sequence code, because this model allows me to stop and modify orientation, but I need to set conditions in order to trigger the signals.

EDIT
I'm using the following code:

Code: Select all

 while dist<pathL and dist<pathL*Stop_Condition do
            local dt=sim.getSimulationTime()-st
            dist=velocity*dt
            st=st+handlePossiblePause(1,dist)
            local p=sim.getPathInterpolatedConfig(pathPositions,pathLengths,dist)
            p=sim.multiplyVector(pathM,p)
            local pp=sim.getObjectPosition(BillHandle,-1)
            local dx=p[1]-pp[1]
            local dy=p[2]-pp[2]
            sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
            sim.setObjectPosition(BillHandle,-1,p)
            moveBody(dist)
            sim.switchThread()
        end
        
        sim.setIntegerSignal('Human_TP',1)
        
        while dist<pathL and dist>pathL*Stop_Condition do
            local dt=sim.getSimulationTime()-st
            dist=velocity*dt
            st=st+waitFunction()+handlePossiblePause(1,dist)
            local p=sim.getPathInterpolatedConfig(pathPositions,pathLengths,dist)
            p=sim.multiplyVector(pathM,p)
            local pp=sim.getObjectPosition(BillHandle,-1)
            local dx=p[1]-pp[1]
            local dy=p[2]-pp[2]
            sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
            sim.setObjectPosition(BillHandle,-1,p)
            moveBody(dist)
            sim.switchThread()
        end
    
        moveBody()
        sim.wait(goalPosPause)
        
        ...
        
        function sysCall_init()
        Stop_Point=sim.getObjectHandle('Stop_Point_4')
        Stop_Point_Coords=sim.getObjectPosition(Stop_Point,-1)
        
        Stop_Point_Path=sim.getClosestPosOnPath(pathPositions,pathLegths,Stop_Point_Coords)
        
        Stop_Condition=Stop_Point_Path/PathL
        
It works, but I cannot make Bill stop when he reaches the pause point.
He walks, then stops when he reaches Stop_Condition I set, then he waits for the robot signal and the, when he restarts, pauses again because he reaches the pausePoint. I would like to make the two pauses to "merge", in order to get a more precise movement.
There's any way to do that?

I also have another problem, I set 4 pausePoints, with the second and the forth in the same position (but obtained using different elements), I set all dir values to 3 (when I set it to 2 the model doesn't stop), but Bill doesn't stop when he reaches the last point.
He basically stops only on the first 3 pausePoints, missing the last one.
How could I fix this?

Again, thank you so much for your support.
Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Tasks sequence

Post by Leizah »

I'm sorry to re-up this topic, but I'm still struggling with the issue described into the previous answer and I would like to know If I I can set to "avoid" the 4th stop point during the first part of the travel of Bill.

Post Reply