Deduplicate joint trajectory points in Pilz Move Group Sequence capab…

…ility (#2943)

* Deduplicate joint trajectory points before sending them to controllers

* Fix max loop value

* Move deduplication code to Pilz

* Clean up

* Add todo

moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h

@@ -244,6 +244,17 @@ class RobotTrajectory
 
   void swap(robot_trajectory::RobotTrajectory& other) noexcept;
 
+  /**
+   * \brief Remove a point from the trajectory
+   * \param index - the index to remove
+   */
+  RobotTrajectory& removeWayPoint(std::size_t index)
+  {
+    waypoints_.erase(waypoints_.begin() + index);
+    duration_from_previous_.erase(duration_from_previous_.begin() + index);
+    return *this;
+  }
+
   RobotTrajectory& clear()
   {
     waypoints_.clear();

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/cartesian_trajectory_point.h

@@ -38,7 +38,6 @@
 
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/twist.hpp>
-#include <geometry_msgs/msg/twist.hpp>
 
 namespace pilz_industrial_motion_planner
 {

moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp

@@ -112,7 +112,26 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst
                               // therefore: "i-1".
                               (i > 0 ? radii.at(i - 1) : 0.));
   }
-  return plan_comp_builder_.build();
+
+  const auto res_vec = plan_comp_builder_.build();
+
+  // De-duplicate trajectory points with the same time value.
+  // This is necessary since some controllers do not allow times that are not monotonically increasing.
+  // TODO: Ideally, we would not need this code if the trajectory segments were created without
+  // duplicate time points in the first place. Leaving this note to revisit this with a more principled fix.
+  for (const auto& traj : res_vec)
+  {
+    for (size_t i = 0; i < traj->size() - 1; ++i)
+    {
+      if (traj->getWayPointDurationFromStart(i) == traj->getWayPointDurationFromStart(i + 1))
+      {
+        RCLCPP_WARN(getLogger(), "Removed duplicate point at time=%f", traj->getWayPointDurationFromStart(i));
+        traj->removeWayPoint(i + 1);
+      }
+    }
+  }
+
+  return res_vec;
 }
 
 bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A,