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.
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.
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
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()
sim.setThreadAutomaticSwitch(false)
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)
sim.switchThread()
end
-- Hide workpiece
sim.setIntegerSignal('Enter_Reached',1)
sim.switchThread()
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)
sim.switchThread()
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)
sim.switchThread()
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)
sim.switchThread()
end
sim.setIntegerSignal('Human_TP',1)
sim.switchThread()
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)
sim.switchThread()
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)
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')
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 path1Data=sim.unpackDoubleTable(sim.readCustomDataBlock(path1Handle,'PATH'))
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 path2Data=sim.unpackDoubleTable(sim.readCustomDataBlock(path2Handle,'PATH'))
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 path3Data=sim.unpackDoubleTable(sim.readCustomDataBlock(path3Handle,'PATH'))
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 path4Data=sim.unpackDoubleTable(sim.readCustomDataBlock(path4Handle,'PATH'))
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 path5Data=sim.unpackDoubleTable(sim.readCustomDataBlock(path5Handle,'PATH'))
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()
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()
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.