Return invalid start state in RRTstar path planning by V-REP OMPL Plugin

[Francis]

Hi,

I'm using the OMPL plugin of CoppeliaSim to plan a path for a diferential robot (Pioneer 3dx) based on RRTstar Algorithm. I refered to the scenes/3DoFHolonomicPathPlanning.ttt and adapted it to my scene, but it doesn't works.

This is part of my code :

Code: Select all

    robotHandle=sim.getObjectHandle('StartConfiguration')
    targetHandle=sim.getObjectHandle('GoalConfiguration')
    initPosit=sim.getObjectPosition(robotHandle,-1)
    initOrient=sim.getObjectOrientation(robotHandle,-1)
    t=simOMPL.createTask('t')
    ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.pose2d,robotHandle,{-2.5,-2.5}, 
    {2.5,2.5},1)}
    simOMPL.setStateSpace(t,ss)
    simOMPL.setAlgorithm(t,simOMPL.Algorithm.RRTstar)
    simOMPL.setCollisionPairs(t,{sim.getObjectHandle('Pioneer_p3dx'),sim.handle_all})
    startposit=sim.getObjectPosition(robotHandle,-1)
    startorient=sim.getObjectOrientation(robotHandle,-1)
    startpose={startposit[1],startposit[2],startorient[3]}
    simOMPL.setStartState(t,startpose)
    goalposit=sim.getObjectPosition(targetHandle,-1)
    goalorient=sim.getObjectOrientation(targetHandle,-1)
    goalpose={goalposit[1],goalposit[2],goalorient[3]}
    simOMPL.setGoalState(t,goalpose)
    ifSolved,path=simOMPL.compute(t,4,-1,800)

After I started my scene, I got the following error:

[simExtOMPL:warning] OMPL: e:\ompl-1.4.1-source\src\ompl\base\src\planner.cpp:248: RRTstar: Skipping invalid start state (invalid state)
[simExtOMPL:error] OMPL: e:\ompl-1.4.1-source\src\ompl\geometric\planners\rrt\src\rrtstar.cpp:193: RRTstar: There are no valid initial states!

I checked my Lua script many times, but I can't find any reasons about the returned error.
So I hope to get some suggestions. Please help me!

Thank you!

[fferri]

As per default state validation, invalid state means that your robot is in collision according to the collision pairs specified with simOMPL.setCollisionPairs(...) and the result of sim.checkCollision(...).

[Francis]

Hi, fferri:

Thank you for the reply! According to your advice, I tried to understand the function simOMPL.setCollisionPairs, and then checked the collision pairs of my scene and the collision properties of every object, but I can't find any problems. Could you look at my scene?

This is my scene file:
https://drive.google.com/file/d/1JjzFok ... sp=sharing

Thanks!

[fferri]

I'd create a collection for the robot (i.s. create a new collection, select "Tree of selected objects, incl. base, select 'Pioneer_p3dx' and add it).

Then change your setCollisionPairs line to:

Code: Select all

simOMPL.setCollisionPairs(t,{sim.getCollectionHandle('robot'),sim.handle_all})

also, change

Code: Select all

robotHandle=sim.getObjectHandle('Pioneer_p3dx')

if you are not willing to keep the robot as a child of StartConfiguration.

[fferri]

By the way, for real-world motion panning use cases, you don't want to move the actual robot around.

That has at least two problems:

• it is slow, as the robot is made of several complex meshes
• OMPL will move the robot around a lot for collision checking, possibly messing up its dynamic representation, e.g. removing and adding it to the physics engine representation many times

Consider instead creating a simplified representation (e.g. a hidden cylinder, or a decimated convex hull).

[Francis]

Hello fferri,

Thank you so much! I'm so happy to receive your reply and advices. Now my simulation can be implemented partly, which is a big progress for me. But there are still some problems, such that the planned path is fail to reach the goal configuration, and the dual wheels of Pioneer_p3dx can't work during the path following. I'm trying to solve these problems. Do you know the wrong reason?

Thanks!

[fferri]

The state space you used (pose2d, equivalent of OMPL SE(2) - rotation and translation in the plane) does not take into account the kinematic constraints of your robot.

Consider using the dubins state space.

[Francis]

OK. Thank very much! I really ignore the kinematic constraints of my robot and the differences between my scene and the referring scene. Now I will try the dubins state space.

By the way, How can I smooth the path planned by RRTstar Planner? I can't find the method in OMPL Plugin API reference.

[fferri]

Smoothing is already done as part of the path simplification routines.