Using desired joint positions as starting point in JointWaypointMotio…

…n for consistency
TimSchneider42 · Jun 3, 2024 · 38aa633 · 38aa633
1 parent 8267b13
commit 38aa633
Showing 1 changed file with 1 addition and 1 deletion.
diff --git 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;
 }