"Computed path is not valid. Invalid states etc".... so let's try another path

[Yoan Mollard]

Hi all,

I'm still confused about the reasons that could make Moveit failing to plan.

I use Baxter with the electric gripper and a simplified collision model of the gripper containing 3 cylinders. Let's assume I've reached a cylinder collision_object1 and want now to reach collision_object2 20 cm away.

On the enclosed capture you see the visual of the gripper with co1 "inside" (inside but not really to avoid problems...) corresponding to the starting state. The goal state corresponds to the grasp of the second collision object. Moveit founds a solution but this solution is almost a linear trajectory so it will fail because the gripper collides with both co1 and co2 along its path. On the capture you also see a well-chosen step of the planned path shown in loop in Moveit, where the collision model of the gripper collides indisputably with the cylinder co1. And iIndeed Moveit complains because the computed path is invalid:

[ INFO] [1409735129.647093022, 9991.697000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1409735129.647180470, 9991.697000000]: Starting state is just outside bounds (joint 'right_s1'). Assuming within bounds.
[ INFO] [1409735129.666577012, 9991.716000000]: No planner specified. Using default.
[ INFO] [1409735129.666764683, 9991.716000000]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1409735129.673802766, 9991.721000000]: LBKPIECE1: Starting with 1 states
[ INFO] [1409735132.262129865, 9994.200000000]: LBKPIECE1: Created 104 (31 start + 73 goal) states in 87 cells (27 start (27 on boundary) + 60 goal (60 on boundary))
[ INFO] [1409735132.262226533, 9994.200000000]: Solution found in 2.595382 seconds
[ INFO] [1409735132.268185064, 9994.206000000]: Path simplification took 0.005870 seconds
[ INFO] [1409735132.268866125, 9994.207000000]: Planning adapters have added states at index positions: [ 0 ]
[ERROR] [1409735132.313300629, 9994.251000000]: Computed path is not valid. Invalid states at index locations: [ 2 3 4 8 ] out of 11. Explanations follow in command line. Contacts are published on /move_group/display_contacts
[ INFO] [1409735132.315719223, 9994.253000000]: Found a contact between 'collision_object1' (type 'Object') and 'right_gripper_r_finger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1409735132.315773983, 9994.253000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1409735132.321978488, 9994.259000000]: Found a contact between 'collision_object1' (type 'Object') and 'right_gripper_r_finger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1409735132.322057903, 9994.259000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1409735132.329804726, 9994.267000000]: Found a contact between 'collision_object1' (type 'Object') and 'right_gripper_r_finger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1409735132.329910988, 9994.267000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1409735132.339982659, 9994.276000000]: Found a contact between 'collision_object2' (type 'Object') and 'right_gripper_l_finger' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1409735132.340073949, 9994.277000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ERROR] [1409735132.345946218, 9994.282000000]: Completed listing of explanations for invalid states.

I ask a tolerence of 0.005 in position, 0.01 in orientation, 0.005 for the joints. I don't understand why it would fail. I guess the best collision-free motion is to move the gripper back to leave co1 free before continuing. Anyway the planned path shown in RViz is almost a direct translation, so indeed the gripper collides with co1 and co2. Thus if there is a collision.... let's try another path! I would especially like to understand why the planner finds a "solution" in a short time (2 seconds over 15 allowed)... and then gives up because it's invalid.

I've changed the kinematics parameters more or less randomly (a higher kinematics_solver_attempts with a higher/lower timeout) without real change, here is my current config:

right_arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_attempts: 6
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.01

Any help or clue about these issues? What parameters should I tune? Why does not Moveit jump to another solution if it finds collisions?

Thanks by advance :-)

Yoan Mollard

Research engineer – 3rd hand project

INRIA Research center, Flowers lab

http://flowers.inria.fr/

http://3rdhandrobot.eu/

[Daniel Solomon]

Yoan

Changing the kinematic solver parameters probably wont make much difference. What is happening here is a common occurrence with MoveIt in these tight planning spaces. The initial plan was found without any collisions:

[ INFO] [1409735132.262226533, 9994.200000000]: Solution found in 2.595382 seconds

but  after simplification it failed. Simplification is a strategy to shortcut and smooth the often jagged paths resulting from random-space planners.  The way MoveIt is currently implemented, it will not replan if a collision is found in the simplification step.

The only strategy I can suggest is to adjust the planner parameters to try to generate plans that will not be shortcut into a collision. In the moveit_config package under config/ompl_planning.yaml add "range: .01" under your planner so that it looks as follows:

  LBKPIECEkConfigDefault:
    type: geometric::LBKPIECE

    range: .2

