using OMPL library for path planning of PKM robot 'irb360'

[Galileo]

Hello there

i have tried to use the OMPL library in order to plan a path for the 'irb360' model.
the path I'm corrently trying to generate shuld overcome an obstacle placed between the robot's platform and the target.

the code used is basically taken from the "simpleManipulatorPathPlanning.ttt" file at the CoppeliaSim scenes directory, with slight modifications.

Code: Select all

    setIkMode(info)

    local simJointHandles = ikDrivingJoints
    local useForProjection = {1, 1, 1, 1}
    
    local config = {-0.4136, -0.0256, -0.0555, 0*math.pi/180}
    
    if configurationValidationCallback(config,info) then
        local msg = "'found a collision-free configuration'"
        sim.addLog(sim.verbosity_scriptinfos,msg)
        
        -- found a collision-free config that matches the desired pose!
        -- Now find a collision-free path (via path planning) that brings us from current config to the found config:
        local task = simOMPL.createTask('task')
        simOMPL.setAlgorithm(task,simOMPL.Algorithm.RRTConnect)
        simOMPL.setStateSpaceForJoints(task,simJointHandles,useForProjection)
        simOMPL.setCollisionPairs(task,{robotCollection,sim.handle_all})
        simOMPL.setStartState(task,getConfig(info))
        simOMPL.setGoalState(task,config)
            
        -- now we could add more goal states with simOMPL.addGoalState, to increase the chance to find a collision-free path. 
        -- But we won't do it for simplicity's sake
        simOMPL.setup(task)
        
        res, path = simOMPL.compute(task,4,-1,300)
        
        if res and path then
            msg = "'found a collision-free path'"
            sim.addLog(sim.verbosity_scriptinfos,msg)
            
            -- We found a collision-free path
            -- Now move along the path in a very simple way:
            local sw = sim.setStepping(true)
            for i=1,simOMPL.getPathStateCount(task,path) do
                local conf = simOMPL.getPathState(task,path,i)
                --setConfig(conf,nil,nil,structuralParameters)
                moveToConfig(maxAngVel,maxAngAccel,maxAngJerk,conf,info)                
                sim.step()
            end
            sim.setStepping(sw)
        else
            local msg = "'haven't found a collision-free path'"
            sim.addLog(sim.verbosity_scriptinfos,msg)
        end
        
        simOMPL.destroyTask(task)
    
    else
        local msg = "'haven't found a collision-free configuration'"
        sim.addLog(sim.verbosity_scriptinfos,msg)
    end

before configure the OMPL task I have checked several things:
1. the robot's objects "linkL(i)"are not collidable.
2. the object is inside the robot's workspace
3. there is a fisible path which doesn't collids within the robot's workspace

as Im new to the program and its API's function, i recon it doesnt as simple as I took it for my first try.
I have wanted to ask whether you would be able to give me a short intoduction regarding what should I configure differently for that task? (if it is not too much)
and/or where can I find information about how I should I use that library?

thanks

[fferri]

Hi,

OMPL has no notion of the robot kinematics, besides knowing a set of joints.

For a robot such as IRB360, since it is a parallel kinematics structure, it is first important to understand how its kinematics are implemented (there are two modes: FK and IK, see the robot model for details).

Then, regardless of which robot kinematics are being used, it is important to compute the robot kinematics at every planning or state interpolation step (i.e. right after writing the robot state and just before checking for collisions). The ideal phase to do this it would be in the state validation callback (set via simOMPL.setStateValidationCallback), which should be overridden to first perform robot's IK and then proceed with the normal state validation, i.e. call sim.checkCollision.

[Galileo]

Hi,

Thanks for your answer.

I'm not sure that I have understood it correctly.
Do you mean that there is a chance the IK might not have been calculated during the OMPL compute, and that is why I'm getting a straight line between the initial and target positions?

If so, why doesn't the "simpleManipulatorPathPlanning.ttt" example need any custom validation callback function in order to compute the IK of the robot? (Subsidiary question: what is the default mode for joints in the dimIK environment? Passive?)

Another question: can I configure special arguments to be taken into the custom callback function? (as in the moveToConfig regular API)

[fferri]

I'm not sure that I have understood it correctly.
Do you mean that there is a chance the IK might not have been calculated during the OMPL compute, and that is why I'm getting a straight line between the initial and target positions?

Correct, OMPL doesn't do any IK by default.

The default state validation function is to write a state, perform collision checking over the specified collision pairs, and restore the state.

Writing a state means simply to call sim.setObjectPosition, sim.setObjectPose, or sim.setJointPosition, depending on the component of the state space. No IK is handled automatically in this step.

If so, why doesn't the "simpleManipulatorPathPlanning.ttt" example need any custom validation callback function in order to compute the IK of the robot? (Subsidiary question: what is the default mode for joints in the dimIK environment? Passive?)

When you have a serial kinematic chain, simply setting the joints positions is sufficient to set the robot in a specific configuration, so the default state validation is fine.

Another question: can I configure special arguments to be taken into the custom callback function? (as in the moveToConfig regular API)

No, but you can set a global variable which will be read in your custom callback.

[Galileo]

Thank you very much!

Your answers have helped me a lot. I have got it running, i.e., finding a path that complies with the task requirements. However, it is not always choosing the right path and does not always finish the entire path through the desired position either.

I have a couple more queries regarding OMPL:

1. How do simOMPL.writeState/getState differ from setObjectPosition/getObjectPosition or similar functions? Is there any preference for particular cases in general?

2. I would like to start using MATLAB as a remote API for CoppeliaSim. Can the callback function for OMPL.compute be written as a MATLAB function, or does it have to be written as an embedded script?

[fferri]

simOMPL.writeState is a general purpose function that writes a state for a compound state space. For every component type of the state space it will use the correct CoppeliaSim API, namely:

• simOMPL.StateSpaceType.position2d => sim.setObjectPosition (state space is R²)
• simOMPL.StateSpaceType.pose2d => sim.setObjectPosition + sim.setObjectOrientation (state space is SE(2))
• simOMPL.StateSpaceType.position3d => sim.setObjectPosition (state space is R³)
• simOMPL.StateSpaceType.pose3d => sim.setObjectPosition + sim.setObjectQuaternion (state space is SE(3))
• simOMPL.StateSpaceType.joint_position => sim.setJointPosition (state space is a bounded real vector)
• simOMPL.StateSpaceType.cyclic_joint_position => sim.setJointPosition (state space is SO(2))
• simOMPL.StateSpaceType.dubins => similar to pose2d, but computes the kinematics for a dubins car

It is highly recommended to use a callback in the embedded script and not in the remote script.

OMPL utilizes a lot of CPU, and a remote callback is significantly slower than an embedded callback, resulting in OMPL being extremely slow that way.

[Galileo]

thanks again!

again, your answers are very informative and precise.