I am working on a script trying to find motion plans using OMPL. I use python remote API to call functions in a non-threaded childscript in coppeliasim on the same machine, which is desired to return the planned path to python call. I used to make this happen (also able to save and load path to replay as shown in jaco demo but now failed and got error when loading the path saying wrong argument), but somehow now I can see the path is generated in coppeliasim but cannot be passed to python (verified this by printing path out).
Some thoughts and analysis: I got return state value as 3 which I checked might be no return value tag and timeout tag. The path length I got is 2100 which I think is not super long, and I indeed saw the function returns to python before the values are printed out in coppeliasim functions. Will the network be the issue? I guess on the same machine the speed should not be the problem, but is there a timer for remote call to return? Also I am using blocking mode and I guess it should be.
I attach my python caller function, coppeliasim motion planning function here for reference. Thanks advance for your help!
python:
Code: Select all
def plan(self, target_pose, pathfilename='latest_path', load_path=0, save_path=0):
emptyBuff = bytearray()
# not used
approach_vector = [0, 0, 0] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
max_configs_desired_pose = 10 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state
max_trials_config_search = 300 # a parameter needed for finding appropriate goal states
search_count = 30 # how many times OMPL will run for a given task
min_configs_path_planning = 400 # interpolation states for the OMPL path
min_configs_ik_path = 100 # interpolation states for the linear approach path
collision_checking = 1 # whether collision checking is on or off
# Do the path planning here (between a start state and a goal pose, including a linear approach phase):
inInts = [self.robot_target_handle, collision_checking, min_configs_ik_path, min_configs_path_planning,
max_configs_desired_pose, max_trials_config_search, search_count, load_path, save_path]
inFloats = target_pose.flatten().tolist() + approach_vector
# print('inInts', inInts)
# print('inFloats', inFloats)
res, retInts, path, retStrings, retBuffer = sim.simxCallScriptFunction(self.sim_client,
'remoteApiCommandServer',
sim.sim_scripttype_childscript,
'findPath_goalIsPose',
inInts, inFloats, [pathfilename],
self.empty_buff,
sim.simx_opmode_blocking) #sim.simx_opmode_oneshot_wait)Code: Select all
findPath=function(task1, startConfig,goalConfigs,cnt)
-- Here we do path planning between the specified start and goal configurations. We run the search cnt times,
-- and return the shortest path, and its length
local task=simOMPL.createTask('task')
simOMPL.setAlgorithm(task,simOMPL.Algorithm.SBL)
local j1_space=simOMPL.createStateSpace('j1_space',simOMPL.StateSpaceType.joint_position,task1.jh[1],{-170*math.pi/180},{170*math.pi/180},1)
local j2_space=simOMPL.createStateSpace('j2_space',simOMPL.StateSpaceType.joint_position,task1.jh[2],{-120*math.pi/180},{120*math.pi/180},2)
local j3_space=simOMPL.createStateSpace('j3_space',simOMPL.StateSpaceType.joint_position,task1.jh[3],{-170*math.pi/180},{170*math.pi/180},3)
local j4_space=simOMPL.createStateSpace('j4_space',simOMPL.StateSpaceType.joint_position,task1.jh[4],{-120*math.pi/180},{120*math.pi/180},0)
local j5_space=simOMPL.createStateSpace('j5_space',simOMPL.StateSpaceType.joint_position,task1.jh[5],{-170*math.pi/180},{170*math.pi/180},0)
local j6_space=simOMPL.createStateSpace('j6_space',simOMPL.StateSpaceType.joint_position,task1.jh[6],{-120*math.pi/180},{120*math.pi/180},0)
local j7_space=simOMPL.createStateSpace('j7_space',simOMPL.StateSpaceType.joint_position,task1.jh[7],{-170*math.pi/180},{170*math.pi/180},0)
simOMPL.setStateSpace(task,{j1_space,j2_space,j3_space,j4_space,j5_space,j6_space,j7_space})
simOMPL.setCollisionPairs(task, task1.collisionPairs)
simOMPL.setStartState(task,startConfig)
simOMPL.setGoalState(task,goalConfigs[1])
for i=2,#goalConfigs,1 do
simOMPL.addGoalState(task,goalConfigs[i])
end
local path=nil
local l=999999999999
for i=1,cnt,1 do
local res,_path=simOMPL.compute(task,4,-1,300)
if res and _path then
local _l=getPathLength(task1, _path)
if _l<l then
l=_l
path=_path
end
end
end
simOMPL.destroyTask(task)
return path,l
end
findShortestPath=function(task, startConfig,goalConfigs,searchCntPerGoalConfig)
-- This function will search for several paths between the specified start configuration,
-- and several of the specified goal configurations. The shortest path will be returned
local thePath,pathlength=findPath(task, startConfig,goalConfigs,searchCntPerGoalConfig)
return thePath
end
findSeveralCollisionFreeConfigsAndCheckApproach=function(task, matrix,trialCnt,maxConfigs)
-- Here we search for several robot configurations...
-- 1. ..that matches the desired pose (matrix)
-- 2. ..that does not collide in that configuration
-- 3. ..that does not collide and that can perform the IK linear approach
sim.setObjectMatrix(task.ikTarget,-1,matrix)
local cc=getConfig(task.jh)
local cs={}
local l={}
for i=1,trialCnt,1 do
--sim.addStatusbarMessage(tostring(i))
local c=findCollisionFreeConfigAndCheckApproach(task, matrix)
if c then
local dist=getConfigConfigDistance(cc,c,task)
local p=0
local same=false
for j=1,#l,1 do
if math.abs(l[j]-dist)<0.001 then
-- we might have the exact same config. Avoid that
same=true
for k=1,#task.jh,1 do
if math.abs(cs[j][k]-c[k])>0.01 then
same=false
break
end
end
end
if same then
break
end
end
if not same then
cs[#cs+1]=c
l[#l+1]=dist
end
end
if #l>=maxConfigs then
break
end
end
if #cs==0 then
cs=nil
end
return cs
end
findPath_goalIsPose=function(inInts,inFloats,inStrings,inBuffer)
local task={}
local currentState={-1,-1,-1,-1,-1,-1,-1}
local jh={-1,-1,-1,-1,-1,-1,-1}
for i=1,7,1 do
jh[i]=sim.getObjectHandle('LBR4p_joint'..i)
currentState[i]=sim.getObjectPosition(jh[i], -1)
end
task.jh = jh
task.currentState=currentState
metric={0.5,1,1,0.5,0.1,0.2,0.1}
task.metric = metric
task.robotHandle=inInts[1]
local collisionChecking=inInts[2]>0
task.minConfigsForIkPath=inInts[3]
task.minConfigsForPathPlanningPath=inInts[4]
task.maxConfigsForDesiredPose=inInts[5]
task.maxTrialsForConfigSearch=inInts[6]
task.searchCountPerConfig=inInts[7]
local load_path=inInts[8]
local save_path=inInts[9]
local load_path_filename=inStrings[1]
local goalPose={}
for i=1,12,1 do goalPose[i]=inFloats[i] end
task.goalPose=goalPose
print('target: ')
print(task.goalPose)
local approachVector={}
for i=1,3,1 do approachVector[i]=inFloats[i+12] end
task.approachVector=approachVector
local fullRobotName=sim.getObjectName(task.robotHandle)
local suff,robotName=sim.getNameSuffix(fullRobotName)
task.ikGroup=sim.getIkGroupHandle('IK_LBR4p')
task.ikTip=sim.getObjectHandle('LBR4p_tip')
task.ikTarget=sim.getObjectHandle('ik_target')
task.collisionPairs={sim.getCollectionHandle('LBR4p'),sim.getCollectionHandle('environment0')}
if not collisionChecking then
task.collisionPairs={}
end
local goalConfigs=findSeveralCollisionFreeConfigsAndCheckApproach(task, task.goalPose, task.maxTrialsForConfigSearch, task.maxConfigsForDesiredPose)
if goalConfigs == nil then
sim.addStatusbarMessage("Can't find any goal configurations given target pose")
return {0},{},{},''
end
sim.addStatusbarMessage('Number of found goal configs: '..tostring(#goalConfigs))
local path
print('load_path='..load_path)
if load_path == 1 then
print('load precalculated path')
path=loadPath(load_path_filename,task.robotHandle)
else
print('calculate new path')
path=findShortestPath(task, getConfig(task.jh), goalConfigs, task.searchCountPerConfig)
end
if path then
sim.addStatusbarMessage("number of path points: "..tostring(#path))
for k, v in pairs(path) do
if k < 10 then
print(k, v)
end
end
return {1},path,{},''
else
return {0},{},{},''
end
end