The range is related to the longest motion added to the planning tree (I don't remember if it is total joint motion, or each joint).  This will cause the planner to create small steps, and can drastically increase planning time if set too small (planning time may need to increase in order to complete in time). Play with this value to see if you can generate plans that are not invalidated during simplification.

[Yoan Mollard]

 The way MoveIt is currently implemented, it will not replan if a collision is found in the simplification step.

Well there is this replanning function, isn't it? I tried with replanning, and indeed some "replannings" are correct. We only miss the function to set the number of attempts from python but in some cases indeed the planner finds a path after 2 or 3 attempts.

The only strategy I can suggest is to adjust the planner parameters to try to generate plans that will not be shortcut into a collision. In the moveit_config package under config/ompl_planning.yaml add "range: .01" under your planner so that it looks as follows:

  LBKPIECEkConfigDefault:
    type: geometric::LBKPIECE

    range: .2

The range is related to the longest motion added to the planning tree (I don't remember if it is total joint motion, or each joint).  This will cause the planner to create small steps, and can drastically increase planning time if set too small (planning time may need to increase in order to complete in time). Play with this value to see if you can generate plans that are not invalidated during simplification.

I see. I'll try to tune this parameter.

I also realised that sometimes the final path does not avoid collisions because the two colliding objects are thin (eg a unique finger of the robot and a door handle) and the step used to compute collisions is too big so it checks the collision state just before and just after the collision but does not point out the moment were the objects actually colllide. If I understand well, this step is exactly the parameter you are talking about. Do you know its unit? Radians for the joint angles?

I'm about to ask other questions in a separated mail also :)

Thanks

[Daniel Solomon]

AFAIK, the replanning flag refers to something slightly different. If the environment updates during execution (and maybe during planning?), the replanning flag is supposed to enable the planner to replan and move through the new environment: http://moveit.ros.org/doxygen/classmoveit_1_1planning__interface_1_1MoveGroup.html#af97c72ed88c1b57bcc6b453099a44b8b

I also realised that sometimes the final path does not avoid collisions because the two colliding objects are thin (eg a unique finger of the robot and a door handle) and the step used to compute collisions is too big so it checks the collision state just before and just after the collision but does not point out the moment were the objects actually colllide. If I understand well, this step is exactly the parameter you are talking about. Do you know its unit? Radians for the joint angles?

During the random space planning step, joint points are chosen that can be very far apart from each other. When each point is added to the graph, collisions are checked at a much finer discretization. The error you posted, however, occurred after the entire solve, in the simplification step. This step probably rounded off a corner, and that is where the collision occurred. Setting a smaller range can provide a smoother path that may also do a better job of avoiding obstacles, and hopefully the simplification step will find it unnecessary to shortcut into a collision state.

-Dan

[Yoan Mollard]

Hi Daniel,

The problem I'm experiencing right now is the fingers of my grippers colliding with objects. They are just narrow cylinders (diam. 4mm, length:12cm) in the collision model and most of the time RRT manages to find a solution where the fingers cross objects despite they are represented in the collision model, so I get this error "found contact between the object and the left finger"...

I try to tune the solver parameters to get acceptable solution in a reasonable time. However it's difficult to tune parameters at random. Is there a documentation about these?

I assume some of them are generic and others are specific to each planner.

Or even if it's not documented yet, any idea where to find clues about what they are and their unit, in the source code for instance?

I'm also wondering what are longest_valid_segment_fraction and projection_evaluator?

Thanks

Yoan Mollard

Research engineer – 3rd hand project

INRIA Research center, Flowers lab

http://flowers.inria.fr/

http://3rdhandrobot.eu/

[Dave Hershberger]

Hi Yoan,

I think longest_valid_segment_fraction is the missing link you are looking for. Or at least, it is the parameter that I tweaked when I got the problem you describe.

That parameter was set to 0.05 by default on the robot I was using, which means 5cm. That is the maximum distance between valid states that the planner checks. The error you see is because a later validation step after the planner checks with a much smaller step size. So if you are working at small scales or close to objects, the planner will often take a 5cm step over/through an obstacle. Often it looks like sort of rounding off a corner.

Anyway, I just changed that to 0.005 (5mm) and that solved it for me.

It is true that a better solution would involve changing the planning code to integrate the finer-resolution check into the planning process, but in the meantime we can just tweak that parameter.

Dave
________________________________________
From: moveit...@googlegroups.com [moveit...@googlegroups.com] on behalf of Yoan Mollard [yoan.m...@inria.fr]
Sent: Friday, November 28, 2014 4:28 AM
To: Daniel Solomon
Cc: moveit...@googlegroups.com

Subject: Re: "Computed path is not valid. Invalid states etc".... so let's try another path

Hi Daniel,

The problem I'm experiencing right now is the fingers of my grippers colliding with objects. They are just narrow cylinders (diam. 4mm, length:12cm) in the collision model and most of the time RRT manages to find a solution where the fingers cross objects despite they are represented in the collision model, so I get this error "found contact between the object and the left finger"...

I try to tune the solver parameters to get acceptable solution in a reasonable time. However it's difficult to tune parameters at random. Is there a documentation about these?
I assume some of them are generic and others are specific to each planner.
Or even if it's not documented yet, any idea where to find clues about what they are and their unit, in the source code for instance?

I'm also wondering what are longest_valid_segment_fraction and projection_evaluator?

Thanks

Yoan Mollard
Research engineer – 3rd hand project

[https://iww.inria.fr/dircom/inria-logo-mail.jpg]

[Yoan Mollard]

Hi Dave,

Thanks for you feedback.

Actually I tried to tweak this parameter as well, and realised that going under ~0.015 makes computation so longer that it timeouts before (I tried up to 30seconds).

So I'm astonished that you could use a so tiny value. Could you tell me more about your setup? Which OMPL planner you're mainly using, a rough duration you need to plan a path, the number of joints of your robots and your IK solver?

I found that RRT was the faster solver in my case, with one arm of Baxter using KDL dealing in a tabletop workspace with 5 Collision Objects. With the default 5cm for this parameter most solutions are found in less than a second, but generate collisions with the tiny fingers of Baxter's gripper. 1.5cm is longer but acceptable, however it still generates collisions to grasp ~4/5cm bottles. I assume 5mm would be much greater but I cannot get any solution with this value.

The fingers are two narrow cylinders in the collision model but my 5 objects are meshes. Maybe this is what makes computation soooo longer? For info the most complex object has 336 vertices and 668 faces.

Is the trajectory published on /display_planned_path the same than the computed one or has it less points? I can observe the displayed path manifestly stepping over the obstacle and I noticed that the last step between the last two points of the trajectory was bigger than between other points, i.e. exactly when the collision appears. Not sure my explanations are very clear :)

Thanks,
Yoan

Yoan Mollard
Research engineer – 3rd hand project

INRIA Research center, Flowers lab
http://flowers.inria.fr/
http://3rdhandrobot.eu/

[Dave Hershberger]

Hi Yoan,

I tried to think about what may be making it work for me at 0.005 and
not for you.

My setup was probably simpler. 6DOF arm and collision objects that are
boxes.

I've been making a point of using simplified collision meshes for the
links of all the robots I've used. I loaded each mesh in Blender and
used the "decimate" modifier to bring each mesh down to about 1000
triangles or so (depending on the complexity of the geometry). I know
moveit already simplifies the meshes during loading, but I think the
decimation helps anyway. I know it makes loading faster anyway, which
is why I originally started doing it.

Maybe you are already doing this, but we also simplify the planning by
using the planner only up to an "approach pose" which still has the
gripper completely clear of the target object. Then the "grasp" phase
has an approach vector which has been verified in some previous step (or
hard-coded in our case). That approach motion is then a straight-line
move in cartesian space which is what actually gets the fingers on
either side of the target. Similarly, when planning to place an object,
we use an "approach pose" which is a few cm away from the target
placement, and then use a straight-line cartesian motion to do the last
bit of motion. This means the randomized planner can mostly work far
away from obstacles. The randomized planners don't do so well when you
ask them to plan right up close to something. Again when moving away
from an object or surface, we do a linear "retreat" move.

The "approach" and "retreat" motions, though linear, are still checked
for validity by sampling along them and doing collision checking. We
just don't ask the OMPL planner to work right up to the actual start and
end poses, which may be in collision or nearly in collision with something.

We have been using RRTStar or RRTConnect.

I'm running it on a 6-core processor.

I've heard that the video showing how an OMPL planner solved a tricky
puzzle actually took like a half hour to solve. So randomized planners
aren't just always fast no matter what.

That's all I could think of. :)

Dave

[Yoan Mollard]

Hi Dave,

Thanks again for all your thoughts!

I could try decimating more the meshes. But I'm already using low precision meshes.

OK I thought to do this pregrasp/retreat trick but I really thought the planners were able to find a solution. Did you try these high-level actions of move_group pick() and place() ? I know they are already integrating this notion of pregrasp/grasp. I think this is the major explanation of why you get so good results with a precision of 5mm.

Are all OMPL planners randomized?
I didn't mention but I am actually always using a 8-core processor.