Planning direct path plans

[Shaun Edwards]

All,

As was brought up in a previous discussion, the path planned within MoveIt often times takes an indirect route to the goal.  By planning multiple times between the same locations, different path plans can be generated.  Most paths are direct (i.e. near what is a visually desired path), but some small percentage of time the path takes an indirect route (the arm does a few flips before heading toward the goal).  I understand that this is the result of the underlying path planners, but I was wondering if there were any parameters that could be modified to improve this.  In particular, we perform pick and place path planning in a relatively open area.  The majority of the time, a direct path with small variations to account for obstacles would be the desired path.

Thanks for the help,

Shaun

[Dave Coleman]

I'll take a stab at answering this, though please correct me if I make a mistake, there are many here more expert than I:

• The Search Based Planning Library (SBPL), as opposed to OMPL, provides more reliable results in that given the same environment, start, and goal you will always get the same path. It is A*-based, so you will get optimal results up to your chosen search resolution. As far as I know, the SBPL-MoveIt interface has not been catkinized/maintained.
  
• OMPL's default planners do not take into account cost, i.e. an objective function to minimize like distance, work, or smoothness. You can optionally choose from 4 planners in OMPL that support cost: RRT*, PRM*, T-RRT, and PRM, although the last two planners do not provide any theoretic guarantees on their optimality. 
• The difficulty of using these 4 OMPL planners with MoveIt, currently, is that the only cost function currently defined in MoveIt! is distance to closest obstacle (using FCL). I created one a while back that used total joint movement as the cost, but this was never set as the default in MoveIt. This notion of cost - joint movement, would definitely help reduce the indirect route issues. Other notions of cost could also be implemented, but the drawback of planning with cost is of course the large computational performance hit.
• I believe there is also a parameter in MoveIt for the amount of time spent on smoothing paths, but there is still a limit to how much smoothing can help reduce indirect routes.
• Overall, your observations are a result of sampling-based search algorithms that sacrifice optimality and deterministic behavior for computational speed. 

Dave Coleman

[Shaun Edwards]

Dave,

Thanks for the response.  I believe we are in agreement that the search based planners could perform better, probably at the expense of planning time.  Our long term approach is to further develop these types of planners.  In the short term, we would like to "tweak" the parameters of the sampling based OMPL methods to give more direct paths.  My assumption is such parameters exist...I just don't know what they are.

Shaun

[Dave Coleman]

If such parameters exist, I believe they would already be tweaked :-) 

As far as I know, all such parameters that improve the directness of your path do so at great expense to your planning time. These include smoothing (as I mentioned), using RRT* (which is probabilistically optimal with time), and any other of the settings used by the particular algorithm you are using in OMPL, for example, in KPIECE (I believe on of Ioan's favorites :-) you have the following parameters:

• failedExpansionScoreFactor_ - When extending a motion from a cell, the extension can fail. If it is, the score of the cell is multiplied by this factor. 
  
• goalBias_ - The fraction of time the goal is picked as the state to expand towards (if such a state is available) 
  
• minValidPathFraction_ - When extending a motion, the planner can decide to keep the first valid part of it, even if invalid states are found, as long as the valid part represents a sufficiently large fraction from the original motion.  
  
• maxDistance_
  

:: dave | 251.463.2345 c

[Ioan Sucan]

Hello Shaun, Dave,

I am very sorry for the delays in getting back to you. The  comments  above are mostly correct -- other than parameters being tuned -- they are not.

There are three things that I want to mention:

1) the joint space parametrization. Sometimes, e.g., kuka arms, we have > 2pi range per joint. even close to 4pi. This is defined in the URDF as a revolute but not continuous joint, and all of this gets translated into a subset of R^7.

The reason I am mentioning this is that when IK solutions are produced, if we get solutions where a joint is 2pi away from the current state, the motion plan can get very weird.

2) actual smoothing (this is the more likely culprit).
   - In the request for computing a motion plan there is the number of plans to generate (num_planning_attempts). If this number is >1, multiple plans are generated and hybridized -- combined so that shortest part of each is used to make the shortest plan possible.
In the pick&place pipeline, the plan_stage uses one single plan. To get better plans, we need to add a param for this value, possibly exposed in the pick & place params, and set this to something like 3 - 5.

Once we have this in, the chance of getting a bad plan is much less.

  -  The time spent for smoothing can be increased. In fact, I sometimes hack the code and simply call the OMPL routines for smoothing multiple times. If we just call those routines a few times, tweak the time spent on the different types of shortening / smoothing (see PathSimplifier class in OMPL), we can get very high quality solutions. This is not a parameter either, and a little code needs to be written to expose more of the OMPL functionality for smoothing.

