OMPL Setup

[polender]

Hello all,
I'm attempting to set up OMPL within the sim and can't seem to figure it out. I've read over the documentation here: https://www.coppeliarobotics.com/helpFi ... odules.htm

However, I'm unsure how to create a wrapper around the OMPL library. From other forum posts, I know that OMPL is included by default. Can anyone provide instructions on how to set this up within a python environment?

I'd really appreciate it!

[coppelia]

Hello,

you are very vague with your question.
Open your the scene scenes/pathPlanning/holonomicPathPlanning3dof.ttt and have a look at the Lua code. The corresponding Python code (that you can simply replace in the child script) would look very similar, like:

Code: Select all

#python

def visualizePath(path):
    global lineContainer
    global initPos
    sim.addDrawingObjectItem(lineContainer,None)
    if path is not None:
        pc=len(path)//3
        for i in range(len(path)//3):
            lineDat=[path[3*i+0],path[3*i+1],initPos[2]]
            sim.addDrawingObjectItem(lineContainer,lineDat)

def sysCall_thread():
    global lineContainer
    global initPos
    lineContainer=sim.addDrawingObject(sim.drawing_linestrip,3,0,-1,0,[0.2,0.2,0.2])
    robotHandle = sim.getObject('.')
    targetHandle = sim.getObject('/GoalConfiguration')
    initPos = sim.getObjectPosition(robotHandle,sim.handle_world)
    initOrient = sim.getObjectOrientation(robotHandle,sim.handle_world)
    t = simOMPL.createTask('t')
    ss =[simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.pose2d,robotHandle,[-0.5,-0.5],[0.5,0.5],1)]
    simOMPL.setStateSpace(t,ss)
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)
    simOMPL.setCollisionPairs(t,[sim.getObject('./start'),sim.handle_all])
    startpos = sim.getObjectPosition(robotHandle,sim.handle_world)
    startorient = sim.getObjectOrientation(robotHandle,sim.handle_world)
    startpose = [startpos[0],startpos[1],startorient[2]]
    simOMPL.setStartState(t,startpose)
    goalpos = sim.getObjectPosition(targetHandle,sim.handle_world)
    goalorient = sim.getObjectOrientation(targetHandle,sim.handle_world)
    goalpose = [goalpos[0],goalpos[1],goalorient[2]]
    simOMPL.setGoalState(t,goalpose)
    simOMPL.setup(t)
    r,path = simOMPL.compute(t,4,-1,800)
    if path != None:
        visualizePath(path)
        # Simply jump through the path points, no interpolation here:
        for i in range(len(path)//3-1):
            pos = [path[3*i+0],path[3*i+1],initPos[2]]
            orient = [initOrient[0],initOrient[1],path[3*i+2]]
            sim.setObjectPosition(robotHandle,sim.handle_world,pos)
            sim.setObjectOrientation(robotHandle,sim.handle_world,orient)
            sim.switchThread()

Cheers

[fferri]

If the task defines a 3D projection, simOMPL.drawPath can be used to visualize the path.

[polender]

Okay, that helps a little bit, sorry about the vagueness.

To expand:
I'm using an external python environment through the ZMQ API to control the robot in the simulation and would like to keep all my code in that external environment if possible.

The part that I am confused about however is how to create the simOMPL object that is provided in the documentation as well as your previous example. Is this possible to do through the external python env so that no code will reside within the sim environment?

I can try to clarify further if need be.

Edit: It seems that OMPL functionality could be provided through the regular API call of sim.loadModule(), can this be confirmed and can an instruction for how this is used be provided?


Thanks!

[coppelia]

You have 2 possibilities, not sure which one you are referring to:

1. Keep a Python child script, but with external source. You'd do something like:

Code: Select all

#python
include myExternalFile

# myExternalFile is the pythonScript name or path (absolute or relative), without quotes nor the ending '.py'
# searched paths include:
# <CoppeliaSim executable path>/ 
# <CoppeliaSim executable path>/python 
# <current scene path>/ 
# <additional path>/ (see system/usrset.txt and value 'additionalPythonPath')
# additional include paths passed via #luaExec additionalIncludePaths={'c:/Python38'}

2. Or use the ZeroMQ remote API. In that case, you need to explicitly fetch the simOMPL command space with:

Code: Select all

simOMPL = client.getObject('simOMPL')

Cheers