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