## Bill on path model interaction with objects

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

### Bill on path model interaction with objects

Hi,

I'm using the Bill on Path model, I would like to make it pick up some objects from the scene, to carry them along the path and to release them on the floor at the end of the path.
How could I get this kind of interaction? There's some tutorial scene that I could use as reference?

coppelia
Posts: 8881
Joined: 14 Dec 2012, 00:25

### Re: Bill on path model interaction with objects

Hello,

you can attach the object to the human model via sim.setObjectParent, if the object is not dynamically enabled. Otherwise, you should first attach a force sensor to the human model, and attach the shape to that sensor, also via sim.setObjectParent.

Cheers

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

### Re: Bill on path model interaction with objects

Maybe I put the question wrong, I have a Bill on Path model (The one that has also stop points from v2.3) that walks through his path, while the object stands near a certain point of the path (around 30%).
I would like to have that, when Bill reaches the position of the object, he picks up it with both hands. Then, he has to carry this object until he reaches an other point of the path (around 45%), when he has to lift the object in a specific pose.

There is not parent-child relation between Bill and the object, but I've not understood how your solution works...

Cheers

coppelia
Posts: 8881
Joined: 14 Dec 2012, 00:25

### Re: Bill on path model interaction with objects

If you need the human model to move his two hands to pick up something, you'll first have to heavily modify that model. Currently the model does simple keyframe interpolation for the walking movement.

Cheers

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

### Re: Bill on path model interaction with objects

What should I modify in order to get this result or, at least, a pick operation with just one hand?
I don't need the actual pick operation, I just need that the operation looks like a pick, so I could also just set that, when Bill reaches the pick-up point, he extends his arms towards the object and takes this pose until he reaches the lift point.
There's a way to get this?

Cheers

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

### Re: Bill on path model interaction with objects

Hi,

I'm considering the option to split the original path in different sub-paths, in order to obtain the following sequence:

PRESENT SEQUENCE (SINGLE PATH):
- At 30% pick up the object
- At 45% lift the object
- At 75% pick up the object
- At 100% lift the object

PROPOSED SEQUENCE (MULTIPLE PATHS)
PATH 1 (0% to 30% of the single path)
- Bill follows the path using the existing code of the 'Bill on path' model. He then stops at the end of the path.

PATH 2 (30% to 45% of the single path)
- During the walk, Bill arms are placed alongside the bust, with the forearms at 90°, in order to simulate the carry phase. He then stops at the end of the path.

PATH 3 (45% to 75% of the single path)
- Bill follows the path using the existing code of the 'Bill on path' model. He then stops at the end of the path.

PATH 4 (75% to 100% of the single path)
- During the walk, Bill arms are placed alongside the bust, with the forearms at 90°, in order to simulate the carry phase. He then stops at the end of the path.

Is it possible to make this path-division? And how to manage it into the code?

Cheers

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

### Re: Bill on path model interaction with objects

Hi, I've tried to develop the solution that I've proposed into the previous answer, but I got some problems related to the time sequence.
Following the tips provided into this topic, I made several wait functions, but the sequence is still "shattered" (Bill continues to walk "invisible" and appears in different positions of the paths).
Also at the beginning of the movements, there are some glitchy movements.
There are 5 subpaths, with the ending point of the n path coincident with the starting point of the n+1 path.
Here's the code I used:

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
end

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 setPausePointVisibility(l)
for i=1,#pausePoints,1 do
local pp=pausePoints[i]
sim.setObjectInt32Param(pp.handle,sim.objintparam_visibility_layer,l)
end
end

