TrajOptTaskflow Failure

[marip8]

Discussed in https://github.com/ros-industrial-consortium/tesseract_planning/discussions/100

^{Originally posted by mohamedsayed18 June 24, 2021}

Error:   TrajOptTaskflow Failure: 
         at line 51 in space/src/motion-planner/tesseract_planning/tesseract_process_managers/src/core/utils.cpp

I got this error when I try to do a linear motion from the start position to a point, followed by a linear motion to another point

Waypoint wp0 = CartesianWaypoint(getPoses()[0]);
Waypoint wp1 = CartesianWaypoint(getPoses()[1]);

These waypoints are VectorIsometry3d poses

PlanInstruction plan_f0(wp0, PlanInstructionType::LINEAR, "FREESPACE");
PlanInstruction plan_f1(wp1, PlanInstructionType::LINEAR, "RASTER");
program.push_back(plan_f0);
program.push_back(plan_f1);

The program generates a path but it has an unexpected motion between the two points and the path is not straight line

But when changing the motion of the first instruction from LINEAR to FREESPACE, it works fine and the path is straight

[marip8]

@mohamedsayed18 are you using the TrajOptTaskflow as the process planner? Also, what planning profiles are you using?

[mohamedsayed18]

I'm using the default process planners planning_server.loadDefaultProcessPlanners(); and for the request name request.name = tesseract_planning::process_planner_names::TRAJOPT_PLANNER_NAME; Is request name TRAJOPT_PLANNER_NAME is the same as the TrajOptTaskflow?

[marip8]

Yes, it appears that TRAJOPT_PLANNER_NAME is associated with the TrajOptTaskflow process planner:

https://github.com/ros-industrial-consortium/tesseract_planning/blob/5b09248852f861c57dbdb20e5c9cabe6647e957a/tesseract_process_managers/src/core/process_planning_server.cpp#L75

To confirm, this is what you did to solve the issue?

-PlanInstruction plan_f0(wp0, PlanInstructionType::LINEAR, "FREESPACE");
+PlanInstruction plan_f0(wp0, PlanInstructionType::FREESPACE, "FREESPACE");
PlanInstruction plan_f1(wp1, PlanInstructionType::LINEAR, "RASTER");

What happens if you run this instead?:

PlanInstruction plan_f0(wp0, PlanInstructionType::LINEAR, "FREESPACE");
-PlanInstruction plan_f1(wp1, PlanInstructionType::LINEAR, "RASTER");
+PlanInstruction plan_f1(wp1, PlanInstructionType::LINEAR, "FREESPACE");

[mohamedsayed18]

To confirm, this is what you did to solve the issue?

Yes this is what I did

PlanInstruction plan_f0(wp0, PlanInstructionType::FREESPACE, "FREESPACE");
PlanInstruction plan_f1(wp1, PlanInstructionType::LINEAR, "RASTER");

When I tried to run the following I got the same error

PlanInstruction plan_f0(wp0, PlanInstructionType::LINEAR, "FREESPACE");
PlanInstruction plan_f1(wp1, PlanInstructionType::LINEAR, "FREESPACE");

[mohamedsayed18]

In another experiment I did, I made the robot move in a square trajectory

PlanInstruction plan_f0(wp0, PlanInstructionType::FREESPACE, "FREESPACE");
PlanInstruction plan_f1(wp1, PlanInstructionType::LINEAR, "RASTER");
PlanInstruction plan_f2(wp2, PlanInstructionType::LINEAR, "RASTER");
PlanInstruction plan_f3(wp3, PlanInstructionType::LINEAR, "RASTER");
PlanInstruction plan_f4(wp0, PlanInstructionType::LINEAR, "RASTER");

It fails even though the first instruction is FREESPACE I tried the same trajectory with different plannerCARTESIAN_PLANNER_NAME It worked fine

When I removed the OPW from the srdf file it worked fine, it worked with the TRAJOPT_PLANNER_NAME

<group name="manipulator">
    <chain base_link="base_link" tip_link="tool0" />
</group>

[mohamedsayed18]

Is the error caused by the following function? https://github.com/ros-industrial-consortium/tesseract_planning/blob/5b09248852f861c57dbdb20e5c9cabe6647e957a/tesseract_process_managers/src/taskflow_generators/raster_global_taskflow.cpp#L59-L61

[mohamedsayed18]

