Gravity feed-forward delivery and CAN driver generalization

.github/workflows/ci.yml

@@ -115,9 +115,9 @@ jobs:
       CCACHE_MAXSIZE: 500M
       CMAKE_CXX_COMPILER_LAUNCHER: ccache
       ASAN_TEST_REGEX: >-
-        ^(DriverCanTransport.RejectsTruncatedFrames|DriverCanTransport.RejectsInvalidBuffersBeforeIo|DriverArxTransport.RejectsTruncatedReceivedFrames|DriverArxTransport.RejectsServoDataIndexOutsideReceiveCache|DriverArxTransport.SendCommandRejectsInvalidCanFrameBeforeWrite|ServoDmParser.*|TopicZmqPublishBoundsTest.*|TopicZmqStep.ReportsMalformedJointPayload|TopicZmqJoystickCount.RejectsInvalidCountsBeforeDispatch|DeviceConfigTest.MalformedJsonIsRejectedNotThrown)$
+        ^(DriverCanTransport.RejectsTruncatedFrames|DriverCanTransport.RejectsInvalidBuffersBeforeIo|DriverCanMitTransport.RejectsTruncatedReceivedFrames|DriverCanMitTransport.RejectsServoDataIndexOutsideReceiveCache|DriverCanMitTransport.SendCommandRejectsInvalidCanFrameBeforeWrite|ServoDmParser.*|TopicZmqPublishBoundsTest.*|TopicZmqStep.ReportsMalformedJointPayload|TopicZmqJoystickCount.RejectsInvalidCountsBeforeDispatch|DeviceConfigTest.MalformedJsonIsRejectedNotThrown)$
       TSAN_TEST_REGEX: >-
-        ^(DriverCanLifecycle.ConcurrentCloseDoesNotRetireSocketDuringSend|DriverCanLifecycle.ConcurrentCloseDoesNotRetireSocketDuringRead|DriverArxConcurrency.ConcurrentCloseAndSendCommandAreRaceFree|DriverArxConcurrency.ConcurrentCloseAndResetZeroPositionAreRaceFree|DriverArxConcurrency.ConcurrentCloseAndEnableAreRaceFree|DriverArxConcurrency.ConcurrentDmEnablesDoNotInterleaveHandshakes|DriverArxConcurrency.SendCommandDoesNotInterleaveEnableHandshake|DriverArxConcurrency.ResetDoesNotInterleaveEnableHandshake)$
+        ^(DriverCanLifecycle.ConcurrentCloseDoesNotRetireSocketDuringSend|DriverCanLifecycle.ConcurrentCloseDoesNotRetireSocketDuringRead|DriverCanMitConcurrency.ConcurrentCloseAndSendCommandAreRaceFree|DriverCanMitConcurrency.ConcurrentCloseAndResetZeroPositionAreRaceFree|DriverCanMitConcurrency.ConcurrentCloseAndEnableAreRaceFree|DriverCanMitConcurrency.ConcurrentDmEnablesDoNotInterleaveHandshakes|DriverCanMitConcurrency.SendCommandDoesNotInterleaveEnableHandshake|DriverCanMitConcurrency.ResetDoesNotInterleaveEnableHandshake)$
     strategy:
       fail-fast: false
       matrix:

native/pi_control/CMakeLists.txt

