From f460de23038f53440cf9d7475b4bc52a1fb8b26d Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Sun, 6 Oct 2024 18:33:23 +0200
Subject: [PATCH] Fix Cartesian interpolation with multiple waypoints

continue from latest RobotState
---
 moveit_core/robot_state/src/cartesian_interpolator.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp
index c2825a3cc1..276e45c37d 100644
--- a/moveit_core/robot_state/src/cartesian_interpolator.cpp
+++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp
@@ -298,6 +298,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
       percentage_solved += wp_percentage_solved / static_cast<double>(waypoints.size());
       break;
     }
+    start_state = traj.back().get();
   }
 
   return percentage_solved;