.
Code: Select all
-- do some initialization here
-- Get handles and postions of dummies
tip = sim.getObjectHandle("tip")
ownHandle = sim.getObjectHandle("Target")
startPath1=sim.getObjectHandle("Path1_1")
startPath2=sim.getObjectHandle("Path1_2")
mittelPath1=sim.getObjectHandle("Path2_1")
mittelPath2=sim.getObjectHandle("Path2_2")
endPath1=sim.getObjectHandle("Path3_1")
endPath2=sim.getObjectHandle("Path3_2")
pathLength1=sim.getPathLength(startPath1)
pathLength2=sim.getPathLength(startPath2)
pathLength3=sim.getPathLength(mittelPath1)
pathLength4=sim.getPathLength(mittelPath2)
pathLength5=sim.getPathLength(endPath1)
pathLength6=sim.getPathLength(endPath2)
speed=0.02 -- end-effector velocity in m/s
sim.setUserParameter(ownHandle,'@enable','')
timestep=sim.getSimulationTimeStep() -- in s
currentPositionRelative1=0
currentPositionRelative2=0
currentPositionRelative3=0
currentPositionRelative4=0
currentPositionRelative5=0
currentPositionRelative6=0
moveForward=true
epsilon1=0.01
epsilon2=0.01
epsilon3=0.01
epsilon4=0.01
epsilon5=0.01
epsilon6=0.01
function sysCall_threadmain()
-- Put some initialization code here
while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
slowerWork=sim.getStringSignal("slowerWork")
if slowerWork=="1" then
slowerWork=true
else
slowerWork=false
end
-- path1
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
if currentPositionRelative1 < 1-epsilon1 and moveForward then
currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
if currentPositionRelative1 < 1-epsilon1 and moveForward then
currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
-- path2
if currentPositionRelative1 >= 1-epsilon1 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength2
if currentPositionRelative2 < 1-epsilon2 and moveForward then
currentPositionRelative2=currentPositionRelative2+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath2,currentPositionRelative2)
pathOrientation=sim.getOrientationOnPath(startPath2,currentPositionRelative2)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength2
if currentPositionRelative2 < 1-epsilon2 and moveForward then
currentPositionRelative2=currentPositionRelative2+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath2,currentPositionRelative2)
pathOrientation=sim.getOrientationOnPath(startPath2,currentPositionRelative2)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path3
if currentPositionRelative2 >= 1-epsilon2 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength3
if currentPositionRelative3 < 1-epsilon3 and moveForward then
currentPositionRelative3=currentPositionRelative3+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath1,currentPositionRelative3)
pathOrientation=sim.getOrientationOnPath(mittelPath1,currentPositionRelative3)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength3
if currentPositionRelative3 < 1-epsilon3 and moveForward then
currentPositionRelative3=currentPositionRelative3+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath1,currentPositionRelative3)
pathOrientation=sim.getOrientationOnPath(mittelPath1,currentPositionRelative3)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path4
if currentPositionRelative3 >= 1-epsilon3 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength4
if currentPositionRelative4 < 1-epsilon4 and moveForward then
currentPositionRelative4=currentPositionRelative3+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath1,currentPositionRelative4)
pathOrientation=sim.getOrientationOnPath(endPath1,currentPositionRelative4)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength4
if currentPositionRelative4 < 1-epsilon4 and moveForward then
currentPositionRelative4=currentPositionRelative4+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath1,currentPositionRelative4)
pathOrientation=sim.getOrientationOnPath(endPath1,currentPositionRelative4)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path5
if currentPositionRelative4 >= 1-epsilon4 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength5
if currentPositionRelative5 < 1-epsilon5 and moveForward then
currentPositionRelative5=currentPositionRelative5+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath2,currentPositionRelative5)
pathOrientation=sim.getOrientationOnPath(endPath2,currentPositionRelative5)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength5
if currentPositionRelative5 < 1-epsilon5 and moveForward then
currentPositionRelative5=currentPositionRelative5+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath2,currentPositionRelative5)
pathOrientation=sim.getOrientationOnPath(endPath2,currentPositionRelative5)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path6
if currentPositionRelative5 >= 1-epsilon5 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength6
if currentPositionRelative6 < 1-epsilon6 and moveForward then
currentPositionRelative6=currentPositionRelative6+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath2,currentPositionRelative6)
pathOrientation=sim.getOrientationOnPath(mittelPath2,currentPositionRelative6)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength6
if currentPositionRelative6 < 1-epsilon6 and moveForward then
currentPositionRelative6=currentPositionRelative6+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath2,currentPositionRelative6)
pathOrientation=sim.getOrientationOnPath(mittelPath2,currentPositionRelative6)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
if currentPositionRelative6 >= 1-epsilon6 then
currentPositionRelative1=0
currentPositionRelative2=0
currentPositionRelative3=0
currentPositionRelative4=0
currentPositionRelative5=0
currentPositionRelative6=0
end
end
end
function sysCall_cleanup()
-- Put some clean-up code here
end
-- See the user manual or the available code snippets for additional callback functions and details