OMPL Path Ignoring Collisions

[TrimItOut]

Hi,

I am trying to generate an OMPL path and the path is successfully created but constantly ignores all objects in the scene with the resulting path not being collision free.

I'm not entirely sure I am going about this the correct way though, I'm using the 3DoFHolonomicPathPlanning demo scene as reference and I have replaced the start configuration with the line tracer robot and I am using a primitive cylinder (Start_Config_Cylinder) as a simple representation of the robot for collision checking - instead of using the robot's complex geometry.

GoalConfiguration is a dummy which contains a cylinder identical to Start_Config_Cylinder called Goal_Config_Cylinder.

Here is the code:

Code: Select all

function sysCall_init()
    
    robot = sim.getObjectHandle('LineTracer#0')

    --path planning  variables
    start_config_cylinder_handle = sim.getObjectHandle('Start_Config_Cylinder#1')
    goal_config_dummy_handle = sim.getObjectHandle('GoalConfiguration#1')    
    
    i = 0 -- counter so path is only generated once 
    
end

visualizePath=function(path)
    if not _lineContainer then
        _lineContainer=sim.addDrawingObject(sim.drawing_lines,3,0,-1,99999,{0.2,0.2,0.2})
    end
    sim.addDrawingObjectItem(_lineContainer,nil)
    if path then
        local pc=#path/3
        for i=1,pc-1,1 do
            lineDat={path[(i-1)*3+1],path[(i-1)*3+2],initPos[3],path[i*3+1],path[i*3+2],initPos[3]}
            sim.addDrawingObjectItem(_lineContainer,lineDat)
        end
    end
end


function sysCall_actuation()
    --generate OMPL Path
    if i == 0 then
        initPos=sim.getObjectPosition(robot,-1)
        initOrient=sim.getObjectOrientation(robot,-1)
        t=simOMPL.createTask('t')
        ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.dubins,robot,{-5,-5},{5,5},1)}
        simOMPL.setStateSpace(t,ss)
        simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)
        simOMPL.setCollisionPairs(t,{sim.getObjectHandle('Start_Config_Cylinder#1'),sim.handle_all})
        startpos=sim.getObjectPosition(robot,-1)
        startorient=sim.getObjectOrientation(robot,-1)
        startpose={startpos[1],startpos[2],startorient[3]}
        simOMPL.setStartState(t,startpose)
        goalpos=sim.getObjectPosition(goal_config_dummy_handle,-1)
        goalorient=sim.getObjectOrientation(goal_config_dummy_handle,-1)
        goalpose={goalpos[1],goalpos[2],goalorient[3]}
        simOMPL.setGoalState(t,goalpose)
        r,path=simOMPL.compute(t,4,-1,800) 
    end

    i = i + 1
    
    visualizePath(path)
end

function sysCall_sensing()
    -- put your sensing code here
end

function sysCall_cleanup()
    -- do some clean-up here
end

-- See the user manual or the available code snippets for additional callback functions and details

I have a few questions regarding the OMPL path generation procedure, firstly if I wanted to use the primitive cylinder as a simple representation of the robot for OMPL does it have to be a child of the dummy (or robot in this case)or can it be 'loose' and just referenced in simOMPL.setCollisionPairs?

Secondly does the GoalConfiguration dummy have to contain a copy of the object used in simOMPL.setCollisionPairs or is the dummy sufficient?

Here is the scene file which will hopefully help everything I've mentioned make sense:

https://drive.google.com/file/d/1Jl0FE_ ... sp=sharing

I'm sure it is a simple fix or misinterpretation by myself, but I am struggling to wrap my head around it.

Thanks!

[fferri]

Code: Select all

ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.dubins,robot,{-5,-5},{5,5},1)}

here you tell OMPL to move the robot object...

Code: Select all

simOMPL.setCollisionPairs(t,{sim.getObjectHandle('Start_Config_Cylinder#1'),sim.handle_all})

but you check collisions for a different object (the cylinder) which does not move, hence all the states expanded by OMPL will be all valid (or all invalid depending on the initial collision state of that cylinder).

[TrimItOut]

Thanks for your response!

I have changed the code so it is only calculating the path for the start_config_cylinder instead of the robot, which is what I somewhat intended.

Modified code:

Code: Select all

initPos=sim.getObjectPosition(start_config_cylinder_handle,-1)
        initOrient=sim.getObjectOrientation(start_config_cylinder_handle,-1)
        t=simOMPL.createTask('t')
        ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.dubins,start_config_cylinder_handle,{-5,-5},{5,5},1)}
        simOMPL.setStateSpace(t,ss)
        simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTConnect)
        simOMPL.setCollisionPairs(t,{sim.getObjectHandle('Start_Config_Cylinder#1'),sim.handle_all})
        startpos=sim.getObjectPosition(start_config_cylinder_handle,-1)
        startorient=sim.getObjectOrientation(start_config_cylinder_handle,-1)
        startpose={startpos[1],startpos[2],startorient[3]}
        simOMPL.setStartState(t,startpose)
        goalpos=sim.getObjectPosition(goal_config_dummy_handle,-1)
        goalorient=sim.getObjectOrientation(goal_config_dummy_handle,-1)
        goalpose={goalpos[1],goalpos[2],goalorient[3]}
        simOMPL.setGoalState(t,goalpose)
        r,path=simOMPL.compute(t,4,-1,800) 

However, now the path is generated from start_config_cylinder's position instead of the robot's and when I set the cylinder position to the robot's then an invalid start state error is returned as obviously the cylinder is colliding with the robot.

My question is can I set it so that in setCollisionPairs so that start_config_cylinder does NOT check for collision with robot but DOES check for collisions with everything else using sim.handle_all?

[coppelia]

Hello,

probably the easiest would be to separate the robot from the path planning task and the cylinder. If not, then you absolutely need to have the cylinder child of the robot (or at least in the robot's hierarchy), otherwise it won't be moved around when OMPL moves the robot to random configurations to check if a collision occurs.

But if your cylinder is separate from the robot, then you need to have the cylinder ignore collisions with the robot. The easiest would be to create a collection in that case: the collection should include all objects in the scene, minus the cylinder, minus the tree starting at the robot's base. Then, check collision between the cylinder and that collection.

Cheers