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; }