From 303a000516b970bb78a83646882f6a5125a65159 Mon Sep 17 00:00:00 2001 From: Nicolas Gres Date: Tue, 17 Nov 2026 18:29:44 +0000 Subject: [PATCH] Fix the joint position move (homing): no start jerk, no failed moves at the target - Start the motion from q_d instead of q. Starting from the measured position makes the first command jump by q - q_d (audible jerk at the start, or a velocity/acceleration discontinuity abort when the gap is larger, e.g. after the arm sagged in Idle). - Skip the move if the arm is already within tolerance. A motion that finishes in its first cycle is rejected by libfranka ("Motion finished commanded, but the robot is still moving"). - Stop retrying after a successful move. The loop ran 5 times even on success, each extra run started at the target and failed as above. - Return false when all retries fail instead of reporting the target as reached. Co-Authored-By: Claude Opus 5.5 --- src/control_mode/abstract_control_mode.cpp | 24 ++++++++++++++++++- .../joint_position_motion_generator.cpp | 5 ++-- 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index e5d02e1..48d5c54 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -119,11 +119,28 @@ bool AbstractControlMode::moveToJointPosition(const std::array // {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, // {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, // {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); + // a motion that finishes in its first cycle is rejected by libfranka ("still moving") + const franka::RobotState state = robot_->readOnce(); + state_buffer_->write(state); + double max_error = 0.0; + for (size_t i = 0; i < NUM_DOFS; i++) + { + max_error = std::max(max_error, std::abs(target_q[i] - state.q[i])); + } + if (max_error < tolerance) + { + zlc::info("[{}] Already at target joint position.", getModeName()); + startControl(); + return true; + } + bool reached = false; for (size_t i = 0; i < 5; i++) { try { JointPositionMotionGenerator motion_generator(max_velocity, target_q, *state_buffer_, tolerance); robot_->control(motion_generator); + reached = true; + break; }catch (const franka::Exception& e) { zlc::error("Error when move joint position {}", e.what()); } @@ -134,8 +151,13 @@ bool AbstractControlMode::moveToJointPosition(const std::array break; } } - zlc::info("[{}] Reached target joint position.", getModeName()); startControl(); + if (!reached) + { + zlc::error("[{}] Move to joint position failed, target not reached.", getModeName()); + return false; + } + zlc::info("[{}] Reached target joint position.", getModeName()); return true; } diff --git a/src/motion_generator/joint_position_motion_generator.cpp b/src/motion_generator/joint_position_motion_generator.cpp index e658355..cb8257f 100644 --- a/src/motion_generator/joint_position_motion_generator.cpp +++ b/src/motion_generator/joint_position_motion_generator.cpp @@ -25,12 +25,13 @@ franka::JointPositions JointPositionMotionGenerator::operator()(const franka::Ro time_ += period.toSec(); if (time_ == 0.0) { - q_start_ = JointPositionMotionGenerator::Vector7d(robot_state.q.data()); + // q_d, not q: starting from q makes the first command jump by q - q_d (discontinuity) + q_start_ = JointPositionMotionGenerator::Vector7d(robot_state.q_d.data()); delta_q_ = q_goal_ - q_start_; // Check if already within tolerance - skip motion if so if (delta_q_.cwiseAbs().maxCoeff() < tolerance_) { - franka::JointPositions output(robot_state.q); + franka::JointPositions output(robot_state.q_d); output.motion_finished = true; return output; }