3) Used algorithm. We can use RRT* for instance, to generate a short path from the beginning. That might be a bit slow, but it would be worth running different OMPL planners in paralel. Unfortunately that is not exposed in MoveIt -- the same planner is run in parallel, although OMPL supports running different planners in parallel (ParallelPlan class).


Ioan

[Carlos J. Rosales Gallegos]

Hi All,

We were experiencing something similar here. I assume you are providing a cartesian pose as a goal, as we are doing as well.

We believe is not a problem of the planner, but the IK solution for the pose target, which is not aware of the closeness to the start state as Ioan mention in point 1.

What we did is the following (while keeping in mind that we wanted to minimize hacking and keep using the moveit libraries).

We use the computeCartesianPath with the pose target as the only waypoint. This function allows you to limit jumps in the IK solution, and also avoid collisions, and it returns a very nice direct path like this

Then you could do something like this https://github.com/ros-planning/moveit_core/issues/141 to generate the trajectory.

However, what we do is to check for the fraction of the path to be very close to 1, and if so, we take the final waypoint in joint state as the goal in terms of joint constraint for the planner, and so far so good.

Carlos

[Adolfo Rodríguez Tsouroukdissian]

On Wednesday, March 19, 2014 6:49:27 AM UTC+1, Ioan Alexandru Sucan wrote:

Hello Shaun, Dave,

I am very sorry for the delays in getting back to you. The  comments  above are mostly correct -- other than parameters being tuned -- they are not.

There are three things that I want to mention:

1) the joint space parametrization. Sometimes, e.g., kuka arms, we have > 2pi range per joint. even close to 4pi. This is defined in the URDF as a revolute but not continuous joint, and all of this gets translated into a subset of R^7.

The reason I am mentioning this is that when IK solutions are produced, if we get solutions where a joint is 2pi away from the current state, the motion plan can get very weird.

2) actual smoothing (this is the more likely culprit).
   - In the request for computing a motion plan there is the number of plans to generate (num_planning_attempts). If this number is >1, multiple plans are generated and hybridized -- combined so that shortest part of each is used to make the shortest plan possible.
In the pick&place pipeline, the plan_stage uses one single plan. To get better plans, we need to add a param for this value, possibly exposed in the pick & place params, and set this to something like 3 - 5.

Once we have this in, the chance of getting a bad plan is much less.

  -  The time spent for smoothing can be increased. In fact, I sometimes hack the code and simply call the OMPL routines for smoothing multiple times. If we just call those routines a few times, tweak the time spent on the different types of shortening / smoothing (see PathSimplifier class in OMPL), we can get very high quality solutions. This is not a parameter either, and a little code needs to be written to expose more of the OMPL functionality for smoothing.

3) Used algorithm. We can use RRT* for instance, to generate a short path from the beginning. That might be a bit slow, but it would be worth running different OMPL planners in paralel. Unfortunately that is not exposed in MoveIt -- the same planner is run in parallel, although OMPL supports running different planners in parallel (ParallelPlan class).

Hey everybody,

This thread has so far focused on path simplification. I've observed that even when paths look good, their time parameterization is not as smooth as I would like. I've opened an issue (ros-planning/moveit_core#167) to track this other aspect of a motion plan's smoothness.

Adolfo.

--
Adolfo Rodríguez Tsouroukdissian
Senior robotics engineer
adolfo.r...@pal-robotics.com
http://www.pal-robotics.com

PAL ROBOTICS S.L
c/ Pujades 77-79, 4º4ª
08005 Barcelona, Spain.
Tel. +34.93.414.53.47
Fax.+34.93.209.11.09
Skype: adolfo.pal-robotics
Facebook - Twitter - PAL Robotics YouTube Channel

AVISO DE CONFIDENCIALIDAD: Este mensaje y sus documentos adjuntos, pueden contener información privilegiada y/o confidencial que está dirigida exclusivamente a su destinatario. Si usted recibe este mensaje y no es el destinatario indicado, o el empleado encargado de su entrega a dicha persona, por favor, notifíquelo inmediatamente y remita el mensaje original a la dirección de correo electrónico indicada. Cualquier copia, uso o distribución no autorizados de esta comunicación queda estrictamente prohibida.

CONFIDENTIALITY NOTICE: This e-mail and the accompanying document(s) may contain confidential information which is privileged and intended only for the individual or entity to whom they are addressed.  If you are not the intended recipient, you are hereby notified that any disclosure, copying, distribution or use of this e-mail and/or accompanying document(s) is strictly prohibited.  If you have received this e-mail in error, please immediately notify the sender at the above e-mail address.