Correct way to add optimization progress plotting callbacks

[cpetersmeier]

Hello everyone,

I would like to use the plotting callbacks to visualize optimization progress. A few approaches I have tried are forwarding the callbacks through the solver profile (all member functions are const though), as well as constructing them manually in the TrajOptMotionPlanner class (circular dependency when creating a plotter with tesseract_rosutils).

Is there a recommended way to approach this?

[Levi-Armstrong]

Unfortunately, I have not used these in a long time but I will see if I can find an examples of using them that works.

[Levi-Armstrong]

Update, Now that a solver profile exist this can be implemented. I will have PR against TrajOpt and Tesseract planning to support this very soon.

[Levi-Armstrong]

The trajopt PR

[Levi-Armstrong]

The tesseract_planning PR

[Levi-Armstrong]

Now you can add a solver profile where you add a callback for plotting

TrajOptDefaultSolverProfile solver_profile;
solver_profile.callbacks.push_back(trajopt::PlotCallback(tesseract_ros::ROSPlotting(root_link, topic_namespace));

Then add this profile to the profile dictionary and make sure to use this profile in your composite instruction.

[cpetersmeier]

Awesome! Thanks a lot. Interestingly, I observe the following behaviour:

When setting the display mode to 'trail', the first callback call plots the entire environment (including collision geometries), but the pose of the links are all over the place. When switching from 'loop' to 'trail' mode during optimization, I do not encounter this phenomenon.

[Levi-Armstrong]

Are you able to share a screen shot of what is happening?

[Levi-Armstrong]

@cpetersmeier Can this be closed?

[cpetersmeier]

I was able to reproduce the effect with the basic_cartesian_example by adding a collision object to the lbr_iiwa_14_r820_macro.xacro file like so:

<!-- Plotter Debug -->
<link name="${prefix}box">
    <visual>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <geometry>
      <box size="2.0 2.0 0.5"/>
    </geometry>
    <material name="ex_material">
      <color rgba="1.0 0.0 0.0 0.2"/>
    </material>
  </visual>
  <collision>
    <geometry>
      <box size="2.0 2.0 0.5"/>
    </geometry>
  </collision>
</link>

<joint name="${prefix}box-world" type="fixed">
  <origin xyz="2 2 2" rpy="0 0 0"/>
  <parent link="${prefix}base"/>
  <child link="${prefix}box"/>
</joint>
<!-- Debugging end -->

Then I added the plot callback to the basic_cartesian_example.cpp file:

    auto trajopt_solver_profile = std::make_shared<tesseract_planning::TrajOptDefaultSolverProfile>();
    trajopt_solver_profile->opt_info.max_iter = 500;
    trajopt_solver_profile->opt_info.min_approx_improve = 1e-3;
    trajopt_solver_profile->opt_info.min_trust_box_size = 1e-3; 
    trajopt_solver_profile->callbacks.push_back(trajopt::PlotCallback(plotter_));
    planning_server.getProfiles()->addProfile<tesseract_planning::TrajOptSolverProfile>(
        tesseract_planning::profile_ns::TRAJOPT_DEFAULT_NAMESPACE, 
        "cartesian_program", 
        trajopt_solver_profile
    );

I removed the manual inputs for pausing the solver here:

  // plotter->waitForInput();

When I plot the trajectory using the trail setting I get:

Edit:
The impression of 'random' positioning was most likely due to my model origins not being at the center of the collision models like it is the case with this box.