function handlePossiblePause_Path_1(op,dist)
local st=sim.getSimulationTime()
if op==0 then
for i=1,#pausePoints,1 do
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local d=sim.getClosestPosOnPath(path1Positions,path1Lengths,p)
pausePoints[i].dist=d
pausePoints[i].visited=false
end
else
for i=1,#pausePoints,1 do
local pp=pausePoints[i]
if not pp.visited and (op&pp.dir~=0) then
if (op==1 and dist>pp.dist) or (op==2 and dist<pp.dist) then
pp.visited=true
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local b=sim.getObjectPosition(BillHandle,-1)
local dx=p[1]-b[1]
local dy=p[2]-b[2]
sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
moveBody()
sim.wait(pp.duration)
end
end
end
end
return sim.getSimulationTime()-st
end

function handlePossiblePause_Path_2(op,dist)
local st=sim.getSimulationTime()
if op==0 then
for i=1,#pausePoints,1 do
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local d=sim.getClosestPosOnPath(path2Positions,path2Lengths,p)
pausePoints[i].dist=d
pausePoints[i].visited=false
end
else
for i=1,#pausePoints,1 do
local pp=pausePoints[i]
if not pp.visited and (op&pp.dir~=0) then
if (op==1 and dist>pp.dist) or (op==2 and dist<pp.dist) then
pp.visited=true
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local b=sim.getObjectPosition(BillHandle,-1)
local dx=p[1]-b[1]
local dy=p[2]-b[2]
sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
moveBody()
sim.wait(pp.duration)
end
end
end
end
return sim.getSimulationTime()-st
end

function handlePossiblePause_Path_3(op,dist)
local st=sim.getSimulationTime()
if op==0 then
for i=1,#pausePoints,1 do
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local d=sim.getClosestPosOnPath(path3Positions,path3Lengths,p)
pausePoints[i].dist=d
pausePoints[i].visited=false
end
else
for i=1,#pausePoints,1 do
local pp=pausePoints[i]
if not pp.visited and (op&pp.dir~=0) then
if (op==1 and dist>pp.dist) or (op==2 and dist<pp.dist) then
pp.visited=true
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local b=sim.getObjectPosition(BillHandle,-1)
local dx=p[1]-b[1]
local dy=p[2]-b[2]
sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
moveBody()
sim.wait(pp.duration)
end
end
end
end
return sim.getSimulationTime()-st
end

function handlePossiblePause_Path_4(op,dist)
local st=sim.getSimulationTime()
if op==0 then
for i=1,#pausePoints,1 do
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local d=sim.getClosestPosOnPath(path4Positions,path4Lengths,p)
pausePoints[i].dist=d
pausePoints[i].visited=false
end
else
for i=1,#pausePoints,1 do
local pp=pausePoints[i]
if not pp.visited and (op&pp.dir~=0) then
if (op==1 and dist>pp.dist) or (op==2 and dist<pp.dist) then
pp.visited=true
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local b=sim.getObjectPosition(BillHandle,-1)
local dx=p[1]-b[1]
local dy=p[2]-b[2]
sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
moveBody()
sim.wait(pp.duration)
end
end
end
end
return sim.getSimulationTime()-st
end

function handlePossiblePause_Path_5(op,dist)
local st=sim.getSimulationTime()
if op==0 then
for i=1,#pausePoints,1 do
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local d=sim.getClosestPosOnPath(path5Positions,path5Lengths,p)
pausePoints[i].dist=d
pausePoints[i].visited=false
end
else
for i=1,#pausePoints,1 do
local pp=pausePoints[i]
if not pp.visited and (op&pp.dir~=0) then
if (op==1 and dist>pp.dist) or (op==2 and dist<pp.dist) then
pp.visited=true
local p=sim.getObjectPosition(pausePoints[i].handle,-1)
local b=sim.getObjectPosition(BillHandle,-1)
local dx=p[1]-b[1]
local dy=p[2]-b[2]
sim.setObjectOrientation(BillHandle,-1,{0,0,math.atan2(dy,dx)})
moveBody()
sim.wait(pp.duration)
end
end
end
end
return sim.getSimulationTime()-st
end