I noticed that when there is an obstacle in the path, The same error appears.

[savolla]

@mohamedsayed18 did you resolve the issue? I'm getting the same error. I create a very simple rectangular path and put a simple cube collision object. It solves the path without collision object and fails when I enable it..

[mohamedsayed18]

@savolla Sorry, I can't help with this, The code has many updates since I opened this issue, make sure you are working with the newest version. Check this issue maybe you find a solution Another tip try to make your path a bit away from the robot body to avoid singularity, also experiment with smaller obstacle/cube and see if this generates the desired path

[marip8]

For reference, the PlanInstructionType generally determines how the motion planner input (i.e. Cartesian poses) gets interpolated to a more dense input by the SimplePlanner for use by the other motion planners. PlanInstructionType::Linear means that new poses are generated in a straight line between two waypoints, and PlanInstructionType::Freespace means that waypoints are generated along a joint interpolated arc between two waypoints. Both strategies are valid for all combinations of joint and Cartesian waypoints (i.e. joint to joint, joint to Cartesian, Cartesian to joint, and Cartesian to Cartesian) and IK/FK are used to get the waypoints into the correct state space.

The default SimplePlanner (which is a part of most planning taskflows, i.e. process planners) performs interpolation based on these strategies using a "longest-valid-segment" technique, meaning it adds an interpolated point a some discrete Cartesian or joint distance (e.g. every 25mm or every 1 degree of joint space movement). That's why your video shows many Cartesian waypoints when you only actually gave the planner 2 waypoints. There are other SimplePlanner profiles that have different behavior, such as copying the first waypoint n times, interpolating over n states, etc.

Regarding planner success with collision obstacles, only the TrajOpt motion planner is capable of moving these interpolated waypoints out of collision. The OMPL planner does not require interpolation because it generates unconstrained collision-free intermediate states between the start and goal waypoints as a part of the planning process. The Descartes planner usually benefits from interpolation, but only samples valid robot poses at each interpolated waypoint and cannot move that waypoint out of collision.

Some general guidance about what the common planning taskflows (i.e. process planners) do

• FreespaceTaskflow (FREESPACE_PLANNER_NAME)
  • For "freespace" moves between waypoints where it doesn't really matter what path the robot takes to get there
  • Interpolates with a SimplePlanner (based on the PlanInstructionType described above)
  • First attempts planning with the TrajOpt planner (which can, and probably will, adjust the positions of the interpolated waypoints)
  • If TrajOpt fails, attempts planning with OMPL (disregarding the interpolated path from the SimplePlanner) to generate a new collision-free trajectory
  • If OMPL succeeds, uses TrajOpt to improve the trajectory from OMPL (where TrajOpt can, and probably will, adjust the intermediate joint states of the trajectory)
    • You can control the behavior of TrajOpt by defining your own TrajOpt profiles that get added to the planning request and are referenced by your original waypoints
• CartesianTaskflow (CARTESIAN_PLANNER_NAME)
  • For move between waypoints where the robot must move in a specific pre-defined way where no adjustment to the path is allowed. Most useful for systems with more than 6 degrees of freedom or underconstrained applications (i.e. tool z orientation is not constrained)
  • Interpolates with a SimplePlanner (based on the PlanInstructionType described above)
    • Depending on how dense your initial tool path is, you may not actually want to interpolate it. In this case you would want to create a custom SimplePlanner profile that does not attempt interpolation or create a custom planning taskflow that does not include a simple planner
  • Uses the Descartes planner to sample robot poses (i.e. discretely sampling extra degrees of freedom or poses about an unconstrained dimension) for each waypoint in the path and construct a valid joint trajectory through that path. Note Descartes cannot adjust the positions of these waypoints to make them collision-free. It can only choose robot configurations that achieve the waypoint position in a collision-free manner.
  • Uses TrajOpt to improve the trajectory from Descartes
    • Again you can configure the behavior of TrajOpt by defining a custom profile and associating your original waypoints with those profiles. By default we don't allow TrajOpt to adjust the Cartesian positions of the waypoints in this planner, but we do allow it to move the extra degrees of freedom of the system (e.g. a rail or gantry) to produce a smoother trajectory

Hopefully this sheds some light on what the planners are actually doing to help resolve this issue. If you are not already doing this, you may need to modify the values of the default planner profiles to get the behavior you are looking for.

[Levi-Armstrong]

Reopen if still an issue.