Adding Trajopt_Ifopt option to all examples

[rjoomen]

Examples

• car_seat_example
• freespace_hybrid_example
• pick_and_place_example
• puzzle_piece_auxillary_axes_example (but "TrajOpt IFOPT currently does not support dynamic cartesian waypoints!")
• puzzle_piece_example

Other examples

• basic_cartesian_example (already present)
• glass_upright_example (already present)
• online_planning_example (always Ifopt)
• scene_graph_example (no planning involved)
• freespace_ompl_example (OMPL only)
• glass_upright_ompl_example (OMPL only)

Other

• Add FreespaceIfoptPipeline and FreespaceIfoptTask to task_composer_plugins.yaml so the OMPL hybrid example can use Trajopt_Ifopt
• Modify all tesseract_ros2 examples to be able to test this (Adding Trajopt_Ifopt option to all examples tesseract_ros2#91)

[rjoomen]

@Levi-Armstrong I can't get the car_seat_example to place the seat using Ifopt. Could you take a look? Maybe my choice of parameters is wrong.

[Levi-Armstrong]

@Levi-Armstrong I can't get the car_seat_example to place the seat using Ifopt. Could you take a look? Maybe my choice of parameters is wrong.

I will take a look, but I am not surprised that this was is not working with the Ifopt version based on our discussion about the collision implementation. I am working on moving the collision utilities to trajopt_common so we can update the legacy code to help compare the two.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Comparison is base (981dd03) 85.62% compared to head (97fb925) 85.62%.

Additional details and impacted files

@@           Coverage Diff           @@
##           master     #389   +/-   ##
=======================================
  Coverage   85.62%   85.62%           
=======================================
  Files         221      221           
  Lines       14681    14681           
=======================================
  Hits        12571    12571           
  Misses       2110     2110           

[Levi-Armstrong]

I imagine some the issues my be resolved with the latest fixes in TrajOpt 0.7.2 release. Does this release fix any of the examples?

[rjoomen]

All applicable examples have an Ifopt option added now. I got the car_seat_example working, the puzzle_piece_example and pick_and_place_example do not converge, so I disabled their unit tests.

This is ready for review now.

(BTW: The CI fails on the unrelated LoadAndExportPluginTest.)

[Levi-Armstrong]

@rjoomen I think you just need to update a unit test because you modified the task_composer_plugins.yaml.

[Levi-Armstrong]

@rjoomen It looks like another test is failing.

2024-01-17T19:43:15.0335977Z 5: [ RUN      ] TesseractExamples.GlassUprightTrajOptExampleUnit
2024-01-17T19:43:15.2321985Z 5: Info:    glass upright plan example
2024-01-17T19:43:15.2390217Z 5: Program: Composite Instruction, Description: Tesseract Composite Instruction
2024-01-17T19:43:15.2391605Z 5: Program: {
2024-01-17T19:43:15.2394743Z 5: Program:   Move Instruction, Move Type: 0, State WP: Pos=   -0.4  0.2762       0 -1.3348       0  1.4959       0
2024-01-17T19:43:15.2396646Z 5: , Description: Start Instruction
2024-01-17T19:43:15.2399950Z 5: Program:   Move Instruction, Move Type: 0, State WP: Pos=    0.4  0.2762       0 -1.3348       0  1.4959       0
2024-01-17T19:43:15.2401575Z 5: , Description: freespace_plan
2024-01-17T19:43:15.2402084Z 5: Program: }
2024-01-17T19:43:15.2882494Z 5: Info:    1 JointVelTermInfo coeffs given. Applying to all 7 joints
2024-01-17T19:43:15.2883310Z 5: Info:    1 JointVelTermInfo targets given. Applying to all 7 joints
2024-01-17T19:43:15.6667157Z 5: Info:    TrajOptMotionPlannerTask motion planning failed (Failed to find valid solution) for process input: Tesseract Composite Instruction
2024-01-17T19:43:15.6673985Z 5: Info:    Planning took 0.404566 seconds.
2024-01-17T19:43:15.6674984Z 5: Info:    Final trajectory is collision free
2024-01-17T19:43:15.6696994Z 5: /__w/tesseract_planning/tesseract_planning/target_ws/src/tesseract_examples/test/glass_upright_example_unit.cpp:25: Failure
2024-01-17T19:43:15.6698239Z 5: Value of: example.run()
2024-01-17T19:43:15.6698726Z 5:   Actual: false
2024-01-17T19:43:15.6699117Z 5: Expected: true
2024-01-17T19:43:15.6706477Z 5: [  FAILED  ] TesseractExamples.GlassUprightTrajOptExampleUnit (639 ms)

[rjoomen]

Yes, weird, because it does pass on stable. I'll look into it.

[rjoomen]

I worked a fix this weekend and should have PR up this coming weekend.

All examples are working now (with PR tesseract-robotics/trajopt#387), I think this is good to go once that PR is merged, Thanks, Levi.

[Levi-Armstrong]

@rjoomen Does the car seat work with Ifopt?

[Levi-Armstrong]

I will review this over the weekend.

[Levi-Armstrong]

Please squash merge, after CI is passing.

[Levi-Armstrong]

It looks like the pick and place example for ifopt is failing.

[Levi-Armstrong]

@rjoomen I pushed a commit fixing the pick and place example. I just had to update the configuration to match the original trajopt and everything was able to plan. It is starting to look like the ifopt version now aligns with the legacy trajopt.

[rjoomen]

I am not allowed to merge, as the stable checks are marked as required and did not pass. (Is it correct they should be required?)

[Levi-Armstrong]

@rjoomen Thank you!