From 899e8e3ee771c389b5354a83be4ecd40cd65f9ee Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 11 Nov 2025 22:07:59 +0100 Subject: [PATCH] fix(franka,controllers): fix osc vibration and trajectory jumping This commit resolves two issues in the franka controllers: 1. OSC Vibration: The order of torque limiting operations in osc() and joint_controller() was reversed. Torques are now first clamped by TorqueSafetyGuardFn (with new per-joint limits) and then rate-limited. This improves stability. TorqueSafetyGuardFn was also enhanced to support per-joint min/max torque arrays. 2. Trajectory Jumping: The reset() methods in LinearPoseTrajInterpolator and LinearJointPositionTrajInterpolator were refactored. They now correctly use the robot's current position as the start for new trajectories, eliminating abrupt jumps. Redundant internal state variables were removed. 3. adds avoidance forces and removes hard push away from the limits. --- extensions/rcs_fr3/src/hw/Franka.cpp | 77 +++++++++++++++--------- include/rcs/LinearPoseTrajInterpolator.h | 39 ++---------- 2 files changed, 55 insertions(+), 61 deletions(-) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index 53210249..054fa7e0 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -147,13 +147,14 @@ void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv, M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose()); } -void TorqueSafetyGuardFn(std::array &tau_d_array, double min_torque, - double max_torque) { +void TorqueSafetyGuardFn(std::array &tau_d_array, + const std::array &min_torques, + const std::array &max_torques) { for (size_t i = 0; i < tau_d_array.size(); i++) { - if (tau_d_array[i] < min_torque) { - tau_d_array[i] = min_torque; - } else if (tau_d_array[i] > max_torque) { - tau_d_array[i] = max_torque; + if (tau_d_array[i] < min_torques[i]) { + tau_d_array[i] = min_torques[i]; + } else if (tau_d_array[i] > max_torques[i]) { + tau_d_array[i] = max_torques[i]; } } } @@ -419,18 +420,20 @@ void Franka::osc() { dist2joint_max = joint_max_.matrix() - q; dist2joint_min = q - joint_min_.matrix(); + double activation_distance = 0.25; for (int i = 0; i < 7; i++) { - if (dist2joint_max[i] < 0.25 && dist2joint_max[i] > 0.1) - avoidance_force[i] += -avoidance_weights_[i] * dist2joint_max[i]; - if (dist2joint_min[i] < 0.25 && dist2joint_min[i] > 0.1) - avoidance_force[i] += avoidance_weights_[i] * dist2joint_min[i]; + if (dist2joint_max[i] < activation_distance) { + avoidance_force[i] += + -avoidance_weights_[i] * + (1 / std::max(dist2joint_max[i], 0.001) - 1 / activation_distance); + } + if (dist2joint_min[i] < activation_distance) { + avoidance_force[i] += + avoidance_weights_[i] * + (1 / std::max(dist2joint_min[i], 0.001) - 1 / activation_distance); + } } tau_d << tau_d + Nullspace * avoidance_force; - for (int i = 0; i < 7; i++) { - if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.; - if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.; - } - std::array tau_d_array{}; Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; @@ -439,14 +442,16 @@ void Franka::osc() { std::chrono::high_resolution_clock::now(); auto time = std::chrono::duration_cast(t2 - t1); + // clamp the torques to a safe range. + std::array min_torques = {-10.0, -10.0, -10.0, -10.0, + -5.0, -5.0, -5.0}; + std::array max_torques = {10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0}; + TorqueSafetyGuardFn(tau_d_array, min_torques, max_torques); + + // limit the rate of change of the clamped torques. std::array tau_d_rate_limited = franka::limitRate( franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); - // deoxys/config/control_config.yml - double min_torque = -5; - double max_torque = 5; - TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque); - return tau_d_rate_limited; }); } @@ -514,16 +519,31 @@ void Franka::joint_controller() { tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq); + // Add joint avoidance potential + Eigen::Array avoidance_weights; + avoidance_weights << 1., 1., 1., 1., 1., 10., 10.; + Eigen::Matrix avoidance_force; + avoidance_force.setZero(); Eigen::Matrix dist2joint_max; Eigen::Matrix dist2joint_min; dist2joint_max = joint_max_.matrix() - q; dist2joint_min = q - joint_min_.matrix(); + double activation_distance = 0.25; for (int i = 0; i < 7; i++) { - if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.; - if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.; + if (dist2joint_max[i] < activation_distance) { + avoidance_force[i] += + -avoidance_weights[i] * + (1 / std::max(dist2joint_max[i], 0.001) - 1 / activation_distance); + } + if (dist2joint_min[i] < activation_distance) { + avoidance_force[i] += + avoidance_weights[i] * + (1 / std::max(dist2joint_min[i], 0.001) - 1 / activation_distance); + } } + tau_d << tau_d + avoidance_force; std::array tau_d_array{}; Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; @@ -533,14 +553,17 @@ void Franka::joint_controller() { std::chrono::high_resolution_clock::now(); auto time = std::chrono::duration_cast(t2 - t1); + // deoxys/config/control_config.yml + // First clamp the torques to a safe range. + std::array min_torques = {-10.0, -10.0, -10.0, -10.0, + -5.0, -5.0, -5.0}; + std::array max_torques = {10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0}; + TorqueSafetyGuardFn(tau_d_array, min_torques, max_torques); + + // Then limit the rate of change of the clamped torques. std::array tau_d_rate_limited = franka::limitRate( franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d); - // deoxys/config/control_config.yml - double min_torque = -5; - double max_torque = 5; - TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque); - return tau_d_rate_limited; }); } diff --git a/include/rcs/LinearPoseTrajInterpolator.h b/include/rcs/LinearPoseTrajInterpolator.h index cecaa754..1bada3a9 100644 --- a/include/rcs/LinearPoseTrajInterpolator.h +++ b/include/rcs/LinearPoseTrajInterpolator.h @@ -10,19 +10,16 @@ class LinearPoseTrajInterpolator { Eigen::Vector3d p_start_; Eigen::Vector3d p_goal_; Eigen::Vector3d last_p_t_; - Eigen::Vector3d prev_p_goal_; Eigen::Quaterniond q_start_; Eigen::Quaterniond q_goal_; Eigen::Quaterniond last_q_t_; - Eigen::Quaterniond prev_q_goal_; double dt_; double last_time_; double max_time_; double start_time_; bool start_; - bool first_goal_; public: inline LinearPoseTrajInterpolator() @@ -30,8 +27,7 @@ class LinearPoseTrajInterpolator { last_time_(0.), max_time_(1.), start_time_(0.), - start_(false), - first_goal_(true){}; + start_(false){}; inline ~LinearPoseTrajInterpolator(){}; @@ -49,22 +45,8 @@ class LinearPoseTrajInterpolator { start_ = false; - if (first_goal_) { - p_start_ = p_start; - q_start_ = q_start; - - prev_p_goal_ = p_start; - prev_q_goal_ = q_start; - first_goal_ = false; - } else { - // If the goal is already set, use prev goal as the starting point of - // interpolation. - prev_p_goal_ = p_goal_; - prev_q_goal_ = q_goal_; - - p_start_ = prev_p_goal_; - q_start_ = prev_q_goal_; - } + p_start_ = p_start; + q_start_ = q_start; p_goal_ = p_goal; q_goal_ = q_goal; @@ -104,14 +86,12 @@ class LinearJointPositionTrajInterpolator { Vector7d q_goal_; Vector7d last_q_t_; - Vector7d prev_q_goal_; double dt_; double last_time_; double max_time_; double start_time_; bool start_; - bool first_goal_; double interpolation_fraction_; // fraction of actual interpolation within an // interval @@ -122,8 +102,7 @@ class LinearJointPositionTrajInterpolator { last_time_(0.), max_time_(1.), start_time_(0.), - start_(false), - first_goal_(true){}; + start_(false){}; inline ~LinearJointPositionTrajInterpolator(){}; @@ -141,15 +120,7 @@ class LinearJointPositionTrajInterpolator { start_ = false; - if (first_goal_) { - q_start_ = q_start; - prev_q_goal_ = q_start; - first_goal_ = false; - // std::cout << "First goal of the interpolation" << std::endl; - } else { - prev_q_goal_ = q_goal_; - q_start_ = prev_q_goal_; - } + q_start_ = q_start; q_goal_ = q_goal; };