function coroutineMain()
for journey=1,maxJourneys,1 do
if journey==1 then
sim.wait(startPosInitialPause)
else
sim.wait(startPosPause)
end
handlePossiblePause_Path_1(0)
handlePossiblePause_Path_2(0)
handlePossiblePause_Path_3(0)
handlePossiblePause_Path_4(0)
handlePossiblePause_Path_5(0)
local dist=0
local st=sim.getSimulationTime()

while dist<=path1L do
local dt=sim.getSimulationTime()-st
dist=velocity*dt
st=st+handlePossiblePause_Path_1(1,dist)
local p=sim.getPathInterpolatedConfig(path1Positions,path1Lengths,dist)
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

-- Hide workpiece
sim.setIntegerSignal('Enter_Reached',1)

while dist<=path2L and dist>path1L  do
local dt=sim.getSimulationTime()-st
dist=velocity*dt
st=st+waitFunction_Enter()+handlePossiblePause_Path_2(1,dist)
local p=sim.getPathInterpolatedConfig(path2Positions,path2Lengths,dist)
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

-- Hide workpiece
sim.setIntegerSignal('Object_Reached',1)
-- Change shirt color
TorsoOriginalColorData=sim.changeEntityColor(ShirtTorsoHandle,{0.37,0.46,0.74},sim.colorcomponent_ambient_diffuse)
LeftShoulderOriginalColorData=sim.changeEntityColor(ShirtLeftShoulderHandle,{0.37,0.46,0.74},sim.colorcomponent_ambient_diffuse)
LeftArmOriginalColorData=sim.changeEntityColor(ShirtLeftArmHandle,{0.37,0.46,0.74},sim.colorcomponent_ambient_diffuse)
RightShoulderOriginalColorData=sim.changeEntityColor(ShirtRightShoulderHandle,{0.37,0.46,0.74},sim.colorcomponent_ambient_diffuse)
RightArmOriginalColorData=sim.changeEntityColor(ShirtRightArmHandle,{0.37,0.46,0.74},sim.colorcomponent_ambient_diffuse)
BeltOriginalColorData=sim.changeEntityColor(ShirtBeltHandle,{0.37,0.46,0.74},sim.colorcomponent_ambient_diffuse)

while dist<=path3L and dist>path2L do
local dt=sim.getSimulationTime()-st
dist=velocity*dt
st=st+waitFunction_Object_1()+handlePossiblePause_Path_3(1,dist)
local p=sim.getPathInterpolatedConfig(path3Positions,path3Lengths,dist)
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<=path4L and dist>path3L do
local dt=sim.getSimulationTime()-st
dist=velocity*dt
st=st+waitFunction_Robot()+handlePossiblePause_Path_4(1,dist)
local p=sim.getPathInterpolatedConfig(path4Positions,path4Lengths,dist)
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.setIntegerSignal('Exit_Reached',1)
sim.wait(goalPosPause)

local dist=path5L
local st=sim.getSimulationTime()
while dist>0 do
local dt=sim.getSimulationTime()-st
dist=path5L-velocity*dt
st=st+handlePossiblePause_Path_5(2,dist)
local p=sim.getPathInterpolatedConfig(path5Positions,path5Lengths,dist)
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')
path1Handle=sim.getObjectHandle('Bill_path_1')
path2Handle=sim.getObjectHandle('Bill_path_2')
path3Handle=sim.getObjectHandle('Bill_path_3')
path4Handle=sim.getObjectHandle('Bill_path_4')
path5Handle=sim.getObjectHandle('Bill_path_5')

ShirtTorsoHandle=sim.getObjectHandle('Mesh17')
ShirtLeftShoulderHandle=sim.getObjectHandle('Group28')
ShirtLeftArmHandle=sim.getObjectHandle('Group30')
ShirtRightShoulderHandle=sim.getObjectHandle('Group33')
ShirtRightArmHandle=sim.getObjectHandle('Group7')
ShirtBeltHandle=sim.getObjectHandle('Group8')

