update nullspace ctrl

ICube-Robotics · Aug 27, 2024 · 1fbbecc · 1fbbecc
1 parent c12907f
commit 1fbbecc
Showing 1 changed file with 17 additions and 5 deletions.
diff --git a/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp b/cartesian_vic_controller/src/rules/vanilla_cartesian_impedance_rule.cpp
@@ -255,11 +255,23 @@ bool VanillaCartesianImpedanceRule::compute_controls(
     auto error_position_nullspace = \
       vic_input_data.nullspace_desired_joint_positions - vic_input_data.joint_state_position;
     // Add nullspace contribution to joint accelerations
-    vic_command_data.joint_command_acceleration += nullspace_projection_ * M_inv_nullspace_ * (
-      -D_nullspace_ * vic_input_data.joint_state_velocity +
-      K_nullspace_ * error_position_nullspace +
-      external_joint_torques_
-    );
+    if (vic_input_data.has_external_torque_sensor()) {
+      RCLCPP_DEBUG(logger_, "Cmd nullspace joint acc with external torques...");
+      vic_command_data.joint_command_acceleration += nullspace_projection_ * M_inv_nullspace_ * (
+        - D_nullspace_ * vic_input_data.joint_state_velocity
+        + K_nullspace_ * error_position_nullspace
+        + external_joint_torques_
+      );
+    } else {
+      // Use natural joint space inertia
+      RCLCPP_DEBUG(
+        logger_,
+        "Cmd nullspace joint acc with natural joint inertia (no ext torque sensor)...");
+      vic_command_data.joint_command_acceleration += nullspace_projection_ * (
+        - D_nullspace_ * vic_input_data.joint_state_velocity
+        + K_nullspace_ * error_position_nullspace
+      );
+    }
   } else {
     // Pure (small) damping in nullspace for stability
     RCLCPP_WARN_THROTTLE(