I don't really understand how I have the problem.
I have the subscriber in a loop which breaks once the callback receives some data:
and then after this, it runs the path finding and execution. So I don't understand how having the publisher still running slows down the motion of the arm but it speeds up as soon as I kill the python script.
Would you be able to explain more what this means in terms of what I should do?
Code: Select all
function sysCall_init()
rosInterfacePresent=simROS
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
--************************* ROS pub/sub functions ****************************************
function py_cop_callback(msg) -- retrieves data published by python
pydata=nil
pydata=(msg.data)
if pydata then
recieved_data=true -- has recieved new data from python
end
end
function recieve_data() -- subscribes to python
recieved_data=false
while not (recieved_data==true) do
py_cop_sub=simROS.subscribe('/new_pose','std_msgs/Float64MultiArray','py_cop_callback')
end
return pydata
end
--****************Collision Avoidance ****************************************
function visualizePath(path) --draws the path
if not _lineContainer then
_lineContainer=sim.addDrawingObject(sim.drawing_lines,3,0,-1,99999,{1,1,0})
end
sim.addDrawingObjectItem(_lineContainer,nil)
if path then
local lb=sim.setThreadAutomaticSwitch(false)
local initConfig=getConfig()
local l=#jh
local pc=#path/l
for i=1,pc-1,1 do
local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7],path[(i-1)*l+8]}
local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7],path[i*l+8]}
setConfig(config1)
local lineDat=sim.getObjectPosition(simTip,-1)
setConfig(config2)
local p=sim.getObjectPosition(simTip,-1)
lineDat[4]=p[1]
lineDat[5]=p[2]
lineDat[6]=p[3]
sim.addDrawingObjectItem(_lineContainer,lineDat)
end
setConfig(initConfig)
sim.setThreadAutomaticSwitch(lb)
end
sim.switchThread()
end
--************** Path Finding Functions ****************************************
function _getJointPosDifference(startValue,goalValue,isRevolute)
local dx=goalValue-startValue
if (isRevolute) then
if (dx>=0) then
dx=math.mod(dx+math.pi,2*math.pi)-math.pi
else
dx=math.mod(dx-math.pi,2*math.pi)+math.pi
end
end
return(dx)
end
function _applyJoints(jointHandles,joints)
for i=1,#jointHandles,1 do
sim.setJointTargetPosition(jointHandles[i],joints[i])
end
end
function generatePathLengths(path)
-- Returns a table that contains a distance along the path for each path point
local d=0
local l=#jh
local pc=#path/l
local retLengths={0}
for i=1,pc-1,1 do
local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7],path[(i-1)*l+8]}
local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7],path[i*l+8]}
d=d+getConfigConfigDistance(config1,config2)
retLengths[i+1]=d
end
return retLengths
end
function validationCb(config,auxData)
collisionColors={{1,0,0},{1,0,1}} -- collider and collidee
local retVal=true
local prev={}
for i=1,#jh,1 do
prev[i]=sim.getJointPosition(jh[i])
sim.setJointPosition(jh[i],config[i])
end
for i=1,#collisionPairs/2,1 do
local result,pairHandles=sim.checkCollision(collisionPairs[(i-1)*2+1],collisionPairs[(i-1)*2+2])
if result>0 then
print('Robot is colliding. Colliding pair is '..sim.getObjectName(pairHandles[1])..','..sim.getObjectName(pairHandles[2]))
-- Change color of the collider and collidee objects:
-- changePairColor({pairHandles[1],pairHandles[2]},collisionColors)
retVal=false
break
else
--print('not colliding')
end
end
for i=1,#jh,1 do
sim.setJointPosition(jh[i],prev[i])
end
return retVal
end
function getConfig()
-- Returns the current robot configuration
local config={}
for i=1,#jh,1 do
config[i]=sim.getJointPosition(jh[i])
end
return config
end
function setConfig(config)
-- Applies the specified configuration to the robot
if config then
for i=1,#jh,1 do
sim.setJointPosition(jh[i],config[i])
end
end
end
function getConfigConfigDistance(config1,config2)
-- Returns the distance (in configuration space) between two configurations
local d=0
for i=1,#jh,1 do
local dx=config1[i]-config2[i]
d=d+dx*dx
end
return math.sqrt(d)
end
function getPathLength(path)
-- Returns the length of the path in configuration space
local d=0
local l=#jh --number of joints
local pc=#path/l -- path is list of joint agles; 1pc = 1 pose set of joint angles
for i=1,pc-1,1 do -- loop over all poses in path
local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7],path[(i-1)*l+8]} -- pose in list
local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7],path[i*l+8]} -- next pose in list
d=d+getConfigConfigDistance(config1,config2) -- each next pose in list adds the distance
end
return d
end
function _findPath(startConfig,goalConfig)
local jointLimitsL={-170,-120,-170,-120,-170,-120,-175,-90}
local jointLimitsH={170,120,170,120,170,120,175,90}
local task=simOMPL.createTask('task')
simOMPL.setAlgorithm(task,OMPLAlgo)
local jSpaces={}
for i=1,#jh,1 do
local proj=i
if i>3 then proj=0 end -- creates a projection for only the 1st 3 joints?
jSpaces[#jSpaces+1]=simOMPL.createStateSpace('j_space'..i,simOMPL.StateSpaceType.joint_position,jh[i],{jointLimitsL[i]},{jointLimitsH[i]},proj)
end
simOMPL.setStateSpace(task,jSpaces)
simOMPL.setCollisionPairs(task,collisionPairs)
simOMPL.setStartState(task,startConfig)
simOMPL.setGoalState(task,goalConfig)-- sets goal state
simOMPL.setup(task)
local l=nil
local res,path=simOMPL.compute(task,maxOMPLCalculationTime,-1,200) -- return a min of 200 states --
-- it finds path to each goal state but stops once it reaches goal (doesn't check all goal states)
if #path >0 then
visualizePath(path)
l=getPathLength(path)
end
simOMPL.destroyTask(task)
return path,l
end
function findPath(startConfig,goalConfig)
-- This function will search for a path between the specified start configuration,
-- and several of the specified goal configurations.
local onePath,onePathLength=_findPath(startConfig,goalConfig)
return onePath,generatePathLengths(onePath) -- returns path and table of point to point lenghts on path
end
function path_attempt(maxtry,pose)
returnpath={}
for t=1,maxtry,1 do
print('planning to config ')
path,lengths=findPath(initConfig,pose) -- using OMPL to find the path between the current pose and multiple valid goal poses
if (path and #path>0) then -- if a non zero path has been found
print('found a path')
pathfail=1 -- default to failed
poseinpath={}
for colcheck=1,#path,#jh do -- loops through path to see if any poses in path are colliding
for ja=0,#jh-1,1 do
poseinpath[ja+1] = path[colcheck+ja]
end
notcollide = validationCb(poseinpath)
if (notcollide == false) then --has collided
pathfail=1
print('found a path to the target but it collides ')
break -- out of collision check loop over this path
else
pathfail=0
end
end
if (pathfail==0) then -- no collision
print('found a valid path')
returnpath=path
break -- out of loop through try to plan a path for this pose
end
else
print('found no path to config')
end
end -- loop to next attempt out of maxtry
return returnpath
end
--******Move robot *****************************************************************************
function executeMotion(path,lengths,maxVel,maxAccel,maxJerk)
dt=sim.getSimulationTimeStep()
-- 1. Make sure we are not going too fast for each individual joint (i.e. calculate a correction factor (velCorrection)):
jointsUpperVelocityLimits={}
for j=1,8,1 do
jointsUpperVelocityLimits[j]=sim.getObjectFloatParam(jh[j],sim.jointfloatparam_upper_limit) -- gets the joint velocity limit from joint handle
end
velCorrection=1
sim.setThreadSwitchTiming(200)
while true do
posVelAccel={0,0,0}
targetPosVel={lengths[#lengths],0}
pos=0
res=0
previousQ={path[1],path[2],path[3],path[4],path[5],path[6],path[7],path[8]}
local rMax=0
rmlHandle=sim.rmlPos(1,0.0001,-1,posVelAccel,{maxVel*velCorrection,maxAccel,maxJerk},{1},targetPosVel)
while res==0 do
res,posVelAccel,sync=sim.rmlStep(rmlHandle,dt)
if (res>=0) then
l=posVelAccel[1]
for i=1,#lengths-1,1 do
l1=lengths[i]
l2=lengths[i+1]
if (l>=l1)and(l<=l2) then
t=(l-l1)/(l2-l1)
for j=1,8,1 do
q=path[8*(i-1)+j]+_getJointPosDifference(path[8*(i-1)+j],path[8*i+j],jt[j]==sim.joint_revolute_subtype)*t
dq=_getJointPosDifference(previousQ[j],q,jt[j]==sim.joint_revolute_subtype)
previousQ[j]=q
r=math.abs(dq/dt)/jointsUpperVelocityLimits[j]
if (r>rMax) then
rMax=r
end
end
break
end
end
end
end
sim.rmlRemove(rmlHandle)
if rMax>1.001 then
velCorrection=velCorrection/rMax
else
break
end
end
sim.setThreadSwitchTiming(2)
-- 2. Execute the movement:
posVelAccel={0,0,0}
targetPosVel={lengths[#lengths],0}
pos=0
res=0
jointPos={}
rmlHandle=sim.rmlPos(1,0.0001,-1,posVelAccel,{maxVel*velCorrection,maxAccel,maxJerk},{1},targetPosVel)
while res==0 do
dt=sim.getSimulationTimeStep()
res,posVelAccel,sync=sim.rmlStep(rmlHandle,dt)
if (res>=0) then
l=posVelAccel[1]
for i=1,#lengths-1,1 do
l1=lengths[i]
l2=lengths[i+1]
if (l>=l1)and(l<=l2) then
t=(l-l1)/(l2-l1)
for j=1,8,1 do
jointPos[j]=path[8*(i-1)+j]+_getJointPosDifference(path[8*(i-1)+j],path[8*i+j],jt[j]==sim.joint_revolute_subtype)*t
end
_applyJoints(jh,jointPos)
break
end
end
end
sim.switchThread()
end
sim.rmlRemove(rmlHandle)
end
-- *************************************START HERE:********************************************************************
function coroutineMain()
--** initialise send/recieve variables
pydata={}
recieved_data=false
--**joint setups
jh={-1,-1,-1,-1,-1,-1,-1,-1}
jt={-1,-1,-1,-1,-1,-1,-1,-1}
for i=1,8,1 do
jh[i]=sim.getObjectHandle('LBR_iiwa_14_j'..i)
jt[i]=sim.getJointType(jh[i])
end
--** IK setups
simBase=sim.getObjectHandle(sim.handle_self)
simTarget=sim.getObjectHandle('LBR_iiwa_target')
simTip=sim.getObjectHandle('LBR_iiwa_tip')
ikEnv=simIK.createEnvironment()
ikGroup=simIK.createIkGroup(ikEnv)
local ikElement,simToIkMap=simIK.addIkElementFromScene(ikEnv,ikGroup,simBase,simTip,simTarget,simIK.constraint_pose)
ikJoints={}
for i=1,#jh,1 do
ikJoints[i]=simToIkMap[jh[i]]
end
ikTip=simToIkMap[simTip]
--**Collision Pairs
local collection=sim.createCollection(0)
sim.addItemToCollection(collection,sim.handle_tree,simBase,0)
-- 2 collision pairs: the first for robot self-collision detection, the second for robot-environment collision detection:
collisionPairs={collection,collection,collection,sim.handle_all}
--** Trajectory setup
maxVel=0.1
maxAccel=0.1
maxJerk=80
ikSteps=20 --steps for IK trajectory
--** OMPL Parameters
maxOMPLCalculationTime=4 -- for one calculation. Higher is better, but takes more time
OMPLAlgo=simOMPL.Algorithm.KPIECE1 -- the OMPL algorithm to use
--** Start pose
initConfig=getConfig() -- function to find joint positions
print('found current robot pose')
--** Recieve joint angles for goal pose
print('waiting for goal poses')
recieve_data()
-- process poses array
poses_array=pydata
pose={}
e=1
for e=1,#jh,1 do
pose[e]=poses_array[e]*math.pi/180 -- convert to rad
end
print(pose)
--** For sent pose try up to a set number of times to find a path
maxtry=10
path_solution={}
path_solution=path_attempt(maxtry, pose)
if #path_solution > 0 then
visualizePath(path_solution)
executeMotion(path_solution,lengths,maxVel,maxAccel,maxJerk) --joint speed position controller
print('path motion completed')
else
print('no path found')
end
sim.wait(45)
sim.stopSimulation()
sim.wait(5)
end