From 38aa633801ebde52bad1f9b09dbd64422a37352c Mon Sep 17 00:00:00 2001
From: Tim Schneider <tim.schneider1@tu-darmstadt.de>
Date: Mon, 3 Jun 2024 16:39:59 +0200
Subject: [PATCH] Using desired joint positions as starting point in
 JointWaypointMotion for consistency

---
 src/motion/joint_waypoint_motion.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/motion/joint_waypoint_motion.cpp b/src/motion/joint_waypoint_motion.cpp
index 170eb6d4..0e183a85 100644
--- a/src/motion/joint_waypoint_motion.cpp
+++ b/src/motion/joint_waypoint_motion.cpp
@@ -19,7 +19,7 @@ void JointWaypointMotion::initWaypointMotion(
   if (previous_command.has_value())
     input_parameter.current_position = previous_command->q;
   else
-    input_parameter.current_position = robot_state.q;
+    input_parameter.current_position = robot_state.q_d;
   input_parameter.current_velocity = robot_state.dq_d;
   input_parameter.current_acceleration = robot_state.ddq_d;
 }