Virtual endstops simple

idls/ControlDebug.idl

@@ -32,6 +32,13 @@ module dls2_interface
       double terrain_height_world;
       double terrain_roll;
       double terrain_pitch;
+      // Calculated joint torques in Nm with full MPC GRF, before enable blending, indexed like joints_position.
+      sequence<double> current_desired_torques;
+      // Measured controller inputs after simulated noise and drift.
+      double dp[3];
+      double omega[3];
+      // Riccati feedback K (x - x0) added to the MPC GRF, in N, world frame, per leg; zero when not applied.
+      sequence<dls2_interface::msg::Vec3> riccati_grf_world;
     };
   };
 };

idls/HalSafetyDebug.idl

@@ -0,0 +1,23 @@
+#include "DlsHeader.idl"
+
+module dls2_interface
+{
+  module msg
+  {
+    // Per-joint state of the HAL virtual joint limit, for debugging.
+    struct HalSafetyDebug
+    {
+      dls2_interface::msg::DlsHeader header;
+
+      uint8 safety_state; // RobotSafety::SafetyState
+
+      sequence<boolean> joint_pos_limit_violation_predicted;
+      sequence<long> virtual_joint_limit_side; // +1 braking toward position_max, -1 toward position_min, 0 inactive
+      sequence<double> virtual_joint_limit_anchor; // [rad], NaN when inactive
+      sequence<double> virtual_joint_limit_k; // [Nm/rad]
+      sequence<double> virtual_joint_limit_d; // [Nm*s/rad]
+      sequence<boolean> virtual_joint_limit_damping_only; // latched damping only (damping_after_stop)
+      sequence<double> virtual_joint_limit_torque; // [Nm] computed before the torque limits clamp, NaN when not overridden
+    };
+  };
+};

idls/StanceStatus.idl

@@ -11,6 +11,30 @@ module dls2_interface
 
       // data
       sequence<boolean> stance_status;
+      // Estimated GRFs in base frame, flattened as [Fx, Fy, Fz] per limb id.
+      sequence<double> estimated_grfs;
+      // Filtered joint accelerations used by the GRF estimator [rad/s^2].
+      sequence<double> joints_acceleration;
+      // Joint-space dynamics contributions [Nm], indexed by joint id.
+      sequence<double> gravity_torques;
+      sequence<double> coriolis_torques;
+      sequence<double> joint_inertia_torques;
+      sequence<double> base_inertia_torques;
+      sequence<double> friction_torques;
+      // Raw measured joint torques from blind_state [Nm].
+      sequence<double> measured_joint_torques;
+      // Dynamics contributions mapped to base-frame GRFs [Fx, Fy, Fz] per limb.
+      sequence<double> gravity_grfs;
+      // Coriolis and centrifugal contribution: nonlinear effects minus gravity.
+      sequence<double> coriolis_grfs;
+      // Joint-acceleration contribution M_jj*qdd.
+      sequence<double> joint_inertia_grfs;
+      // Floating-base acceleration contribution M_jb*a_base.
+      sequence<double> base_inertia_grfs;
+      // Friction contribution added to the dynamics residual.
+      sequence<double> friction_grfs;
+      // Raw measured joint torques mapped through J^T, without model terms.
+      sequence<double> joint_torque_grfs;
     };
   };
 };