@@ -76,7 +76,7 @@ set(COMMON_SOURCES
   src/pi_topic.cpp
   src/pi_topic_zmq.cpp
   src/pi_driver.cpp
-  src/pi_driver_arx.cpp
+  src/pi_driver_can_mit.cpp
   src/pi_driver_arx_encoder.cpp
   src/pi_driver_can.cpp
   src/pi_driver_controller.cpp
@@ -94,12 +94,12 @@ set(COMMON_SOURCES
   src/pi_servo_can_encoder.cpp
   src/pi_device.cpp
   src/pi_device_arm.cpp
-  src/pi_device_arm_arx.cpp
-  src/pi_device_arm_nello.cpp
+  src/pi_device_arm_can.cpp
+  src/pi_device_arm_serial.cpp
   src/pi_device_effector.cpp
-  src/pi_device_effector_arx.cpp
+  src/pi_device_effector_can.cpp
   src/pi_device_effector_controller.cpp
-  src/pi_device_effector_nello.cpp
+  src/pi_device_effector_serial.cpp
   src/pi_device_config.cpp
   src/pi_algo.cpp
   src/pi_algo_pino.cpp
@@ -155,7 +155,8 @@ if(OPENPI_CONTROL_BUILD_TESTING)
   list(REMOVE_ITEM PI_TOPIC_ZMQ_TEST_SOURCES src/pi_control_node.cpp)
 
   add_executable(pi_topic_zmq_tests
-    tests/test_driver_arx.cpp
+    tests/test_command_line_args.cpp
+    tests/test_driver_can_mit.cpp
     tests/test_topic_zmq.cpp
     ${PI_TOPIC_ZMQ_TEST_SOURCES}
   )

native/pi_control/include/pi_algo.hpp

@@ -20,8 +20,7 @@ class Joint;
  * @brief Enumeration for different trajectory planning strategies.
  */
 enum class TrajectoryPlanningType {
-    NONE,                     ///< No trajectory planning; target position is sent directly to the servo.
-    SLEW_POS_GRAVITY          ///< Velocity-limited follower tracking with gravity compensation.
+    NONE                      ///< No trajectory planning; target position is sent directly to the servo.
 };
 
 /*!
@@ -68,6 +67,16 @@ class Algo {
         return ReturnCode::SUCCESS;
     }
 
+    /*!
+     * @brief Whether gravity_compensation() computes real model torques.
+     *
+     * The base class is a no-op (zero torques), so callers that require actual
+     * compensation (follower gravity feed-forward) must check this and fail
+     * fast instead of silently commanding zero torque.
+     * @return True when a dynamics model (URDF) is loaded.
+     */
+    virtual bool has_gravity_model() const { return false; }
+
     /*!
      * @brief Initializes the algorithm with the given device configuration.
      * @param p_config_model Pointer to the device model configuration containing URDF path, joint

native/pi_control/include/pi_algo_pino.hpp

@@ -54,6 +54,12 @@ class AlgoPino : public Algo {
     ReturnCode gravity_compensation(const std::vector<float>& joint_positions,
                                     std::vector<float>& calculated_torques) override;
 
+    /*!
+     * @brief AlgoPino computes RNEA gravity torques from the loaded URDF.
+     * @return Always true (init fails without a valid URDF).
+     */
+    bool has_gravity_model() const override { return true; }
+
    private:
     pinocchio::Model model_;                    ///< Pinocchio model structure.
     pinocchio::Data data_;                      ///< Pinocchio data structure.

native/pi_control/include/pi_command_line_args.hpp

@@ -4,6 +4,7 @@
  */
 #pragma once
 #include <string>
+#include <vector>
 
 #define OPT_ROLE                                  "role"              ///< Device role option.
 #define OPT_ROLE_LEADER                           "leader"            ///< Leader role value.
@@ -56,7 +57,9 @@
 #define OPT_DEFAULT_NONE                          "None"             ///< Default "none" value.
 #define OPT_PLANING_TYPE                          "planning_type"    ///< Planning type option.
 #define OPT_PLANING_TYPE_DEFAULT                  "config"           ///< Default planning type value.
-#define OPT_ARM_PLANNING_TYPE                     "arm_planning_type"  ///< Planning type override applied to ARM devices only.
+#define OPT_FOLLOWER_GRAVITY_COMPENSATION         "follower_gravity_compensation"  ///< Follower gravity feed-forward override (config/true/false).
+#define OPT_FOLLOWER_GRAVITY_COMPENSATION_DEFAULT "config"           ///< Default: the arm's individual config JSON decides.
+#define OPT_TORQ_RESCALE                          "torq_rescale"     ///< Per-joint torq_rescale override (comma-separated floats).
 #define OPT_FORCE_FEEDBACK                        "force_feedback"   ///< Force feedback option.
 #define OPT_TOPIC_STATE                           "topic_state"
 #define OPT_TOPIC_LIVE_COMMAND                    "topic_live_command"
@@ -129,7 +132,14 @@ class CommandLineArgs {
     bool safety_feature_off;              ///< Disable safety features flag.
     bool safety_torque_mode = false;       ///< Enable sustained measured-torque protective stops.
     std::string planning_type;            ///< Planning type.
-    std::string arm_planning_type;        ///< Planning type override for ARM devices only (empty/None = model config).
+    ///< Follower gravity feed-forward override: "config" leaves the decision to the
+    ///< follower_gravity_compensation field of the arm's individual config JSON;
+    ///< "true"/"false" (from devices.toml) force it regardless of the JSON value.
+    std::string follower_gravity_compensation_override = OPT_FOLLOWER_GRAVITY_COMPENSATION_DEFAULT;
+    ///< Per-joint torq_rescale override (from the devices.toml [arms] torq_rescale
+    ///< array). Empty when unset; otherwise one value per arm joint, applied after
+    ///< the model and individual configs (highest precedence).
+    std::vector<float> torq_rescale_override;
     float force_feedback;                 ///< Force feedback parameter.
     std::string topic_state;
     std::string topic_live_command;
@@ -157,6 +167,14 @@ class CommandLineArgs {
      */
     CommandLineArgs(int argc, char** argv);
 
+    /*!
+     * @brief Parses a comma-separated torq_rescale override list.
+     *
+     * @param csv Comma-separated floats, e.g. "0.8,0.8,0.8,1.5,1.5,1.5".
+     * @return One value per joint; empty when any token is not a nonnegative finite float.
+     */
+    static std::vector<float> parse_torq_rescale_csv(const std::string& csv);
+
     // Default constructor.
     CommandLineArgs() = default;
 };

native/pi_control/include/pi_control.hpp

@@ -63,8 +63,8 @@
 // Servo-side CAN communication-loss protection. The DM TIMEOUT register (0x09) auto-disables
 // the motor (latched 0xD error) when no frame arrives within the window; the ENCOS heartbeat
 // window behaves equivalently. The window survives in DM servo RAM between runs while power
-// stays on, so DriverArx::enable() explicitly disarms it (writes DM_SERVO_CAN_TIMEOUT_DISARM)
-// right after each enable handshake, and DriverArx::arm_comm_loss_protection() asserts the
+// stays on, so DriverCanMit::enable() explicitly disarms it (writes DM_SERVO_CAN_TIMEOUT_DISARM)
+// right after each enable handshake, and DriverCanMit::arm_comm_loss_protection() asserts the
 // per-device policy in one pass right before the command stream starts.
 //
 // Policy (Device::wants_comm_loss_stop()): what "keep executing the last command" means on a
@@ -91,19 +91,19 @@
 // control loop compares the wall-clock age of each servo's most recent parsed status frame
 // (ReceivedServoData::last_update_perf_) against these thresholds. Used by BOTH the per-servo
 // path (ServoDm::read_hardware_values -> SAFE_MODE_SIG) and the bulk path
-// (DriverArx::group_read_hardware_values -> dead_servo_ids -> device recovery), so the two
+// (DriverCanMit::group_read_hardware_values -> dead_servo_ids -> device recovery), so the two
 // detectors agree. INITIAL applies until the first frame has ever been parsed for the driver
 // (bus may still be coming up after the enable handshakes); NORMAL applies afterwards and
 // never relaxes back.
-#define ARX_STALE_FRAME_AGE_NORMAL_MS             10000                ///< Frame age (ms) before a known-alive servo is declared dead. 10 s.
-#define ARX_STALE_FRAME_AGE_INITIAL_MS            2500                 ///< Frame age (ms) for the start-up phase (before any frame has ever been parsed). 2.5 s.
+#define CAN_MIT_STALE_FRAME_AGE_NORMAL_MS             10000                ///< Frame age (ms) before a known-alive servo is declared dead. 10 s.
+#define CAN_MIT_STALE_FRAME_AGE_INITIAL_MS            2500                 ///< Frame age (ms) for the start-up phase (before any frame has ever been parsed). 2.5 s.
 
-// Warn-only telemetry-stall diagnostic (DriverArx::group_read_hardware_values): a servo whose
+// Warn-only telemetry-stall diagnostic (DriverCanMit::group_read_hardware_values): a servo whose
 // newest frame is older than this gets one edge-triggered PI_WARN ("telemetry stalled") and one
 // on recovery ("resumed after N ms"). Far below the dead thresholds above on purpose -- the
 // point is to leave evidence in the node log for stalls that silently feed a cached position
 // to the policy but never trip the 10 s dead detector.
-#define ARX_STALL_WARN_AGE_MS                     250                  ///< Frame age (ms) that triggers the warn-only stall log.
+#define CAN_MIT_STALL_WARN_AGE_MS                     250                  ///< Frame age (ms) that triggers the warn-only stall log.
 
 // Whole-arm controller (DriverController) stall watchdog. Vendor controller
 // stacks (Trossen iNerve etc.) keep streaming the last command from a driver-

native/pi_control/include/pi_device.hpp

@@ -236,6 +236,12 @@ class Device {
      *        keep holding instead of collapsing detorqued. Followers are
      *        position-commanded, leaders are torque-commanded (gravity
      *        compensation), hence the role-based default.
+     *
+     * Followers stay disarmed even with gravity feed-forward enabled: the
+     * frozen command (pos, kp>0, kd>0, gravity torque for that pose) is a
+     * stable position-anchored equilibrium, not a runaway, while arming would
+     * detorque the arm into an undamped free fall. The reference controller
+     * ships the same policy (ENCOS timeout 0 with follower gravity feed-forward).
      * @return True when the protection window must be armed.
      */
     virtual bool wants_comm_loss_stop() { return role_ != Role::FOLLOWER; }
@@ -268,6 +274,16 @@ class Device {
     virtual ReturnCode publish_device_info(int info_key, std::vector<float>* p_float_data = nullptr,
                                            std::vector<int>* p_int_data = nullptr);
 
+    /*!
+     * @brief Publishes ONE joint's servo parameter report (DEVICE_INFO_SERVO_PARAM).
+     *
+     * Called on its own cadence by the node loop and rotates through the joints —
+     * one message per call, never a burst: the status sockets run with a tiny HWM
+     * (2), so publishing all joints at once would drop everything but the tail.
+     * Devices without MIT-codec servos publish nothing (default no-op).
+     */
+    virtual ReturnCode publish_next_servo_param() { return ReturnCode::SUCCESS; }
+
     /*!
      * @brief Requests a \"move-to-ready\" re-entry from the current pose.
      *
@@ -620,6 +636,41 @@ class Device {
 
     virtual ReturnCode set_runtime_force_feedback_gain(float gain);
 
+    /// Default runaway threshold of the calibration gravity float: the control loop
+    /// re-engages HOLD the moment any joint drifts this far from the float-entry pose.
+    static constexpr float kGravityFloatAbortRadDefault = 0.25f;
+
+    /*!
+     * @brief Toggles the follower calibration gravity float (gravity_tune / arm_check).
+     *
+     * When enabled, the arm joints drop to the gravity feed-forward alone (no position PD),
+     * matching the leader's disengaged float. The control loop watches the drift from the
+     * float-entry pose and re-engages HOLD itself the moment any joint exceeds
+     * ``abort_drift_rad`` — the client is too slow for this judgment (ZMQ round trip),
+     * so the runaway stop must live in the loop. HOLD or any move-to-ready path also
+     * re-engages position control. Only arm followers support this.
+     *
+     * @param enabled Enter (true) or leave (false) the float.
+     * @param abort_drift_rad Runaway threshold in radians; ignored when disabling.
+     */
+    virtual ReturnCode set_runtime_gravity_float(bool enabled, float abort_drift_rad) {
+        (void)enabled;
+        (void)abort_drift_rad;
+        return ReturnCode::NOT_SUPPORTED;
+    }
+
+    /*!
+     * @brief Updates the per-joint torq_rescale at runtime (calibration tools).
+     *
+     * Lets gravity_tune try a new gravity-delivery candidate instantly, without a node
+     * restart, so the arm never waits unpowered between candidates. One value per arm
+     * joint; the count must match the arm DOF.
+     */
+    virtual ReturnCode set_runtime_torq_rescale(const std::vector<float>& values) {
+        (void)values;
+        return ReturnCode::NOT_SUPPORTED;
+    }
+
     virtual ReturnCode runtime_hold();
 
     /*!
@@ -751,6 +802,22 @@ class Device {
     MovingMode moving_mode_ = MovingMode::INVALID;  ///< Strategy for moving joints (SEQUENTIAL or PARALLEL).
     TrajectoryPlanningType planning_type_;  ///< Trajectory planning type.
 
+    /// Follower gravity feed-forward: when set (DeviceArm::init, follower role
+    /// with the individual config's follower_gravity_compensation field, or the
+    /// --follower_gravity_compensation true/false override from devices.toml),
+    /// move() forwards the caller's gravity torque with every position command.
+    /// Independent of the planning type; never set on effectors or leaders.
+    bool follower_gravity_compensation_ = false;
+
+    /// Follower calibration gravity float (set_runtime_gravity_float): while true,
+    /// the arm joints run the leader-style gravity feed-forward alone instead of
+    /// position tracking. Cleared by HOLD, by every move-to-ready path, and by the
+    /// in-loop runaway watchdog (drift from gravity_float_baseline_ beyond
+    /// gravity_float_abort_rad_).
+    bool gravity_float_active_ = false;
+    std::vector<float> gravity_float_baseline_;  ///< Joint positions at float entry (rad).
+    float gravity_float_abort_rad_ = kGravityFloatAbortRadDefault;  ///< Runaway threshold (rad).
+
     MsgType msg_type_ = MsgType::INVALID;  ///< Message type for teleoperation communication.
 
     int control_frequency_ = 0;  ///< Control loop frequency in Hz.

native/pi_control/include/pi_device_arm.hpp

@@ -141,6 +141,35 @@ class DeviceArm : public Device {
 
     ReturnCode set_runtime_force_feedback_gain(float gain) override;
 
+    /*!
+     * @brief Follower calibration gravity float (gravity_tune / arm_check float runs).
+     *
+     * Enable: the arm joints switch to leader control mode (no position PD) and
+     * operate_as_follower() applies the model gravity feed-forward alone, matching the
+     * leader's disengaged float. The float-entry pose is captured as the runaway
+     * baseline; the control loop re-engages HOLD itself the moment any joint drifts
+     * beyond ``abort_drift_rad``. Disable (also via runtime_hold() or any move-to-ready
+     * path) re-engages follower position control at the current pose.
+     */
+    ReturnCode set_runtime_gravity_float(bool enabled, float abort_drift_rad) override;
+
+    /*!
+     * @brief Runtime per-joint torq_rescale update (calibration tools; no node restart).
+     */
+    ReturnCode set_runtime_torq_rescale(const std::vector<float>& values) override;
+
+    /*!
+     * @brief Publishes the next joint's DEVICE_INFO_SERVO_PARAM message (round-robin).
+     *
+     * Carries the effective MIT codec ranges and the motor-reported firmware ranges so
+     * clients (gravity_tune) can compare two arms' servo parameters before assuming one
+     * torq_rescale calibration fits both. One joint per call: the status sockets run
+     * with a tiny HWM, so a per-joint burst would be dropped down to its tail.
+     */
+    ReturnCode publish_next_servo_param() override;
+
+    ReturnCode runtime_hold() override;
+
     /*!
      * @brief UI-facing progress estimate for any active move-to-ready.
      *
@@ -164,6 +193,17 @@ class DeviceArm : public Device {
         return ReturnCode::SUCCESS;
     }
 
+    /*!
+     * @brief Whether this arm's joints accept a torque feed-forward with position commands.
+     *
+     * Gravity feed-forward streamed from this node requires it. DeviceArm::init
+     * fast-fails a --follower_gravity_compensation request on serial arms
+     * (position-only bus); controller arms are accepted without torque streaming
+     * because the vendor controller applies its own compensation.
+     * @return True only for MIT-mode CAN arms.
+     */
+    virtual bool supports_torque_feed_forward() const { return false; }
+
    protected:
     /*!
      * @brief Reads current hardware values from all joints and servos.
@@ -210,6 +250,8 @@ class DeviceArm : public Device {
     std::vector<float> tele_tor_;  ///< Teleoperation target torques (Nm).
     std::vector<float> max_vel_;   ///< Maximum velocity limits (rad/s).
 
+    int servo_param_publish_index_ = 0;  ///< Round-robin cursor of publish_next_servo_param().
+
     std::vector<float> follower_pos_;         ///< Follower target positions (relative radians).
     std::vector<float> follower_vel_;         ///< Follower target velocities (rad/s).
     std::vector<float> follower_tor_;         ///< Follower target torques (Nm).