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

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
[*]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
Posts: 8893
Joined: 14 Dec 2012, 00:25

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()
local ok,errorMsg=coroutine.resume(corout)
if errorMsg then
error(debug.traceback(corout,errorMsg),2)
end
end
end

function coroutineMain()

sim.setInt32Signal("syncSignal",1)
while sim.getInt32Signal("syncSignal")==1 do
end

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

-- etc.
end

Code: Select all

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

function sysCall_actuation()
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
end

local s=sim.getInt32Signal("syncSignal")+1
while sim.getInt32Signal("syncSignal")==s do
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()
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
end

canMove=true
sim.wait(5)
canMove=false

local s=sim.getInt32Signal("syncSignal")+1
while sim.getInt32Signal("syncSignal")==s do
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

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
Posts: 8893
Joined: 14 Dec 2012, 00:25

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()
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)
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)
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 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()
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

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
Posts: 8893
Joined: 14 Dec 2012, 00:25

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)
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

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
Posts: 8893
Joined: 14 Dec 2012, 00:25

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

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)
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)
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