feat: Add new parameters for the controllers and fix reference frame when using world base jacobian

src/cartesian_controller.cpp

@@ -122,7 +122,7 @@ CartesianController::update(const rclcpp::Time &time,
   J.setZero();
   auto reference_frame = params_.use_local_jacobian
                              ? pinocchio::ReferenceFrame::LOCAL
-                             : pinocchio::ReferenceFrame::WORLD;
+                             : pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED;
   pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id,
                                   reference_frame, J);
 
@@ -151,19 +151,21 @@ CartesianController::update(const rclcpp::Time &time,
 
     pinocchio::computeMinverse(model_, data_, q_pin);
     auto Mx_inv = J * data_.Minv * J.transpose();
-    auto Mx = pseudo_inverse(Mx_inv);
+    auto Mx = pseudo_inverse(Mx_inv, params_.operational_space_regularization);
 
     tau_task << J.transpose() * Mx * (stiffness * error - damping * (J * dq));
   } else {
     tau_task << J.transpose() * (stiffness * error - damping * (J * dq));
   }
 
-  if (model_.nq != model_.nv) {
-    // TODO: Then we have some continouts joints, not being handled for now
+  if (model_.nq != model_.nv || !params_.joint_limit_repulsion.enabled) {
+    // Skip joint limit repulsion if disabled or if continuous joints are present (nq != nv)
     tau_joint_limits = Eigen::VectorXd::Zero(model_.nv);
   } else {
-    tau_joint_limits = get_joint_limit_torque(q, model_.lowerPositionLimit,
-                                              model_.upperPositionLimit);
+    tau_joint_limits = get_joint_limit_torque(
+        q, model_.lowerPositionLimit, model_.upperPositionLimit,
+        params_.joint_limit_repulsion.safe_range,
+        params_.joint_limit_repulsion.max_torque);
   }
 
   tau_secondary << nullspace_stiffness * (q_ref - q) +
@@ -197,8 +199,8 @@ CartesianController::update(const rclcpp::Time &time,
   if (params_.limit_torques) {
     tau_d = saturateTorqueRate(tau_d, tau_previous, params_.max_delta_tau);
   }
-  /*tau_d = exponential_moving_average(tau_d, tau_previous,*/
-  /*                                   params_.filter.output_torque);*/
+  tau_d = exponential_moving_average(tau_d, tau_previous,
+                                     params_.filter.output_torque);
 
   if (not params_.stop_commands) {
     for (size_t i = 0; i < num_joints; ++i) {

src/cartesian_impedance_controller.yaml

@@ -17,6 +17,13 @@ cartesian_impedance_controller:
     default_value: False
     description: "Whether we use operational space control or cartesian impedance control"
 
+  operational_space_regularization:
+    type: double
+    default_value: 0.1
+    description: "Regularization (damping) for Mx pseudo-inverse. Higher values improve stability near singularities but reduce accuracy. Valid range is 0.001-1.0; typical useful values are in the 0.01-0.5 range."
+    validation:
+      bounds<>: [0.001, 1.0]
+
   task:
     k_pos_x:
       type: double
@@ -258,6 +265,24 @@ cartesian_impedance_controller:
     default_value: true
     description: "Limit torques"
 
+  joint_limit_repulsion:
+    enabled:
+      type: bool
+      default_value: true
+      description: "Enable joint limit repulsion torques that push joints away from their limits"
+    safe_range:
+      type: double
+      default_value: 0.1
+      description: "Distance from joint limit where repulsion starts [rad]"
+      validation:
+        bounds<>: [0.01, 1.0]
+    max_torque:
+      type: double
+      default_value: 5.0
+      description: "Maximum repulsion torque at joint limit [Nm]"
+      validation:
+        bounds<>: [0.0, 50.0]
+
   max_delta_tau:
     type: double
     default_value: 0.5