pausePoints={}
pausePoints[1]={handle=sim.getObjectHandle('Bill_Enter_Pause'),duration=3,dir=3}
pausePoints[2]={handle=sim.getObjectHandle('Bill_Object_Pause_1'),duration=3,dir=3}
pausePoints[3]={handle=sim.getObjectHandle('Bill_TP_Pause'),duration=3,dir=3}
pausePoints[4]={handle=sim.getObjectHandle('Bill_Object_Pause_2'),duration=3,dir=0}
setPausePointVisibility(0)

StandHandle=sim.getObjectHandle('Stand_Enter')

-- Path_1
local m=Matrix(#path1Data//7,7,path1Data)
path1Pose=sim.getObjectPose(path1Handle,-1)
path1Positions=m:slice(1,1,m:rows(),3):data()
path1Positions=sim.multiplyVector(path1Pose,path1Positions)
path1Lengths,path1L=sim.getPathLengths(path1Positions,3)

-- Path_2
local m=Matrix(#path2Data//7,7,path2Data)
path2Pose=sim.getObjectPose(path2Handle,-1)
path2Positions=m:slice(1,1,m:rows(),3):data()
path2Positions=sim.multiplyVector(path2Pose,path2Positions)
path2Lengths,path2L=sim.getPathLengths(path2Positions,3)

-- Path_3
local m=Matrix(#path3Data//7,7,path3Data)
path3Pose=sim.getObjectPose(path3Handle,-1)
path3Positions=m:slice(1,1,m:rows(),3):data()
path3Positions=sim.multiplyVector(path3Pose,path3Positions)
path3Lengths,path3L=sim.getPathLengths(path3Positions,3)

-- Path_4
local m=Matrix(#path4Data//7,7,path4Data)
path4Pose=sim.getObjectPose(path4Handle,-1)
path4Positions=m:slice(1,1,m:rows(),3):data()
path4Positions=sim.multiplyVector(path4Pose,path4Positions)
path4Lengths,path4L=sim.getPathLengths(path4Positions,3)

-- Path_5
local m=Matrix(#path5Data//7,7,path5Data)
path5Pose=sim.getObjectPose(path5Handle,-1)
path5Positions=m:slice(1,1,m:rows(),3):data()
path5Positions=sim.multiplyVector(path5Pose,path5Positions)
path5Lengths,path5L=sim.getPathLengths(path5Positions,3)

velocity=1
setRandomColors=false
startPosInitialPause=0
startPosPause=3
goalPosPause=3
maxJourneys=99999

-- 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 waitFunction_Enter()
local st=sim.getSimulationTime()
sim.waitForSignal('Enter_Reached')
return sim.getSimulationTime()-st
end

function waitFunction_Object_1()
local st=sim.getSimulationTime()
sim.waitForSignal('Object_Reached')
return sim.getSimulationTime()-st
end

function waitFunction_TP()
local st=sim.getSimulationTime()
sim.waitForSignal('TP_Reached')
return sim.getSimulationTime()-st
end

function waitFunction_Robot()
local st=sim.getSimulationTime()
sim.waitForSignal('Robot_Finish')
return sim.getSimulationTime()-st
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()
setPausePointVisibility(1)
-- 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

I hope someone can help me.
Cheers.

coppelia
Posts: 8881
Joined: 14 Dec 2012, 00:25

### Re: Bill on path model interaction with objects

Hello again,

you will likely get more replies if you post a simple, short question. Expecting people to read through your algorithm, understand it, reproduce and test it, does usually lead to very few replies, because people willing to spend more than 5 minutes on a reply are rare.

cheers

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

### Re: Bill on path model interaction with objects

coppelia wrote:
04 Oct 2021, 08:55
Hello again,

you will likely get more replies if you post a simple, short question. Expecting people to read through your algorithm, understand it, reproduce and test it, does usually lead to very few replies, because people willing to spend more than 5 minutes on a reply are rare.

cheers