fix: use current q7 for IK in move to pose (#32)

JeanElsner · May 21, 2024 · 2f5c538 · 2f5c538
1 parent e6c84bc
commit 2f5c538
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/controllers/trajectory.cpp b/src/controllers/trajectory.cpp
@@ -14,8 +14,8 @@ Trajectory::Trajectory(std::shared_ptr<motion::PandaTrajectory> trajectory,
 franka::Torques Trajectory::step(const franka::RobotState &robot_state,
                                  franka::Duration &duration) {
   Vector7d q = Eigen::Map<const Vector7d>(robot_state.q.data());
-  auto q_d = traj_->getJointPositions(getTime(), q);
-  auto dq_d = traj_->getJointVelocities(getTime(), q);
+  auto q_d = traj_->getJointPositions(getTime(), q, q[6]);
+  auto dq_d = traj_->getJointVelocities(getTime(), q, q[6]);
   setControl(q_d, dq_d);
   auto torques = JointPosition::step(robot_state, duration);
   if (getTime() > traj_->getDuration()) {