Remote API calling OMPL cannot get value returned, also got weird return state value

Typically: "How do I... ", "How can I... " questions
Post Reply
cxt
Posts: 8
Joined: 08 Sep 2020, 04:12

Remote API calling OMPL cannot get value returned, also got weird return state value

Post by cxt »

Hi Coppelia,

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)
coppeliasim:

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

coppelia
Site Admin
Posts: 8371
Joined: 14 Dec 2012, 00:25

Re: Remote API calling OMPL cannot get value returned, also got weird return state value

Post by coppelia »

Hello,

how do you set-up the remote API client? What are the arguments to simxStart? Try out using a larger negative number for the timeOutInMs argument.

Additionally, for such longer operations, it would be best to send a non-blocking request to CoppeliaSim, have CoppeliaSim handle the request, and inform the client when data is available. Then the client can retrieve the data.

Cheers

cxt
Posts: 8
Joined: 08 Sep 2020, 04:12

Re: Remote API calling OMPL cannot get value returned, also got weird return state value

Post by cxt »

Hi,

Thanks for your reply! I use "self.sim_client = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)" in python client. I found the API: def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs). It seems I set 5000ms in timeout now. I will try to enlarge that, and also try non-blocking request, thanks very much!

coppelia
Site Admin
Posts: 8371
Joined: 14 Dec 2012, 00:25

Re: Remote API calling OMPL cannot get value returned, also got weird return state value

Post by coppelia »

and try with a negative argument as mentioned!

Cheers

Post Reply