How to avoid obstacles??

[AmiQuijano]

Hello,
I'm new to Tesseract and I've been testing it out. I noticed that when using a goal position that is inside the sphere (obstacle) in the freespace_hybrid_example the robot will still move inside the sphere. I tried to test it with ifopt set to true but the trajectory was exactly the same.

Is there a way to modify this example (which uses OMPL) in order to effectively avoid obstacles?

Thanks a lot!

[Levi-Armstrong]

I recently ran into this and I believe it is reporting success but actually failed because the pipeline is not configured correctly to trigger an abort on failure. Can you save the dotgraph and post?

[AmiQuijano]

Yes, the message "Final trajectory is collision free" is reported
This is the dot graph for the freespace_hybrid_example

[Levi-Armstrong]

It is as suspected becaus trajopt fails but abort is never triggered. I believe the
configuration file is missing a trigger abort entry.

[Levi-Armstrong]

I have investigate further and the config is fine but the examples where incorrectly always reporting the message that the trajectory was collision free. I will have a PR shortly which update the messaging in the examples.

[Levi-Armstrong]

You will see the following message after the MR is merged.

Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution: OPT_FAILED) for process input: Tesseract Composite Instruction
Debug:   Error
Info:    Planning took 6.984464 seconds.
Error:   BasicCartesianExample failed

[Levi-Armstrong]

Fixed by this PR

[AmiQuijano]

Thank you for fixing the messaging.
However, the problem is not the messaging but the output trajectory which is in collision.

When defining a goal that causes collisions I would expect, for example, for the robot not to move (abort). However, a trajectory in collision is still showed as output.
For example, in basic_cartesian_example, when using:

CartesianWaypoint wp3{ Eigen::Isometry3d::Identity() * Eigen::Translation3d(0.55, 0.3, 0.3) *
                         Eigen::Quaterniond(0, 0, 1.0, 0) };

The terminal shows:

[tesseract_ros_examples_basic_cartesian_example_node-2] You can set logging level with TRAJOPT_LOG_THRESH. Valid values: FATAL ERROR WARN INFO DEBUG TRACE. Defaulting to ERROR
[tesseract_ros_examples_basic_cartesian_example_node-2] Program: Composite Instruction, Description: Tesseract Composite Instruction
[tesseract_ros_examples_basic_cartesian_example_node-2] Program: {
[tesseract_ros_examples_basic_cartesian_example_node-2] Program:   Move Instruction, Move Type: 1, State WP: Pos=   -0.4  0.2762       0 -1.3348       0  1.4959       0
[tesseract_ros_examples_basic_cartesian_example_node-2] , Description: Start Instruction
[tesseract_ros_examples_basic_cartesian_example_node-2] Program:   Move Instruction, Move Type: 1, Cart WP: xyz=0.55, 0.3, 0.3
[tesseract_ros_examples_basic_cartesian_example_node-2] , Description: from_start_plan
[tesseract_ros_examples_basic_cartesian_example_node-2] Program: }
[tesseract_ros_examples_basic_cartesian_example_node-2] Info:    basic cartesian plan example
[tesseract_ros_examples_basic_cartesian_example_node-2] Info:    1 JointVelTermInfo coeffs given. Applying to all 7 joints
[tesseract_ros_examples_basic_cartesian_example_node-2] Info:    1 JointVelTermInfo targets given. Applying to all 7 joints
[tesseract_ros_examples_basic_cartesian_example_node-2] [WARN] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[tesseract_ros_examples_basic_cartesian_example_node-2] [WARN] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[tesseract_ros_examples_basic_cartesian_example_node-2] [WARN] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[tesseract_ros_examples_basic_cartesian_example_node-2] [WARN] convex solver failed! set TRAJOPT_LOG_THRESH=DEBUG to see solver output. saving model to /tmp/fail.lp and IIS to /tmp/fail.ilp
[tesseract_ros_examples_basic_cartesian_example_node-2] [ERROR] The convex solver failed you one too many times.
[tesseract_ros_examples_basic_cartesian_example_node-2] Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution: OPT_FAILED) for process input: Tesseract Composite Instruction
[tesseract_ros_examples_basic_cartesian_example_node-2] Info:    Planning took 5.465325 seconds.
[tesseract_ros_examples_basic_cartesian_example_node-2] [ERROR] [1752467894.183358937] [basic_cartesian_example_node]: BasicCartesianExample failed

BUT in RViz2 the trajectory causes the robot to enter in collision:

[Levi-Armstrong]

This is the intended behavior for the examples to show the final state of the planned trajectory regardless of success or failure. In your application you would need to check the return success bool and if planning failed do something different.