State Validation for simIK/simOMPL using Python+Lua

[dav_paul]

Good day!

I have been running into an issue with a dual Python+Lua solution where state validation callbacks are needed.

I have a scene where I am using simIK.findConfigs to find a valid configuration for moving a robot to a pre-grasping pose, after which I use simOMPL.compute() to find a collision-free path to that configuration.

Everything is stored in a script object that is written in Python with some Lua-equivalent functions to run via Lua (for possible speedup as suggested in the stateValidationCallback-python scene in the scenes/pathPlanning folder). I've got everything running well if I do everything on Python, but running and calling the equivalent methods in Lua only work without a stateValidationCallback method provided.

In my Python code, I have a flag set to check if to use the Lua version or Python version:

Code: Select all

def findIKConfig(args):
    # -- Prepare robot collection:
    self.robot_collection = sim.createCollection()
    sim.addItemToCollection(self.robot_collection, sim.handle_tree, self.robot, 0)

    # -- prepare an ik task (in order to be able to find configs that match specific end-effector poses):
    ikEnv = simIK.createEnvironment()
    ikGroup = simIK.createGroup(ikEnv)
    ikElement, simToIkObjectMapping, _ = simIK.addElementFromScene(ikEnv,ikGroup,self.robot,self.tip,self.goal,simIK.constraint_pose)
    simIK.syncFromSim(ikEnv, [ikGroup])

    # -- get a few handles from the IK world:
    ikJointHandles = []
    for J in range(len(self.joint_handles)):
        ikJointHandles.append(simToIkObjectMapping[self.joint_handles[J]])

    ikGoal=simToIkObjectMapping[self.goal]
    ikBase=simToIkObjectMapping[self.robot]
    ikTip=simToIkObjectMapping[self.tip]

    pose = sim.getObjectPose(self.goal, self.robot)

    simIK.setObjectPose(ikEnv,ikGoal,ikBase,pose)

    if self.use_lua:
        # -- run simIK.findConfigs from the Lua side:
        configs = sim.callScriptFunction(
            "luaFindIKConfig",
            sim.handle_self,
            {
                "ikEnv": ikEnv, 
                "ikGroup": ikGroup,
                "ikJointHandles": ikJointHandles,
                "robot_collection": self.robot_collection,
                "joint_handles": self.joint_handles,
            },
        )
        
    else:
        # -- run simIK.findConfigs from the Python side:
        params = {
            'maxDist': 0.1,
            'maxTime': 10,
            'findMultiple': False, # -- change to True to find multiple solutions
            'pMetric': [0.05,0.05,0.05,0.1],
            'cb': stateValidationCollision_python,
        }
        configs = simIK.findConfigs(ikEnv, ikGroup, ikJointHandles, params)
    
    # NOTE: check here for more info on how a valid configuration is found via IK: 
    #   https://manual.coppeliarobotics.com/en/simIK.htm#simIK.findConfigs
    if bool(configs):
        # -- found a robot config that matches the desired pose!
        return configs[0]
    
    return None

If the use_lua flag is set to True, then I would call this function in Lua:

Code: Select all

function getConfig_lua()
    -- get the current joint configuration
    local retVal={}
    for J=1,#joint_handles,1 do
        retVal[J]=sim.getJointPosition(joint_handles[J])
    end
    return retVal
end


function setConfig_lua(config)
    -- apply a joint configuration to the robot
    for J=1,#joint_handles,1 do
        sim.setJointPosition(joint_handles[J],config[J])
    end
end


function stateValidationCollision_lua(config)
    -- check if a configuration is valid, i.e., doesn't collide:
    -- save current config:    
    local tmp = getConfig_lua()

    -- apply new config:
    setConfig_lua(config)

    -- does new config collide?
    local is_collision, _ = sim.checkCollision(robot_collection,sim.handle_all)

    -- restore original config:
    setConfig_lua(tmp)

    return (is_collision > 0)
end


function luaFindIKConfig(data)    
    -- get necessary handles for the state validation portion:
    local ikEnv = data["ikEnv"]
    local ikJointHandles = data["ikJointHandles"]
    local ikGroup = data["ikGroup"]

    -- passing these items for the getConfig and setConfig functions above; making them global in Lua scope:
    robot_collection = data["robot_collection"]
    joint_handles = data["joint_handles"]

    params = {
        maxDist = 0.1,
        maxTime = 10,
        findMultiple = false, -- change to true to find multiple solutions
        pMetric = {0.05,0.05,0.05,0.1},
        --cb = 'stateValidationCollision_lua'  -- NOTE: if this is commented, it works!
    }
        
    return simIK.findConfigs(ikEnv, ikGroup, ikJointHandles, params)
end

As in the comments, if the "cb" element is not provided to simIK.findConfigs, it works. An issue also happens whenever I try to use simOMPL.setStateValidationCallback using the same Lua callback. I get an error like:

Code: Select all

[simOMPL:error] OMPL: D:\coppeliaRobotics\coppeliaSim\programming\simOMPL\build\ompl-1.5.0-prefix\src\ompl-1.5.0\src\ompl\geometric\planners\rrt\src\RRTConnect.cpp:207: RRTConnect: Motion planning start tree could not be initialized!

Am I perhaps using the stateValidationCallback in a weird way since I am invoking a call to a Lua function from Python using a callback function also in Lua? Or perhaps is this not permitted to run from a Python setup?

Just to give a bit more context, I have the entire scene here. In this scene, I have a dummy object containing a script with all the motion planning stuff, and I have a Python file that sends a call to the dummy's script functions. My goal was to make OMPL planning more accessible and easier to use.

I sincerely appreciate any tips on how to fix this, thank you! I'd just like to see if I can make planning faster.

[coppelia]

Hello,

please excuse the late reply. I am now looking at your scene.
First, make sure to return true/True when a state is valid: in your code, e.g. in function stateValidationCollision_lua, you return true when there is a collision. Instead return:

Code: Select all

return (is_collision == 0)

Also, consider to have 2 separate state validation callback functions, one for the OMPL task, and one for the simIK.findConfigs function: this way you can debug more easily a possible source of problems.

But using the lua callback (i.e. setting cb = stateValidationCollision_lua) it seems the callback is called.

Cheers

[dav_paul]

Hi Coppelia Team, thank you so much for your response!

I missed that bug in the Lua function, thank you for pointing it out. Perhaps that was the reason why the callback was not working well before. After making the fix and some other misc. changes, I managed to get the motion planning working a lot faster now that I am using Lua.

Thanks again!