diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp
index 9a94fbdfc9c2daac8a58eae89376bb5835b75b9b..96108e3c76d78d9edf2e9c98df3183c13bf78ed2 100644
--- a/src/TimedPlanner.cpp
+++ b/src/TimedPlanner.cpp
@@ -121,6 +121,7 @@ void TimedPlanner::doMotion() {
             ROS_WARN_STREAM("terminating current trajectory");
             return;
           }
+          group->getCurrentState()->copyJointGroupPositions("panda_arm", trajectory.joint_trajectory.points[0].positions);
           ROS_INFO_STREAM(
               "Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
           TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);