Fix Cartesian interpolation (moveit#3652)

* Reduce code duplication * Fix Cartesian interpolation with multiple waypoints continue from latest RobotState
ubi-agni · Oct 6, 2024 · 65e2f9e · 65e2f9e
1 parent 2e5b70c
commit 65e2f9e
Showing 1 changed file with 13 additions and 20 deletions.
diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp
@@ -197,23 +197,20 @@ double CartesianInterpolator::computeCartesianPath(
     double wp_percentage_solved =
         computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
                              precision, validCallback, options, link_offset);
+
+    std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
+    if (i > 0 && !waypoint_traj.empty())
+      std::advance(start, 1);
+    traj.insert(traj.end(), start, waypoint_traj.end());
+
     if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
-    {
       percentage_solved = (double)(i + 1) / (double)waypoints.size();
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
-    }
     else
     {
       percentage_solved += wp_percentage_solved / (double)waypoints.size();
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
       break;
     }
+    start_state = traj.back().get();
   }
 
   return percentage_solved;
@@ -360,21 +357,17 @@ double CartesianInterpolator::computeCartesianPath(
         computeCartesianPath(start_state, group, waypoint_traj, link, waypoints[i], global_reference_frame, max_step,
                              NO_JOINT_SPACE_JUMP_TEST, validCallback, options, link_offset);
 #pragma GCC diagnostic pop
+
+    std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
+    if (i > 0 && !waypoint_traj.empty())
+      std::advance(start, 1);
+    traj.insert(traj.end(), start, waypoint_traj.end());
+
     if (fabs(wp_percentage_solved - 1.0) < std::numeric_limits<double>::epsilon())
-    {
       percentage_solved = (double)(i + 1) / (double)waypoints.size();
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
-    }
     else
     {
       percentage_solved += wp_percentage_solved / (double)waypoints.size();
-      std::vector<RobotStatePtr>::iterator start = waypoint_traj.begin();
-      if (i > 0 && !waypoint_traj.empty())
-        std::advance(start, 1);
-      traj.insert(traj.end(), start, waypoint_traj.end());
       break;
     }
   }