Finding mapping between collision_<idx> and the link names in TrajOpt Motion Planner

[fjonath1]

Hi all,

I just started setting up tesseract_planning (using pick_and_place_example as a starting point) and I'm wondering how to tell which links are in collision by looking at the TaskComposer output.

For instance, PickAndPlace example output with roslaunch tesseract_ros_examples pick_and_place_example.launch

[INFO] iteration 1
[INFO]  

| =========================================================================== |
|                               ROS Industrial                                |
|                           TrajOpt Motion Planning                           |
| =========================================================================== |
|      merit |   oldexact |  new_exact |    dapprox |     dexact |      ratio | -                
| --------------------------------------------------------------------------- | COSTS
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_0     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_1     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_2     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_3     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_4     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_5     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_6     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_7     
| ---------- |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_8     
| ---------- |  2.270e-01 |  1.912e-01 | -8.153e-05 |  3.585e-02 | -4.397e+02 | collision_9     
| ---------- |  2.615e+00 |  2.595e+00 |  2.008e-02 |  2.008e-02 |  1.000e+00 | joint_vel_cost  
| ---------- |  1.275e-01 |  9.133e-02 |  3.618e-02 |  3.618e-02 |  1.000e+00 | joint_accel_cost 
| ---------- |  2.550e-01 |  1.492e-01 |  2.134e-01 |  1.059e-01 |  4.961e-01 | joint_jerk_cost 

If, hypothetically collision_9 is in collision, we would see an output similar to this:

[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_9
[INFO] New merit_error_coeffs: (10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 1e+06)
[INFO] optimization couldn't satisfy all constraints
[INFO] 
==================
Optimization results:
status: PENALTY_ITERATION_LIMIT
cost values: (0, 0, 0, 0, 0, 0, 0, 0, 0, 18.3728, 6.05356)
constraint violations: (2.41203e-31, 1.70252e-30, 2.61926e-31, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3.27455)
n func evals: 20
n qp solves: 19
==================
Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution) for process input: Tesseract Composite Instruction
Debug:   Error

Is the there a way to tell which links are associated with collision_index?
In other word, how tell which links are causing the planning failure?

Thank you!

[Levi-Armstrong]

I do not think this is currently possible without attaching a debugger. The index number corresponds the the joint state index in the trajectory.

[fjonath1]

I see! Thank you for the clarification.

When Moveit is planning, I believe they output some debugging information that output which joints are colliding as it is trying to solve the trajectory, so I was wondering if there are similar feature/flag built in tesseract planner that I can just enable.

I guess I can also throw in tesseract's collision checker into the offending waypoint index based on the collision_ to inspect which one is failing when I'm debugging, so this is not a big deal.

[Levi-Armstrong]

@marrts I know you did work on printing collision details; is this possible to enable via a flag?

[marip8]

To my knowledge there is no flag for trajopt to enable the same type of debugging that we have in descartes. We would have to write a few similar lines of code here after we convert the trajopt result to a tesseract trajectory