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.

Author: juelg

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<double, 7> &tau_d_array, double min_torque,
-                         double max_torque) {
+void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array,
+                         const std::array<double, 7> &min_torques,
+                         const std::array<double, 7> &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<double, 7> 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<std::chrono::microseconds>(t2 - t1);
 
+    // clamp the torques to a safe range.
+    std::array<double, 7> min_torques = {-10.0, -10.0, -10.0, -10.0,
+                                         -5.0,  -5.0,  -5.0};
+    std::array<double, 7> 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<double, 7> 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<double, 7, 1> avoidance_weights;
+    avoidance_weights << 1., 1., 1., 1., 1., 10., 10.;
+    Eigen::Matrix<double, 7, 1> avoidance_force;
+    avoidance_force.setZero();
     Eigen::Matrix<double, 7, 1> dist2joint_max;
     Eigen::Matrix<double, 7, 1> 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<double, 7> 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<std::chrono::microseconds>(t2 - t1);
 
+    // deoxys/config/control_config.yml
+    // First clamp the torques to a safe range.
+    std::array<double, 7> min_torques = {-10.0, -10.0, -10.0, -10.0,
+                                         -5.0,  -5.0,  -5.0};
+    std::array<double, 7> 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<double, 7> 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;
   });
 }

include/rcs/LinearPoseTrajInterpolator.h

@@ -10,28 +10,24 @@ 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()
       : dt_(0.),
         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;
   };