trajopt takes 5 seconds to plan

[balakumar-s]

I am trying to benchmark tesseract's motion planning on the motion benchmaker dataset. I am specifically looking at running with the Franka Panda robot and want to make sure I am using the correct interface. Here is how I am running planning:

1. Call tesseract ompl to get a seed trajectory
2. Send seed to trajopt planner to get an optimized plan.

This pipeline gives me solutions, however it's taking around 6 seconds to get a plan. Most of the time is spent in trajopt. I am using continuous collision checking with a longest valid length of 0.1. I am only using the collision cost and not the constraint (as constraint was failing in many situations).

1. Is 5 seconds reasonable to get a trajectory in a constrained environment?
2. I am using the python bindings, so I do not have access to trajopt_ifopt. Will trajopt_ifopt be faster?

[Levi-Armstrong]

What tags are you using from tesseract, tesseract_planning and trajopt? I would have to look at the trajopt tables that can be produced for each iteration to identify what the issue might be. There a few things that can cause this type of behavior.

• The number of state passed to trajopt
• A contact distance which conflicts with the allowed collision matrix
  • In some cases for a given contact distance it is possible to have two links being detected being near collision. In this case you can set a link pair contact distance to a smaller distance to improve performance.
• Velocity smoothing being enabled
  • This directly conflicts with collisiion avoidance and should be turned of by default but has not yet. I recommend turning off velocity smoothing and enabling acceleration and jerk smoothing

[balakumar-s]

• I am using the collision avoidance config from here: https://github.com/ros-planning/panda_moveit_config/blob/melodic-devel/config/panda_arm.xacro#L44 which seems to filter out adjacent links.
• I did have velocity smoothing enabled as I wanted to minimize path length. I have disabled it now and that does give better performance.
• I was using very high resolution meshes, which was slowing down trajopt. Now the timing is around 1 second to plan (which seems reasonable to me, let me if that's still on the slower end).
• Some optimized trajectories, still ended up in collision so I am now running a second trajopt call with collision as constraint and seed this with the collision as cost solution. This seems to give collision-free solutions.

[Levi-Armstrong]

Some optimized trajectories, still ended up in collision so I am now running a second trajopt call with collision as constraint and seed this with the collision as cost solution. This seems to give collision-free solutions.

In stead of chaining them I would try enabling both collision as a cost and a constraint in the same plan which is typically what I do. I will configure the cost to have a larger contact distance that the constraint. Also recommend setting the contact margin buffer to something greater than zero which will allow it to converge faster.

[Levi-Armstrong]

The trajopt_ifopt is faster per iteration for the same problem but currently it has an upfront cost of 0.1 second time cost which the original version does not. I plan to profile and figure why this is the case.

[balakumar-s]

Thanks. I will stick to trajopt for now (no ifopt).