diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 062e4ed..d884fe5 100644 --- a/.github/workflows/ci.yml +++ b/.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: diff --git a/native/pi_control/CMakeLists.txt b/native/pi_control/CMakeLists.txt index 8b98965..9f6c3d7 100644 --- a/native/pi_control/CMakeLists.txt +++ b/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} ) diff --git a/native/pi_control/include/pi_algo.hpp b/native/pi_control/include/pi_algo.hpp index f21bfdd..eca7594 100644 --- a/native/pi_control/include/pi_algo.hpp +++ b/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 diff --git a/native/pi_control/include/pi_algo_pino.hpp b/native/pi_control/include/pi_algo_pino.hpp index bb8fbe2..552e99a 100644 --- a/native/pi_control/include/pi_algo_pino.hpp +++ b/native/pi_control/include/pi_algo_pino.hpp @@ -54,6 +54,12 @@ class AlgoPino : public Algo { ReturnCode gravity_compensation(const std::vector& joint_positions, std::vector& 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. diff --git a/native/pi_control/include/pi_command_line_args.hpp b/native/pi_control/include/pi_command_line_args.hpp index c145069..e9f13ed 100644 --- a/native/pi_control/include/pi_command_line_args.hpp +++ b/native/pi_control/include/pi_command_line_args.hpp @@ -4,6 +4,7 @@ */ #pragma once #include +#include #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 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 parse_torq_rescale_csv(const std::string& csv); + // Default constructor. CommandLineArgs() = default; }; diff --git a/native/pi_control/include/pi_control.hpp b/native/pi_control/include/pi_control.hpp index 0fd953d..d30369f 100644 --- a/native/pi_control/include/pi_control.hpp +++ b/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- diff --git a/native/pi_control/include/pi_device.hpp b/native/pi_control/include/pi_device.hpp index 4b41379..4f2803d 100644 --- a/native/pi_control/include/pi_device.hpp +++ b/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* p_float_data = nullptr, std::vector* 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& 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 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. diff --git a/native/pi_control/include/pi_device_arm.hpp b/native/pi_control/include/pi_device_arm.hpp index c005aa3..279270d 100644 --- a/native/pi_control/include/pi_device_arm.hpp +++ b/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& 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 tele_tor_; ///< Teleoperation target torques (Nm). std::vector max_vel_; ///< Maximum velocity limits (rad/s). + int servo_param_publish_index_ = 0; ///< Round-robin cursor of publish_next_servo_param(). + std::vector follower_pos_; ///< Follower target positions (relative radians). std::vector follower_vel_; ///< Follower target velocities (rad/s). std::vector follower_tor_; ///< Follower target torques (Nm). diff --git a/native/pi_control/include/pi_device_arm_arx.hpp b/native/pi_control/include/pi_device_arm_arx.hpp deleted file mode 100644 index 450c938..0000000 --- a/native/pi_control/include/pi_device_arm_arx.hpp +++ /dev/null @@ -1,33 +0,0 @@ -/*! - * @file pi_device_arm_arx.hpp - * @brief Defines the DeviceArmArx class for ARX robotic arm device. - */ -#pragma once -#include "pi_device_arm.hpp" - -/*! - * @class DeviceArmArx - * @brief Concrete implementation of DeviceArm for ARX robotic arm devices. - */ -class DeviceArmArx : public DeviceArm { - public: - /*! - * @brief Constructs a new DeviceArmArx instance. - * @param cla Command-line arguments containing device configuration parameters such as - */ - DeviceArmArx(const CommandLineArgs& cla); - - // Destroys the DeviceArmArx instance. - ~DeviceArmArx(); - - /*! - * @brief Sets control mode for ARX arm. - * - * ARX family does not require explicit leader/follower mode switching at this level. - * Keep as a no-op (commands still flow through normal Joint/Servo path). - */ - virtual ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; - - private: -}; - diff --git a/native/pi_control/include/pi_device_arm_can.hpp b/native/pi_control/include/pi_device_arm_can.hpp new file mode 100644 index 0000000..d4b7acb --- /dev/null +++ b/native/pi_control/include/pi_device_arm_can.hpp @@ -0,0 +1,39 @@ +/*! + * @file pi_device_arm_can.hpp + * @brief Defines the DeviceArmCan class for MIT-mode CAN arm device. + */ +#pragma once +#include "pi_device_arm.hpp" + +/*! + * @class DeviceArmCan + * @brief Concrete implementation of DeviceArm for MIT-mode CAN arm devices. + */ +class DeviceArmCan : public DeviceArm { + public: + /*! + * @brief Constructs a new DeviceArmCan instance. + * @param cla Command-line arguments containing device configuration parameters such as + */ + DeviceArmCan(const CommandLineArgs& cla); + + // Destroys the DeviceArmCan instance. + ~DeviceArmCan(); + + /*! + * @brief Sets control mode for MIT-mode CAN arm. + * + * MIT-mode CAN devices do not require explicit leader/follower mode switching at this level. + * Keep as a no-op (commands still flow through normal Joint/Servo path). + */ + virtual ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; + + /*! + * @brief MIT-mode CAN frames carry a torque feed-forward field. + * @return Always true. + */ + bool supports_torque_feed_forward() const override { return true; } + + private: +}; + diff --git a/native/pi_control/include/pi_device_arm_nello.hpp b/native/pi_control/include/pi_device_arm_nello.hpp deleted file mode 100644 index f5f5b28..0000000 --- a/native/pi_control/include/pi_device_arm_nello.hpp +++ /dev/null @@ -1,42 +0,0 @@ -/*! - * @file pi_device_arm_nello.hpp - * @brief Defines the DeviceArmNello class for Nello robotic arm device. - */ -#pragma once -#include "pi_device_arm.hpp" - -/*! - * @class DeviceArmNello - * @brief Nello robotic arm device implementation. - */ -class DeviceArmNello : public DeviceArm { - public: - /*! - * @brief Constructs a new DeviceArmNello instance. - * - * @param cla Command-line arguments containing device configuration parameters. - */ - DeviceArmNello(const CommandLineArgs& cla); - - // Destroys the DeviceArmNello instance. - ~DeviceArmNello(); - - /*! - * @brief Moves the arm to the ready position using Nello-specific movement sequence. - * - * @return ReturnCode::SUCCESS if successful, otherwise an error code. - */ - ReturnCode move_to_ready_position() override; - - /*! - * @brief Sets control mode for Nello arm. - * - * Nello: leader and follower require different servo operation modes. - * - NORMAL_OPERATION: follow the target_role policy. - * - READY_MOVE_OVERRIDE: force a safe position-based mode so the arm can move to home from current pose. - */ - ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; - - private: -}; - diff --git a/native/pi_control/include/pi_device_arm_serial.hpp b/native/pi_control/include/pi_device_arm_serial.hpp new file mode 100644 index 0000000..fb997b7 --- /dev/null +++ b/native/pi_control/include/pi_device_arm_serial.hpp @@ -0,0 +1,42 @@ +/*! + * @file pi_device_arm_serial.hpp + * @brief Defines the DeviceArmSerial class for serial bus-servo arm device. + */ +#pragma once +#include "pi_device_arm.hpp" + +/*! + * @class DeviceArmSerial + * @brief serial bus-servo arm device implementation. + */ +class DeviceArmSerial : public DeviceArm { + public: + /*! + * @brief Constructs a new DeviceArmSerial instance. + * + * @param cla Command-line arguments containing device configuration parameters. + */ + DeviceArmSerial(const CommandLineArgs& cla); + + // Destroys the DeviceArmSerial instance. + ~DeviceArmSerial(); + + /*! + * @brief Moves the arm to the ready position using serial-arm movement sequence. + * + * @return ReturnCode::SUCCESS if successful, otherwise an error code. + */ + ReturnCode move_to_ready_position() override; + + /*! + * @brief Sets control mode for serial bus-servo arm. + * + * Serial bus-servo devices: leader and follower require different servo operation modes. + * - NORMAL_OPERATION: follow the target_role policy. + * - READY_MOVE_OVERRIDE: force a safe position-based mode so the arm can move to home from current pose. + */ + ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; + + private: +}; + diff --git a/native/pi_control/include/pi_device_config.hpp b/native/pi_control/include/pi_device_config.hpp index 94a56de..c15c1d7 100644 --- a/native/pi_control/include/pi_device_config.hpp +++ b/native/pi_control/include/pi_device_config.hpp @@ -21,7 +21,7 @@ enum class DeviceConfigType { EFFECTOR ///< Attached end-effector configuration. }; -#define CURRENT_CONFIG_VERSION "1.1.1" ///< Currently supported configuration file format version. +#define CURRENT_CONFIG_VERSION "1.3.1" ///< Currently supported configuration file format version. /*! * @class DeviceConfig @@ -33,7 +33,8 @@ class DeviceConfig { const std::string fn_device_model = "device_model"; ///< Field name for device model name. const std::string fn_device_id = "device_id"; ///< Field name for device id. const std::string fn_spring_effect = "spring_effect"; ///< Field name for spring_effect on/off. - const std::string fn_gravity_compensation = "gravity_compensation"; ///< Field name for gravity_compensation on/off. + const std::string fn_gravity_compensation = "gravity_compensation"; ///< Field name for leader gravity compensation on/off. + const std::string fn_follower_gravity_compensation = "follower_gravity_compensation"; ///< Field name for follower gravity feed-forward on/off. const std::string fn_read_only = "read_only"; ///< Field name for read_only on/off. const std::string fn_publishes_joystick = "publishes_joystick"; ///< Top-level boolean: declares this effector hosts a joystick servo and must publish MsgJoystick. When true, pi_control_node auto-derives `--topic_joystick` from the per-handle `joystick_side`. Absent/false => legacy servo_model scan fallback. @@ -42,20 +43,20 @@ class DeviceConfig { const std::string val_device_type_effector = "effector"; ///< Value for device type effector. const std::string fn_arm_type = "arm_type"; ///< Field name for arm type. - const std::string val_arm_type_arx = "arx"; ///< Value for arm type arx. + const std::string val_arm_type_can = "can"; ///< Value for MIT-mode CAN arms. const std::string val_arm_type_controller = "controller"; ///< Value for arms managed by a whole-arm controller (DriverController). - const std::string val_arm_type_nello = "nello"; ///< Value for arm type nello (serial bus-servo arms, e.g. SO-ARM101). + const std::string val_arm_type_serial = "serial"; ///< Value for serial bus-servo arms (e.g. SO-ARM101). const std::string fn_effector_type = "effector_type"; ///< Field name for effector type. - const std::string val_effector_type_arx = "arx"; ///< Value for effector type arx. + const std::string val_effector_type_can = "can"; ///< Value for MIT-mode CAN effectors. const std::string val_effector_type_controller = "controller"; ///< Value for effectors managed by a whole-arm controller (DriverController). - const std::string val_effector_type_nello = "nello"; ///< Value for effector type nello (serial bus-servo grippers, e.g. SO-ARM101). + const std::string val_effector_type_serial = "serial"; ///< Value for serial bus-servo grippers (e.g. SO-ARM101). const std::string val_effector_type_none = "None"; ///< Value for effector type none. const std::string fn_effector_control_mode = "control_mode"; ///< Field name for effector control mode. const std::string val_effector_control_mode_torque = "torque"; ///< Value for effector control mode torque. const std::string val_effector_control_mode_position = "position"; ///< Value for effector control mode position. const std::string fn_effector_dist_to_torque_const = "dist_to_torque_const"; ///< Field name for effector distance to torque constant. - const std::string fn_effector_grip_spring_offset = "grip_spring_offset"; ///< Field name for the torque-mode spring offset (rad), subtracted from the position error (monopi ControlFollowGripper "offset"). Optional; default 0. + const std::string fn_effector_grip_spring_offset = "grip_spring_offset"; ///< Field name for the torque-mode spring offset (rad), subtracted from the position error. Optional; default 0. const std::string fn_effector_open_at_min = "open_at_min"; ///< Field name to specify the open side is at min position. (default is false) @@ -77,7 +78,6 @@ class DeviceConfig { const std::string fn_planning_type = "planning_type"; ///< Field name for moving trajectory planning type. const std::string val_planning_type_none = "None"; ///< Value for no planning. - const std::string val_planning_type_slew_pos_gravity = "slew_pos_gravity"; ///< Value for synchronized velocity-limited position tracking + gravity compensation torque. const std::string fn_joint_init_sequence = "joint_init_sequence"; ///< Field name for joint initialization sequence. const std::string fn_joints = "joints"; ///< Field name for joints. @@ -143,6 +143,7 @@ class DeviceConfig { const std::string fn_servo_resolution = "servo_resolution"; ///< Field name for servo resolution. const std::string fn_servo_prof_accel = "prof_accel"; ///< Field name for acceleration for servo profile control. const std::string fn_servo_dir_invert = "dir_invert"; ///< Field name for servo direction invert: inverted = -1, not inverted = 1. + const std::string fn_servo_abs_position = "abs_position"; ///< Field name for sign-agnostic position reads (read-only encoders whose sign varies per unit, e.g. the ARX_ENC gripper). Optional; default false. const std::string fn_servo_zero_pos = "zero_pos"; ///< Field name for servo zero position (absolute radian). const std::string fn_servo_position_wrap_period = "position_wrap_period"; ///< Optional single-turn feedback wrap period (relative radian). const std::string fn_servo_spring_home_pos = "spring_home_pos"; ///< Field name for servo home position (relative radian). diff --git a/native/pi_control/include/pi_device_effector.hpp b/native/pi_control/include/pi_device_effector.hpp index 3d5d8b8..9e74351 100644 --- a/native/pi_control/include/pi_device_effector.hpp +++ b/native/pi_control/include/pi_device_effector.hpp @@ -311,6 +311,6 @@ class DeviceEffector : public Device { DeviceArm* p_arm_ = nullptr; ///< Pointer to the attached arm device. float distance_to_torque_ = 0.0f; ///< Distance-to-torque conversion factor (Nm/rad). - float grip_spring_offset_ = 0.0f; ///< Torque-mode spring offset (rad), subtracted from the position error (monopi ControlFollowGripper "offset"; a per-installation zero trim -- prefer adjusting the servo zero). + float grip_spring_offset_ = 0.0f; ///< Torque-mode spring offset (rad), subtracted from the position error; a per-installation zero trim -- prefer adjusting the servo zero. bool open_at_min_ = false; ///< Open side of the effector: true if open at min position, false if open at max position. }; diff --git a/native/pi_control/include/pi_device_effector_arx.hpp b/native/pi_control/include/pi_device_effector_can.hpp similarity index 67% rename from native/pi_control/include/pi_device_effector_arx.hpp rename to native/pi_control/include/pi_device_effector_can.hpp index 196133f..4fe7d43 100644 --- a/native/pi_control/include/pi_device_effector_arx.hpp +++ b/native/pi_control/include/pi_device_effector_can.hpp @@ -1,26 +1,26 @@ /*! - * @file pi_device_effector_arx.hpp - * @brief ARX effector device implementation. + * @file pi_device_effector_can.hpp + * @brief MIT-mode CAN effector device implementation. */ #pragma once #include "pi_device_effector.hpp" /*! - * @brief ARX effector device implementation. + * @brief MIT-mode CAN effector device implementation. */ -class DeviceEffectorArx : public DeviceEffector { +class DeviceEffectorCan : public DeviceEffector { public: /*! * @brief Constructor. * @param cla Command-line arguments. */ - DeviceEffectorArx(const CommandLineArgs& cla); + DeviceEffectorCan(const CommandLineArgs& cla); /*! * @brief Destructor. */ - ~DeviceEffectorArx(); + ~DeviceEffectorCan(); // // Override functions @@ -35,9 +35,9 @@ class DeviceEffectorArx : public DeviceEffector { virtual ReturnCode move_joint_with_torque(Joint *p_joint, float target_pos) override; /*! - * @brief Sets control mode for ARX effector. + * @brief Sets control mode for MIT-mode CAN effector. * - * ARX family does not require special leader/follower mode switching here; enabling and + * MIT-mode CAN devices do not require special leader/follower mode switching here; enabling and * the regular command path is sufficient. We keep this as a no-op to satisfy Device API. */ virtual ReturnCode set_control_mode(Role target_role, ControlModeIntent intent) override; diff --git a/native/pi_control/include/pi_device_effector_controller.hpp b/native/pi_control/include/pi_device_effector_controller.hpp index 42ceef9..40c6574 100644 --- a/native/pi_control/include/pi_device_effector_controller.hpp +++ b/native/pi_control/include/pi_device_effector_controller.hpp @@ -4,20 +4,20 @@ */ #pragma once -#include "pi_device_effector_arx.hpp" +#include "pi_device_effector_can.hpp" /*! * @class DeviceEffectorController * @brief Effector device for grippers driven by a whole-arm controller (DriverController). * - * Inherits the ARX torque-gripper motion logic (distance-to-torque via + * Inherits the CAN torque-gripper motion logic (distance-to-torque via * apply_torque_with_damping(), which ServoController maps to controller - * external efforts). Unlike ARX, controller-managed grippers need real + * external efforts). Unlike CAN grippers, controller-managed grippers need real * leader/follower mode switching (position vs external effort on the vendor * controller), so set_control_mode() restores the DeviceEffector base * behavior of delegating to Joint::change_control_mode_for_{leader,follower}(). */ -class DeviceEffectorController : public DeviceEffectorArx { +class DeviceEffectorController : public DeviceEffectorCan { public: /*! * @brief Constructor. @@ -32,7 +32,7 @@ class DeviceEffectorController : public DeviceEffectorArx { /*! * @brief Delegates mode switching to the joints (DeviceEffector base - * behavior), undoing the ARX no-op override. + * behavior), undoing the CAN no-op override. * @param target_role Target role (LEADER or FOLLOWER). * @param intent Control mode intent. * @return ReturnCode indicating success or failure. diff --git a/native/pi_control/include/pi_device_effector_nello.hpp b/native/pi_control/include/pi_device_effector_serial.hpp similarity index 78% rename from native/pi_control/include/pi_device_effector_nello.hpp rename to native/pi_control/include/pi_device_effector_serial.hpp index aeffbf0..c858810 100644 --- a/native/pi_control/include/pi_device_effector_nello.hpp +++ b/native/pi_control/include/pi_device_effector_serial.hpp @@ -1,32 +1,32 @@ /*! - * @file pi_device_effector_nello.hpp - * @brief Nello effector device implementation. + * @file pi_device_effector_serial.hpp + * @brief serial bus-servo effector device implementation. */ #pragma once #include "pi_device_effector.hpp" /*! - * @brief Nello effector device implementation. + * @brief serial bus-servo effector device implementation. */ -class DeviceEffectorNello : public DeviceEffector { +class DeviceEffectorSerial : public DeviceEffector { public: /*! * @brief Constructor. * @param cla Command-line arguments. */ - DeviceEffectorNello(const CommandLineArgs& cla); + DeviceEffectorSerial(const CommandLineArgs& cla); /*! * @brief Destructor. */ - ~DeviceEffectorNello(); + ~DeviceEffectorSerial(); // // Override functions // /*! - * @brief Initializes the Nello effector device. + * @brief Initializes the serial bus-servo effector device. * @param cla Command-line arguments. * @param argc Argument count. * @param argv Argument values. @@ -52,9 +52,9 @@ class DeviceEffectorNello : public DeviceEffector { virtual ReturnCode move_joint_with_torque(Joint* p_joint, float target_pos) override; /*! - * @brief Sets control mode for Nello effector. + * @brief Sets control mode for serial bus-servo effector. * - * Nello: leader and follower require different servo operation modes, and follower behavior also depends on + * Serial bus-servo devices: leader and follower require different servo operation modes, and follower behavior also depends on * effector control type (torque vs position). READY_MOVE_OVERRIDE forces a safe position-based mode via the * base-class override flag. */ diff --git a/native/pi_control/include/pi_driver.hpp b/native/pi_control/include/pi_driver.hpp index 9349896..80b3b82 100644 --- a/native/pi_control/include/pi_driver.hpp +++ b/native/pi_control/include/pi_driver.hpp @@ -162,7 +162,7 @@ class Driver { protected: // RegisteredServo / lock_registered_servo are protected (not private) so - // driver subclasses (e.g. DriverArx::arm_comm_loss_protection) can take a + // driver subclasses (e.g. DriverCanMit::arm_comm_loss_protection) can take a // locked registry snapshot with the same discipline as the parsers. class RegisteredServo { public: diff --git a/native/pi_control/include/pi_driver_arx_encoder.hpp b/native/pi_control/include/pi_driver_arx_encoder.hpp index 22b7457..4eb2141 100644 --- a/native/pi_control/include/pi_driver_arx_encoder.hpp +++ b/native/pi_control/include/pi_driver_arx_encoder.hpp @@ -5,21 +5,21 @@ #pragma once -#include "pi_driver_arx.hpp" +#include "pi_driver_can_mit.hpp" /*! * @brief Read-only CAN driver for an ARX encoder leader arm. * * Each joint carries a passive encoder that broadcasts a fixed 2-byte * mechanical angle at 200 Hz on its own CAN id (no servo, no torque, no - * enable handshake). This driver inherits ``DriverArx`` to reuse its + * enable handshake). This driver inherits ``DriverCanMit`` to reuse its * ``open``/``close`` reception loop, the ``ReceivedServoData`` cache, * ``read_hardware_values`` (angle -> ``curr_pos_abs_``) and the staleness * scan in ``group_read_hardware_values``. Only the per-frame parse differs * (different CAN id range and payload), and all actuation entry points are * no-ops so nothing is ever written to the bus. */ -class DriverArxEncoder : public DriverArx { +class DriverArxEncoder : public DriverCanMit { public: /*! * @brief Constructor. diff --git a/native/pi_control/include/pi_driver_arx.hpp b/native/pi_control/include/pi_driver_can_mit.hpp similarity index 92% rename from native/pi_control/include/pi_driver_arx.hpp rename to native/pi_control/include/pi_driver_can_mit.hpp index 746c588..2c35861 100644 --- a/native/pi_control/include/pi_driver_arx.hpp +++ b/native/pi_control/include/pi_driver_can_mit.hpp @@ -1,6 +1,6 @@ /*! - * @file pi_driver_arx.hpp - * @brief DriverArx class for ARX device communication via CAN interface. + * @file pi_driver_can_mit.hpp + * @brief DriverCanMit class for MIT-mode CAN device communication. */ #pragma once @@ -26,7 +26,7 @@ class ServoDm; * ``ServoCanPassiveEncoder::parse_encoder_status``). A default-constructed * value (detect via ``Profile::is_zero``) means no frame has ever been * parsed for this slot. The control loop reads this via - * ``DriverArx::get_last_update_perf`` to detect a CAN-dead servo + * ``DriverCanMit::get_last_update_perf`` to detect a CAN-dead servo * regardless of the cached pos / vel / tor magnitude. */ class ReceivedServoData { @@ -58,21 +58,21 @@ class PassiveEncoderRoute { }; /*! - * @brief Driver implementation for ARX devices using CAN bus communication. + * @brief Driver implementation for MIT-mode CAN devices. */ -class DriverArx : public DriverCan { +class DriverCanMit : public DriverCan { public: /*! * @brief Constructor. * @param p_device Pointer to the Device instance. * @param cla Command-line arguments. */ - explicit DriverArx(Device* p_device, const CommandLineArgs& cla); + explicit DriverCanMit(Device* p_device, const CommandLineArgs& cla); /*! * @brief Destructor. */ - ~DriverArx(); + ~DriverCanMit(); /*! * @brief Opens the CAN control port and starts message reception. @@ -98,12 +98,12 @@ class DriverArx : public DriverCan { * @brief Frame-age based bulk read. * * Unlike DXL where ``group_read_hardware_values()`` actually probes the - * bus, the ARX/CAN path receives status frames asynchronously on a - * background thread (see ``DriverArx::handle_received_message``). This + * bus, the MIT CAN path receives status frames asynchronously on a + * background thread (see ``DriverCanMit::handle_received_message``). This * override therefore scans the cached ``last_update_perf_`` of every * servo bound to this driver: if any servo's most recent frame is older - * than the threshold (`ARX_STALE_FRAME_AGE_NORMAL_MS` once any motor has - * responded at least once, `ARX_STALE_FRAME_AGE_INITIAL_MS` until then), + * than the threshold (`CAN_MIT_STALE_FRAME_AGE_NORMAL_MS` once any motor has + * responded at least once, `CAN_MIT_STALE_FRAME_AGE_INITIAL_MS` until then), * the servo is inserted into ``dead_servo_ids_``, * ``last_failed_servo_id_`` is updated to the lowest stale id, and * ``FAIL`` is returned. ``DeviceArm::read_hardware_values`` / @@ -329,6 +329,18 @@ class DriverArx : public DriverCan { */ void drain_startup_frames(); + /*! + * @brief Queries one ENCOS servo's MIT SPD/TOR codec ranges and adopts them. + * + * The wire ranges are per-motor firmware parameters (technical document + * 9.2.6/9.2.7), so the compiled defaults can mis-scale torque commands and + * feedback by the ratio of the actual range to the assumed one. Runs inside + * the arm_comm_loss_protection() pass (reception stopped, transaction lock + * held) so replies are drained synchronously. A failed query keeps the + * compiled default and logs loudly; it is non-fatal. + */ + void query_and_adopt_encos_mit_ranges(int id); + protected: /*! * @brief Callback function to handle received CAN messages from servos. @@ -394,8 +406,8 @@ class DriverArx : public DriverCan { /*! * @brief Latched flag: ``true`` once any servo on this driver has * produced at least one parsed status frame. Picks between - * ``ARX_STALE_FRAME_AGE_INITIAL_MS`` (start-up, longer tolerance) - * and ``ARX_STALE_FRAME_AGE_NORMAL_MS`` (steady state, tighter + * ``CAN_MIT_STALE_FRAME_AGE_INITIAL_MS`` (start-up, longer tolerance) + * and ``CAN_MIT_STALE_FRAME_AGE_NORMAL_MS`` (steady state, tighter * tolerance) inside ``group_read_hardware_values()``. */ bool any_motor_moved_ = false; diff --git a/native/pi_control/include/pi_joint.hpp b/native/pi_control/include/pi_joint.hpp index c4dd7b2..1d1209d 100644 --- a/native/pi_control/include/pi_joint.hpp +++ b/native/pi_control/include/pi_joint.hpp @@ -297,6 +297,22 @@ class Joint { return servos_[reference_servo_index_]->get_tor_nm(); } + /*! + * @brief MIT codec report of the reference servo (effective codec + motor-reported ranges). + * @return true when the reference servo has an MIT codec, false otherwise. + */ + bool get_mit_codec_report(Servo::MitCodecReport& report) { + if (servos_.size() == 0 || reference_servo_index_ >= (int)servos_.size()) { + PI_ERROR("Servo is not initialized in get_mit_codec_report(): Joint%d", id_); + return false; + } + if (!servos_[reference_servo_index_]->get_mit_codec_report(report)) { + return false; + } + report.torq_rescale = torq_rescale_; + return true; + } + /*! * @brief Age of the hardware frame backing the reference servo's position. * @return Frame age in milliseconds, or -1 when unknown/untracked. diff --git a/native/pi_control/include/pi_servo.hpp b/native/pi_control/include/pi_servo.hpp index baf87bd..8448981 100644 --- a/native/pi_control/include/pi_servo.hpp +++ b/native/pi_control/include/pi_servo.hpp @@ -4,6 +4,7 @@ */ #pragma once +#include #include #include @@ -71,6 +72,7 @@ class Servo { float kv_ = 0; ///< Velocity conversion constant: servo_value * kv_ = velocity in rad/sec. float ka_ = 0; ///< Current conversion constant: servo_value * ka_ = current in mA. int dir_invert_ = 1; ///< Direction inversion flag: 1 = normal direction, -1 = inverted direction. + bool abs_position_ = false; ///< Sign-agnostic position reads: relative positions are reported as their absolute value. For read-only encoders whose feedback sign varies per unit (ARX_ENC gripper: the vendor reference applies abs() so left/right gripper hardware read identically). Never set on actuated servos: the relative-to-absolute command conversion stays sign-preserving. float zero_pos_abs_ = 0; ///< Zero/home position in absolute radian (reference for relative coordinates). float position_wrap_period_ = 0; ///< Optional single-turn feedback period; 0 disables startup unwrapping. float position_wrap_offset_rel_ = 0; ///< Runtime whole-turn offset added in the relative position frame. @@ -107,6 +109,40 @@ class Servo { */ virtual ~Servo(); + /*! + * @brief Snapshot of a servo's MIT codec ranges and the motor-reported firmware ranges. + * + * The effective codec ranges scale every wire command/status; the reported ranges come + * from the motor's own firmware registers (ENCOS range query) and expose batch + * differences (e.g. ENCOS TOR registers of 30 vs 42 Nm) that make one torq_rescale + * calibration invalid for another arm. Reported ranges are absent for motor families + * without a range query (valid flags false). + */ + struct MitCodecReport { + float codec_vel_min = 0.0f; ///< Effective codec velocity range minimum (rad/s). + float codec_vel_max = 0.0f; ///< Effective codec velocity range maximum (rad/s). + float codec_tor_min = 0.0f; ///< Effective codec torque range minimum (Nm). + float codec_tor_max = 0.0f; ///< Effective codec torque range maximum (Nm). + bool reported_spd_valid = false; ///< True when the motor answered the SPD-range query. + float reported_spd_min = 0.0f; ///< Motor-reported SPD range minimum (rad/s). + float reported_spd_max = 0.0f; ///< Motor-reported SPD range maximum (rad/s). + bool reported_tor_valid = false; ///< True when the motor answered the TOR-range query. + float reported_tor_min = 0.0f; ///< Motor-reported TOR range minimum (Nm). + float reported_tor_max = 0.0f; ///< Motor-reported TOR range maximum (Nm). + float pos_kp = 0.0f; ///< Effective position kp sent in position frames. + float pos_kd = 0.0f; ///< Position kd sent in position frames. + float torq_rescale = 1.0f; ///< Applied gravity-delivery torque rescale (filled by Joint). + }; + + /*! + * @brief Fills the MIT codec report for this servo. + * @return true when the servo has an MIT codec (DM/ENCOS families), false otherwise. + */ + virtual bool get_mit_codec_report(MitCodecReport& report) const { + (void)report; + return false; + } + /*! * @brief Safely parks the servo before shutdown. * @return ReturnCode indicating success or failure. @@ -212,7 +248,7 @@ class Servo { /*! * @brief Enables or disables torque output of the servo. * - * Virtual so device code (e.g. DeviceEffectorNello) can toggle torque on + * Virtual so device code (e.g. DeviceEffectorSerial) can toggle torque on * any bus-servo family without casting to a concrete type. The default * implementation fast-fails for servo families without a torque enable * register. @@ -229,7 +265,7 @@ class Servo { * @brief Applies torque while retaining the servo's configured derivative damping. * * Servo families that do not support a separate damping gain fall back to - * their normal torque command. This is used by monopi-style gripper + * their normal torque command. This is used by torque-spring gripper * control; arm gravity/torque commands continue to use apply_torque(). * * @param torque Torque to be applied in Nm. @@ -242,7 +278,8 @@ class Servo { * @return Current position in relative radian. */ float get_pos_rad_relative() { - return (curr_pos_abs_ - zero_pos_abs_) * dir_invert_ + position_wrap_offset_rel_; + const float relative = (curr_pos_abs_ - zero_pos_abs_) * dir_invert_ + position_wrap_offset_rel_; + return abs_position_ ? std::fabs(relative) : relative; } /*! @@ -251,7 +288,8 @@ class Servo { * @return The converted relative radian value. */ float get_pos_rad_relative(float rad_absolute) { - return (rad_absolute - zero_pos_abs_) * dir_invert_ + position_wrap_offset_rel_; + const float relative = (rad_absolute - zero_pos_abs_) * dir_invert_ + position_wrap_offset_rel_; + return abs_position_ ? std::fabs(relative) : relative; } /*! @@ -346,7 +384,7 @@ class Servo { * cached position. * * Servo types whose driver keeps a receive-time stamp (ServoDm via the - * DriverArx cache) override this; the base returns -1 (unknown) so + * DriverCanMit cache) override this; the base returns -1 (unknown) so * publishers can tell "freshness not tracked" apart from a real age. * * @return Frame age in milliseconds, or -1 when the servo type does not @@ -374,9 +412,9 @@ class Servo { * Move-to-ready and emergency recovery must NOT inherit that weak spring: * those moves have to carry the arm against gravity, and at gain 0.1-0.3 * the wrist joints get kp 1-3, the move stalls, per-joint stuck detection - * latches, and the device force-parks mid-recovery and falls. The ARX + * latches, and the device force-parks mid-recovery and falls. The CAN-MIT * family has no control-mode switch to escape through - * (DeviceArmArx::set_control_mode is a no-op), so the escape lives here. + * (DeviceArmCan::set_control_mode is a no-op), so the escape lives here. * * @return Adjusted position proportional gain (Kp). */ diff --git a/native/pi_control/include/pi_servo_can_encoder.hpp b/native/pi_control/include/pi_servo_can_encoder.hpp index 65bbf10..42561a9 100644 --- a/native/pi_control/include/pi_servo_can_encoder.hpp +++ b/native/pi_control/include/pi_servo_can_encoder.hpp @@ -14,7 +14,7 @@ #pragma once #include -#include "pi_driver_arx.hpp" +#include "pi_driver_can_mit.hpp" #include "pi_servo.hpp" #define PASSIVE_ENCODER_RESPONSE_LEN 6 ///< Response payload: device_id (u8) + position (i16) + velocity (i16) + digital_inputs (u8), big-endian. @@ -130,7 +130,7 @@ class ServoCanPassiveEncoder : public Servo { using Clock = std::chrono::steady_clock; - DriverArx* p_driver_can_ = nullptr; ///< Pointer to the CAN driver (cast from base Driver pointer). + DriverCanMit* p_driver_can_ = nullptr; ///< Pointer to the CAN driver (cast from base Driver pointer). int response_can_id_ = -1; ///< CAN id the encoder answers on (default id + 1). int button_num_ = 2; ///< Number of buttons carried in the digital-inputs byte. uint32_t last_update_count_ = 0; ///< Cache update counter at the previous read. diff --git a/native/pi_control/include/pi_servo_dm.hpp b/native/pi_control/include/pi_servo_dm.hpp index 1a7052f..5d997e5 100644 --- a/native/pi_control/include/pi_servo_dm.hpp +++ b/native/pi_control/include/pi_servo_dm.hpp @@ -4,7 +4,9 @@ */ #pragma once -#include "pi_driver_arx.hpp" +#include + +#include "pi_driver_can_mit.hpp" #include "pi_servo.hpp" /*! @@ -132,7 +134,7 @@ class ServoDm : public Servo { /*! * @brief Age of the newest parsed CAN frame for this servo, from the - * DriverArx receive cache (``last_update_perf_``). Note this is + * DriverCanMit receive cache (``last_update_perf_``). Note this is * bus liveness, not position freshness: some frame types (ENCOS * config acks, non-position ack statuses) refresh the stamp * without updating the cached position. @@ -144,7 +146,7 @@ class ServoDm : public Servo { * @brief Confirms that ``received_servo_data_`` for this servo has been * populated by at least one parsed status frame. DM motors do not * broadcast status spontaneously, so without a successful - * ``DriverArx::enable()`` response parse the cache stays zero and + * ``DriverCanMit::enable()`` response parse the cache stays zero and * ``curr_pos_abs_`` would be a stale 0. The check looks at * ``motor_id_`` which is zero only when the cache has never been * touched (real DM IDs are 1+). @@ -206,12 +208,12 @@ class ServoDm : public Servo { * @param p_frame Pointer to the received CAN frame. * @param p_received_servo_data Pointer to the buffer where parsed servo data will be stored. * @param p_find_data_index Function pointer to find the data buffer index for a given servo ID. - * @param p_driver_arx Pointer to the CAN driver object. + * @param p_driver_can_mit Pointer to the CAN driver object. * @return ReturnCode indicating success or failure. */ static ReturnCode parse_dm_servo_status(DriverCan::can_frame_t* p_frame, ReceivedServoData* p_received_servo_data, - DriverArx::func_find_data_index_t p_find_data_index, - DriverArx* p_driver_arx); + DriverCanMit::func_find_data_index_t p_find_data_index, + DriverCanMit* p_driver_can_mit); /*! * @brief Parses an Encos servo status message from a CAN frame. @@ -222,7 +224,7 @@ class ServoDm : public Servo { */ static ReturnCode parser_encos_servo_status(DriverCan::can_frame_t* p_frame, ReceivedServoData* p_received_servo_data, - DriverArx::func_find_data_index_t p_find_data_index); + DriverCanMit::func_find_data_index_t p_find_data_index); /*! * @brief Constructs a CAN command frame for DM servo control. @@ -329,6 +331,68 @@ class ServoDm : public Servo { static ReturnCode parse_can_timeout_reply_encos_servo(const DriverCan::can_frame_t& can_frame, uint16_t motor_id, uint16_t& timeout_ms); + // ENCOS MIT-protocol range queries (technical document 9.3 / reply type 5, + // 10.5). The wire codec ranges are per-motor firmware parameters, so the + // compiled defaults may not match a given motor (some motor batches ship + // with a wider TOR range than the EC-A4310-P2-36 factory default). + static constexpr uint8_t ENCOS_QUERY_SPD_RANGE = 26; ///< Query code: MIT SPD range (int16 pair, scale 100). + static constexpr uint8_t ENCOS_QUERY_TOR_RANGE = 27; ///< Query code: MIT TOR range (int16 pair, scale 10). + static constexpr float ENCOS_SPD_RANGE_SCALE = 0.01f; ///< rad/s per raw count in a SPD-range reply. + static constexpr float ENCOS_TOR_RANGE_SCALE = 0.1f; ///< Nm per raw count in a TOR-range reply. + + /*! + * @brief Constructs an ENCOS config-query frame for an MIT-protocol range. + * @param can_frame Reference to the CAN frame structure to be filled. + * @param motor_id The CAN ID of the target motor. + * @param query_code ENCOS_QUERY_SPD_RANGE or ENCOS_QUERY_TOR_RANGE. + * @return ReturnCode indicating success or failure. + */ + static ReturnCode can_frame_to_get_mit_range_encos_servo(DriverCan::can_frame_t& can_frame, uint16_t motor_id, + uint8_t query_code); + + /*! + * @brief Parses the ACK_QUERY reply to an MIT-range query (codes 26/27). + * @param can_frame The received reply frame. + * @param motor_id Expected motor CAN ID. + * @param query_code Expected query code echoed in the reply. + * @param scale Physical units per raw count (ENCOS_SPD_RANGE_SCALE / ENCOS_TOR_RANGE_SCALE). + * @param range_min Out: queried range minimum in physical units. + * @param range_max Out: queried range maximum in physical units. + * @return ReturnCode::SUCCESS when the frame is a matching reply, FAIL otherwise. + */ + static ReturnCode parse_mit_range_reply_encos_servo(const DriverCan::can_frame_t& can_frame, uint16_t motor_id, + uint8_t query_code, float scale, float& range_min, + float& range_max); + + /*! + * @brief Handles a motor-reported MIT codec range for this servo's encode/decode. + * + * SPD ranges are adopted: the compiled parameter set is copied into a + * per-servo override on first use so both the command builder and the + * status parser scale against the motor's actual firmware range. + * + * TOR ranges are verify-and-log only: delivered-torque conformance testing + * showed the physical torque full scale does not follow the firmware register + * (ENCOS A4310 delivers over +-42 Nm while reporting +-30), and the model + * JSON torq_rescale factors are calibrated against the compiled codec, so + * adopting the register would silently shift the gravity feed-forward + * delivery. A mismatch is logged loudly and the compiled codec is kept. + * + * @param query_code ENCOS_QUERY_SPD_RANGE or ENCOS_QUERY_TOR_RANGE. + * @param range_min Queried range minimum in physical units. + * @param range_max Queried range maximum in physical units. + * @return ReturnCode::SUCCESS, or FAIL when the values are not a sane range. + */ + ReturnCode adopt_encos_mit_range(uint8_t query_code, float range_min, float range_max); + + /*! + * @brief MIT codec report: effective codec ranges plus the motor-reported firmware ranges. + * + * Lets clients (gravity_tune) compare the servo parameters of two arms before assuming + * one torq_rescale calibration fits both (ENCOS batches report different TOR registers). + */ + bool get_mit_codec_report(MitCodecReport& report) const override; + protected: /*! * @brief Initializes the current estimation system for the DM servo. @@ -342,9 +406,20 @@ class ServoDm : public Servo { const char* trigger); ReturnCode reject_if_thermal_fault_latched() const; - DriverArx* p_driver_can_ = nullptr; ///< Pointer to the CAN driver (cast from base Driver pointer). + DriverCanMit* p_driver_can_ = nullptr; ///< Pointer to the CAN driver (cast from base Driver pointer). HoldChecker checker_motor_no_response_; ///< Checker for detecting motor communication failures. bool motor_moved_ = false; ///< Flag indicating whether the motor has moved from its initial position. uint8_t last_reported_fault_code_ = 0; ///< Last DM fault logged, to avoid repeating it every control loop. bool thermal_fault_latched_ = false; ///< Thermal effector fault: output was disabled and must not be re-enabled. + /// Per-servo codec parameter override, populated when an ENCOS MIT-range + /// query reports a range that must replace the compiled default. + std::optional encos_param_override_; + /// Motor-reported MIT firmware ranges (ENCOS range query), kept verbatim for the + /// client servo-parameter report even when the compiled codec is retained. + bool reported_spd_range_valid_ = false; + float reported_spd_min_ = 0.0f; ///< Motor-reported SPD range minimum (rad/s). + float reported_spd_max_ = 0.0f; ///< Motor-reported SPD range maximum (rad/s). + bool reported_tor_range_valid_ = false; + float reported_tor_min_ = 0.0f; ///< Motor-reported TOR range minimum (Nm). + float reported_tor_max_ = 0.0f; ///< Motor-reported TOR range maximum (Nm). }; diff --git a/native/pi_control/include/pi_topic.hpp b/native/pi_control/include/pi_topic.hpp index 0fa8bdf..836cc45 100644 --- a/native/pi_control/include/pi_topic.hpp +++ b/native/pi_control/include/pi_topic.hpp @@ -31,6 +31,7 @@ #define DEVICE_COMMAND_SET_FORCE_FEEDBACK_GAIN 32 #define DEVICE_COMMAND_HOLD 33 #define DEVICE_COMMAND_HEARTBEAT 34 ///< Client-liveness heartbeat; payload-free, arms the dead-client watchdog. +#define DEVICE_COMMAND_SET_TORQ_RESCALE 35 ///< Runtime per-joint torq_rescale update (float params, one per arm joint). #define DEVICE_INFO_READY_NOW 1 ///< Device is ready; param_int[0] is the completed lifecycle request id, if any. #define DEVICE_INFO_EFFECTOR 10 ///< Device info code indicating that the device is an effector. @@ -59,6 +60,16 @@ #define DEVICE_INFO_PROTOCOL_HANDSHAKE 30 #define DEVICE_INFO_COMMAND_ACK 31 #define DEVICE_INFO_RUNTIME_MODE 32 +// Per-joint servo parameter report (one message per arm joint, re-announced with the +// handshake so late subscribers always receive it): +// param_int[0] = joint index (0-based) +// param_int[1] = motor answered the SPD-range query (0/1) +// param_int[2] = motor answered the TOR-range query (0/1) +// param_float[0..1] = effective codec velocity range min/max (rad/s) +// param_float[2..3] = effective codec torque range min/max (Nm) +// param_float[4..5] = motor-reported SPD range min/max (rad/s; valid per param_int[1]) +// param_float[6..7] = motor-reported TOR range min/max (Nm; valid per param_int[2]) +#define DEVICE_INFO_SERVO_PARAM 33 #define PI_CONTROL_PROTOCOL_VERSION_MAJOR 1 #define PI_CONTROL_PROTOCOL_VERSION_MINOR 1 diff --git a/native/pi_control/src/pi_command_line_args.cpp b/native/pi_control/src/pi_command_line_args.cpp index e6c8508..c891e65 100644 --- a/native/pi_control/src/pi_command_line_args.cpp +++ b/native/pi_control/src/pi_command_line_args.cpp @@ -6,6 +6,11 @@ #include "pi_command_line_args.hpp" +#include +#include +#include +#include + #include #include "pi_device.hpp" @@ -14,6 +19,30 @@ namespace po = boost::program_options; +std::vector CommandLineArgs::parse_torq_rescale_csv(const std::string& csv) { + std::vector values; + std::istringstream stream(csv); + std::string token; + while (std::getline(stream, token, ',')) { + std::size_t consumed = 0; + float value = 0.0f; + try { + value = std::stof(token, &consumed); + } catch (const std::invalid_argument&) { + return {}; + } catch (const std::out_of_range&) { + return {}; + } + // Reject trailing garbage (e.g. "0.8x") and non-physical values. + while (consumed < token.size() && std::isspace(static_cast(token[consumed]))) consumed++; + if (consumed != token.size() || !std::isfinite(value) || value < 0.0f) { + return {}; + } + values.push_back(value); + } + return values; +} + CommandLineArgs::CommandLineArgs(int argc, char** argv) { // Parse command line arguments po::options_description desc("Allowed options"); @@ -102,9 +131,12 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { OPT_PLANING_TYPE, po::value()->default_value(OPT_PLANING_TYPE_DEFAULT), "Planning type of waypoint generation")( - OPT_ARM_PLANNING_TYPE, - po::value()->default_value(OPT_DEFAULT_NONE), - "Planning type override applied to ARM devices only")(OPT_FORCE_FEEDBACK, + OPT_FOLLOWER_GRAVITY_COMPENSATION, + po::value()->default_value(OPT_FOLLOWER_GRAVITY_COMPENSATION_DEFAULT), + "Follower gravity feed-forward override: config (arm individual JSON decides), true, or false")( + OPT_TORQ_RESCALE, po::value()->default_value(""), + "Per-joint torq_rescale override, comma-separated (one value per arm joint); " + "empty leaves the model/individual config values")(OPT_FORCE_FEEDBACK, po::value()->default_value(0.0f), "Force feedback parameter")( OPT_TOPIC_STATE, po::value()->default_value(""), @@ -517,11 +549,32 @@ CommandLineArgs::CommandLineArgs(int argc, char** argv) { exit(2); } - if (vm.count(OPT_ARM_PLANNING_TYPE)) { - arm_planning_type = vm[OPT_ARM_PLANNING_TYPE].as(); - if (arm_planning_type != OPT_DEFAULT_NONE) { - PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Arm planning type override: %s", - arm_planning_type.c_str()); + if (vm.count(OPT_FOLLOWER_GRAVITY_COMPENSATION)) { + follower_gravity_compensation_override = vm[OPT_FOLLOWER_GRAVITY_COMPENSATION].as(); + if (follower_gravity_compensation_override != OPT_FOLLOWER_GRAVITY_COMPENSATION_DEFAULT && + follower_gravity_compensation_override != "true" && follower_gravity_compensation_override != "false") { + PI_ERROR("--%s must be one of config/true/false, got '%s'", OPT_FOLLOWER_GRAVITY_COMPENSATION, + follower_gravity_compensation_override.c_str()); + exit(2); + } + PI_INFO("main()", InfoLevel::ESSENTIAL_0, "Follower gravity compensation override: %s", + follower_gravity_compensation_override.c_str()); + } else { + PI_ERROR("--%s is not set", OPT_FOLLOWER_GRAVITY_COMPENSATION); + exit(2); + } + + if (vm.count(OPT_TORQ_RESCALE)) { + const std::string torq_rescale_csv = vm[OPT_TORQ_RESCALE].as(); + if (!torq_rescale_csv.empty()) { + torq_rescale_override = parse_torq_rescale_csv(torq_rescale_csv); + if (torq_rescale_override.empty()) { + PI_ERROR("--%s must be comma-separated nonnegative finite floats, got '%s'", OPT_TORQ_RESCALE, + torq_rescale_csv.c_str()); + exit(2); + } + PI_INFO("main()", InfoLevel::ESSENTIAL_0, "torq_rescale override requested: %s", + torq_rescale_csv.c_str()); } } diff --git a/native/pi_control/src/pi_control_node.cpp b/native/pi_control/src/pi_control_node.cpp index b876cbb..92ca5bf 100644 --- a/native/pi_control/src/pi_control_node.cpp +++ b/native/pi_control/src/pi_control_node.cpp @@ -234,6 +234,7 @@ int main(int argc, char** argv) { const int ready_announce_interval_loops = std::max(1, (int)cla.control_frequency / 10); // ~0.1 sec int handshake_announce_counter = 0; + int servo_param_announce_counter = 0; while (g_terminate_signal_received == 0 && p_device->is_running()) { return_code = p_device->step(); @@ -302,6 +303,15 @@ int main(int argc, char** argv) { p_device->publish_device_info(DEVICE_INFO_PROTOCOL_HANDSHAKE, nullptr, &handshake_data); } + // Servo parameters: one joint per publish on a faster cadence (10 Hz), + // round-robin. Pub/sub gives no replay, so the report repeats forever for + // late subscribers — and the status sockets run with a tiny HWM (2), so a + // per-joint burst would be dropped down to its tail (observed: the client + // received 1 of 6 joints when all were published in one tick). + if ((servo_param_announce_counter++ % std::max(1, cla.control_frequency / 10)) == 0) { + p_device->publish_next_servo_param(); + } + const bool ready_now = p_device->is_ready(); if (!ready_now) { // Allow re-announcement when the device re-enters ready state diff --git a/native/pi_control/src/pi_device.cpp b/native/pi_control/src/pi_device.cpp index 44a60ec..c24f765 100644 --- a/native/pi_control/src/pi_device.cpp +++ b/native/pi_control/src/pi_device.cpp @@ -8,12 +8,12 @@ #include #include "pi_device.hpp" -#include "pi_device_arm_arx.hpp" -#include "pi_device_arm_nello.hpp" +#include "pi_device_arm_can.hpp" +#include "pi_device_arm_serial.hpp" #include "pi_device_config.hpp" -#include "pi_device_effector_arx.hpp" +#include "pi_device_effector_can.hpp" #include "pi_device_effector_controller.hpp" -#include "pi_device_effector_nello.hpp" +#include "pi_device_effector_serial.hpp" #include "pi_info.hpp" Device::Device(const CommandLineArgs& cla) @@ -465,23 +465,14 @@ ReturnCode Device::init(const CommandLineArgs& cla, int argc, char** argv, std:: planning_type = cla.planning_type; } - // Arm-scoped override: lets a client enable e.g. gravity-compensated follower - // planning on the arm without dragging the attached effector onto the same - // planner (a trapezoidal gripper would crawl through its travel) and without - // editing the shared model config that leaders also load. - if (type_ == DeviceType::ARM && !cla.arm_planning_type.empty() - && cla.arm_planning_type != OPT_DEFAULT_NONE) { - planning_type = cla.arm_planning_type; - } - PI_INFO("Device", InfoLevel::HELPFUL_1, "%s_%s: planning_type=%s", model_.c_str(), id_.c_str(), planning_type.c_str()); if (planning_type == p_config_model_->val_planning_type_none) { planning_type_ = TrajectoryPlanningType::NONE; - } else if (planning_type == p_config_model_->val_planning_type_slew_pos_gravity) { - planning_type_ = TrajectoryPlanningType::SLEW_POS_GRAVITY; } else { + // Gravity compensation is no longer a planning type; it is the + // independent --follower_gravity_compensation flag. PI_ERROR("Unsupported planning type '%s' for %s_%s", planning_type.c_str(), model_.c_str(), id_.c_str()); return ReturnCode::NOT_SUPPORTED; } @@ -576,9 +567,9 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf std::string arm_type; return_code = cfg_model.get_field_value(cfg_model.values_, cfg_model.fn_arm_type, arm_type); if (return_code == ReturnCode::SUCCESS) { - if (arm_type == cfg_model.val_arm_type_arx) { - p_device = new DeviceArmArx(cla); - PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmArx for %s_%s", cla.device_model.c_str(), + if (arm_type == cfg_model.val_arm_type_can) { + p_device = new DeviceArmCan(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmCan for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); } else if (arm_type == cfg_model.val_arm_type_controller) { // Whole-arm controller arms use the DeviceArm base directly: @@ -587,9 +578,9 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf p_device = new DeviceArm(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArm (controller) for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); - } else if (arm_type == cfg_model.val_arm_type_nello) { - p_device = new DeviceArmNello(cla); - PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmNello for %s_%s", + } else if (arm_type == cfg_model.val_arm_type_serial) { + p_device = new DeviceArmSerial(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceArmSerial for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid arm type: %s", arm_type.c_str()); @@ -603,17 +594,17 @@ Device* Device::new_device(const DeviceConfig& cfg_model, const DeviceConfig& cf std::string effector_type; return_code = cfg_model.get_field_value(cfg_model.values_, cfg_model.fn_effector_type, effector_type); if (return_code == ReturnCode::SUCCESS) { - if (effector_type == cfg_model.val_effector_type_arx) { - p_device = new DeviceEffectorArx(cla); - PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorArx for %s_%s", + if (effector_type == cfg_model.val_effector_type_can) { + p_device = new DeviceEffectorCan(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorCan for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); } else if (effector_type == cfg_model.val_effector_type_controller) { p_device = new DeviceEffectorController(cla); PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorController for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); - } else if (effector_type == cfg_model.val_effector_type_nello) { - p_device = new DeviceEffectorNello(cla); - PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorNello for %s_%s", + } else if (effector_type == cfg_model.val_effector_type_serial) { + p_device = new DeviceEffectorSerial(cla); + PI_INFO("Device", InfoLevel::DETAIL_2, "Created DeviceEffectorSerial for %s_%s", cla.device_model.c_str(), cla.device_id.c_str()); } else { PI_ERROR("Invalid effector type: %s", effector_type.c_str()); @@ -644,20 +635,21 @@ ReturnCode Device::move(Joint* p_joint, float target_pos, float target_tor, p_joint->adjusted_target_pos_ = target_pos; ReturnCode return_code; - if (planning_type_ == TrajectoryPlanningType::SLEW_POS_GRAVITY) { - p_joint->prev_target_pos_ = target_pos; + // The synchronized follower slew integrates from the last commanded + // target, so keep it current for every path. + p_joint->prev_target_pos_ = target_pos; + if (follower_gravity_compensation_) { + // Gravity feed-forward is independent of the planning type: the + // position command carries the model gravity torque computed by the + // caller (DeviceArm::operate_as_follower fills target_tor). + // torq_rescale matches the leader gravity paths exactly: + // Servo::apply_torque rescales internally but the 3-arg move does + // not, so the leader-validated per-joint factor is applied here. p_joint->target_tor_ = target_tor; - return_code = p_joint->move(target_pos, 0, target_tor); + return_code = p_joint->move(target_pos, 0, target_tor * p_joint->torq_rescale_); p_joint->prev_target_tor_ = target_tor; - } else if (planning_type_ == TrajectoryPlanningType::NONE) { - // The synchronized follower slew integrates from the last commanded - // target for every planning type, so keep it current here too. - p_joint->prev_target_pos_ = target_pos; - return_code = p_joint->move(target_pos); } else { - PI_ERROR("Invalid planning type %d in %s_%s", (int)planning_type_, - model_.c_str(), id_.c_str()); - return ReturnCode::INVALID_PARAM; + return_code = p_joint->move(target_pos); } if (return_code != ReturnCode::SUCCESS) { diff --git a/native/pi_control/src/pi_device_arm.cpp b/native/pi_control/src/pi_device_arm.cpp index 013b17b..86681ba 100644 --- a/native/pi_control/src/pi_device_arm.cpp +++ b/native/pi_control/src/pi_device_arm.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include "pi_device_arm.hpp" @@ -45,7 +46,7 @@ ReturnCode DeviceArm::set_control_mode(Role target_role, ControlModeIntent inten // safely from the current pose. // - NORMAL_OPERATION: follow target_role (leader vs follower). // - // The ARX device subclass may override hardware-specific behavior. + // The CAN device subclass may override hardware-specific behavior. const bool use_follower_like = (intent == ControlModeIntent::READY_MOVE_OVERRIDE) || (target_role == Role::FOLLOWER); ReturnCode rc = ReturnCode::SUCCESS; @@ -75,11 +76,124 @@ ReturnCode DeviceArm::set_control_mode(Role target_role, ControlModeIntent inten if (use_follower_like) { reset_slew_targets_to_current(); + // Any (re-)entry into position control ends a calibration gravity + // float: move-to-ready and emergency recovery must never fight the + // float branch of operate_as_follower(). + gravity_float_active_ = false; } return ReturnCode::SUCCESS; } +ReturnCode DeviceArm::set_runtime_gravity_float(bool enabled, float abort_drift_rad) { + if (role_ != Role::FOLLOWER) { + return ReturnCode::NOT_SUPPORTED; + } + if (enabled == gravity_float_active_) { + return ReturnCode::SUCCESS; + } + if (enabled) { + if (!p_algo_) { + PI_ERROR("%s_%s: gravity float needs a gravity-capable algo (URDF-backed); none is initialized", + model_.c_str(), id_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } + if (!std::isfinite(abort_drift_rad) || abort_drift_rad <= 0.0f) { + PI_ERROR("%s_%s: gravity float abort threshold must be a positive radian value, got %.3f", + model_.c_str(), id_.c_str(), abort_drift_rad); + return ReturnCode::INVALID_PARAM; + } + // The runaway baseline: the loop watches |pos - baseline| per joint and + // re-engages HOLD itself. The client is too slow for this judgment + // (ZMQ round trip), so the stop must live here. + gravity_float_baseline_.clear(); + for (auto& p_joint : joints_) { + gravity_float_baseline_.push_back(p_joint->get_pos_rad_relative()); + } + gravity_float_abort_rad_ = abort_drift_rad; + prof_time_t current_time = Profile::get_time_now(); + for (auto& p_joint : joints_) { + ReturnCode return_code = p_joint->change_control_mode_for_leader(current_time); + if (return_code != ReturnCode::SUCCESS) return return_code; + } + gravity_float_active_ = true; + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, + "%s_%s: entering calibration gravity float (gravity feed-forward only; abort drift %.3f rad; " + "HOLD re-engages position control)", + model_.c_str(), id_.c_str(), abort_drift_rad); + return ReturnCode::SUCCESS; + } + for (auto& p_joint : joints_) { + ReturnCode return_code = p_joint->change_control_mode_for_follower(); + if (return_code != ReturnCode::SUCCESS) return return_code; + } + reset_slew_targets_to_current(); + gravity_float_active_ = false; + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, + "%s_%s: leaving calibration gravity float (position control re-engaged at the current pose)", + model_.c_str(), id_.c_str()); + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceArm::set_runtime_torq_rescale(const std::vector& values) { + if ((int)values.size() != dof_) { + PI_ERROR("%s_%s: runtime torq_rescale update has %d values but the arm has %d joints", model_.c_str(), + id_.c_str(), (int)values.size(), dof_); + return ReturnCode::INVALID_PARAM; + } + for (float value : values) { + if (!std::isfinite(value) || value < 0.0f) { + PI_ERROR("%s_%s: runtime torq_rescale values must be finite and nonnegative, got %.3f", + model_.c_str(), id_.c_str(), value); + return ReturnCode::INVALID_PARAM; + } + } + std::ostringstream summary; + for (size_t i = 0; i < joints_.size(); i++) { + joints_[i]->torq_rescale_ = values[i]; + summary << (i > 0 ? ", " : "") << values[i]; + } + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, "%s_%s: torq_rescale updated at runtime: [%s]", model_.c_str(), + id_.c_str(), summary.str().c_str()); + return ReturnCode::SUCCESS; +} + +ReturnCode DeviceArm::publish_next_servo_param() { + if (joints_.empty()) { + return ReturnCode::SUCCESS; + } + const int index = servo_param_publish_index_ % (int)joints_.size(); + servo_param_publish_index_ = (index + 1) % (int)joints_.size(); + Servo::MitCodecReport report; + if (!joints_[index]->get_mit_codec_report(report)) { + return ReturnCode::SUCCESS; + } + // The gravity feed-forward flag is device-level; it rides every joint's + // report so one message is enough to know the applied state. The wire + // format caps a device-info message at 10 floats, so the position gains + // travel in the int slots as milli-units. + std::vector ints = {index, + report.reported_spd_valid ? 1 : 0, + report.reported_tor_valid ? 1 : 0, + follower_gravity_compensation_ ? 1 : 0, + (int)std::lround(report.pos_kp * 1000.0f), + (int)std::lround(report.pos_kd * 1000.0f)}; + std::vector floats = {report.codec_vel_min, report.codec_vel_max, report.codec_tor_min, + report.codec_tor_max, report.reported_spd_min, report.reported_spd_max, + report.reported_tor_min, report.reported_tor_max, report.torq_rescale}; + return publish_device_info(DEVICE_INFO_SERVO_PARAM, &floats, &ints); +} + +ReturnCode DeviceArm::runtime_hold() { + if (gravity_float_active_) { + ReturnCode return_code = set_runtime_gravity_float(false, 0.0f); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + } + return Device::runtime_hold(); +} + ReturnCode DeviceArm::set_runtime_force_feedback(bool enabled, float gain) { ReturnCode rc = Device::set_runtime_force_feedback(enabled, gain); if (rc != ReturnCode::SUCCESS) { @@ -537,6 +651,24 @@ ReturnCode DeviceArm::init(const CommandLineArgs& cla, int argc, char** argv, st model_.c_str(), id_.c_str()); } + // Follower gravity feed-forward: the individual config JSON declares the default and the + // --follower_gravity_compensation override (fed by devices.toml) takes precedence. + bool follower_gravity_requested; + return_code = p_config_individual_->get_field_value(p_config_individual_->values_, + p_config_individual_->fn_follower_gravity_compensation, + follower_gravity_requested); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("%s_%s: follower_gravity_compensation is not defined in the individual configuration " + "(required since config_version 1.3.0)", + model_.c_str(), id_.c_str()); + return return_code; + } + const char* follower_gravity_source = "individual config default"; + if (cla_.follower_gravity_compensation_override != OPT_FOLLOWER_GRAVITY_COMPENSATION_DEFAULT) { + follower_gravity_requested = (cla_.follower_gravity_compensation_override == "true"); + follower_gravity_source = "devices.toml override (beats the individual config)"; + } + return_code = enable_spring_effect(spring_effect); if (return_code != ReturnCode::SUCCESS) { PI_ERROR("Failed to enable spring effect for %s_%s", model_.c_str(), id_.c_str()); @@ -563,6 +695,37 @@ ReturnCode DeviceArm::init(const CommandLineArgs& cla, int argc, char** argv, st dof_ = (int)joints_.size(); PI_INFO("DeviceArm", InfoLevel::HELPFUL_1, "DeviceArm %s_%s: DOF=%d", model_.c_str(), id_.c_str(), dof_); + // Highest-precedence torq_rescale override (devices.toml [arms] torq_rescale + // -> --torq_rescale): applied after the model and individual configs so a + // per-unit gravity-delivery calibration needs no JSON edit or wheel rebuild. + if (!cla_.torq_rescale_override.empty()) { + if ((int)cla_.torq_rescale_override.size() != dof_) { + PI_ERROR("%s_%s: --%s has %d values but the arm has %d joints", model_.c_str(), id_.c_str(), + OPT_TORQ_RESCALE, (int)cla_.torq_rescale_override.size(), dof_); + return ReturnCode::INVALID_PARAM; + } + for (size_t i = 0; i < joints_.size(); i++) { + joints_[i]->torq_rescale_ = cla_.torq_rescale_override[i]; + } + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, + "%s_%s: torq_rescale overridden by devices.toml [arms] torq_rescale (beats model and " + "individual configs)", + model_.c_str(), id_.c_str()); + } + + // One-line torque-delivery summary so an A/B run comparison can be + // reconstructed from the logs alone: effective wire torque per joint is + // torq_rescale x MIT codec full scale (the codec ranges are logged by each + // servo at connect), clipped to torq_max. + std::ostringstream rescale_summary; + std::ostringstream torq_max_summary; + for (size_t i = 0; i < joints_.size(); i++) { + rescale_summary << (i > 0 ? ", " : "") << joints_[i]->torq_rescale_; + torq_max_summary << (i > 0 ? ", " : "") << joints_[i]->torq_max_; + } + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, "%s_%s: torq_rescale=[%s] torq_max=[%s]", model_.c_str(), + id_.c_str(), rescale_summary.str().c_str(), torq_max_summary.str().c_str()); + for (int i = 0; i < dof_; i++) { tele_pos_.push_back(0); tele_vel_.push_back(0); @@ -671,6 +834,60 @@ ReturnCode DeviceArm::init(const CommandLineArgs& cla, int argc, char** argv, st model_.c_str(), id_.c_str()); } + // Follower gravity feed-forward: model gravity torque is + // sent with every position command so the position gains only correct + // tracking error instead of holding the arm against gravity. Independent + // of the planning type. Fail fast on arms that cannot honor it -- silently + // continuing would run un-compensated gains the operator did not ask for. + std::string arm_type; + return_code = p_config_model_->get_field_value(p_config_model_->values_, p_config_model_->fn_arm_type, arm_type); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("%s_%s: arm type is not defined in the model configuration", model_.c_str(), id_.c_str()); + return return_code; + } + const bool is_controller_arm = (arm_type == p_config_model_->val_arm_type_controller); + + if (role_ == Role::FOLLOWER && follower_gravity_requested) { + if (is_controller_arm) { + // Whole-arm controllers (Trossen) compute gravity/friction compensation inside the + // vendor controller in every active mode, so the request is already satisfied without + // streaming torque from this node. + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, + "%s_%s: follower gravity compensation ON via the vendor controller " + "(built-in, always active; source: %s)", + model_.c_str(), id_.c_str(), follower_gravity_source); + } else if (!supports_torque_feed_forward()) { + PI_ERROR("%s_%s: follower gravity compensation was requested but this arm type cannot " + "take a torque feed-forward (serial bus servos are position-only). Remove the " + "option for this arm.", + model_.c_str(), id_.c_str()); + return ReturnCode::NOT_SUPPORTED; + } else if (!p_algo_ || !p_algo_->has_gravity_model()) { + PI_ERROR("%s_%s: follower gravity compensation was requested but the arm has no dynamics " + "model (a URDF-backed algo such as Pinocchio is required).", + model_.c_str(), id_.c_str()); + return ReturnCode::NOT_INITIALIZED; + } else { + enabled_gravity_compensation_ = true; + follower_gravity_compensation_ = true; + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, + "%s_%s: follower gravity compensation ON (source: %s)", + model_.c_str(), id_.c_str(), follower_gravity_source); + } + } else if (role_ == Role::FOLLOWER && is_controller_arm) { + // The vendor controller has no API to disable its built-in compensation, so an explicit + // "off" cannot be honored on controller arms. Warn instead of failing: position tracking + // still behaves as it always has on this hardware. + PI_WARN("%s_%s: follower gravity compensation is requested off (source: %s), but the vendor " + "controller's built-in compensation cannot be disabled and stays active", + model_.c_str(), id_.c_str(), follower_gravity_source); + } else if (role_ == Role::FOLLOWER) { + // Explicit OFF trace so an A/B comparison can be reconstructed from the logs alone. + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, + "%s_%s: follower gravity compensation OFF (source: %s)", + model_.c_str(), id_.c_str(), follower_gravity_source); + } + return ReturnCode::SUCCESS; } @@ -701,7 +918,7 @@ ReturnCode DeviceArm::verify_servos_operational() { any_probe_sent = true; } if (any_probe_sent) { - // Flush the probe to the bus. CAN drivers (ARX) transmit inside move() and this + // Flush the probe to the bus. CAN drivers (DriverCanMit) transmit inside move() and this // is a no-op there, but the call keeps the sequence correct for any queued // group-write driver added later. if (p_driver_ != nullptr) { @@ -1572,15 +1789,69 @@ ReturnCode DeviceArm::operate_as_follower() { return ReturnCode::NOT_SUPPORTED; } - if (!p_algo_ && planning_type_ == TrajectoryPlanningType::SLEW_POS_GRAVITY) { + if (!p_algo_ && follower_gravity_compensation_) { PI_ERROR("Algorithm handler is not initialized in operate_as_follower()"); return ReturnCode::NOT_INITIALIZED; } + if (gravity_float_active_) { + // Calibration gravity float (gravity_tune / arm_check on a follower + // node): the joints are in leader control mode, so the arm rests on the + // model gravity feed-forward alone -- the exact float the leader path + // applies when teleop is disengaged. Live position commands are ignored + // until HOLD (or a move-to-ready) re-engages position control. + // + // In-loop runaway watchdog: judged here (at the control rate), NOT by + // the client -- a ZMQ round trip is far too slow, and by the time a + // Python monitor reacts the arm has fallen well past the threshold. + int i = 0; + for (auto& p_joint : joints_) { + const float drift = p_joint->get_pos_rad_relative() - gravity_float_baseline_[i]; + if (std::fabs(drift) > gravity_float_abort_rad_) { + PI_INFO("DeviceArm", InfoLevel::ESSENTIAL_0, + "%s_%s: gravity float runaway (joint %d drifted %.3f rad > %.3f) -- re-engaging HOLD " + "at the current pose", + model_.c_str(), id_.c_str(), p_joint->id_, drift, gravity_float_abort_rad_); + return_code = set_runtime_gravity_float(false, 0.0f); + if (return_code != ReturnCode::SUCCESS) { + return return_code; + } + // Rebase the buffered targets too so no stale pre-float command + // yanks the arm after the re-engage. + clear_command_buffers_for_move_to_ready(); + return ReturnCode::SUCCESS; + } + i++; + } + + std::fill(target_tor_.begin(), target_tor_.end(), 0.0f); + i = 0; + for (auto& p_joint : joints_) { + current_motor_positions_[i++] = p_joint->get_pos_rad_relative() * p_joint->get_dir_invert(); + } + return_code = p_algo_->gravity_compensation(current_motor_positions_, target_tor_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Gravity compensation algorithm execution failed for %s_%s", model_.c_str(), id_.c_str()); + return return_code; + } + i = 0; + for (auto& p_joint : joints_) { + target_tor_[i] *= p_joint->gravity_comp_factor_; + return_code = p_joint->apply_torque(target_tor_[i]); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Failed to apply the gravity float torque to joint %d in %s_%s", p_joint->id_, + model_.c_str(), id_.c_str()); + return return_code; + } + i++; + } + return ReturnCode::SUCCESS; + } + // Reuse pre-allocated vector (reset to zero) std::fill(target_tor_.begin(), target_tor_.end(), 0.0f); - if (planning_type_ == TrajectoryPlanningType::SLEW_POS_GRAVITY) { + if (follower_gravity_compensation_) { // Reuse pre-allocated vector int i = 0; for (auto& p_joint : joints_) { @@ -1598,18 +1869,16 @@ ReturnCode DeviceArm::operate_as_follower() { i = 0; for (auto& p_joint : joints_) { target_tor_[i] *= p_joint->gravity_comp_factor_; - if (planning_type_ == TrajectoryPlanningType::SLEW_POS_GRAVITY) { - target_tor_[i] -= p_joint->follow_viscous_damping_ * p_joint->get_vel_rad_sec(); - } + target_tor_[i] -= p_joint->follow_viscous_damping_ * p_joint->get_vel_rad_sec(); i++; } } // Synchronized follower slew: bound the tracking velocity by each joint's - // follow_vel_max for EVERY planning type, not just SLEW_POS_GRAVITY. Direct - // tracking (planning "None") used to hand the raw leader target to the PD - // loop, so a 50 Hz command staircase was traversed as stiff instantaneous - // steps and follow_vel_max was silently ignored. One shared scale keeps the + // follow_vel_max whether or not gravity feed-forward is active. Direct + // tracking used to hand the raw leader target to the PD loop, so a 50 Hz + // command staircase was traversed as stiff instantaneous steps and + // follow_vel_max was silently ignored. One shared scale keeps the // multi-joint motion synchronized (straight line in joint space). if (slew_goal_positions_.size() != joints_.size() || control_frequency_ <= 0) { PI_ERROR("Invalid synchronized slew state in operate_as_follower() for %s_%s", model_.c_str(), diff --git a/native/pi_control/src/pi_device_arm_arx.cpp b/native/pi_control/src/pi_device_arm_arx.cpp deleted file mode 100644 index b66f9ed..0000000 --- a/native/pi_control/src/pi_device_arm_arx.cpp +++ /dev/null @@ -1,20 +0,0 @@ -/*! - * @file pi_device_arm_arx.cpp - * @brief Implementation of the DeviceArmArx class for ARX robotic arm device control. - */ - -#include - -#include "pi_device_arm_arx.hpp" - -DeviceArmArx::DeviceArmArx(const CommandLineArgs& cla) : DeviceArm(cla) {} - -DeviceArmArx::~DeviceArmArx() {} - -ReturnCode DeviceArmArx::set_control_mode(Role target_role, ControlModeIntent intent) { - // ARX family: control-mode switching is not required here. - if (target_role == Role::FOLLOWER || intent == ControlModeIntent::READY_MOVE_OVERRIDE) { - reset_slew_targets_to_current(); - } - return ReturnCode::SUCCESS; -} diff --git a/native/pi_control/src/pi_device_arm_can.cpp b/native/pi_control/src/pi_device_arm_can.cpp new file mode 100644 index 0000000..81edfe8 --- /dev/null +++ b/native/pi_control/src/pi_device_arm_can.cpp @@ -0,0 +1,20 @@ +/*! + * @file pi_device_arm_can.cpp + * @brief Implementation of the DeviceArmCan class for MIT-mode CAN arm device control. + */ + +#include + +#include "pi_device_arm_can.hpp" + +DeviceArmCan::DeviceArmCan(const CommandLineArgs& cla) : DeviceArm(cla) {} + +DeviceArmCan::~DeviceArmCan() {} + +ReturnCode DeviceArmCan::set_control_mode(Role target_role, ControlModeIntent intent) { + // MIT-mode CAN devices: control-mode switching is not required here. + if (target_role == Role::FOLLOWER || intent == ControlModeIntent::READY_MOVE_OVERRIDE) { + reset_slew_targets_to_current(); + } + return ReturnCode::SUCCESS; +} diff --git a/native/pi_control/src/pi_device_arm_nello.cpp b/native/pi_control/src/pi_device_arm_serial.cpp similarity index 78% rename from native/pi_control/src/pi_device_arm_nello.cpp rename to native/pi_control/src/pi_device_arm_serial.cpp index 797a11d..31b8fb2 100644 --- a/native/pi_control/src/pi_device_arm_nello.cpp +++ b/native/pi_control/src/pi_device_arm_serial.cpp @@ -1,21 +1,21 @@ /*! - * @file pi_device_arm_nello.cpp - * @brief Implementation of the DeviceArmNello class for Nello robotic arm device control. + * @file pi_device_arm_serial.cpp + * @brief Implementation of the DeviceArmSerial class for serial bus-servo arm device control. */ #include -#include "pi_device_arm_nello.hpp" +#include "pi_device_arm_serial.hpp" #include "pi_joint.hpp" #include "pi_profile.hpp" -DeviceArmNello::DeviceArmNello(const CommandLineArgs& cla) : DeviceArm(cla) {} +DeviceArmSerial::DeviceArmSerial(const CommandLineArgs& cla) : DeviceArm(cla) {} -DeviceArmNello::~DeviceArmNello() {} +DeviceArmSerial::~DeviceArmSerial() {} -ReturnCode DeviceArmNello::set_control_mode(Role target_role, ControlModeIntent intent) { +ReturnCode DeviceArmSerial::set_control_mode(Role target_role, ControlModeIntent intent) { // Implementation note: - // - Nello leader/follower switching is already expressed via Joint::change_control_mode_for_{leader,follower}(), + // - Serial-arm leader/follower switching is already expressed via Joint::change_control_mode_for_{leader,follower}(), // which maps to servo/driver operation modes. // - For READY_MOVE_OVERRIDE we still treat it as follower-like (position-based) so that move_to_ready_position() // can safely send position targets from the current pose. @@ -42,9 +42,9 @@ ReturnCode DeviceArmNello::set_control_mode(Role target_role, ControlModeIntent // The effector's own step switches its mode once at its ready transition, but that single // attempt can fail -- the passive-leader torque disable races the final ready-move writes // on SO-ARM101 -- so re-applying here at arm-ready gives it a second, later chance. The - // call is idempotent. The follower-like branch above intentionally does not chain: Nello + // call is idempotent. The follower-like branch above intentionally does not chain: the serial arm // follower effector modes depend on the configured effector control type and are handled - // by the effector's own ready transition, matching the long-standing Nello behavior. + // by the effector's own ready transition, matching the long-standing serial-arm behavior. // Skipped during emergency recovery for the same bus-timeout reason as the base class. if (p_effector_ && !is_in_emergency_recovery()) { rc = p_effector_->set_control_mode(p_effector_->get_device_role(), intent); @@ -54,12 +54,12 @@ ReturnCode DeviceArmNello::set_control_mode(Role target_role, ControlModeIntent return ReturnCode::SUCCESS; } -ReturnCode DeviceArmNello::move_to_ready_position() { +ReturnCode DeviceArmSerial::move_to_ready_position() { ReturnCode return_code = ReturnCode::SUCCESS; return_code = DeviceArm::move_to_ready_position(); if (return_code != ReturnCode::SUCCESS) { - PI_ERROR("Failed to move Nello arm to ready position"); + PI_ERROR("Failed to move serial arm to ready position"); return return_code; } diff --git a/native/pi_control/src/pi_device_effector.cpp b/native/pi_control/src/pi_device_effector.cpp index 66c074a..849d031 100644 --- a/native/pi_control/src/pi_device_effector.cpp +++ b/native/pi_control/src/pi_device_effector.cpp @@ -463,7 +463,7 @@ ReturnCode DeviceEffector::init(const CommandLineArgs& cla, int argc, "control mode"); } - // Torque-mode spring offset (rad): monopi ControlFollowGripper "offset", + // Torque-mode spring offset (rad), // subtracted from the position error. Installation-specific zero trim; // optional, default 0 (prefer adjusting the servo zero instead). return_code = p_config_individual_->get_field_value( diff --git a/native/pi_control/src/pi_device_effector_arx.cpp b/native/pi_control/src/pi_device_effector_can.cpp similarity index 77% rename from native/pi_control/src/pi_device_effector_arx.cpp rename to native/pi_control/src/pi_device_effector_can.cpp index e19e7bd..a0349af 100644 --- a/native/pi_control/src/pi_device_effector_arx.cpp +++ b/native/pi_control/src/pi_device_effector_can.cpp @@ -1,31 +1,31 @@ /*! - * @file pi_device_effector_arx.cpp - * @brief Implementation of the DeviceEffectorArx class for ARX effector device control. + * @file pi_device_effector_can.cpp + * @brief Implementation of the DeviceEffectorCan class for MIT-mode CAN effector device control. */ #include -#include "pi_device_effector_arx.hpp" +#include "pi_device_effector_can.hpp" #include "pi_joint.hpp" #define INIT_MOVE_TRY_MAX 200 ///< Maximum number of attempts to move effector to zero position -DeviceEffectorArx::DeviceEffectorArx(const CommandLineArgs& cla) : DeviceEffector(cla) {} +DeviceEffectorCan::DeviceEffectorCan(const CommandLineArgs& cla) : DeviceEffector(cla) {} -DeviceEffectorArx::~DeviceEffectorArx() {} +DeviceEffectorCan::~DeviceEffectorCan() {} -ReturnCode DeviceEffectorArx::set_control_mode(Role target_role, ControlModeIntent intent) { +ReturnCode DeviceEffectorCan::set_control_mode(Role target_role, ControlModeIntent intent) { (void)target_role; (void)intent; ramped_target_initialized_ = false; - // ARX family: control-mode switching is not required here. + // MIT-mode CAN devices: control-mode switching is not required here. return ReturnCode::SUCCESS; } -ReturnCode DeviceEffectorArx::move_joint_with_torque(Joint* p_joint, float target_pos) { +ReturnCode DeviceEffectorCan::move_joint_with_torque(Joint* p_joint, float target_pos) { ReturnCode return_code = ReturnCode::SUCCESS; - // monopi ControlFollowGripper (control_follow.cc): the gripper is a + // Reference gripper controller: the gripper is a // host-side torque spring, // torque = clamp(constant * (goal - measured - offset), +/-bound), // sent as a torque-only command (kp=0; the servo's kd supplies damping). @@ -34,7 +34,7 @@ ReturnCode DeviceEffectorArx::move_joint_with_torque(Joint* p_joint, float targe // current (the i2rt failure mode: overheated gripper motors, snapped // fingers). The torque is linear and continuous through zero error -- // saturation only flattens the tails -- so nothing sign-flips or chatters - // near the target. Match monopi's non-L5 ARX command ramp: initialize at + // near the target. Match the reference non-L5 ARX command ramp: initialize at // the measured position and move the internal goal by at most 1 rad/tick. const float clipped_target_pos = p_joint->clipping(target_pos, p_joint->get_pos_min_relative(), p_joint->get_pos_max_relative()); @@ -56,7 +56,7 @@ ReturnCode DeviceEffectorArx::move_joint_with_torque(Joint* p_joint, float targe // the sign mapped back into the motor frame. torque *= p_joint->get_dir_invert(); - // monopi sends kp=0 with the gripper servo's configured kd=0.1. Keep that + // The reference controller sends kp=0 with the gripper servo's configured kd=0.1. Keep that // damping local to this controller so arm gravity/torque frames retain // their existing zero-kd behavior. return_code = p_joint->apply_torque_with_damping(torque); diff --git a/native/pi_control/src/pi_device_effector_controller.cpp b/native/pi_control/src/pi_device_effector_controller.cpp index da1a2dc..0bae966 100644 --- a/native/pi_control/src/pi_device_effector_controller.cpp +++ b/native/pi_control/src/pi_device_effector_controller.cpp @@ -5,7 +5,7 @@ #include "pi_device_effector_controller.hpp" -DeviceEffectorController::DeviceEffectorController(const CommandLineArgs& cla) : DeviceEffectorArx(cla) {} +DeviceEffectorController::DeviceEffectorController(const CommandLineArgs& cla) : DeviceEffectorCan(cla) {} DeviceEffectorController::~DeviceEffectorController() {} diff --git a/native/pi_control/src/pi_device_effector_nello.cpp b/native/pi_control/src/pi_device_effector_serial.cpp similarity index 85% rename from native/pi_control/src/pi_device_effector_nello.cpp rename to native/pi_control/src/pi_device_effector_serial.cpp index 6e34eb5..3ddb07a 100644 --- a/native/pi_control/src/pi_device_effector_nello.cpp +++ b/native/pi_control/src/pi_device_effector_serial.cpp @@ -1,19 +1,19 @@ /*! - * @file pi_device_effector_nello.cpp - * @brief Implementation of the DeviceEffectorNello class for Nello effector device control. + * @file pi_device_effector_serial.cpp + * @brief Implementation of the DeviceEffectorSerial class for serial bus-servo effector device control. */ #include -#include "pi_device_effector_nello.hpp" +#include "pi_device_effector_serial.hpp" #include "pi_joint.hpp" #include "pi_servo.hpp" -DeviceEffectorNello::DeviceEffectorNello(const CommandLineArgs& cla) : DeviceEffector(cla) {} +DeviceEffectorSerial::DeviceEffectorSerial(const CommandLineArgs& cla) : DeviceEffector(cla) {} -DeviceEffectorNello::~DeviceEffectorNello() {} +DeviceEffectorSerial::~DeviceEffectorSerial() {} -ReturnCode DeviceEffectorNello::set_control_mode(Role target_role, ControlModeIntent intent) { +ReturnCode DeviceEffectorSerial::set_control_mode(Role target_role, ControlModeIntent intent) { ReturnCode rc = ReturnCode::SUCCESS; // READY_MOVE_OVERRIDE forces a safe position-based behavior regardless of configured effector control type. @@ -44,7 +44,7 @@ ReturnCode DeviceEffectorNello::set_control_mode(Role target_role, ControlModeIn return ReturnCode::SUCCESS; } -ReturnCode DeviceEffectorNello::init(const CommandLineArgs& cla, int argc, char** argv, std::shared_ptr p_topic, +ReturnCode DeviceEffectorSerial::init(const CommandLineArgs& cla, int argc, char** argv, std::shared_ptr p_topic, std::shared_ptr p_driver) { ReturnCode return_code = DeviceEffector::init(cla, argc, argv, p_topic, p_driver); if (return_code != ReturnCode::SUCCESS) { @@ -69,11 +69,11 @@ ReturnCode DeviceEffectorNello::init(const CommandLineArgs& cla, int argc, char* return return_code; } -ReturnCode DeviceEffectorNello::move_to_ready_position() { +ReturnCode DeviceEffectorSerial::move_to_ready_position() { ReturnCode return_code = ReturnCode::SUCCESS; if (is_read_only() == true) { - // Read-only Nello effector (e.g. T6_2.0A leader-side or T6_2.0A as a follower + // Read-only serial effector (e.g. T6_2.0A leader-side or T6_2.0A as a follower // gripper that reports position but isn't actuated): mirror the base-class // short-circuit so the device's is_ready_ flag actually flips. Without this, // pi_control_node never publishes DEVICE_INFO_READY_NOW and the UI hangs in @@ -109,7 +109,7 @@ ReturnCode DeviceEffectorNello::move_to_ready_position() { return_code = DeviceEffector::move_to_ready_position(); if (return_code != ReturnCode::SUCCESS) { - PI_ERROR("Failed to move Nello effector to ready position"); + PI_ERROR("Failed to move serial effector to ready position"); return return_code; } @@ -123,7 +123,7 @@ ReturnCode DeviceEffectorNello::move_to_ready_position() { return return_code; } -ReturnCode DeviceEffectorNello::move_joint_with_torque(Joint* p_joint, float target_pos) { +ReturnCode DeviceEffectorSerial::move_joint_with_torque(Joint* p_joint, float target_pos) { ReturnCode return_code = ReturnCode::SUCCESS; if (is_read_only() == true) { @@ -152,7 +152,7 @@ ReturnCode DeviceEffectorNello::move_joint_with_torque(Joint* p_joint, float tar } PI_INFO("DeviceEffector", InfoLevel::FREQUENT_3, - "Nello effector torque control: target_pos_rel=%.3f, clipped_target_pos=%.3f, curr_pos_rel=%.3f, " + "Serial effector torque control: target_pos_rel=%.3f, clipped_target_pos=%.3f, curr_pos_rel=%.3f, " "clipped_curr_pos=%.3f, distance=%.3f, torq_to_apply=%.3f, distance_to_torque_=%.3f", target_pos, clipped_target_pos, p_joint->get_pos_rad_relative(), clipped_curr_pos, distance, torq_to_apply, distance_to_torque_); diff --git a/native/pi_control/src/pi_driver.cpp b/native/pi_control/src/pi_driver.cpp index 0cdad71..6e5e10a 100644 --- a/native/pi_control/src/pi_driver.cpp +++ b/native/pi_control/src/pi_driver.cpp @@ -6,7 +6,7 @@ #include "pi_driver.hpp" #include "pi_device_config.hpp" -#include "pi_driver_arx.hpp" +#include "pi_driver_can_mit.hpp" #include "pi_driver_arx_encoder.hpp" #include "pi_driver_ft.hpp" #include "pi_driver_trossen.hpp" @@ -32,9 +32,9 @@ std::shared_ptr Driver::new_driver(Device* p_device, const DeviceConfig* std::shared_ptr p_driver = nullptr; if (driver_type == p_config->val_driver_type_can) { - auto p_driver_arx = std::make_shared(p_device, cla); - p_driver = p_driver_arx; - PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created CAN 2.0 driver (DriverArx)"); + auto p_driver_can_mit = std::make_shared(p_device, cla); + p_driver = p_driver_can_mit; + PI_INFO("Driver", InfoLevel::HELPFUL_1, "Created CAN 2.0 driver (DriverCanMit)"); } else if (driver_type == p_config->val_driver_type_can_encoder) { auto p_driver_encoder = std::make_shared(p_device, cla); diff --git a/native/pi_control/src/pi_driver_arx_encoder.cpp b/native/pi_control/src/pi_driver_arx_encoder.cpp index f7c63c7..78a09af 100644 --- a/native/pi_control/src/pi_driver_arx_encoder.cpp +++ b/native/pi_control/src/pi_driver_arx_encoder.cpp @@ -11,7 +11,7 @@ #include "pi_driver_arx_encoder.hpp" #include "pi_info.hpp" -DriverArxEncoder::DriverArxEncoder(Device* p_device, const CommandLineArgs& cla) : DriverArx(p_device, cla) {} +DriverArxEncoder::DriverArxEncoder(Device* p_device, const CommandLineArgs& cla) : DriverCanMit(p_device, cla) {} DriverArxEncoder::~DriverArxEncoder() {} diff --git a/native/pi_control/src/pi_driver_arx.cpp b/native/pi_control/src/pi_driver_can_mit.cpp similarity index 91% rename from native/pi_control/src/pi_driver_arx.cpp rename to native/pi_control/src/pi_driver_can_mit.cpp index 7b70d27..13e8678 100644 --- a/native/pi_control/src/pi_driver_arx.cpp +++ b/native/pi_control/src/pi_driver_can_mit.cpp @@ -1,6 +1,6 @@ /*! - * @file pi_driver_arx.cpp - * @brief Implementation of the DriverArx class for ARX device CAN bus communication and servo control. + * @file pi_driver_can_mit.cpp + * @brief Implementation of the DriverCanMit class for MIT-mode CAN bus communication and servo control. */ #include @@ -19,7 +19,7 @@ #include "pi_servo_can_encoder.hpp" #include "pi_servo_dm.hpp" #include "pi_servo_dm_status.hpp" -#include "pi_driver_arx.hpp" +#include "pi_driver_can_mit.hpp" namespace { @@ -60,7 +60,7 @@ int dm_response_motor_id(const DriverCan::can_frame_t& frame) { } // namespace -DriverArx::DriverArx(Device* p_device, const CommandLineArgs& cla) : DriverCan(p_device, cla) { +DriverCanMit::DriverCanMit(Device* p_device, const CommandLineArgs& cla) : DriverCan(p_device, cla) { // ReceivedServoData has in-class member initializers, so default // construction zero-fills the cache. ``last_update_perf_`` is left at // its default ``prof_time_t{}`` sentinel so ``Profile::is_zero`` @@ -70,9 +70,9 @@ DriverArx::DriverArx(Device* p_device, const CommandLineArgs& cla) : DriverCan(p } } -DriverArx::~DriverArx() {} +DriverCanMit::~DriverCanMit() {} -ReturnCode DriverArx::open(int baud_rate) { +ReturnCode DriverCanMit::open(int baud_rate) { (void)baud_rate; ReturnCode return_code = DriverCan::open(baud_rate); @@ -100,7 +100,7 @@ ReturnCode DriverArx::open(int baud_rate) { return ReturnCode::SUCCESS; } -ReturnCode DriverArx::configure_passive_encoders() { +ReturnCode DriverCanMit::configure_passive_encoders() { // request_can_id -> firmware_compat. A single encoder may have several // routes; OR their flags so any route opting into compat mode enables it. std::map request_firmware_compat; @@ -126,7 +126,7 @@ ReturnCode DriverArx::configure_passive_encoders() { return ReturnCode::SUCCESS; } -ReturnCode DriverArx::configure_passive_encoder(int request_can_id, bool firmware_compat) { +ReturnCode DriverCanMit::configure_passive_encoder(int request_can_id, bool firmware_compat) { const uint8_t version_request[] = {kPassiveEncoderAllDevices, kPassiveEncoderReqVersion}; ReturnCode return_code = send_passive_encoder_request(request_can_id, version_request, sizeof(version_request)); @@ -223,7 +223,7 @@ ReturnCode DriverArx::configure_passive_encoder(int request_can_id, bool firmwar return ReturnCode::SUCCESS; } -ReturnCode DriverArx::send_passive_encoder_request(int request_can_id, const uint8_t* p_data, uint8_t data_len) { +ReturnCode DriverCanMit::send_passive_encoder_request(int request_can_id, const uint8_t* p_data, uint8_t data_len) { if (p_data == nullptr || data_len == 0 || data_len > 8) { return ReturnCode::INVALID_PARAM; } @@ -235,7 +235,7 @@ ReturnCode DriverArx::send_passive_encoder_request(int request_can_id, const uin return send_frame(&frame, sizeof(frame)); } -ReturnCode DriverArx::wait_for_passive_encoder_reply(int request_can_id, int expected_device, +ReturnCode DriverCanMit::wait_for_passive_encoder_reply(int request_can_id, int expected_device, uint8_t expected_command, uint8_t expected_len, int timeout_ms, can_frame_t* p_reply) { if (p_reply == nullptr) { @@ -263,7 +263,7 @@ ReturnCode DriverArx::wait_for_passive_encoder_reply(int request_can_id, int exp return ReturnCode::NO_RESPONSE; } -ReturnCode DriverArx::read_passive_encoder_eeprom(int request_can_id, uint8_t device, uint8_t offset, +ReturnCode DriverCanMit::read_passive_encoder_eeprom(int request_can_id, uint8_t device, uint8_t offset, uint8_t* p_value, bool firmware_compat) { if (p_value == nullptr) { return ReturnCode::INVALID_PARAM; @@ -323,7 +323,7 @@ ReturnCode DriverArx::read_passive_encoder_eeprom(int request_can_id, uint8_t de return ReturnCode::NO_RESPONSE; } -ReturnCode DriverArx::read_passive_encoder_frequency(int request_can_id, uint8_t device, uint8_t high_offset, +ReturnCode DriverCanMit::read_passive_encoder_frequency(int request_can_id, uint8_t device, uint8_t high_offset, uint8_t low_offset, int* p_frequency, bool firmware_compat) { if (p_frequency == nullptr) { return ReturnCode::INVALID_PARAM; @@ -353,7 +353,7 @@ ReturnCode DriverArx::read_passive_encoder_frequency(int request_can_id, uint8_t return ReturnCode::SUCCESS; } -void DriverArx::drain_startup_frames() { +void DriverCanMit::drain_startup_frames() { const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(kPassiveEncoderDrainTimeoutMs); while (std::chrono::steady_clock::now() < deadline) { @@ -369,7 +369,7 @@ void DriverArx::drain_startup_frames() { } } -ReturnCode DriverArx::close() { +ReturnCode DriverCanMit::close() { ReturnCode return_code = DriverCan::close(); if (return_code != ReturnCode::SUCCESS) { PI_ERROR("Failed to close CAN driver"); @@ -379,17 +379,17 @@ ReturnCode DriverArx::close() { return ReturnCode::SUCCESS; } -ReturnCode DriverArx::group_read_hardware_values() { +ReturnCode DriverCanMit::group_read_hardware_values() { // Reset the per-cycle output. We need a CLEAN dead_servo_ids_ on every // call so the set always reflects the current cycle's staleness (DXL - // keeps a sticky cache because re-pinging is expensive, but the ARX + // keeps a sticky cache because re-pinging is expensive, but the CAN-MIT // path is just an O(N) timestamp compare so per-cycle is cheap and // simpler). dead_servo_ids_.clear(); last_failed_servo_id_ = -1; if (p_device_ == nullptr) { - PI_ERROR("Device pointer is null in DriverArx::group_read_hardware_values()"); + PI_ERROR("Device pointer is null in DriverCanMit::group_read_hardware_values()"); return ReturnCode::FAIL; } @@ -402,8 +402,8 @@ ReturnCode DriverArx::group_read_hardware_values() { const prof_time_t now = Profile::get_time_now(); const prof_time_msec_t threshold_ms = any_motor_moved_ - ? static_cast(ARX_STALE_FRAME_AGE_NORMAL_MS) - : static_cast(ARX_STALE_FRAME_AGE_INITIAL_MS); + ? static_cast(CAN_MIT_STALE_FRAME_AGE_NORMAL_MS) + : static_cast(CAN_MIT_STALE_FRAME_AGE_INITIAL_MS); bool any_alive_this_cycle = false; { @@ -445,11 +445,11 @@ ReturnCode DriverArx::group_read_hardware_values() { // published position silently repeats the cached value meanwhile) // leave evidence in the node log. Recovery logs the gap length. const auto warned_it = stall_warned_since_.find(servo_id); - if (age_ms > static_cast(ARX_STALL_WARN_AGE_MS)) { + if (age_ms > static_cast(CAN_MIT_STALL_WARN_AGE_MS)) { if (warned_it == stall_warned_since_.end()) { PI_WARN("Servo id=%d: telemetry stalled (newest frame is %ld ms old, warn threshold=%d ms); " "publishing the cached position meanwhile", - servo_id, static_cast(age_ms), ARX_STALL_WARN_AGE_MS); + servo_id, static_cast(age_ms), CAN_MIT_STALL_WARN_AGE_MS); stall_warned_since_[servo_id] = last_update; } } else if (warned_it != stall_warned_since_.end()) { @@ -479,7 +479,54 @@ ReturnCode DriverArx::group_read_hardware_values() { return ReturnCode::FAIL; } -ReturnCode DriverArx::arm_comm_loss_protection() { +void DriverCanMit::query_and_adopt_encos_mit_ranges(int id) { + struct RangeQuery { + uint8_t code; + float scale; + const char* label; + }; + constexpr RangeQuery kQueries[] = { + {ServoDm::ENCOS_QUERY_SPD_RANGE, ServoDm::ENCOS_SPD_RANGE_SCALE, "SPD"}, + {ServoDm::ENCOS_QUERY_TOR_RANGE, ServoDm::ENCOS_TOR_RANGE_SCALE, "TOR"}, + }; + + for (const RangeQuery& query : kQueries) { + can_frame_t frame; + if (ServoDm::can_frame_to_get_mit_range_encos_servo(frame, (uint16_t)id, query.code) != ReturnCode::SUCCESS) { + continue; + } + if (send_frame(&frame, sizeof(frame)) != ReturnCode::SUCCESS) { + PI_ERROR("Servo id=%d: ENCOS MIT %s range query NOT sent; keeping the compiled codec default", id, + query.label); + continue; + } + + can_frame_t reply_frame; + float range_min = 0.0f; + float range_max = 0.0f; + if (read_frame(&reply_frame, sizeof(reply_frame)) != ReturnCode::SUCCESS || + ServoDm::parse_mit_range_reply_encos_servo(reply_frame, (uint16_t)id, query.code, query.scale, range_min, + range_max) != ReturnCode::SUCCESS) { + // Loud but non-fatal: older firmware may not answer the query; the + // servo stays controllable with the compiled default scale. + PI_ERROR("Servo id=%d: ENCOS MIT %s range query got no valid reply; keeping the compiled codec default", + id, query.label); + continue; + } + + RegisteredServo reg = lock_registered_servo(id); + ServoDm* p_servo = dynamic_cast(reg.get()); + if (p_servo == nullptr) { + PI_ERROR("Servo id=%d: not registered as a DM/ENCOS servo; ENCOS MIT %s range not adopted", id, + query.label); + continue; + } + (void)p_servo->adopt_encos_mit_range(query.code, range_min, range_max); + usleep(100); + } +} + +ReturnCode DriverCanMit::arm_comm_loss_protection() { if (!is_socket_open()) { PI_ERROR("CAN socket is not initialized in arm_comm_loss_protection()"); return ReturnCode::NOT_INITIALIZED; @@ -522,6 +569,14 @@ ReturnCode DriverArx::arm_comm_loss_protection() { can_frame_t frame; switch (type) { case ServoType::ENCOS_A4310: { + // The MIT codec ranges are per-motor firmware parameters: adopt the + // motor's actual SPD range before the command stream starts so + // velocity commands and feedback are not mis-scaled. The TOR range + // is verify-and-log only (see ServoDm::adopt_encos_mit_range): the + // physical torque scale is conformance-calibrated via the model + // JSON torq_rescale against the compiled codec. + query_and_adopt_encos_mit_ranges(id); + // Heartbeat window (factory default 500 ms). The setting is persistent // (non-volatile), so query the current window first and only write on // mismatch to limit flash wear. A failed query falls back to an @@ -626,7 +681,7 @@ ReturnCode DriverArx::arm_comm_loss_protection() { return ReturnCode::SUCCESS; } -ReturnCode DriverArx::update_encoder_slot(int data_index, int motor_id, float angle_rad) { +ReturnCode DriverCanMit::update_encoder_slot(int data_index, int motor_id, float angle_rad) { if (data_index < 0 || data_index >= MAX_SERVO_INFO_BUF_SIZE) { PI_ERROR("update_encoder_slot: data_index %d out of range", data_index); return ReturnCode::FAIL; @@ -642,7 +697,7 @@ ReturnCode DriverArx::update_encoder_slot(int data_index, int motor_id, float an return ReturnCode::SUCCESS; } -ReturnCode DriverArx::send_command(ServoDm* p_servo_dm, float kp, float kd, float position, float velocity, +ReturnCode DriverCanMit::send_command(ServoDm* p_servo_dm, float kp, float kd, float position, float velocity, float torque) { if (p_servo_dm == nullptr) { PI_ERROR("Invalid servo pointer in send_command()"); @@ -680,7 +735,7 @@ ReturnCode DriverArx::send_command(ServoDm* p_servo_dm, float kp, float kd, floa } } -ReturnCode DriverArx::enable(int id, int type, bool enable_flag, bool defer_effector_thermal_fault) { +ReturnCode DriverCanMit::enable(int id, int type, bool enable_flag, bool defer_effector_thermal_fault) { std::lock_guard transaction_lock(transaction_mutex_); last_enable_fault_status_ = -1; if (is_socket_open()) { @@ -811,7 +866,7 @@ ReturnCode DriverArx::enable(int id, int type, bool enable_flag, bool defer_effe std::lock_guard lock(received_servo_data_mutex_); ReturnCode parse_rc = ServoDm::parse_dm_servo_status( &response_frame, received_servo_data_, - &DriverArx::find_data_index, this); + &DriverCanMit::find_data_index, this); if (parse_rc != ReturnCode::SUCCESS) { PI_WARN("Failed to parse enable response status for servo id=%d (rc=%d)", id, static_cast(parse_rc)); @@ -947,7 +1002,7 @@ ReturnCode DriverArx::enable(int id, int type, bool enable_flag, bool defer_effe return ReturnCode::SUCCESS; } -ReturnCode DriverArx::send_disable_once(int id, int type) { +ReturnCode DriverCanMit::send_disable_once(int id, int type) { std::lock_guard transaction_lock(transaction_mutex_); if (!is_socket_open()) { PI_ERROR("CAN socket is not initialized"); @@ -972,7 +1027,7 @@ ReturnCode DriverArx::send_disable_once(int id, int type) { return return_code; } -ReturnCode DriverArx::register_passive_encoder(int response_can_id, int encoder_id, int data_index, +ReturnCode DriverCanMit::register_passive_encoder(int response_can_id, int encoder_id, int data_index, bool firmware_compat) { if (data_index < 0 || data_index >= MAX_SERVO_INFO_BUF_SIZE) { PI_ERROR("Passive encoder id=%d: data_index %d out of range [0, %d)", encoder_id, data_index, @@ -1001,7 +1056,7 @@ ReturnCode DriverArx::register_passive_encoder(int response_can_id, int encoder_ return ReturnCode::SUCCESS; } -ReturnCode DriverArx::reset_zero_position(int id, int type) { +ReturnCode DriverCanMit::reset_zero_position(int id, int type) { std::lock_guard transaction_lock(transaction_mutex_); if (is_socket_open()) { can_frame_t frame; @@ -1032,7 +1087,7 @@ ReturnCode DriverArx::reset_zero_position(int id, int type) { return ReturnCode::SUCCESS; } -ReturnCode DriverArx::read_hardware_values(Servo* p_servo) { +ReturnCode DriverCanMit::read_hardware_values(Servo* p_servo) { if (p_servo == nullptr) { PI_ERROR("Invalid servo pointer in read_hardware_values()"); return ReturnCode::FAIL; @@ -1077,7 +1132,7 @@ ReturnCode DriverArx::read_hardware_values(Servo* p_servo) { return DriverCan::read_hardware_values(p_servo); } -void DriverArx::handle_received_message(void* p_data_buf, size_t data_buf_size, size_t read_bytes) { +void DriverCanMit::handle_received_message(void* p_data_buf, size_t data_buf_size, size_t read_bytes) { if (p_data_buf == nullptr) { PI_ERROR("Invalid data buffer in handle_received_message()"); return; @@ -1114,7 +1169,7 @@ void DriverArx::handle_received_message(void* p_data_buf, size_t data_buf_size, if (dm_response_motor_id(*p_frame) >= 0) { return_code = - ServoDm::parse_dm_servo_status(p_frame, received_servo_data_, &DriverArx::find_data_index, this); + ServoDm::parse_dm_servo_status(p_frame, received_servo_data_, &DriverCanMit::find_data_index, this); if (return_code != ReturnCode::SUCCESS) { PI_ERROR("Failed to parse DM servo status message (CAN ID: 0x%02X)", p_frame->can_id); } @@ -1130,7 +1185,7 @@ void DriverArx::handle_received_message(void* p_data_buf, size_t data_buf_size, case 0x06: case 0x07: return_code = - ServoDm::parser_encos_servo_status(p_frame, received_servo_data_, &DriverArx::find_data_index); + ServoDm::parser_encos_servo_status(p_frame, received_servo_data_, &DriverCanMit::find_data_index); if (return_code != ReturnCode::SUCCESS) { PI_ERROR("Failed to parse ENCOS servo status message (CAN ID: 0x%02X)", p_frame->can_id); return; diff --git a/native/pi_control/src/pi_joint.cpp b/native/pi_control/src/pi_joint.cpp index 501d10b..0212bd4 100644 --- a/native/pi_control/src/pi_joint.cpp +++ b/native/pi_control/src/pi_joint.cpp @@ -188,7 +188,7 @@ ReturnCode Joint::init_config_model(const json& joint_config, const DeviceConfig PI_INFO("Joint", InfoLevel::HELPFUL_1, "Joint %d: safe_mode_derating=%.3f", id_, safe_mode_derating_); } - // Load spring invert flag (optional: config format 1.1.1 places it in the model + // Load spring invert flag (optional: config format 1.1.1+ places it in the model // configuration; the individual configuration may still override it) return_code = p_config->get_field_value(joint_config, p_config->fn_joint_spring_invert, spring_invert_); if (return_code == ReturnCode::SUCCESS) { @@ -251,6 +251,26 @@ ReturnCode Joint::init_config_individual(const json& joint_config, const DeviceC return ReturnCode::INVALID_PARAM; } + // Optional gravity-delivery calibration override: torq_rescale is a + // per-unit value (motor batches differ in effective torque full scale, so + // the kp=0 float test calibrates each robot), and a site calibration JSON + // may replace the model default without a wheel rebuild. + float torq_rescale_individual = torq_rescale_; + if (p_config->get_field_value(joint_config, p_config->fn_joint_torq_rescale, torq_rescale_individual) == + ReturnCode::SUCCESS) { + if (!std::isfinite(torq_rescale_individual) || torq_rescale_individual < 0.0f) { + PI_ERROR("Joint %d: torq_rescale override must be finite and nonnegative, but found %.3f", id_, + torq_rescale_individual); + return ReturnCode::INVALID_PARAM; + } + if (torq_rescale_individual != torq_rescale_) { + PI_INFO("Joint", InfoLevel::ESSENTIAL_0, + "Joint %d: torq_rescale overridden by individual config: %.3f -> %.3f", id_, torq_rescale_, + torq_rescale_individual); + } + torq_rescale_ = torq_rescale_individual; + } + return ReturnCode::SUCCESS; } diff --git a/native/pi_control/src/pi_servo.cpp b/native/pi_control/src/pi_servo.cpp index 11ab556..5276758 100644 --- a/native/pi_control/src/pi_servo.cpp +++ b/native/pi_control/src/pi_servo.cpp @@ -141,7 +141,22 @@ ReturnCode Servo::init_config_model(const json& servo_config, } } - // Config format 1.1.1 places the position limits in the model configuration + // Sign-agnostic position reads for read-only encoders whose feedback sign + // varies per unit (e.g. the ARX_ENC gripper: the vendor reference applies + // abs() so left/right gripper hardware read identically). Optional field; + // when present, a malformed value aborts startup. + if (servo_config.contains(p_config->fn_servo_abs_position)) { + return_code = p_config->get_field_value( + servo_config, p_config->fn_servo_abs_position, abs_position_); + if (return_code != ReturnCode::SUCCESS) { + PI_ERROR("Servo ID %d: abs_position must be a boolean in the model configuration file", id_); + return return_code; + } + PI_INFO("Servo", InfoLevel::HELPFUL_1, "Servo ID %d: abs_position=%s (from model config)", + id_, abs_position_ ? "true" : "false"); + } + + // Config format 1.1.1+ places the position limits in the model configuration // servo block; the individual configuration may still override them below. return_code = p_config->get_field_value( servo_config, p_config->fn_servo_pos_min, pos_min_rel_); @@ -204,6 +219,37 @@ ReturnCode Servo::init_config_individual(const json& servo_config, dir_invert_ = dir_invert_individual; } + // Position-gain overrides: an instance config can select a site-specific + // gain profile (e.g. the high-gain kp/kd variant for A/B benchmarking) + // without editing the gains bundled in the model config. + float gain_override = 0; + return_code = p_config->get_field_value( + servo_config, p_config->fn_servo_pos_kp, gain_override); + if (return_code == ReturnCode::SUCCESS) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: pos_kp overridden by individual config: %.3f -> %.3f", + id_, pos_kp_, gain_override); + pos_kp_ = gain_override; + } + + return_code = p_config->get_field_value( + servo_config, p_config->fn_servo_pos_ki, gain_override); + if (return_code == ReturnCode::SUCCESS) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: pos_ki overridden by individual config: %.3f -> %.3f", + id_, pos_ki_, gain_override); + pos_ki_ = gain_override; + } + + return_code = p_config->get_field_value( + servo_config, p_config->fn_servo_pos_kd, gain_override); + if (return_code == ReturnCode::SUCCESS) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: pos_kd overridden by individual config: %.3f -> %.3f", + id_, pos_kd_, gain_override); + pos_kd_ = gain_override; + } + return_code = p_config->get_field_value( servo_config, p_config->fn_servo_pos_min, pos_min_rel_); if (return_code == ReturnCode::SUCCESS) { diff --git a/native/pi_control/src/pi_servo_can_encoder.cpp b/native/pi_control/src/pi_servo_can_encoder.cpp index 74ce35a..9f650d3 100644 --- a/native/pi_control/src/pi_servo_can_encoder.cpp +++ b/native/pi_control/src/pi_servo_can_encoder.cpp @@ -49,7 +49,7 @@ static const ServoParam g_servo_can_passive_encoder_param(DEFAULT_TOLERABLE_POS_ ServoCanPassiveEncoder::ServoCanPassiveEncoder(Device* p_device, Joint* p_joint, Driver* p_driver) : Servo(p_device, p_joint, p_driver) { - p_driver_can_ = dynamic_cast(p_driver); + p_driver_can_ = dynamic_cast(p_driver); } ServoCanPassiveEncoder::~ServoCanPassiveEncoder() {} @@ -139,7 +139,7 @@ ReturnCode ServoCanPassiveEncoder::start_hardware() { } // Presence probe using the same single-outstanding/deadline/backoff rules - // as normal reads. DriverArx has already validated the encoder before + // as normal reads. DriverCanMit has already validated the encoder before // motor enable; this proves that report polling also works after reception // starts. const auto startup_deadline = Clock::now() + PASSIVE_ENCODER_START_TIMEOUT; @@ -343,7 +343,7 @@ ReturnCode ServoCanPassiveEncoder::parse_encoder_status(const DriverCan::can_fra slot.digital_inputs_ = p_data[5]; slot.update_count_++; // Stamp bus liveness for the staleness watchdog - // (DriverArx::group_read_hardware_values / ServoDm::read_hardware_values). + // (DriverCanMit::group_read_hardware_values / ServoDm::read_hardware_values). slot.last_update_perf_ = Profile::get_time_now(); return ReturnCode::SUCCESS; diff --git a/native/pi_control/src/pi_servo_dm.cpp b/native/pi_control/src/pi_servo_dm.cpp index c7a06c8..5345279 100644 --- a/native/pi_control/src/pi_servo_dm.cpp +++ b/native/pi_control/src/pi_servo_dm.cpp @@ -41,7 +41,7 @@ const ServoDmParam g_servo_dm_param_arx_encoder(0.0f, 0.0f, 0.0f, 0.0f, -12.5f, ServoDm::ServoDm(Device* p_device, Joint* p_joint, Driver* p_driver) : Servo(p_device, p_joint, p_driver), checker_motor_no_response_(MAX_CNT_MOTOR_NO_RESPONSE_INITIAL) { - p_driver_can_ = dynamic_cast(p_driver); + p_driver_can_ = dynamic_cast(p_driver); } ServoDm::~ServoDm() { @@ -175,7 +175,7 @@ ReturnCode ServoDm::init_current_estimation(std::string& servo_model, const Devi ReturnCode ServoDm::init_config_model(const json& servo_config, const DeviceConfig* p_config) { if (p_driver_can_ == nullptr) { - PI_ERROR("DM servo requires an ARX CAN driver"); + PI_ERROR("DM servo requires a DriverCanMit driver"); return ReturnCode::NOT_INITIALIZED; } @@ -201,6 +201,16 @@ ReturnCode ServoDm::init_config_model(const json& servo_config, const DeviceConf return ReturnCode::NOT_SUPPORTED; } + // MIT codec full scales in one line: together with the per-arm torq_rescale + // summary this lets the effective gravity feed-forward delivery be + // reconstructed from the logs alone (delivered = rescale x physical / codec). + const ServoDmParam* p_param = (const ServoDmParam*)p_servo_param_; + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d (%s): MIT codec full scales pos [%.1f, %.1f] rad, vel [%.1f, %.1f] rad/s, " + "tor [%.1f, %.1f] Nm", + id_, servo_model_.c_str(), p_param->pos_min_, p_param->pos_max_, p_param->vel_min_, p_param->vel_max_, + p_param->tor_min_, p_param->tor_max_); + return ReturnCode::SUCCESS; } @@ -235,7 +245,7 @@ ReturnCode ServoDm::start_hardware() { get_device_type_belong_to() == DeviceType::EFFECTOR) { const DmServoStatusInfo& status = dm_servo_status_info(static_cast(status_code)); if (status.is_thermal_fault) { - // DriverArx cached the fault response before returning. Refresh the + // DriverCanMit cached the fault response before returning. Refresh the // servo fields so the terminal fault message reports that snapshot. p_driver_can_->read_hardware_values(this); idc_current_ = current_estimation_.estimate_idc_calibrated( @@ -261,7 +271,7 @@ ReturnCode ServoDm::verify_position_fresh() { return ReturnCode::NOT_INITIALIZED; } // DM/ENCOS slot is considered fresh once the asynchronous CAN parser (or the enable - // response path in DriverArx::enable()) has written into received_servo_data_. The + // response path in DriverCanMit::enable()) has written into received_servo_data_. The // motor_id_ field is zero-initialised and motor IDs start at 1, so a non-zero value // proves at least one status frame was parsed. const int cached_id = p_driver_can_->get_received_motor_id(data_index_); @@ -440,8 +450,8 @@ ReturnCode ServoDm::apply_torque_with_damping(float torque) { } ReturnCode ServoDm::parse_dm_servo_status(DriverCan::can_frame_t* p_frame, ReceivedServoData* p_received_servo_data, - DriverArx::func_find_data_index_t p_find_data_index, - DriverArx* p_driver_arx) { + DriverCanMit::func_find_data_index_t p_find_data_index, + DriverCanMit* p_driver_can_mit) { if (p_frame == nullptr) { PI_ERROR("Invalid CAN frame pointer"); return ReturnCode::INVALID_PARAM; @@ -457,8 +467,8 @@ ReturnCode ServoDm::parse_dm_servo_status(DriverCan::can_frame_t* p_frame, Recei return ReturnCode::INVALID_PARAM; } - if (p_driver_arx == nullptr) { - PI_ERROR("Invalid ARX driver pointer"); + if (p_driver_can_mit == nullptr) { + PI_ERROR("Invalid CAN-MIT driver pointer"); return ReturnCode::INVALID_PARAM; } @@ -521,7 +531,7 @@ ReturnCode ServoDm::parse_dm_servo_status(DriverCan::can_frame_t* p_frame, Recei } ReturnCode ServoDm::parser_encos_servo_status(DriverCan::can_frame_t* p_frame, ReceivedServoData* p_received_servo_data, - DriverArx::func_find_data_index_t p_find_data_index) { + DriverCanMit::func_find_data_index_t p_find_data_index) { if (p_frame == nullptr) { PI_ERROR("Invalid CAN frame pointer"); return ReturnCode::INVALID_PARAM; @@ -553,7 +563,7 @@ ReturnCode ServoDm::parser_encos_servo_status(DriverCan::can_frame_t* p_frame, R if (data_len < 8) { // Short frames on an ENCOS channel are config-set acknowledgements (e.g. // the ack for the CAN-timeout write sent by - // DriverArx::arm_comm_loss_protection), not status reports. Byte0[0:4] + // DriverCanMit::arm_comm_loss_protection), not status reports. Byte0[0:4] // is NOT a motor-error field in these frames, so only stamp bus // liveness -- never the error/position/velocity telemetry. p_received_servo_data[data_index].motor_id_ = motor_id; @@ -761,6 +771,149 @@ ReturnCode ServoDm::can_frame_to_get_can_timeout_encos_servo(DriverCan::can_fram return ReturnCode::SUCCESS; } +ReturnCode ServoDm::can_frame_to_get_mit_range_encos_servo(DriverCan::can_frame_t& can_frame, uint16_t motor_id, + uint8_t query_code) { + // ENCOS config-query frame (technical document 9.3): header byte 0 carries the + // motor mode in the top 3 bits (0x07 = CONFIG_GET), byte 1 the query code. + constexpr uint8_t kEncosMotorModeConfigGet = 0x07; + + if (query_code != ENCOS_QUERY_SPD_RANGE && query_code != ENCOS_QUERY_TOR_RANGE) { + PI_ERROR("Unsupported ENCOS MIT-range query code %u (motor ID %d)", query_code, motor_id); + return ReturnCode::INVALID_PARAM; + } + + can_frame = {}; + can_frame.can_dlc = 2; + can_frame.can_id = motor_id; + + can_frame.data[0] = (uint8_t)(kEncosMotorModeConfigGet << 5); + can_frame.data[1] = query_code; + + return ReturnCode::SUCCESS; +} + +ReturnCode ServoDm::parse_mit_range_reply_encos_servo(const DriverCan::can_frame_t& can_frame, uint16_t motor_id, + uint8_t query_code, float scale, float& range_min, + float& range_max) { + // ACK_QUERY reply (technical document 10.5): byte 0 top 3 bits = 5 (query ack), + // byte 1 echoes the query code, bytes 2..5 carry the MIN/MAX pair as + // big-endian int16 values in the query's fixed-point scale. + constexpr uint8_t kEncosAckQuery = 5; + + if (can_frame.can_id != motor_id || can_frame.can_dlc < 6) { + return ReturnCode::FAIL; + } + if ((uint8_t)(can_frame.data[0] >> 5) != kEncosAckQuery || can_frame.data[1] != query_code) { + return ReturnCode::FAIL; + } + const int16_t raw_min = (int16_t)(((uint16_t)can_frame.data[2] << 8) | (uint16_t)can_frame.data[3]); + const int16_t raw_max = (int16_t)(((uint16_t)can_frame.data[4] << 8) | (uint16_t)can_frame.data[5]); + range_min = (float)raw_min * scale; + range_max = (float)raw_max * scale; + return ReturnCode::SUCCESS; +} + +bool ServoDm::get_mit_codec_report(MitCodecReport& report) const { + const ServoDmParam* p_param = (const ServoDmParam*)p_servo_param_; + if (p_param == nullptr) { + return false; + } + report.codec_vel_min = p_param->vel_min_; + report.codec_vel_max = p_param->vel_max_; + report.codec_tor_min = p_param->tor_min_; + report.codec_tor_max = p_param->tor_max_; + report.reported_spd_valid = reported_spd_range_valid_; + report.reported_spd_min = reported_spd_min_; + report.reported_spd_max = reported_spd_max_; + report.reported_tor_valid = reported_tor_range_valid_; + report.reported_tor_min = reported_tor_min_; + report.reported_tor_max = reported_tor_max_; + report.pos_kp = get_effective_pos_kp(); + report.pos_kd = pos_kd_; + return true; +} + +ReturnCode ServoDm::adopt_encos_mit_range(uint8_t query_code, float range_min, float range_max) { + const ServoDmParam* p_current = (const ServoDmParam*)p_servo_param_; + if (p_current == nullptr) { + PI_ERROR("Servo ID %d: cannot adopt ENCOS MIT range before init_config_model", id_); + return ReturnCode::NOT_INITIALIZED; + } + if (!(range_min < range_max)) { + PI_ERROR("Servo ID %d: rejected ENCOS MIT range [%.2f, %.2f] for query code %u (min >= max)", id_, range_min, + range_max, query_code); + return ReturnCode::FAIL; + } + + float current_min = 0.0f; + float current_max = 0.0f; + const char* label = nullptr; + const char* unit = nullptr; + switch (query_code) { + case ENCOS_QUERY_SPD_RANGE: + current_min = p_current->vel_min_; + current_max = p_current->vel_max_; + label = "SPD"; + unit = "rad/s"; + break; + case ENCOS_QUERY_TOR_RANGE: + current_min = p_current->tor_min_; + current_max = p_current->tor_max_; + label = "TOR"; + unit = "Nm"; + break; + default: + PI_ERROR("Servo ID %d: unsupported ENCOS MIT-range query code %u", id_, query_code); + return ReturnCode::INVALID_PARAM; + } + + // Keep the reported firmware range verbatim (even when the compiled codec is + // retained below): the client servo-parameter report compares these across + // arms to detect mixed motor batches (e.g. ENCOS TOR registers of 30 vs 42 Nm). + if (query_code == ENCOS_QUERY_SPD_RANGE) { + reported_spd_range_valid_ = true; + reported_spd_min_ = range_min; + reported_spd_max_ = range_max; + } else { + reported_tor_range_valid_ = true; + reported_tor_min_ = range_min; + reported_tor_max_ = range_max; + } + + constexpr float kRangeMatchEpsilon = 1e-3f; + if (fabs(range_min - current_min) <= kRangeMatchEpsilon && fabs(range_max - current_max) <= kRangeMatchEpsilon) { + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: ENCOS MIT %s range [%.1f, %.1f] %s matches the compiled codec default", id_, label, + range_min, range_max, unit); + return ReturnCode::SUCCESS; + } + + if (query_code == ENCOS_QUERY_TOR_RANGE) { + // Verify-and-log only: delivered-torque conformance testing showed the + // physical torque full scale does not follow the firmware register (the ENCOS + // A4310 delivers over +-42 Nm while reporting +-30), and the model JSON + // torq_rescale factors are calibrated against the compiled codec, so + // adopting the register would silently shift gravity feed-forward + // delivery. + PI_ERROR("Servo ID %d: ENCOS MIT TOR range [%.1f, %.1f] Nm DIFFERS from the compiled default [%.1f, %.1f]; " + "keeping the compiled codec (model torq_rescale is conformance-calibrated against it)", + id_, range_min, range_max, current_min, current_max); + return ReturnCode::SUCCESS; + } + + if (!encos_param_override_.has_value()) { + encos_param_override_ = *p_current; + } + encos_param_override_->vel_min_ = range_min; + encos_param_override_->vel_max_ = range_max; + p_servo_param_ = &encos_param_override_.value(); + PI_INFO("Servo", InfoLevel::ESSENTIAL_0, + "Servo ID %d: ENCOS MIT %s range [%.1f, %.1f] %s DIFFERS from the compiled default [%.1f, %.1f]; " + "codec rescaled to the motor's reported range", + id_, label, range_min, range_max, unit, current_min, current_max); + return ReturnCode::SUCCESS; +} + ReturnCode ServoDm::parse_can_timeout_reply_encos_servo(const DriverCan::can_frame_t& can_frame, uint16_t motor_id, uint16_t& timeout_ms) { // ACK_QUERY reply: byte 0 top 3 bits = 5 (query ack), byte 1 echoes the query code @@ -877,14 +1030,14 @@ ReturnCode ServoDm::read_hardware_values() { // hold, so SAFE_MODE_SIG never fired in steady state. // // Threshold selection: - // ARX_STALE_FRAME_AGE_INITIAL_MS while ``motor_moved_`` is false (the + // CAN_MIT_STALE_FRAME_AGE_INITIAL_MS while ``motor_moved_`` is false (the // servo has not yet produced any status frame for this session -- // bus may still be coming up after enable handshake) - // ARX_STALE_FRAME_AGE_NORMAL_MS once any frame has been seen + // CAN_MIT_STALE_FRAME_AGE_NORMAL_MS once any frame has been seen // (steady-state operation; tighter so a real cable break is // detected within ~10 s rather than ~50 s) // - // Constants live in pi_control.hpp so the DriverArx group_read path and + // Constants live in pi_control.hpp so the DriverCanMit group_read path and // this per-servo path agree on the threshold. prof_time_t last_update_perf; if (p_driver_can_ != nullptr) { @@ -892,8 +1045,8 @@ ReturnCode ServoDm::read_hardware_values() { } const bool last_update_is_zero = Profile::is_zero(last_update_perf); const prof_time_msec_t threshold_ms = motor_moved_ - ? static_cast(ARX_STALE_FRAME_AGE_NORMAL_MS) - : static_cast(ARX_STALE_FRAME_AGE_INITIAL_MS); + ? static_cast(CAN_MIT_STALE_FRAME_AGE_NORMAL_MS) + : static_cast(CAN_MIT_STALE_FRAME_AGE_INITIAL_MS); bool stale = false; prof_time_msec_t age_ms = 0; if (last_update_is_zero) { diff --git a/native/pi_control/src/pi_topic.cpp b/native/pi_control/src/pi_topic.cpp index 0ba00dc..a35c8e2 100644 --- a/native/pi_control/src/pi_topic.cpp +++ b/native/pi_control/src/pi_topic.cpp @@ -180,6 +180,15 @@ ReturnCode Topic::process_leader_msg(const MsgCommand& msg) { } else if (msg.command_ == DEVICE_COMMAND_ENTER_GRAVITY_COMPENSATION) { if (p_device_ == nullptr) return ReturnCode::NOT_INITIALIZED; + if (p_device_->get_device_role() == Role::FOLLOWER) { + // Calibration gravity float (gravity_tune / arm_check float runs on + // a follower node): position control drops to the gravity + // feed-forward alone; HOLD re-engages it. The optional float param + // is the in-loop runaway abort threshold (rad). + const float abort_drift_rad = msg.num_param_float_ > 0 ? msg.param_float_[0] + : Device::kGravityFloatAbortRadDefault; + return p_device_->set_runtime_gravity_float(true, abort_drift_rad); + } return p_device_->set_runtime_force_feedback(false, -1.0f); } else if (msg.command_ == DEVICE_COMMAND_ENABLE_FORCE_FEEDBACK) { @@ -196,6 +205,17 @@ ReturnCode Topic::process_leader_msg(const MsgCommand& msg) { if (p_device_ == nullptr) return ReturnCode::NOT_INITIALIZED; return p_device_->runtime_hold(); + } else if (msg.command_ == DEVICE_COMMAND_SET_TORQ_RESCALE) { + if (p_device_ == nullptr) return ReturnCode::NOT_INITIALIZED; + if (msg.num_param_float_ < 1 || msg.num_param_float_ > (int)msg.param_float_.size()) { + PI_ERROR("Invalid parameter count for the runtime torq_rescale command: received=%d", + msg.num_param_float_); + return ReturnCode::INVALID_PARAM; + } + const std::vector values(msg.param_float_.begin(), + msg.param_float_.begin() + msg.num_param_float_); + return p_device_->set_runtime_torq_rescale(values); + } else if (msg.command_ == DEVICE_COMMAND_SET_EFFECTOR_MIN_MAX_POS) { int num_param_float = msg.num_param_float_; if (num_param_float != 2) { diff --git a/native/pi_control/tests/fuzz/fuzz_servo_status_parsers.cpp b/native/pi_control/tests/fuzz/fuzz_servo_status_parsers.cpp index 1b037e6..3d495a9 100644 --- a/native/pi_control/tests/fuzz/fuzz_servo_status_parsers.cpp +++ b/native/pi_control/tests/fuzz/fuzz_servo_status_parsers.cpp @@ -24,7 +24,7 @@ #include #include "pi_device.hpp" -#include "pi_driver_arx.hpp" +#include "pi_driver_can_mit.hpp" #include "pi_servo_dm.hpp" namespace { @@ -73,7 +73,7 @@ struct FuzzRig { ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -10.0f, 10.0f, -28.0f, 28.0f, 0.2f, 0.3f, 0.1f}; FuzzDevice device{cla}; - DriverArx driver{&device, cla}; + DriverCanMit driver{&device, cla}; std::vector servos; FuzzRig() { @@ -113,8 +113,8 @@ extern "C" int LLVMFuzzerTestOneInput(const uint8_t* data, size_t size) { // iteration to keep the redzone state clean. auto* cache = new ReceivedServoData[MAX_SERVO_INFO_BUF_SIZE](); - (void)ServoDm::parse_dm_servo_status(&frame, cache, &DriverArx::find_data_index, &rig->driver); - (void)ServoDm::parser_encos_servo_status(&frame, cache, &DriverArx::find_data_index); + (void)ServoDm::parse_dm_servo_status(&frame, cache, &DriverCanMit::find_data_index, &rig->driver); + (void)ServoDm::parser_encos_servo_status(&frame, cache, &DriverCanMit::find_data_index); delete[] cache; return 0; diff --git a/native/pi_control/tests/test_command_line_args.cpp b/native/pi_control/tests/test_command_line_args.cpp new file mode 100644 index 0000000..3fd64a0 --- /dev/null +++ b/native/pi_control/tests/test_command_line_args.cpp @@ -0,0 +1,33 @@ +/*! + * @file test_command_line_args.cpp + * @brief Unit tests for CommandLineArgs helpers. + */ + +#include + +#include "pi_command_line_args.hpp" + +TEST(TorqRescaleCsv, ParsesCommaSeparatedFloats) { + const auto values = CommandLineArgs::parse_torq_rescale_csv("0.8,0.8,0.8,1.5,1.5,1.5"); + ASSERT_EQ(values.size(), 6u); + EXPECT_FLOAT_EQ(values[0], 0.8f); + EXPECT_FLOAT_EQ(values[3], 1.5f); +} + +TEST(TorqRescaleCsv, RejectsMalformedTokens) { + EXPECT_TRUE(CommandLineArgs::parse_torq_rescale_csv("0.8,abc").empty()); + EXPECT_TRUE(CommandLineArgs::parse_torq_rescale_csv("0.8x,1.5").empty()); + EXPECT_TRUE(CommandLineArgs::parse_torq_rescale_csv("0.8,,1.5").empty()); +} + +TEST(TorqRescaleCsv, RejectsNonPhysicalValues) { + EXPECT_TRUE(CommandLineArgs::parse_torq_rescale_csv("-0.1,0.8").empty()); + EXPECT_TRUE(CommandLineArgs::parse_torq_rescale_csv("inf,0.8").empty()); + EXPECT_TRUE(CommandLineArgs::parse_torq_rescale_csv("nan").empty()); +} + +TEST(TorqRescaleCsv, AllowsZeroForVirtualJoints) { + const auto values = CommandLineArgs::parse_torq_rescale_csv("0,1.0"); + ASSERT_EQ(values.size(), 2u); + EXPECT_FLOAT_EQ(values[0], 0.0f); +} diff --git a/native/pi_control/tests/test_device_config.cpp b/native/pi_control/tests/test_device_config.cpp index 2b278a4..c61e862 100644 --- a/native/pi_control/tests/test_device_config.cpp +++ b/native/pi_control/tests/test_device_config.cpp @@ -42,7 +42,7 @@ class DeviceConfigTest : public ::testing::Test { TEST_F(DeviceConfigTest, LoadsExplicitModelConfigPath) { const auto path = write_config("model.json", R"({ - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "Yam", "device_type": "arm" })"); @@ -114,7 +114,7 @@ TEST_F(DeviceConfigTest, GetFieldValueRejectsTypeMismatch) { TEST_F(DeviceConfigTest, EffectorOpenAtMinCascadesIntoServoDicts) { const auto path = write_config("effector_01.json", R"({ - "config_version": "1.1.1", + "config_version": "1.3.1", "open_at_min": true, "joints": [ {"servos": [{"servo_id": 1}, {"servo_id": 2, "open_at_min": false}]}, diff --git a/native/pi_control/tests/test_driver_arx.cpp b/native/pi_control/tests/test_driver_can_mit.cpp similarity index 66% rename from native/pi_control/tests/test_driver_arx.cpp rename to native/pi_control/tests/test_driver_can_mit.cpp index 0789649..5ad9c62 100644 --- a/native/pi_control/tests/test_driver_arx.cpp +++ b/native/pi_control/tests/test_driver_can_mit.cpp @@ -19,18 +19,22 @@ #include "pi_device.hpp" #define private public -#include "pi_driver_arx.hpp" +#include "pi_driver_can_mit.hpp" #undef private +#include "pi_algo.hpp" +#include "pi_algo_pino.hpp" +#include "pi_device_arm_can.hpp" +#include "pi_device_arm_serial.hpp" #include "pi_driver_arx_encoder.hpp" #include "pi_servo_can_encoder.hpp" #include "pi_servo_dm.hpp" namespace { -class DriverArxTestDevice : public Device { +class DriverCanMitTestDevice : public Device { public: - explicit DriverArxTestDevice(const CommandLineArgs& cla) : Device(cla) {} + explicit DriverCanMitTestDevice(const CommandLineArgs& cla) : Device(cla) {} ReturnCode apply_action(const MsgJoints&) override { return ReturnCode::SUCCESS; } ReturnCode get_observation(MsgJoints&) override { return ReturnCode::SUCCESS; } @@ -44,17 +48,17 @@ class DriverArxTestDevice : public Device { ReturnCode set_control_mode(Role, ControlModeIntent) override { return ReturnCode::SUCCESS; } }; -class DriverArxTestOtherDriver : public Driver { +class DriverCanMitTestOtherDriver : public Driver { public: - DriverArxTestOtherDriver(Device* p_device, const CommandLineArgs& cla) : Driver(p_device, cla) {} + DriverCanMitTestOtherDriver(Device* p_device, const CommandLineArgs& cla) : Driver(p_device, cla) {} ReturnCode open(int) override { return ReturnCode::SUCCESS; } ReturnCode close() override { return ReturnCode::SUCCESS; } }; -class DriverArxTestServoDm : public ServoDm { +class DriverCanMitTestServoDm : public ServoDm { public: - DriverArxTestServoDm(Device* p_device, Driver* p_driver, const ServoDmParam* p_param, int id, ServoType type) + DriverCanMitTestServoDm(Device* p_device, Driver* p_driver, const ServoDmParam* p_param, int id, ServoType type) : ServoDm(p_device, nullptr, p_driver) { id_ = id; data_index_ = 0; @@ -62,19 +66,20 @@ class DriverArxTestServoDm : public ServoDm { p_servo_param_ = p_param; } - bool has_arx_driver() const { return p_driver_can_ != nullptr; } + bool has_can_mit_driver() const { return p_driver_can_ != nullptr; } + const ServoParam* servo_param() const { return p_servo_param_; } }; -TEST(ServoPositionWrap, FreshArxCacheRecoversYambotOpenGripperReadingsAfterPowerCycle) { +TEST(ServoPositionWrap, FreshCanMitCacheRecoversYambotOpenGripperReadingsAfterPowerCycle) { constexpr float kTwoPi = 6.283185307179586f; CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - DriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; for (const float absolute_position : {1.165f, 0.979f}) { - DriverArxTestServoDm servo(&device, &driver, ¶m, 7, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 7, ServoType::DM_4310); servo.dir_invert_ = -1; servo.zero_pos_abs_ = 0.0f; servo.pos_min_rel_ = 0.0f; @@ -92,11 +97,11 @@ TEST(ServoPositionWrap, FreshArxCacheRecoversYambotOpenGripperReadingsAfterPower TEST(ServoPositionWrap, LeavesTrueOutOfRangePositionForNormalLimitHandling) { constexpr float kTwoPi = 6.283185307179586f; CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - DriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 7, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 7, ServoType::DM_4310); servo.dir_invert_ = -1; servo.zero_pos_abs_ = 0.0f; servo.pos_min_rel_ = 0.0f; @@ -109,16 +114,61 @@ TEST(ServoPositionWrap, LeavesTrueOutOfRangePositionForNormalLimitHandling) { EXPECT_FLOAT_EQ(servo.get_pos_rad_relative(), 12.0f); } -class SocketBackedDriverArx : public DriverArx { +TEST(ServoGainOverride, IndividualConfigOverridesPositionGains) { + // An instance config can select a site-specific gain profile (e.g. the + // high-gain kp/kd variant for A/B benchmarking); fields it omits keep the + // model config values. + CommandLineArgs cla{}; + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); + ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, + 0.2f, 0.3f, 0.1f}; + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 3, ServoType::DM_4310); + servo.pos_kp_ = 150.0f; + servo.pos_ki_ = 0.5f; + servo.pos_kd_ = 12.0f; + + DeviceConfig config; + const json overrides = json::parse(R"({"servo_id": 3, "pos_kp": 40.0, "pos_kd": 1.2})"); + ASSERT_EQ(servo.init_config_individual(overrides, &config), ReturnCode::SUCCESS); + EXPECT_FLOAT_EQ(servo.pos_kp_, 40.0f); + EXPECT_FLOAT_EQ(servo.pos_ki_, 0.5f); + EXPECT_FLOAT_EQ(servo.pos_kd_, 1.2f); + + const json no_overrides = json::parse(R"({"servo_id": 3})"); + ASSERT_EQ(servo.init_config_individual(no_overrides, &config), ReturnCode::SUCCESS); + EXPECT_FLOAT_EQ(servo.pos_kp_, 40.0f); + EXPECT_FLOAT_EQ(servo.pos_ki_, 0.5f); + EXPECT_FLOAT_EQ(servo.pos_kd_, 1.2f); +} + +TEST(FollowerGravityCompensation, CapabilityGatesMatchArmAndAlgoTypes) { + // DeviceArm::init fast-fails a --follower_gravity_compensation request + // unless the arm takes a torque feed-forward AND the algo has a real + // gravity model; these are the two capability probes it consults. + CommandLineArgs cla{}; + DeviceArmCan can_arm(cla); + EXPECT_TRUE(can_arm.supports_torque_feed_forward()); + DeviceArmSerial serial_arm(cla); + EXPECT_FALSE(serial_arm.supports_torque_feed_forward()); + + DriverCanMitTestDevice device(cla); + Algo base_algo(&device, cla); + EXPECT_FALSE(base_algo.has_gravity_model()); + AlgoPino pino_algo(&device, cla); + EXPECT_TRUE(pino_algo.has_gravity_model()); +} + +class SocketBackedDriverCanMit : public DriverCanMit { public: - using DriverArx::DriverArx; + using DriverCanMit::DriverCanMit; void adopt_socket(int socket_fd) { sock_ = socket_fd; } }; -class RestartFailingDriverArx : public DriverArx { +class RestartFailingDriverCanMit : public DriverCanMit { public: - using DriverArx::DriverArx; + using DriverCanMit::DriverCanMit; void adopt_socket(int socket_fd) { sock_ = socket_fd; } @@ -139,9 +189,9 @@ class RestartFailingDriverArx : public DriverArx { } }; -class ZeroDescriptorDriverArx : public DriverArx { +class ZeroDescriptorDriverCanMit : public DriverCanMit { public: - using DriverArx::DriverArx; + using DriverCanMit::DriverCanMit; void adopt_socket(int socket_fd) { sock_ = socket_fd; } void abandon_socket() { sock_ = -1; } @@ -156,9 +206,9 @@ class ZeroDescriptorDriverArx : public DriverArx { size_t send_count_ = 0; }; -class EnableSendFailingDriverArx : public DriverArx { +class EnableSendFailingDriverCanMit : public DriverCanMit { public: - using DriverArx::DriverArx; + using DriverCanMit::DriverCanMit; void adopt_socket(int socket_fd) { sock_ = socket_fd; } bool reception_running() const { return is_running_.load(std::memory_order_acquire); } @@ -171,9 +221,9 @@ class EnableSendFailingDriverArx : public DriverArx { size_t send_count_ = 0; }; -class FailingSendDriverArx : public DriverArx { +class FailingSendDriverCanMit : public DriverCanMit { public: - using DriverArx::DriverArx; + using DriverCanMit::DriverCanMit; void adopt_socket(int socket_fd) { sock_ = socket_fd; } void abandon_socket() { sock_ = -1; } @@ -188,10 +238,10 @@ class FailingSendDriverArx : public DriverArx { size_t send_count_ = 0; }; -class SetupSendFailingDriverArx : public DriverArx { +class SetupSendFailingDriverCanMit : public DriverCanMit { public: - SetupSendFailingDriverArx(Device* p_device, const CommandLineArgs& cla, size_t fail_on_send) - : DriverArx(p_device, cla), fail_on_send_(fail_on_send) {} + SetupSendFailingDriverCanMit(Device* p_device, const CommandLineArgs& cla, size_t fail_on_send) + : DriverCanMit(p_device, cla), fail_on_send_(fail_on_send) {} void adopt_socket(int socket_fd) { sock_ = socket_fd; } void abandon_socket() { sock_ = -1; } @@ -221,10 +271,10 @@ class SetupSendFailingDriverArx : public DriverArx { size_t send_count_ = 0; }; -class ScriptedEnableStatusDriverArx : public DriverArx { +class ScriptedEnableStatusDriverCanMit : public DriverCanMit { public: - ScriptedEnableStatusDriverArx(Device* p_device, const CommandLineArgs& cla, std::vector statuses) - : DriverArx(p_device, cla), statuses_(std::move(statuses)) {} + ScriptedEnableStatusDriverCanMit(Device* p_device, const CommandLineArgs& cla, std::vector statuses) + : DriverCanMit(p_device, cla), statuses_(std::move(statuses)) {} void adopt_socket(int socket_fd) { sock_ = socket_fd; } size_t enable_frame_count() const { return enable_frame_count_; } @@ -269,9 +319,9 @@ class ScriptedEnableStatusDriverArx : public DriverArx { size_t reset_frame_count_ = 0; }; -class ConcurrentEnableProbeDriverArx : public DriverArx { +class ConcurrentEnableProbeDriverCanMit : public DriverCanMit { public: - using DriverArx::DriverArx; + using DriverCanMit::DriverCanMit; void adopt_socket(int socket_fd) { sock_ = socket_fd; } bool overlap_detected() const { return overlap_detected_.load(std::memory_order_acquire); } @@ -332,9 +382,9 @@ class ConcurrentEnableProbeDriverArx : public DriverArx { std::atomic overlap_detected_{false}; }; -class EnableTransactionProbeDriverArx : public DriverArx { +class EnableTransactionProbeDriverCanMit : public DriverCanMit { public: - using DriverArx::DriverArx; + using DriverCanMit::DriverCanMit; void adopt_socket(int socket_fd) { sock_ = socket_fd; } @@ -397,17 +447,17 @@ namespace { // handle_received_message is protected (invoked by the receive loop only); // expose it for direct exercise in tests. -class DriverArxMessageProbe : public DriverArx { +class DriverCanMitMessageProbe : public DriverCanMit { public: - using DriverArx::DriverArx; - using DriverArx::handle_received_message; + using DriverCanMit::DriverCanMit; + using DriverCanMit::handle_received_message; }; } // namespace -TEST(DriverArxTransport, RejectsTruncatedReceivedFrames) { +TEST(DriverCanMitTransport, RejectsTruncatedReceivedFrames) { CommandLineArgs cla{}; - DriverArxMessageProbe driver(nullptr, cla); + DriverCanMitMessageProbe driver(nullptr, cla); std::vector truncated_frame(1); driver.handle_received_message(truncated_frame.data(), truncated_frame.size(), truncated_frame.size()); @@ -427,10 +477,10 @@ class DriverArxEncoderMessageProbe : public DriverArxEncoder { TEST(DriverArxEncoderDecode, DecodesTwoByteAngleFrameIntoCacheSlot) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); + DriverCanMitTestDevice device(cla); DriverArxEncoderMessageProbe driver(&device, cla); ServoDmParam param{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1537, ServoType::ARX_ENCODER); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1537, ServoType::ARX_ENCODER); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); // raw = ARX_ENCODER_ZERO_RAW + 2048 -> +2048 * pi / 4096 = +pi/2 rad. @@ -451,10 +501,10 @@ TEST(DriverArxEncoderDecode, DecodesTwoByteAngleFrameIntoCacheSlot) { TEST(DriverArxEncoderDecode, DropsFramesWithWrongDlcOrUnmappedIds) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); + DriverCanMitTestDevice device(cla); DriverArxEncoderMessageProbe driver(&device, cla); ServoDmParam param{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1537, ServoType::ARX_ENCODER); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1537, ServoType::ARX_ENCODER); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); // Wrong DLC on a mapped id: not an encoder report, must not touch the slot. @@ -476,10 +526,10 @@ TEST(DriverArxEncoderDecode, DropsFramesWithWrongDlcOrUnmappedIds) { TEST(DriverArxEncoderLifecycle, ActuationEntryPointsAreNoOps) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); + DriverCanMitTestDevice device(cla); DriverArxEncoder driver(&device, cla); ServoDmParam param{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1537, ServoType::ARX_ENCODER); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1537, ServoType::ARX_ENCODER); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); // Pre-populate the slot so enable() returns without exhausting the warmup window. driver.received_servo_data_[servo.data_index_].motor_id_ = servo.id_; @@ -494,7 +544,7 @@ TEST(DriverArxEncoderLifecycle, ActuationEntryPointsAreNoOps) { TEST(DriverArxEncoderLifecycle, EnableFailsFastForUnmappedIds) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); + DriverCanMitTestDevice device(cla); DriverArxEncoder driver(&device, cla); EXPECT_EQ(driver.enable(1999, static_cast(ServoType::ARX_ENCODER), true), ReturnCode::FAIL); @@ -502,7 +552,7 @@ TEST(DriverArxEncoderLifecycle, EnableFailsFastForUnmappedIds) { TEST(ServoDmParser, DmStatusRejectsNullIndexLookup) { CommandLineArgs cla{}; - DriverArx driver(nullptr, cla); + DriverCanMit driver(nullptr, cla); DriverCan::can_frame_t frame{}; ReceivedServoData cache[MAX_SERVO_INFO_BUF_SIZE]{}; frame.can_dlc = 8; @@ -522,11 +572,11 @@ TEST(ServoDmParser, EncosStatusRejectsNullIndexLookup) { TEST(ServoDmParser, DmStatusRejectsNullDriver) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - DriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); DriverCan::can_frame_t frame{}; @@ -534,27 +584,27 @@ TEST(ServoDmParser, DmStatusRejectsNullDriver) { frame.can_dlc = 8; frame.data[0] = static_cast(servo.id_); - EXPECT_EQ(ServoDm::parse_dm_servo_status(&frame, cache, &DriverArx::find_data_index, nullptr), + EXPECT_EQ(ServoDm::parse_dm_servo_status(&frame, cache, &DriverCanMit::find_data_index, nullptr), ReturnCode::INVALID_PARAM); Driver::register_servo_data_index(servo.id_, -1, nullptr); } -TEST(DriverArxTransport, RejectsServoDataIndexOutsideReceiveCache) { +TEST(DriverCanMitTransport, RejectsServoDataIndexOutsideReceiveCache) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - DriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); servo.data_index_ = MAX_SERVO_INFO_BUF_SIZE; EXPECT_EQ(driver.read_hardware_values(&servo), ReturnCode::INVALID_PARAM); } -TEST(DriverArxTransport, RejectsNullEncoderSnapshotOutputs) { +TEST(DriverCanMitTransport, RejectsNullEncoderSnapshotOutputs) { CommandLineArgs cla{}; - DriverArx driver(nullptr, cla); + DriverCanMit driver(nullptr, cla); float position = 0; float velocity = 0; uint8_t digital_inputs = 0; @@ -566,10 +616,10 @@ TEST(DriverArxTransport, RejectsNullEncoderSnapshotOutputs) { EXPECT_FALSE(driver.get_received_encoder_data(0, &position, &velocity, &digital_inputs, nullptr)); } -TEST(DriverArxTransport, RejectsNonDmServoReads) { +TEST(DriverCanMitTransport, RejectsNonDmServoReads) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - DriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); ServoCanPassiveEncoder servo(&device, nullptr, &driver); servo.id_ = 1; servo.data_index_ = 0; @@ -577,25 +627,25 @@ TEST(DriverArxTransport, RejectsNonDmServoReads) { EXPECT_EQ(driver.read_hardware_values(&servo), ReturnCode::INVALID_PARAM); } -TEST(ServoDmConstruction, RejectsNonArxDrivers) { +TEST(ServoDmConstruction, RejectsNonCanMitDrivers) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - DriverArxTestOtherDriver driver(&device, cla); + DriverCanMitTestDevice device(cla); + DriverCanMitTestOtherDriver driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); - ASSERT_FALSE(servo.has_arx_driver()); + ASSERT_FALSE(servo.has_can_mit_driver()); EXPECT_EQ(servo.start_hardware(), ReturnCode::NOT_INITIALIZED); } -TEST(ServoDmConstruction, RejectsNonArxDriversDuringModelInitialization) { +TEST(ServoDmConstruction, RejectsNonCanMitDriversDuringModelInitialization) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - DriverArxTestOtherDriver driver(&device, cla); + DriverCanMitTestDevice device(cla); + DriverCanMitTestOtherDriver driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); DeviceConfig config; const json servo_config = { {config.fn_servo_id, 1}, @@ -606,20 +656,121 @@ TEST(ServoDmConstruction, RejectsNonArxDriversDuringModelInitialization) { EXPECT_EQ(servo.init_config_model(servo_config, &config), ReturnCode::NOT_INITIALIZED); } -TEST(DriverArxLifecycle, EnablePropagatesReceptionRestartFailure) { +TEST(EncosMitRangeQuery, BuildsConfigGetFramesForSpdAndTorCodes) { + DriverCan::can_frame_t frame{}; + + ASSERT_EQ(ServoDm::can_frame_to_get_mit_range_encos_servo(frame, 3, ServoDm::ENCOS_QUERY_TOR_RANGE), + ReturnCode::SUCCESS); + EXPECT_EQ(frame.can_id, 3u); + EXPECT_EQ(frame.can_dlc, 2); + EXPECT_EQ(frame.data[0], 0x07 << 5); + EXPECT_EQ(frame.data[1], ServoDm::ENCOS_QUERY_TOR_RANGE); + + ASSERT_EQ(ServoDm::can_frame_to_get_mit_range_encos_servo(frame, 3, ServoDm::ENCOS_QUERY_SPD_RANGE), + ReturnCode::SUCCESS); + EXPECT_EQ(frame.data[1], ServoDm::ENCOS_QUERY_SPD_RANGE); + + EXPECT_EQ(ServoDm::can_frame_to_get_mit_range_encos_servo(frame, 3, 99), ReturnCode::INVALID_PARAM); +} + +TEST(EncosMitRangeQuery, ParsesTypeFiveTorRangeReply) { + DriverCan::can_frame_t reply{}; + reply.can_id = 3; + reply.can_dlc = 6; + reply.data[0] = 5 << 5; + reply.data[1] = ServoDm::ENCOS_QUERY_TOR_RANGE; + // TOR range [-42.0, 42.0] Nm at scale 10: raw -420 / 420, big-endian int16. + reply.data[2] = 0xFE; + reply.data[3] = 0x5C; + reply.data[4] = 0x01; + reply.data[5] = 0xA4; + + float range_min = 0.0f; + float range_max = 0.0f; + ASSERT_EQ(ServoDm::parse_mit_range_reply_encos_servo(reply, 3, ServoDm::ENCOS_QUERY_TOR_RANGE, + ServoDm::ENCOS_TOR_RANGE_SCALE, range_min, range_max), + ReturnCode::SUCCESS); + EXPECT_FLOAT_EQ(range_min, -42.0f); + EXPECT_FLOAT_EQ(range_max, 42.0f); +} + +TEST(EncosMitRangeQuery, RejectsMismatchedRangeReplies) { + DriverCan::can_frame_t reply{}; + reply.can_id = 3; + reply.can_dlc = 6; + reply.data[0] = 5 << 5; + reply.data[1] = ServoDm::ENCOS_QUERY_TOR_RANGE; + + float range_min = 0.0f; + float range_max = 0.0f; + + // Wrong motor id. + EXPECT_EQ(ServoDm::parse_mit_range_reply_encos_servo(reply, 4, ServoDm::ENCOS_QUERY_TOR_RANGE, + ServoDm::ENCOS_TOR_RANGE_SCALE, range_min, range_max), + ReturnCode::FAIL); + // Wrong query code echoed (a SPD reply while expecting TOR). + EXPECT_EQ(ServoDm::parse_mit_range_reply_encos_servo(reply, 3, ServoDm::ENCOS_QUERY_SPD_RANGE, + ServoDm::ENCOS_SPD_RANGE_SCALE, range_min, range_max), + ReturnCode::FAIL); + // Not a query-ack frame (type 1 telemetry). + reply.data[0] = 1 << 5; + EXPECT_EQ(ServoDm::parse_mit_range_reply_encos_servo(reply, 3, ServoDm::ENCOS_QUERY_TOR_RANGE, + ServoDm::ENCOS_TOR_RANGE_SCALE, range_min, range_max), + ReturnCode::FAIL); + // Truncated frame. + reply.data[0] = 5 << 5; + reply.can_dlc = 4; + EXPECT_EQ(ServoDm::parse_mit_range_reply_encos_servo(reply, 3, ServoDm::ENCOS_QUERY_TOR_RANGE, + ServoDm::ENCOS_TOR_RANGE_SCALE, range_min, range_max), + ReturnCode::FAIL); +} + +TEST(EncosMitRangeAdoption, AdoptsSpdButOnlyVerifiesTorAgainstTheCompiledCodec) { CommandLineArgs cla{}; - RestartFailingDriverArx driver(nullptr, cla); + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); + // Compiled ENCOS EC-A4310-P2-36 defaults: SPD +-18 rad/s, TOR +-30 Nm. + ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -18.0f, 18.0f, -30.0f, 30.0f, + 0.2f, 0.3f, 0.1f}; + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::ENCOS_A4310); + + // A matching reported range keeps the compiled parameter object. + EXPECT_EQ(servo.adopt_encos_mit_range(ServoDm::ENCOS_QUERY_TOR_RANGE, -30.0f, 30.0f), ReturnCode::SUCCESS); + EXPECT_EQ(servo.servo_param(), ¶m); + + // A different TOR range is logged but never adopted: torq_rescale in the + // model JSON is conformance-calibrated against the compiled codec, so the + // register value must not shift the gravity feed-forward delivery. + EXPECT_EQ(servo.adopt_encos_mit_range(ServoDm::ENCOS_QUERY_TOR_RANGE, -42.0f, 42.0f), ReturnCode::SUCCESS); + EXPECT_EQ(servo.servo_param(), ¶m); + EXPECT_FLOAT_EQ(((const ServoDmParam*)servo.servo_param())->tor_max_, 30.0f); + + // A different SPD range still repoints the codec to a per-servo override. + EXPECT_EQ(servo.adopt_encos_mit_range(ServoDm::ENCOS_QUERY_SPD_RANGE, -20.0f, 20.0f), ReturnCode::SUCCESS); + const ServoDmParam* p_adopted = (const ServoDmParam*)servo.servo_param(); + ASSERT_NE(p_adopted, ¶m); + EXPECT_FLOAT_EQ(p_adopted->vel_max_, 20.0f); + EXPECT_FLOAT_EQ(p_adopted->tor_max_, 30.0f); // untouched fields keep the compiled values + + // A nonsense range is rejected and leaves the codec unchanged. + EXPECT_EQ(servo.adopt_encos_mit_range(ServoDm::ENCOS_QUERY_SPD_RANGE, 5.0f, -5.0f), ReturnCode::FAIL); + EXPECT_FLOAT_EQ(((const ServoDmParam*)servo.servo_param())->vel_max_, 20.0f); +} + +TEST(DriverCanMitLifecycle, EnablePropagatesReceptionRestartFailure) { + CommandLineArgs cla{}; + RestartFailingDriverCanMit driver(nullptr, cla); driver.adopt_socket(42); EXPECT_EQ(driver.enable(1, static_cast(ServoType::DM_4310), false), ReturnCode::FAIL); } -TEST(DriverArxLifecycle, EnableRecoversOneStaleCommunicationLossFault) { +TEST(DriverCanMitLifecycle, EnableRecoversOneStaleCommunicationLossFault) { int sockets[2]; ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - ScriptedEnableStatusDriverArx driver(nullptr, cla, {0xD, 0x1}); + ScriptedEnableStatusDriverCanMit driver(nullptr, cla, {0xD, 0x1}); driver.adopt_socket(sockets[0]); EXPECT_EQ(driver.enable(1, static_cast(ServoType::DM_4310), true), ReturnCode::SUCCESS); @@ -631,12 +782,12 @@ TEST(DriverArxLifecycle, EnableRecoversOneStaleCommunicationLossFault) { close(sockets[1]); } -TEST(DriverArxLifecycle, EnableFailsAfterOneResetForPersistentCommunicationLoss) { +TEST(DriverCanMitLifecycle, EnableFailsAfterOneResetForPersistentCommunicationLoss) { int sockets[2]; ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - ScriptedEnableStatusDriverArx driver(nullptr, cla, {0xD}); + ScriptedEnableStatusDriverCanMit driver(nullptr, cla, {0xD}); driver.adopt_socket(sockets[0]); EXPECT_EQ(driver.enable(1, static_cast(ServoType::DM_4310), true), ReturnCode::HARDWARE_FAULT); @@ -648,12 +799,12 @@ TEST(DriverArxLifecycle, EnableFailsAfterOneResetForPersistentCommunicationLoss) close(sockets[1]); } -TEST(DriverArxLifecycle, DisableDoesNotResetCommunicationLossFault) { +TEST(DriverCanMitLifecycle, DisableDoesNotResetCommunicationLossFault) { int sockets[2]; ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - ScriptedEnableStatusDriverArx driver(nullptr, cla, {0xD}); + ScriptedEnableStatusDriverCanMit driver(nullptr, cla, {0xD}); driver.adopt_socket(sockets[0]); EXPECT_EQ(driver.enable(1, static_cast(ServoType::DM_4310), false), ReturnCode::HARDWARE_FAULT); @@ -665,9 +816,9 @@ TEST(DriverArxLifecycle, DisableDoesNotResetCommunicationLossFault) { close(sockets[1]); } -TEST(DriverArxLifecycle, ResetZeroPositionAcceptsDescriptorZero) { +TEST(DriverCanMitLifecycle, ResetZeroPositionAcceptsDescriptorZero) { CommandLineArgs cla{}; - ZeroDescriptorDriverArx driver(nullptr, cla); + ZeroDescriptorDriverCanMit driver(nullptr, cla); driver.adopt_socket(0); EXPECT_EQ(driver.reset_zero_position(1, static_cast(ServoType::DM_4310)), ReturnCode::SUCCESS); @@ -676,9 +827,9 @@ TEST(DriverArxLifecycle, ResetZeroPositionAcceptsDescriptorZero) { driver.abandon_socket(); } -TEST(DriverArxTransport, ResetZeroPositionPropagatesSendFailure) { +TEST(DriverCanMitTransport, ResetZeroPositionPropagatesSendFailure) { CommandLineArgs cla{}; - FailingSendDriverArx driver(nullptr, cla); + FailingSendDriverCanMit driver(nullptr, cla); driver.adopt_socket(42); EXPECT_EQ(driver.reset_zero_position(1, static_cast(ServoType::DM_4310)), ReturnCode::BUSY); @@ -686,14 +837,14 @@ TEST(DriverArxTransport, ResetZeroPositionPropagatesSendFailure) { driver.abandon_socket(); } -TEST(DriverArxTransport, SendCommandPropagatesSendFailure) { +TEST(DriverCanMitTransport, SendCommandPropagatesSendFailure) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - FailingSendDriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + FailingSendDriverCanMit driver(&device, cla); driver.adopt_socket(42); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); EXPECT_EQ(driver.send_command(&servo, 1.0f, 0.1f, 0.5f, 0.0f, 0.0f), ReturnCode::BUSY); @@ -702,9 +853,9 @@ TEST(DriverArxTransport, SendCommandPropagatesSendFailure) { driver.abandon_socket(); } -TEST(DriverArxTransport, SendCommandRejectsNullServo) { +TEST(DriverCanMitTransport, SendCommandRejectsNullServo) { CommandLineArgs cla{}; - FailingSendDriverArx driver(nullptr, cla); + FailingSendDriverCanMit driver(nullptr, cla); driver.adopt_socket(42); EXPECT_EQ(driver.send_command(nullptr, 1.0f, 0.1f, 0.5f, 0.0f, 0.0f), ReturnCode::INVALID_PARAM); @@ -713,16 +864,16 @@ TEST(DriverArxTransport, SendCommandRejectsNullServo) { driver.abandon_socket(); } -TEST(DriverArxConcurrency, ConcurrentCloseAndSendCommandAreRaceFree) { +TEST(DriverCanMitConcurrency, ConcurrentCloseAndSendCommandAreRaceFree) { #if !defined(__linux__) GTEST_SKIP() << "socket-backed concurrency assertion currently runs on Linux"; #else CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - SocketBackedDriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + SocketBackedDriverCanMit driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::DM_4310); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); constexpr size_t kRoundCount = 32; @@ -776,12 +927,12 @@ TEST(DriverArxConcurrency, ConcurrentCloseAndSendCommandAreRaceFree) { #endif } -TEST(DriverArxConcurrency, ConcurrentCloseAndResetZeroPositionAreRaceFree) { +TEST(DriverCanMitConcurrency, ConcurrentCloseAndResetZeroPositionAreRaceFree) { #if !defined(__linux__) GTEST_SKIP() << "socket-backed concurrency assertion currently runs on Linux"; #else CommandLineArgs cla{}; - SocketBackedDriverArx driver(nullptr, cla); + SocketBackedDriverCanMit driver(nullptr, cla); constexpr size_t kRoundCount = 32; constexpr size_t kResetterCount = 16; @@ -835,16 +986,16 @@ TEST(DriverArxConcurrency, ConcurrentCloseAndResetZeroPositionAreRaceFree) { #endif } -TEST(DriverArxConcurrency, ConcurrentCloseAndEnableAreRaceFree) { +TEST(DriverCanMitConcurrency, ConcurrentCloseAndEnableAreRaceFree) { #if !defined(__linux__) GTEST_SKIP() << "socket-backed concurrency assertion currently runs on Linux"; #else CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - SocketBackedDriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + SocketBackedDriverCanMit driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::ENCOS_A4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::ENCOS_A4310); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); constexpr size_t kRoundCount = 32; @@ -901,7 +1052,7 @@ TEST(DriverArxConcurrency, ConcurrentCloseAndEnableAreRaceFree) { #endif } -TEST(DriverArxConcurrency, ConcurrentDmEnablesDoNotInterleaveHandshakes) { +TEST(DriverCanMitConcurrency, ConcurrentDmEnablesDoNotInterleaveHandshakes) { #if !defined(__linux__) GTEST_SKIP() << "socket-backed concurrency assertion currently runs on Linux"; #else @@ -909,7 +1060,7 @@ TEST(DriverArxConcurrency, ConcurrentDmEnablesDoNotInterleaveHandshakes) { ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - ConcurrentEnableProbeDriverArx driver(nullptr, cla); + ConcurrentEnableProbeDriverCanMit driver(nullptr, cla); driver.adopt_socket(sockets[0]); std::atomic ready{0}; @@ -943,7 +1094,7 @@ TEST(DriverArxConcurrency, ConcurrentDmEnablesDoNotInterleaveHandshakes) { #endif } -TEST(DriverArxConcurrency, SendCommandDoesNotInterleaveEnableHandshake) { +TEST(DriverCanMitConcurrency, SendCommandDoesNotInterleaveEnableHandshake) { #if !defined(__linux__) GTEST_SKIP() << "socket-backed concurrency assertion currently runs on Linux"; #else @@ -951,12 +1102,12 @@ TEST(DriverArxConcurrency, SendCommandDoesNotInterleaveEnableHandshake) { ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - EnableTransactionProbeDriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + EnableTransactionProbeDriverCanMit driver(&device, cla); driver.adopt_socket(sockets[0]); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 2, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 2, ServoType::DM_4310); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); ReturnCode enable_result = ReturnCode::FAIL; @@ -978,7 +1129,7 @@ TEST(DriverArxConcurrency, SendCommandDoesNotInterleaveEnableHandshake) { #endif } -TEST(DriverArxConcurrency, ResetDoesNotInterleaveEnableHandshake) { +TEST(DriverCanMitConcurrency, ResetDoesNotInterleaveEnableHandshake) { #if !defined(__linux__) GTEST_SKIP() << "socket-backed concurrency assertion currently runs on Linux"; #else @@ -986,7 +1137,7 @@ TEST(DriverArxConcurrency, ResetDoesNotInterleaveEnableHandshake) { ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - EnableTransactionProbeDriverArx driver(nullptr, cla); + EnableTransactionProbeDriverCanMit driver(nullptr, cla); driver.adopt_socket(sockets[0]); ReturnCode enable_result = ReturnCode::FAIL; @@ -1010,14 +1161,14 @@ TEST(DriverArxConcurrency, ResetDoesNotInterleaveEnableHandshake) { TEST(DriverRegistryLifetime, DestroyedServosAreUnregistered) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - FailingSendDriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + FailingSendDriverCanMit driver(&device, cla); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; constexpr int kServoId = 13; { - DriverArxTestServoDm servo(&device, &driver, ¶m, kServoId, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, kServoId, ServoType::DM_4310); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); ASSERT_EQ(Driver::find_servo(kServoId), &servo); ASSERT_EQ(Driver::find_data_index(kServoId), servo.data_index_); @@ -1033,14 +1184,14 @@ TEST(DriverRegistryLifetime, DestroyedServosAreUnregistered) { } } -TEST(DriverArxTransport, SendCommandRejectsInvalidCanFrameBeforeWrite) { +TEST(DriverCanMitTransport, SendCommandRejectsInvalidCanFrameBeforeWrite) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - FailingSendDriverArx driver(&device, cla); + DriverCanMitTestDevice device(cla); + FailingSendDriverCanMit driver(&device, cla); driver.adopt_socket(42); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 15, ServoType::DM_4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 15, ServoType::DM_4310); Driver::register_servo_data_index(servo.id_, -1, nullptr); EXPECT_EQ(driver.send_command(&servo, 1.0f, 0.1f, 0.5f, 0.0f, 0.0f), ReturnCode::INVALID_PARAM); @@ -1049,14 +1200,14 @@ TEST(DriverArxTransport, SendCommandRejectsInvalidCanFrameBeforeWrite) { driver.abandon_socket(); } -TEST(DriverArxTransport, EncosEnablePropagatesSetupSendFailure) { +TEST(DriverCanMitTransport, EncosEnablePropagatesSetupSendFailure) { CommandLineArgs cla{}; - DriverArxTestDevice device(cla); - SetupSendFailingDriverArx driver(&device, cla, 1); + DriverCanMitTestDevice device(cla); + SetupSendFailingDriverCanMit driver(&device, cla, 1); driver.adopt_socket(42); ServoDmParam param{0.0f, 500.0f, 0.0f, 5.0f, -12.5f, 12.5f, -30.0f, 30.0f, -10.0f, 10.0f, 0.2f, 0.3f, 0.1f}; - DriverArxTestServoDm servo(&device, &driver, ¶m, 1, ServoType::ENCOS_A4310); + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1, ServoType::ENCOS_A4310); Driver::register_servo_data_index(servo.id_, servo.data_index_, &servo); EXPECT_EQ(driver.enable(servo.id_, static_cast(ServoType::ENCOS_A4310)), ReturnCode::BUSY); @@ -1066,9 +1217,9 @@ TEST(DriverArxTransport, EncosEnablePropagatesSetupSendFailure) { driver.abandon_socket(); } -TEST(DriverArxTransport, EncosEnableRejectsInvalidSetupFrameBeforeWrite) { +TEST(DriverCanMitTransport, EncosEnableRejectsInvalidSetupFrameBeforeWrite) { CommandLineArgs cla{}; - ZeroDescriptorDriverArx driver(nullptr, cla); + ZeroDescriptorDriverCanMit driver(nullptr, cla); driver.adopt_socket(42); Driver::register_servo_data_index(31, -1, nullptr); @@ -1078,12 +1229,12 @@ TEST(DriverArxTransport, EncosEnableRejectsInvalidSetupFrameBeforeWrite) { driver.abandon_socket(); } -TEST(DriverArxTransport, DmEnableStopsAfterFailedSetupWrite) { +TEST(DriverCanMitTransport, DmEnableStopsAfterFailedSetupWrite) { int sockets[2]; ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - SetupSendFailingDriverArx driver(nullptr, cla, 1); + SetupSendFailingDriverCanMit driver(nullptr, cla, 1); driver.adopt_socket(sockets[0]); EXPECT_EQ(driver.enable(1, static_cast(ServoType::DM_4310), false), ReturnCode::BUSY); @@ -1094,12 +1245,12 @@ TEST(DriverArxTransport, DmEnableStopsAfterFailedSetupWrite) { close(sockets[1]); } -TEST(DriverArxLifecycle, FailedEnableRestartsActiveReception) { +TEST(DriverCanMitLifecycle, FailedEnableRestartsActiveReception) { int sockets[2]; ASSERT_EQ(socketpair(AF_UNIX, SOCK_DGRAM, 0, sockets), 0); CommandLineArgs cla{}; - EnableSendFailingDriverArx driver(nullptr, cla); + EnableSendFailingDriverCanMit driver(nullptr, cla); driver.adopt_socket(sockets[0]); std::atomic callback_entered{false}; @@ -1124,4 +1275,73 @@ TEST(DriverArxLifecycle, FailedEnableRestartsActiveReception) { close(sockets[1]); } +TEST(ServoAbsPosition, ParsesOptionalAbsPositionFieldFromModelConfig) { + CommandLineArgs cla{}; + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); + ServoDmParam param{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + DeviceConfig config; + + // Field present (E_ARX_ENC gripper): sign-agnostic reads are enabled. + { + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1543, ServoType::ARX_ENCODER); + const json servo_config = {{"servo_id", 1543}, + {"data_index", 6}, + {"servo_model", config.val_servo_model_arx_encoder}, + {"abs_position", true}}; + ASSERT_EQ(servo.init_config_model(servo_config, &config), ReturnCode::SUCCESS); + EXPECT_TRUE(servo.abs_position_); + Driver::register_servo_data_index(servo.id_, -1, nullptr); + } + + // Field absent (arm joints): sign-preserving reads stay the default. + { + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1537, ServoType::ARX_ENCODER); + const json servo_config = { + {"servo_id", 1537}, {"data_index", 0}, {"servo_model", config.val_servo_model_arx_encoder}}; + ASSERT_EQ(servo.init_config_model(servo_config, &config), ReturnCode::SUCCESS); + EXPECT_FALSE(servo.abs_position_); + Driver::register_servo_data_index(servo.id_, -1, nullptr); + } +} + +TEST(ServoAbsPosition, RejectsNonBooleanAbsPositionValues) { + CommandLineArgs cla{}; + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); + ServoDmParam param{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + DeviceConfig config; + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1543, ServoType::ARX_ENCODER); + const json servo_config = {{"servo_id", 1543}, + {"data_index", 6}, + {"servo_model", config.val_servo_model_arx_encoder}, + {"abs_position", "yes"}}; + EXPECT_NE(servo.init_config_model(servo_config, &config), ReturnCode::SUCCESS); + Driver::register_servo_data_index(servo.id_, -1, nullptr); +} + +TEST(ServoAbsPosition, ReportsSignAgnosticRelativePositionsWhenEnabled) { + CommandLineArgs cla{}; + DriverCanMitTestDevice device(cla); + DriverCanMit driver(&device, cla); + ServoDmParam param{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; + DriverCanMitTestServoDm servo(&device, &driver, ¶m, 1543, ServoType::ARX_ENCODER); + servo.zero_pos_abs_ = 0.0f; + servo.dir_invert_ = 1; + + // A right-hand ARX_ENC gripper reads negative angles; abs_position folds them positive + // so left/right hardware report the same opening (vendor reference applies abs()). + servo.abs_position_ = true; + servo.curr_pos_abs_ = -0.5f; + EXPECT_NEAR(servo.get_pos_rad_relative(), 0.5f, 1e-6f); + EXPECT_NEAR(servo.get_pos_rad_relative(-0.5f), 0.5f, 1e-6f); + servo.curr_pos_abs_ = 0.5f; + EXPECT_NEAR(servo.get_pos_rad_relative(), 0.5f, 1e-6f); + + // Default keeps the sign for regular joints. + servo.abs_position_ = false; + servo.curr_pos_abs_ = -0.5f; + EXPECT_NEAR(servo.get_pos_rad_relative(), -0.5f, 1e-6f); +} + } // namespace diff --git a/pyproject.toml b/pyproject.toml index 7cf7cef..2a9a272 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build" [project] name = "openpi-control" -version = "0.1.1" +version = "0.1.3" description = "OpenPI native control runtime for YAM and ARX robot arms" readme = "README.md" requires-python = ">=3.11" diff --git a/src/openpi_control/__init__.py b/src/openpi_control/__init__.py index 2038f9e..fbc27a9 100644 --- a/src/openpi_control/__init__.py +++ b/src/openpi_control/__init__.py @@ -36,6 +36,7 @@ ArmState, EffectorState, InputState, + JointServoReport, JointState, PositionCommand, ) @@ -60,6 +61,7 @@ "HardwareFaultError", "InputLayout", "InputState", + "JointServoReport", "JointState", "LeaderArm", "NativeProcessError", diff --git a/src/openpi_control/arms.py b/src/openpi_control/arms.py index 4daad47..45fa7bc 100644 --- a/src/openpi_control/arms.py +++ b/src/openpi_control/arms.py @@ -2,6 +2,7 @@ from __future__ import annotations +import time from threading import RLock from .backend import ArmBackend @@ -9,7 +10,15 @@ from .exceptions import CommandRejectedError, ConfigurationError from .native import NativeArmBackend from .protocol import ArmTopics -from .types import ArmCapabilities, ArmMode, ArmRole, ArmState, InputState, PositionCommand +from .types import ( + ArmCapabilities, + ArmMode, + ArmRole, + ArmState, + InputState, + JointServoReport, + PositionCommand, +) class _Arm: @@ -134,6 +143,53 @@ def hold(self) -> None: with self._dispatch_lock: self._backend.hold() + def enter_gravity_compensation(self, drift_abort_rad: float | None = None) -> None: + """Calibration gravity float: the arm rests on the gravity feed-forward alone. + + Position control (and the live command stream) is suspended until + hold() or move_to_ready() re-engages it at the current pose. The native + control loop itself re-engages HOLD the moment any joint drifts beyond + ``drift_abort_rad`` (node default when None) — the runaway judgment is + deliberately not done client-side, a round trip is too slow to save the + arm. Used by the gravity_tune torq_rescale calibration and follower + float checks. + """ + with self._dispatch_lock: + self._backend.enter_gravity_float(drift_abort_rad) + + def set_torq_rescale(self, values: tuple[float, ...] | list[float]) -> None: + """Runtime per-joint torq_rescale update (one value per arm joint). + + Applied by the native node within one control tick, without a restart, + so calibration candidates switch while the arm keeps holding. The node + rejects a count that does not match the arm DOF. + """ + with self._dispatch_lock: + self._backend.set_torq_rescale(tuple(float(value) for value in values)) + + def servo_reports(self, timeout_s: float = 5.0) -> dict[int, JointServoReport]: + """Per-joint servo parameter reports (codec + motor-reported firmware ranges). + + The node announces one joint per publish (round-robin at ~10 Hz — the + status sockets run with a tiny HWM, so a burst would be dropped), so a + complete set arrives within about one second of connect. Raises + TimeoutError when fewer than DOF reports arrive in time. Used by + gravity_tune to compare two arms' servo parameters before assuming one + torq_rescale calibration fits both. + """ + dof = self.capabilities.dof + deadline = time.monotonic() + timeout_s + while True: + reports = self._backend.servo_reports() + if len(reports) >= dof: + return reports + if time.monotonic() >= deadline: + raise TimeoutError( + f"{self.name}: received servo parameter reports for {len(reports)} of " + f"{dof} joints within {timeout_s:g}s" + ) + time.sleep(0.05) + def move_to_ready(self) -> None: with self._dispatch_lock: self._backend.move_to_ready() diff --git a/src/openpi_control/backend.py b/src/openpi_control/backend.py index 480b7cf..edcc203 100644 --- a/src/openpi_control/backend.py +++ b/src/openpi_control/backend.py @@ -6,7 +6,15 @@ from .config import ArmConfig from .protocol import ArmTopics -from .types import ArmCapabilities, ArmMode, ArmRole, ArmState, InputState, PositionCommand +from .types import ( + ArmCapabilities, + ArmMode, + ArmRole, + ArmState, + InputState, + JointServoReport, + PositionCommand, +) class ArmBackend(ABC): @@ -45,6 +53,18 @@ def pause_live_input(self, paused: bool) -> None: ... @abstractmethod def set_mode(self, mode: ArmMode) -> None: ... + def enter_gravity_float(self, drift_abort_rad: float | None = None) -> None: + """Follower calibration gravity float with an in-loop runaway threshold (rad).""" + raise NotImplementedError + + def set_torq_rescale(self, values: tuple[float, ...]) -> None: + """Runtime per-joint torq_rescale update (calibration tools; no node restart).""" + raise NotImplementedError + + def servo_reports(self) -> dict[int, JointServoReport]: + """Per-joint servo parameter reports received so far (may be incomplete).""" + raise NotImplementedError + @abstractmethod def set_force_feedback_gain(self, gain: float) -> None: ... diff --git a/src/openpi_control/config.py b/src/openpi_control/config.py index 145ec0d..4764e9a 100644 --- a/src/openpi_control/config.py +++ b/src/openpi_control/config.py @@ -4,6 +4,7 @@ import ipaddress import json +import math import re from dataclasses import dataclass from importlib.resources import files @@ -206,11 +207,19 @@ class ArmConfig: # device init (observed on physical YAM followers), so the default leaves # cold starts room to finish. connect_timeout_s: float = 120.0 - # Followers only: use synchronized velocity-limited position tracking with - # model-based gravity feedforward and configured motor-side damping (native - # slew_pos_gravity planning on the arm device; the attached effector keeps - # its own planner). Requires the arm model to ship a gravity algo. - follower_gravity_compensation: bool = False + # Followers only: model-based gravity feedforward torque (plus configured + # motor-side damping) sent with every position command, independent of the + # planning type. None leaves the decision to the follower_gravity_compensation + # field of the arm's individual config JSON; an explicit True/False overrides + # it. MIT CAN arms need a URDF-backed gravity algo (the native node fails + # fast otherwise); controller arms (Trossen) satisfy the flag via the vendor + # controller's built-in compensation; serial arms reject an explicit True. + follower_gravity_compensation: bool | None = None + # Per-joint gravity-delivery calibration override (one value per arm joint). + # None keeps the model/instance config values; a sequence is passed to the + # native node and beats both configs (devices.toml [arms] + # follower_torq_rescale / leader_torq_rescale, per the arm's role). + torq_rescale: tuple[float, ...] | None = None # Leaders only: model-based gravity feedforward torque. Defaults on for # YAM, off for every other model unless explicitly enabled by the caller. leader_gravity_compensation: bool | None = None @@ -243,6 +252,14 @@ def __post_init__(self) -> None: raise ConfigurationError("leader_gravity_compensation must be a boolean") if not isinstance(self.safety_torque_mode, bool): raise ConfigurationError("safety_torque_mode must be a boolean") + if self.torq_rescale is not None: + values = tuple(float(value) for value in self.torq_rescale) + if not values or any(not math.isfinite(value) or value < 0.0 for value in values): + raise ConfigurationError( + "torq_rescale must be a non-empty sequence of nonnegative finite floats " + "(one value per arm joint)" + ) + object.__setattr__(self, "torq_rescale", values) for field_name in ("instance_config", "effector_instance_config", "urdf"): value = getattr(self, field_name) if value is not None: diff --git a/src/openpi_control/models/arms/ARX_ENC/ARX_ENC.json b/src/openpi_control/models/arms/ARX_ENC/ARX_ENC.json index ccd176c..2d935f9 100644 --- a/src/openpi_control/models/arms/ARX_ENC/ARX_ENC.json +++ b/src/openpi_control/models/arms/ARX_ENC/ARX_ENC.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "ARX_ENC", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "topic_type": "ROS", "driver_type": "CAN_ENCODER", "algo_type": "KDL", diff --git a/src/openpi_control/models/arms/ARX_ENC/ARX_ENC_01.json b/src/openpi_control/models/arms/ARX_ENC/ARX_ENC_01.json index 9b36f61..7390204 100644 --- a/src/openpi_control/models/arms/ARX_ENC/ARX_ENC_01.json +++ b/src/openpi_control/models/arms/ARX_ENC/ARX_ENC_01.json @@ -1,10 +1,11 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "ARX_ENC", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "spring_effect": false, "gravity_compensation": false, + "follower_gravity_compensation": false, "base_rpy": [ 0, 0, diff --git a/src/openpi_control/models/arms/ARX_ENC/ARX_ENC_right_01.json b/src/openpi_control/models/arms/ARX_ENC/ARX_ENC_right_01.json new file mode 100644 index 0000000..7390204 --- /dev/null +++ b/src/openpi_control/models/arms/ARX_ENC/ARX_ENC_right_01.json @@ -0,0 +1,88 @@ +{ + "config_version": "1.3.1", + "device_model": "ARX_ENC", + "device_type": "arm", + "arm_type": "can", + "spring_effect": false, + "gravity_compensation": false, + "follower_gravity_compensation": false, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 0, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1537, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0 + } + ] + }, + { + "joint_id": 1, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1538, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0 + } + ] + }, + { + "joint_id": 2, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1539, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0 + } + ] + }, + { + "joint_id": 3, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1540, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0 + } + ] + }, + { + "joint_id": 4, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1541, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0 + } + ] + }, + { + "joint_id": 5, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1542, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/ARX_L5/ARX_L5.json b/src/openpi_control/models/arms/ARX_L5/ARX_L5.json index 768b7d1..9aea5ea 100644 --- a/src/openpi_control/models/arms/ARX_L5/ARX_L5.json +++ b/src/openpi_control/models/arms/ARX_L5/ARX_L5.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "ARX_L5", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "topic_type": "ROS", "driver_type": "CAN", "algo_type": "Pinocchio", @@ -24,17 +24,17 @@ "joints": [ { "joint_id": 1, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.5, - "torq_min": -15, - "torq_max": 15, + "torq_min": -27, + "torq_max": 27, "safe_torq_min": 0, "safe_torq_max": 0, "torq_rescale": 1.4, "pos_rescale": 1, "pos_error_margin": 0.05, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 3, "spring_preload": 0.0, "spring_force_config": true, @@ -47,29 +47,31 @@ "servo_id": 1, "data_index": 0, "dir_invert": 1, + "pos_min": -2.1, + "pos_max": 3.1, "response_delay": 0.025, "kT": 1, "kA": 1, "kV": 1, "pos_kp": 150, "pos_ki": 0, - "pos_kd": 12 + "pos_kd": 5 } ] }, { "joint_id": 2, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.6, - "torq_min": -15, - "torq_max": 15, + "torq_min": -27, + "torq_max": 27, "safe_torq_min": 0, "safe_torq_max": 0, "torq_rescale": 1.4, "pos_rescale": 1, "pos_error_margin": 0.05, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "zero_pos": 0, "spring_home_pos": 0.523598775598299, "spring_constant": 0, @@ -84,29 +86,31 @@ "servo_id": 2, "data_index": 1, "dir_invert": 1, + "pos_min": 0.0, + "pos_max": 3.63, "response_delay": 0.025, "kT": 1, "kA": 1, "kV": 1, "pos_kp": 150, "pos_ki": 0, - "pos_kd": 12 + "pos_kd": 5 } ] }, { "joint_id": 3, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.8, - "torq_min": -15, - "torq_max": 15, + "torq_min": -27, + "torq_max": 27, "safe_torq_min": 0, "safe_torq_max": 0, "torq_rescale": 1.4, "pos_rescale": 1, "pos_error_margin": 0.05, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.6, "spring_preload": -0.0349065850398866, "spring_force_config": true, @@ -119,29 +123,31 @@ "servo_id": 4, "data_index": 3, "dir_invert": 1, + "pos_min": 0.0, + "pos_max": 3.2, "response_delay": 0.025, "kT": 1, "kA": 1, "kV": 1, "pos_kp": 150, "pos_ki": 0, - "pos_kd": 12 + "pos_kd": 5 } ] }, { "joint_id": 4, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, - "torq_min": -15, - "torq_max": 15, + "torq_min": -7, + "torq_max": 7, "safe_torq_min": 0, "safe_torq_max": 0, "torq_rescale": 1.4, "pos_rescale": 1, "pos_error_margin": 0.05, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, "spring_force_config": false, @@ -154,6 +160,8 @@ "servo_id": 5, "data_index": 4, "dir_invert": 1, + "pos_min": -1.45, + "pos_max": 1.35, "response_delay": 0.025, "kT": 2.36, "kA": 1, @@ -166,17 +174,17 @@ }, { "joint_id": 5, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, - "torq_min": -15, - "torq_max": 15, + "torq_min": -7, + "torq_max": 7, "safe_torq_min": 0, "safe_torq_max": 0, "torq_rescale": 1.4, "pos_rescale": 1, "pos_error_margin": 0.05, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, "spring_force_config": false, @@ -189,11 +197,13 @@ "servo_id": 6, "data_index": 5, "dir_invert": 1, + "pos_min": -1.58, + "pos_max": 1.58, "response_delay": 0.025, "kT": 2.36, "kA": 1, "kV": 1, - "pos_kp": 30, + "pos_kp": 25, "pos_ki": 0, "pos_kd": 0.8 } @@ -201,17 +211,17 @@ }, { "joint_id": 6, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, - "torq_min": -15, - "torq_max": 15, + "torq_min": -7, + "torq_max": 7, "safe_torq_min": 0, "safe_torq_max": 0, "torq_rescale": 1.4, "pos_rescale": 1, "pos_error_margin": 0.05, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "zero_pos": 0, "spring_home_pos": 0, "spring_constant": 0.0, @@ -226,13 +236,15 @@ "servo_id": 7, "data_index": 6, "dir_invert": 1, + "pos_min": -2.05, + "pos_max": 2.05, "response_delay": 0.025, "kT": 2.36, "kA": 1, "kV": 1, - "pos_kp": 30, + "pos_kp": 10, "pos_ki": 0, - "pos_kd": 0.8 + "pos_kd": 1 } ] } diff --git a/src/openpi_control/models/arms/ARX_L5/ARX_L5_01.json b/src/openpi_control/models/arms/ARX_L5/ARX_L5_01.json index 85c3de5..e54dd89 100644 --- a/src/openpi_control/models/arms/ARX_L5/ARX_L5_01.json +++ b/src/openpi_control/models/arms/ARX_L5/ARX_L5_01.json @@ -1,10 +1,11 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "ARX_L5", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "spring_effect" : false, "gravity_compensation" : true, + "follower_gravity_compensation": true, "base_rpy": [0, 0, 0], diff --git a/src/openpi_control/models/arms/ARX_L5/ARX_L5_high_gain_01.json b/src/openpi_control/models/arms/ARX_L5/ARX_L5_high_gain_01.json new file mode 100644 index 0000000..7a69dc0 --- /dev/null +++ b/src/openpi_control/models/arms/ARX_L5/ARX_L5_high_gain_01.json @@ -0,0 +1,118 @@ +{ + "config_version": "1.3.1", + "device_model": "ARX_L5", + "device_type": "arm", + "arm_type": "can", + "spring_effect": false, + "gravity_compensation": true, + "follower_gravity_compensation": true, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 1, + "reference_servo_index": 0, + "spring_invert": false, + "servos": [ + { + "servo_id": 1, + "pos_min": -3.14, + "pos_max": 2.618, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 150, + "pos_kd": 12 + } + ] + }, + { + "joint_id": 2, + "reference_servo_index": 0, + "spring_invert": false, + "servos": [ + { + "servo_id": 2, + "pos_min": -0.1, + "pos_max": 3.14, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0.523598775598299, + "pos_kp": 150, + "pos_kd": 12 + } + ] + }, + { + "joint_id": 3, + "reference_servo_index": 0, + "spring_invert": false, + "servos": [ + { + "servo_id": 4, + "pos_min": -0.1, + "pos_max": 3.14, + "zero_pos": 0.002, + "home_pos": 0, + "spring_home_pos": 0.07457828521728516, + "pos_kp": 150, + "pos_kd": 12 + } + ] + }, + { + "joint_id": 4, + "reference_servo_index": 0, + "spring_invert": false, + "servos": [ + { + "servo_id": 5, + "pos_min": -1.571, + "pos_max": 1.571, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 30, + "pos_kd": 0.8 + } + ] + }, + { + "joint_id": 5, + "reference_servo_index": 0, + "spring_invert": false, + "servos": [ + { + "servo_id": 6, + "pos_min": -1.57, + "pos_max": 1.57, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 30, + "pos_kd": 0.8 + } + ] + }, + { + "joint_id": 6, + "reference_servo_index": 0, + "spring_invert": false, + "servos": [ + { + "servo_id": 7, + "pos_min": -1.57, + "pos_max": 1.57, + "zero_pos": 0.0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 30, + "pos_kd": 0.8 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/ARX_X5/ARX_X5.json b/src/openpi_control/models/arms/ARX_X5/ARX_X5.json index c251b92..48c7b1d 100644 --- a/src/openpi_control/models/arms/ARX_X5/ARX_X5.json +++ b/src/openpi_control/models/arms/ARX_X5/ARX_X5.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "ARX_X5", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "topic_type": "ROS", "driver_type": "CAN", "algo_type": "KDL", @@ -25,17 +25,17 @@ { "joint_id": 0, "nominal_stall_ok": false, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.5, - "torq_min": -7, - "torq_max": 7, + "torq_min": -36, + "torq_max": 36, "safe_torq_min": -15, "safe_torq_max": 15, - "torq_rescale": 0.8, + "torq_rescale": 0.803, "pos_rescale": 1, "pos_error_margin": 0.1, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 3, "spring_preload": 0.0, "spring_force_config": true, @@ -49,33 +49,33 @@ "servo_id": 1, "data_index": 0, "dir_invert": 1, - "pos_min": -2.0944, - "pos_max": 3.1416, + "pos_min": -2.1, + "pos_max": 3.1, "reverse_flag": false, "response_delay": 0.025, "kT": 0.72, "kA": 1, "kV": 1, - "pos_kp": 150, + "pos_kp": 40, "pos_ki": 0, - "pos_kd": 12 + "pos_kd": 1.2 } ] }, { "joint_id": 1, "nominal_stall_ok": true, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.6, - "torq_min": -7, - "torq_max": 7, + "torq_min": -36, + "torq_max": 36, "safe_torq_min": -7, "safe_torq_max": 7, - "torq_rescale": 0.8, + "torq_rescale": 0.803, "pos_rescale": 1, "pos_error_margin": 0.1, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0, "spring_preload": 0.0, "spring_force_config": false, @@ -90,32 +90,32 @@ "data_index": 1, "dir_invert": 1, "pos_min": 0.0, - "pos_max": 3.5256, + "pos_max": 3.63, "reverse_flag": false, "response_delay": 0.025, "kT": 0.72, "kA": 1, "kV": 1, - "pos_kp": 150, + "pos_kp": 40, "pos_ki": 0, - "pos_kd": 12 + "pos_kd": 1.2 } ] }, { "joint_id": 2, "nominal_stall_ok": true, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.8, - "torq_min": -7, - "torq_max": 7, + "torq_min": -36, + "torq_max": 36, "safe_torq_min": -7, "safe_torq_max": 7, - "torq_rescale": 0.8, + "torq_rescale": 0.803, "pos_rescale": 1, "pos_error_margin": 0.1, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.6, "spring_preload": -0.0349065850398866, "spring_force_config": true, @@ -130,32 +130,32 @@ "data_index": 3, "dir_invert": 1, "pos_min": 0.0, - "pos_max": 4.7647, + "pos_max": 3.2, "reverse_flag": false, "response_delay": 0.025, "kT": 0.72, "kA": 1, "kV": 1, - "pos_kp": 150, + "pos_kp": 32, "pos_ki": 0, - "pos_kd": 12 + "pos_kd": 1.0 } ] }, { "joint_id": 3, "nominal_stall_ok": true, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, "torq_min": -7, "torq_max": 7, "safe_torq_min": -15, "safe_torq_max": 15, - "torq_rescale": 1.5, + "torq_rescale": 1.51, "pos_rescale": 1, "pos_error_margin": 0.1, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, "spring_force_config": false, @@ -169,14 +169,14 @@ "servo_id": 5, "data_index": 4, "dir_invert": 1, - "pos_min": -1.5882, - "pos_max": 1.5882, + "pos_min": -1.45, + "pos_max": 1.35, "reverse_flag": false, "response_delay": 0.025, "kT": 2.36, "kA": 1, "kV": 1, - "pos_kp": 30, + "pos_kp": 10, "pos_ki": 0, "pos_kd": 0.8 } @@ -185,17 +185,17 @@ { "joint_id": 4, "nominal_stall_ok": false, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, "torq_min": -7, "torq_max": 7, "safe_torq_min": -15, "safe_torq_max": 15, - "torq_rescale": 1.5, + "torq_rescale": 1.51, "pos_rescale": 1, "pos_error_margin": 0.1, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, "spring_force_config": false, @@ -209,14 +209,14 @@ "servo_id": 6, "data_index": 5, "dir_invert": 1, - "pos_min": -1.5882, - "pos_max": 1.5882, + "pos_min": -1.58, + "pos_max": 1.58, "reverse_flag": false, "response_delay": 0.025, "kT": 2.36, "kA": 1, "kV": 1, - "pos_kp": 25, + "pos_kp": 10, "pos_ki": 0, "pos_kd": 0.8 } @@ -225,17 +225,17 @@ { "joint_id": 5, "nominal_stall_ok": false, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, "torq_min": -7, "torq_max": 7, "safe_torq_min": -15, "safe_torq_max": 15, - "torq_rescale": 1.5, + "torq_rescale": 1.51, "pos_rescale": 1, "pos_error_margin": 0.1, "safe_mode_derating": 0.1, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, "spring_force_config": false, @@ -249,8 +249,8 @@ "servo_id": 7, "data_index": 6, "dir_invert": 1, - "pos_min": -2.0944, - "pos_max": 2.0944, + "pos_min": -2.05, + "pos_max": 2.05, "reverse_flag": false, "response_delay": 0.025, "kT": 2.36, diff --git a/src/openpi_control/models/arms/ARX_X5/ARX_X5_01.json b/src/openpi_control/models/arms/ARX_X5/ARX_X5_01.json index f2110d2..fced60d 100644 --- a/src/openpi_control/models/arms/ARX_X5/ARX_X5_01.json +++ b/src/openpi_control/models/arms/ARX_X5/ARX_X5_01.json @@ -1,10 +1,11 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "ARX_X5", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "spring_effect": false, "gravity_compensation": true, + "follower_gravity_compensation": true, "base_rpy": [ 0, 0, diff --git a/src/openpi_control/models/arms/ARX_X5/ARX_X5_high_gain_01.json b/src/openpi_control/models/arms/ARX_X5/ARX_X5_high_gain_01.json new file mode 100644 index 0000000..3087b5a --- /dev/null +++ b/src/openpi_control/models/arms/ARX_X5/ARX_X5_high_gain_01.json @@ -0,0 +1,100 @@ +{ + "config_version": "1.3.1", + "device_model": "ARX_X5", + "device_type": "arm", + "arm_type": "can", + "spring_effect": false, + "gravity_compensation": true, + "follower_gravity_compensation": true, + "base_rpy": [ + 0, + 0, + 0 + ], + "joints": [ + { + "joint_id": 0, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 1, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 150, + "pos_kd": 12 + } + ] + }, + { + "joint_id": 1, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 2, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0.523598775598299, + "pos_kp": 150, + "pos_kd": 12 + } + ] + }, + { + "joint_id": 2, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 4, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0.349065850398866, + "pos_kp": 150, + "pos_kd": 12 + } + ] + }, + { + "joint_id": 3, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 5, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 30, + "pos_kd": 0.8 + } + ] + }, + { + "joint_id": 4, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 6, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 25, + "pos_kd": 0.8 + } + ] + }, + { + "joint_id": 5, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 7, + "zero_pos": 0, + "home_pos": 0, + "spring_home_pos": 0, + "pos_kp": 10, + "pos_kd": 1 + } + ] + } + ] +} diff --git a/src/openpi_control/models/arms/SO101/SO101.json b/src/openpi_control/models/arms/SO101/SO101.json index 4f8e3e3..45593f3 100644 --- a/src/openpi_control/models/arms/SO101/SO101.json +++ b/src/openpi_control/models/arms/SO101/SO101.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "SO101", "device_type": "arm", - "arm_type": "nello", + "arm_type": "serial", "topic_type": "ROS", "driver_type": "FEETECH", "algo_type": "Pinocchio", diff --git a/src/openpi_control/models/arms/SO101/SO101_01.json b/src/openpi_control/models/arms/SO101/SO101_01.json index d236cc7..c092126 100644 --- a/src/openpi_control/models/arms/SO101/SO101_01.json +++ b/src/openpi_control/models/arms/SO101/SO101_01.json @@ -1,10 +1,11 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "SO101", "device_type": "arm", - "arm_type": "nello", + "arm_type": "serial", "spring_effect": false, "gravity_compensation": false, + "follower_gravity_compensation": false, "base_rpy": [ 0, 0, diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json index 2ce0345..61a7703 100644 --- a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl.json @@ -1,5 +1,5 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "Trossen_wai_ctrl", "device_type": "arm", "arm_type": "controller", diff --git a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json index 1daf2fa..de7f871 100644 --- a/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json +++ b/src/openpi_control/models/arms/Trossen_wai_ctrl/Trossen_wai_ctrl_01.json @@ -1,10 +1,11 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "Trossen_wai_ctrl", "device_type": "arm", "arm_type": "controller", "spring_effect": false, - "gravity_compensation": false, + "gravity_compensation": true, + "follower_gravity_compensation": true, "base_rpy": [ 0, 0, diff --git a/src/openpi_control/models/arms/Yam/Yam.json b/src/openpi_control/models/arms/Yam/Yam.json index cc11822..685f6e4 100644 --- a/src/openpi_control/models/arms/Yam/Yam.json +++ b/src/openpi_control/models/arms/Yam/Yam.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "Yam", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "topic_type" : "ROS", "driver_type" : "CAN", "algo_type": "Pinocchio", @@ -22,12 +22,12 @@ "joint_id": 1, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.5, "follow_viscous_damping": 0.7777778, - "torq_min": -10, - "torq_max": 10, + "torq_min": -27, + "torq_max": 27, "safe_torq_min": -7, "safe_torq_max": 7, "torq_rescale": 1.1, @@ -37,7 +37,7 @@ "pos_error_margin": 0.05, "safe_mode_derating": 0.05, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.1, "spring_preload": 0.0, @@ -70,12 +70,12 @@ "joint_id": 2, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.6, "follow_viscous_damping": 0.7777778, - "torq_min": -10, - "torq_max": 10, + "torq_min": -27, + "torq_max": 27, "safe_torq_min": -7, "safe_torq_max": 7, "torq_rescale": 1.1, @@ -85,7 +85,7 @@ "pos_error_margin": 0.05, "safe_mode_derating": 0.05, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0, "spring_preload": 0, @@ -121,12 +121,12 @@ "joint_id": 3, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 2.8, "follow_viscous_damping": 0.7777778, - "torq_min": -10, - "torq_max": 10, + "torq_min": -27, + "torq_max": 27, "safe_torq_min": -7, "safe_torq_max": 7, "torq_rescale": 1.1, @@ -136,7 +136,7 @@ "pos_error_margin": 0.05, "safe_mode_derating": 0.05, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant":0.4, "spring_preload": 0, @@ -169,7 +169,7 @@ "joint_id": 4, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, "follow_viscous_damping": 0.0, @@ -184,7 +184,7 @@ "pos_error_margin": 0.05, "safe_mode_derating": 0.05, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, @@ -217,7 +217,7 @@ "joint_id": 5, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, "follow_viscous_damping": 0.0, @@ -232,7 +232,7 @@ "pos_error_margin": 0.05, "safe_mode_derating": 0.05, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, @@ -265,7 +265,7 @@ "joint_id": 6, - "vel_max": 2.0, + "vel_max": 20.0, "follow_vel_max": 6.0, "follow_viscous_damping": 0.0, @@ -280,7 +280,7 @@ "pos_error_margin": 0.05, "safe_mode_derating": 0.05, - "accel_max": 1.0, + "accel_max": 40.0, "spring_constant": 0.0, "spring_preload": 0.0, diff --git a/src/openpi_control/models/arms/Yam/Yam_01.json b/src/openpi_control/models/arms/Yam/Yam_01.json index c13d35b..34e0986 100644 --- a/src/openpi_control/models/arms/Yam/Yam_01.json +++ b/src/openpi_control/models/arms/Yam/Yam_01.json @@ -1,10 +1,11 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "Yam", "device_type": "arm", - "arm_type": "arx", + "arm_type": "can", "spring_effect": false, "gravity_compensation": true, + "follower_gravity_compensation": true, "base_rpy": [ 0, 0, diff --git a/src/openpi_control/models/effectors/E_ARX/E_ARX.json b/src/openpi_control/models/effectors/E_ARX/E_ARX.json index 3242e79..bb444f8 100644 --- a/src/openpi_control/models/effectors/E_ARX/E_ARX.json +++ b/src/openpi_control/models/effectors/E_ARX/E_ARX.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_ARX", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "topic_type": "ROS", "driver_type": "CAN", "algo_type": "Algo", @@ -17,6 +17,7 @@ { "joint_id": 6, "vel_max": 3.0, + "grip_torque_limit": 1.11, "torq_min": -7, "torq_max": 7, "safe_torq_min": 0, diff --git a/src/openpi_control/models/effectors/E_ARX/E_ARX_01.json b/src/openpi_control/models/effectors/E_ARX/E_ARX_01.json index 85b23aa..87b1e12 100644 --- a/src/openpi_control/models/effectors/E_ARX/E_ARX_01.json +++ b/src/openpi_control/models/effectors/E_ARX/E_ARX_01.json @@ -1,11 +1,12 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_ARX", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "spring_effect": false, "control_mode": "torque", - "dist_to_torque_const": 2.0, + "dist_to_torque_const": 6.67, + "grip_spring_offset": 0.2, "base_rpy": [ 0, 0, diff --git a/src/openpi_control/models/effectors/E_ARX/E_ARX_l5_01.json b/src/openpi_control/models/effectors/E_ARX/E_ARX_l5_01.json new file mode 100644 index 0000000..4b78c08 --- /dev/null +++ b/src/openpi_control/models/effectors/E_ARX/E_ARX_l5_01.json @@ -0,0 +1,34 @@ +{ + "config_version": "1.3.1", + "device_model": "E_ARX", + "device_type": "effector", + "effector_type": "can", + "spring_effect": false, + "control_mode": "torque", + "dist_to_torque_const": 4.44, + "grip_spring_offset": 0.1, + "base_rpy": [ + 0, + 0, + 0 + ], + "rpy_invert": [ + 1, + 1, + 1 + ], + "joints": [ + { + "joint_id": 6, + "reference_servo_index": 0, + "servos": [ + { + "servo_id": 8, + "zero_pos": 0.019, + "home_pos": 0, + "spring_home_pos": 4.9 + } + ] + } + ] +} diff --git a/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC.json b/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC.json index 74d841c..bc03e2d 100644 --- a/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC.json +++ b/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_ARX_ENC", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "topic_type": "ROS", "driver_type": "CAN_ENCODER", "algo_type": "Algo", @@ -42,6 +42,7 @@ "servo_id": 1543, "data_index": 6, "dir_invert": 1, + "abs_position": true, "reverse_flag": false, "pos_max": 1.2379, "pos_min": 0, diff --git a/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC_01.json b/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC_01.json index 7660957..6382b0a 100644 --- a/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC_01.json +++ b/src/openpi_control/models/effectors/E_ARX_ENC/E_ARX_ENC_01.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_ARX_ENC", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "spring_effect": false, "control_mode": "torque", "dist_to_torque_const": 2.0, diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101.json b/src/openpi_control/models/effectors/E_SO101/E_SO101.json index 9810016..2df6aa3 100644 --- a/src/openpi_control/models/effectors/E_SO101/E_SO101.json +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_SO101", "device_type": "effector", - "effector_type": "nello", + "effector_type": "serial", "topic_type": "ROS", "driver_type": "FEETECH", "algo_type": "Algo", diff --git a/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json b/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json index 9fa6d12..097e171 100644 --- a/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json +++ b/src/openpi_control/models/effectors/E_SO101/E_SO101_01.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_SO101", "device_type": "effector", - "effector_type": "nello", + "effector_type": "serial", "spring_effect": false, "control_mode": "position", "base_rpy": [ diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json index bb0ff0c..9980d8c 100644 --- a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl.json @@ -1,5 +1,5 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Trossen_ctrl", "device_type": "effector", "effector_type": "controller", diff --git a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json index ba7c9c3..71c6e7f 100644 --- a/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json +++ b/src/openpi_control/models/effectors/E_Trossen_ctrl/E_Trossen_ctrl_01.json @@ -1,5 +1,5 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Trossen_ctrl", "device_type": "effector", "effector_type": "controller", diff --git a/src/openpi_control/models/effectors/E_Yam/E_Yam.json b/src/openpi_control/models/effectors/E_Yam/E_Yam.json index cd7dcfe..cb922a2 100644 --- a/src/openpi_control/models/effectors/E_Yam/E_Yam.json +++ b/src/openpi_control/models/effectors/E_Yam/E_Yam.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Yam", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "topic_type" : "ROS", "driver_type" : "CAN", "algo_type": "Algo", diff --git a/src/openpi_control/models/effectors/E_Yam/E_Yam_01.json b/src/openpi_control/models/effectors/E_Yam/E_Yam_01.json index 916cf94..0c2536e 100644 --- a/src/openpi_control/models/effectors/E_Yam/E_Yam_01.json +++ b/src/openpi_control/models/effectors/E_Yam/E_Yam_01.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Yam", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "spring_effect": false, "control_mode": "position", diff --git a/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle.json b/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle.json index 6e823ec..bb02baf 100644 --- a/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle.json +++ b/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Yam_Handle", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "topic_type": "ROS", "driver_type": "CAN", "algo_type": "Algo", diff --git a/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle_01.json b/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle_01.json index 97e8344..794c7d1 100644 --- a/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle_01.json +++ b/src/openpi_control/models/effectors/E_Yam_Handle/E_Yam_Handle_01.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Yam_Handle", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "spring_effect": false, "control_mode": "position", diff --git a/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat.json b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat.json index 77fa7ec..80a48b0 100644 --- a/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat.json +++ b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Yam_Handle_compat", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "topic_type": "ROS", "driver_type": "CAN", "algo_type": "Algo", diff --git a/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_01.json b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_01.json index bedc6fc..b43aeb6 100644 --- a/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_01.json +++ b/src/openpi_control/models/effectors/E_Yam_Handle_compat/E_Yam_Handle_compat_01.json @@ -1,8 +1,8 @@ { - "config_version": "1.1.1", + "config_version": "1.3.1", "device_model": "E_Yam_Handle_compat", "device_type": "effector", - "effector_type": "arx", + "effector_type": "can", "spring_effect": false, "control_mode": "position", diff --git a/src/openpi_control/native.py b/src/openpi_control/native.py index 9ac9b70..b435f4a 100644 --- a/src/openpi_control/native.py +++ b/src/openpi_control/native.py @@ -62,6 +62,7 @@ ArmState, EffectorState, InputState, + JointServoReport, JointState, PositionCommand, ) @@ -249,6 +250,7 @@ def __init__(self) -> None: self._inputs_sequence = 0 self._input_layout = InputLayout() self._capabilities: ArmCapabilities | None = None + self._servo_params: dict[int, JointServoReport] = {} self._condition = threading.Condition() self._connect_lock = threading.RLock() self._close_lock = threading.Lock() @@ -299,6 +301,7 @@ def _prepare_connect(self) -> None: self._inputs = None self._inputs_sequence = 0 self._capabilities = None + self._servo_params = {} self._acks.clear() self._pending_ack_request_id = None self._request_id = 0 @@ -446,11 +449,25 @@ def _connect_prepared( ] if self._paired_follower_state_topic: args.extend(["--paired_follower_state_topic", self._paired_follower_state_topic]) - if role is ArmRole.FOLLOWER and config.follower_gravity_compensation: - # Arm-device-scoped MonoPi-style synchronized slew tracking plus - # gravity/damping feedforward. The attached effector keeps the - # planner from its own model config. - args.extend(["--arm_planning_type", "slew_pos_gravity"]) + if config.torq_rescale is not None: + # Highest-precedence gravity-delivery calibration (devices.toml + # [arms] follower_torq_rescale / leader_torq_rescale, per the + # arm's role): the node applies it after the model and individual + # configs. Passed for every role so the arm_check gravity-float + # calibration run sees the same values. + args.extend(["--torq_rescale", ",".join(f"{value:g}" for value in config.torq_rescale)]) + if role is ArmRole.FOLLOWER and config.follower_gravity_compensation is not None: + # Gravity/damping feedforward on every follower + # position command, independent of the planning type. Scoped to + # the arm device: the attached effector never receives it. When + # None, the flag stays at "config" and the arm's individual config + # JSON (follower_gravity_compensation field) decides. + args.extend( + [ + "--follower_gravity_compensation", + "true" if config.follower_gravity_compensation else "false", + ] + ) if role is ArmRole.LEADER and config.leader_gravity_compensation: args.append("--leader_gravity_compensation") if config.safety_torque_mode: @@ -788,6 +805,19 @@ def _consume_status(self, payload: bytes) -> None: # State and status use independent ZMQ topics, so consume a # state sample after the correlated completion before returning. self._pending_ready_state_generation = self._state_generation + elif status is NativeStatus.SERVO_PARAM and len(ints) >= 6 and len(floats) >= 9: + # The wire format caps a device-info message at 10 floats, so + # the position gains travel in the int slots as milli-units. + self._servo_params[ints[0]] = JointServoReport( + codec_vel_range=(floats[0], floats[1]), + codec_tor_range=(floats[2], floats[3]), + reported_spd_range=(floats[4], floats[5]) if ints[1] else None, + reported_tor_range=(floats[6], floats[7]) if ints[2] else None, + torq_rescale=floats[8], + pos_kp=ints[4] / 1000.0, + pos_kd=ints[5] / 1000.0, + gravity_feed_forward=bool(ints[3]), + ) elif status is NativeStatus.MODE and self._state and ints: mode_values = list(ArmMode) if 0 <= ints[0] < len(mode_values): @@ -1006,6 +1036,27 @@ def set_mode(self, mode: ArmMode) -> None: raise CommandRejectedError(f"native runtime cannot directly enter {mode}") self._replace_mode(mode, expected_reader=reader) + def enter_gravity_float(self, drift_abort_rad: float | None = None) -> None: + # Follower calibration float: the runaway judgment lives in the native + # control loop (a client round trip is too slow to save the arm), so + # the threshold is shipped with the command instead of being enforced + # here. + with self._mode_lock: + with self._condition: + reader = self._reader + floats = () if drift_abort_rad is None else (float(drift_abort_rad),) + self._send_lifecycle(NativeCommand.ENTER_GRAVITY_COMPENSATION, floats=floats) + self._replace_mode(ArmMode.GRAVITY_COMPENSATION, expected_reader=reader) + + def set_torq_rescale(self, values: tuple[float, ...]) -> None: + self._send_lifecycle( + NativeCommand.SET_TORQ_RESCALE, floats=tuple(float(value) for value in values) + ) + + def servo_reports(self) -> dict[int, JointServoReport]: + with self._condition: + return dict(self._servo_params) + def set_force_feedback_gain(self, gain: float) -> None: self._send_lifecycle(NativeCommand.SET_FORCE_FEEDBACK_GAIN, floats=(gain,)) diff --git a/src/openpi_control/protocol.py b/src/openpi_control/protocol.py index 09bf85a..c833f6d 100644 --- a/src/openpi_control/protocol.py +++ b/src/openpi_control/protocol.py @@ -36,6 +36,7 @@ class NativeCommand(IntEnum): SET_FORCE_FEEDBACK_GAIN = 32 HOLD = 33 HEARTBEAT = 34 + SET_TORQ_RESCALE = 35 class NativeStatus(IntEnum): @@ -47,6 +48,7 @@ class NativeStatus(IntEnum): HANDSHAKE = 30 COMMAND_ACK = 31 MODE = 32 + SERVO_PARAM = 33 CAP_DIRECT = 1 << 0 diff --git a/src/openpi_control/types.py b/src/openpi_control/types.py index 989d03c..a45539b 100644 --- a/src/openpi_control/types.py +++ b/src/openpi_control/types.py @@ -186,6 +186,33 @@ def __post_init__(self) -> None: raise ConfigurationError("effector command must be normalized to [0, 1]") +@dataclass(frozen=True, slots=True) +class JointServoReport: + """One joint's servo parameters as the native node reports them. + + The codec ranges scale every wire command/status; the reported ranges come + from the motor's own firmware registers (ENCOS range query) and expose + batch differences (e.g. TOR registers of 30 vs 42 Nm) that make one + torq_rescale calibration invalid for another arm. Reported ranges are None + for motor families without a range query. + + torq_rescale, pos_kp and pos_kd are the values the node actually applies + (after every config layer and runtime update), so consumers such as the + rollout dump can record the effective calibration without re-deriving it + from config files. gravity_feed_forward is device-level (the follower + gravity feed-forward on/off state) and repeats on every joint's report. + """ + + codec_vel_range: tuple[float, float] + codec_tor_range: tuple[float, float] + reported_spd_range: tuple[float, float] | None + reported_tor_range: tuple[float, float] | None + torq_rescale: float + pos_kp: float + pos_kd: float + gravity_feed_forward: bool + + @dataclass(frozen=True, slots=True) class ArmCapabilities: protocol_version: tuple[int, int] diff --git a/tests/sil/fake_dm_servo.py b/tests/sil/fake_dm_servo.py index e525fa7..79b6899 100644 --- a/tests/sil/fake_dm_servo.py +++ b/tests/sil/fake_dm_servo.py @@ -249,7 +249,7 @@ def set_torque_mobile(self, motor_id: int, rad_per_nm_per_frame: float = 0.045) Real DM servos accelerate under torque-only MIT frames; kp-gated teleporting cannot represent that, so torque-mode consumers (the - monopi-style gripper spring) opt their motor into ``pos += torque * + gripper torque spring) opt their motor into ``pos += torque * gain`` per command frame. Stuck motors stay stuck (a blocked gripper). """ with self._lock: diff --git a/tests/sil/test_native_sil.py b/tests/sil/test_native_sil.py index 83bbb41..fe927f5 100644 --- a/tests/sil/test_native_sil.py +++ b/tests/sil/test_native_sil.py @@ -15,6 +15,7 @@ import sys import time from pathlib import Path +from typing import Any import pytest @@ -242,7 +243,10 @@ def test_killed_node_surfaces_as_native_process_error(fake_bus, session_factory) time.sleep(0.1) -def test_native_node_gracefully_stops_when_python_parent_dies(fake_bus, tmp_path): +def test_native_node_gracefully_stops_when_python_parent_dies( + fake_bus: Any, + tmp_path: Path, +) -> None: """Exercise the real inherited-FD lifeline across an abrupt Python exit.""" native_pid_file = tmp_path / "native-pid" client_script = """ @@ -678,13 +682,14 @@ def assert_bounded(target: float, expected_window: float) -> None: def test_follower_gravity_compensation_feeds_torque(fake_bus): - # With follower_gravity_compensation enabled, the arm device switches to - # slew_pos_gravity planning: every follower position command carries a - # model-based (pinocchio RNEA over Yam.urdf) gravity feedforward torque plus - # MonoPi-style viscous damping on joints 1-3. Park the fake arm bent so the - # pitch joints are gravity-loaded, then assert the MIT command frames carry - # torque on the loaded elbow and that position tracking still works through - # the synchronized velocity slew limiter. + # With follower_gravity_compensation enabled (independent of the planning + # type), every follower position command carries a model-based (pinocchio + # RNEA over Yam.urdf) gravity feedforward torque plus viscous + # damping on joints 1-3, all scaled by the leader-validated per-joint + # torq_rescale. Park the fake arm bent so the pitch joints are + # gravity-loaded, then assert the MIT command frames carry torque on the + # loaded elbow and that position tracking still works through the + # synchronized velocity slew limiter. from openpi_control import ArmConfig, ArmSession, PositionCommand, SocketCanConnection for motor_id in (2, 3): @@ -715,8 +720,10 @@ def test_follower_gravity_compensation_feeds_torque(fake_bus): joint4_baseline = fake_bus.last_torque(4) fake_bus.set_reported_velocity(1, 1.0) fake_bus.set_reported_velocity(4, 1.0) + # Wire torque = follow_viscous_damping (0.7777778) x torq_rescale (1.1), + # the same per-joint rescale the leader gravity paths apply. wait_for( - lambda: abs((fake_bus.last_torque(1) - joint1_baseline) + 0.7777778) < 0.1, + lambda: abs((fake_bus.last_torque(1) - joint1_baseline) + 0.8555556) < 0.1, timeout_s=5.0, what="joint-1 viscous damping to oppose measured velocity", ) @@ -1392,7 +1399,7 @@ def test_over_torque_warns_once_and_continues_by_default(fake_bus, session_facto session.connect() follower.read_state(timeout_s=10.0) - fake_bus.set_reported_torque(2, 12.0) # joint 2 torq_max is 10 Nm + fake_bus.set_reported_torque(2, 27.5) # joint 2 torq_max is 27 Nm warning = "Torque safe mode disabled: sustained measured torque exceeds limit" def warning_count() -> int: @@ -1411,7 +1418,7 @@ def warning_count() -> int: assert "launch with --safety_torque_mode" in warning_lines[0] assert "servo ID 2" in warning_lines[0] assert "current=" in warning_lines[0] - assert "torq_max=10.000 Nm" in warning_lines[0] + assert "torq_max=27.000 Nm" in warning_lines[0] assert "Protective stop: Torque limit exceeded" not in log target = [0.2, 0.25, 0.3, 0.0, 0.1, -0.1] @@ -1430,7 +1437,7 @@ def warning_count() -> int: timeout_s=60.0, what="over-torque warning hysteresis to re-arm", ) - fake_bus.set_reported_torque(2, 12.0) + fake_bus.set_reported_torque(2, 27.5) wait_for(lambda: warning_count() == 2, timeout_s=90.0, what="re-armed over-torque warning") assert process.poll() is None fake_bus.set_reported_torque(2, 0.0) @@ -1476,7 +1483,7 @@ def test_over_torque_on_middle_joint_escalates_to_protective_stop(fake_bus, sess session.connect() follower.read_state(timeout_s=10.0) - fake_bus.set_reported_torque(2, 12.0) # joint 2 torq_max is 10 Nm + fake_bus.set_reported_torque(2, 27.5) # joint 2 torq_max is 27 Nm def protective_stop_logged() -> bool: log = "".join(follower._backend._log_lines) # noqa: SLF001 @@ -1557,7 +1564,7 @@ def test_leader_recovery_goes_silent_instead_of_steering_the_follower(fake_bus_w session.connect() leader.read_state(timeout_s=10.0) - bus.set_reported_torque(2, 12.0) + bus.set_reported_torque(2, 27.5) def gate_logged() -> bool: return "leader observation publishing gated" in "".join( diff --git a/tests/test_arms.py b/tests/test_arms.py new file mode 100644 index 0000000..9add41b --- /dev/null +++ b/tests/test_arms.py @@ -0,0 +1,62 @@ +"""Unit tests for the role-specific public arm handles.""" + +from openpi_control.arms import FollowerArm +from openpi_control.config import ArmConfig, SocketCanConnection +from openpi_control.protocol import topics_for +from openpi_control.types import ArmMode + + +class _RecordingBackend: + """Minimal ArmBackend stand-in that records the dispatched calls.""" + + def __init__(self) -> None: + self.modes: list[ArmMode] = [] + self.holds = 0 + self.float_thresholds: list[float | None] = [] + self.torq_rescales: list[tuple[float, ...]] = [] + + def configure_pair(self, *, follower_state_topic: str) -> None: + del follower_state_topic + + def set_mode(self, mode: ArmMode) -> None: + self.modes.append(mode) + + def enter_gravity_float(self, drift_abort_rad: float | None = None) -> None: + self.float_thresholds.append(drift_abort_rad) + + def set_torq_rescale(self, values: tuple[float, ...]) -> None: + self.torq_rescales.append(values) + + def hold(self) -> None: + self.holds += 1 + + +def test_follower_arm_exposes_the_calibration_gravity_float() -> None: + # gravity_tune floats a follower (gravity feed-forward only, no position + # PD) between position moves; the native node accepts + # ENTER_GRAVITY_COMPENSATION on followers since 00.00.91, watches the + # runaway drift in its own control loop (a client round trip is too slow), + # and HOLD re-engages position control at the current pose. + config = ArmConfig("follower", "ARX_X5", SocketCanConnection("test")) + backend = _RecordingBackend() + arm = FollowerArm(config, topics_for("session", "follower"), backend=backend) + + arm.enter_gravity_compensation() + arm.enter_gravity_compensation(drift_abort_rad=0.2) + arm.hold() + + assert backend.float_thresholds == [None, 0.2] + assert backend.holds == 1 + + +def test_follower_arm_updates_torq_rescale_at_runtime() -> None: + # gravity_tune switches calibration candidates without a node restart: the + # native node applies the new per-joint values within one control tick + # while the arm keeps holding. + config = ArmConfig("follower", "ARX_X5", SocketCanConnection("test")) + backend = _RecordingBackend() + arm = FollowerArm(config, topics_for("session", "follower"), backend=backend) + + arm.set_torq_rescale([0.8, 0.8, 0.8, 1.5, 1.5, 1.5]) + + assert backend.torq_rescales == [(0.8, 0.8, 0.8, 1.5, 1.5, 1.5)] diff --git a/tests/test_config.py b/tests/test_config.py index 2a358de..2f608d4 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -131,7 +131,7 @@ def test_arx_gripper_uses_training_normalized_range() -> None: assert joint["servos"][0]["pos_max"] == 5.077 -def test_yam_uses_monopi_style_follower_tracking_constants() -> None: +def test_yam_uses_reference_follower_tracking_constants() -> None: config = ArmConfig("follower", "Yam", SocketCanConnection("test")) model = json.loads(config.resolve_assets().model_config.read_text()) @@ -139,11 +139,14 @@ def test_yam_uses_monopi_style_follower_tracking_constants() -> None: assert [joint["follow_viscous_damping"] for joint in model["joints"]] == pytest.approx( [0.7777778, 0.7777778, 0.7777778, 0.0, 0.0, 0.0] ) - assert [joint["vel_max"] for joint in model["joints"]] == [2.0] * 6 + # Reference-controller planning envelope (00.00.80); follow_vel_max above + # is the operative follower speed limit. + assert [joint["vel_max"] for joint in model["joints"]] == [20.0] * 6 + assert [joint["torq_max"] for joint in model["joints"]] == [27, 27, 27, 7, 7, 7] @pytest.mark.parametrize("model_name", ["ARX_X5", "ARX_L5"]) -def test_arx_uses_monopi_style_follower_tracking_limits(model_name: str) -> None: +def test_arx_uses_reference_follower_tracking_limits(model_name: str) -> None: # ARX_X5 and ARX_L5 mirror the Yam velocity limits (X5 restored in # 00.00.36, L5 in 00.00.57): the 0.3 rad/s robot-test cap made follower # tracking far too sluggish for policy execution. @@ -151,7 +154,119 @@ def test_arx_uses_monopi_style_follower_tracking_limits(model_name: str) -> None model = json.loads(config.resolve_assets().model_config.read_text()) assert [joint["follow_vel_max"] for joint in model["joints"]] == [2.5, 2.6, 2.8, 6.0, 6.0, 6.0] - assert [joint["vel_max"] for joint in model["joints"]] == [2.0] * 6 + # Reference-controller planning envelope and base-joint torque limits + # (00.00.80): X5 base joints are ENCOS A4310 (datasheet peak 36 Nm), L5 + # base joints are DM J4340 (27 Nm inside the +-28 Nm MIT codec range); + # wrists stay +-7 Nm. + assert [joint["vel_max"] for joint in model["joints"]] == [20.0] * 6 + base_torque = 36 if model_name == "ARX_X5" else 27 + assert [joint["torq_max"] for joint in model["joints"]] == [base_torque] * 3 + [7] * 3 + + +@pytest.mark.parametrize("model_name", ["ARX_X5", "ARX_L5"]) +def test_arx_follower_position_limits_match_reference_urdf(model_name: str) -> None: + # Reference URDF joint bounds (00.00.83/85): the operational envelope + # derived from successful-episode data; commands outside are clipped + # natively. + config = ArmConfig("follower", model_name, SocketCanConnection("test")) + model = json.loads(config.resolve_assets().model_config.read_text()) + + expected = [(-2.1, 3.1), (0.0, 3.63), (0.0, 3.2), (-1.45, 1.35), (-1.58, 1.58), (-2.05, 2.05)] + actual = [ + (joint["servos"][0]["pos_min"], joint["servos"][0]["pos_max"]) + for joint in model["joints"] + ] + assert actual == expected + + +@pytest.mark.parametrize("model_name", ["ARX_X5", "ARX_L5"]) +def test_arx_torque_rescale_uses_float_test_calibration(model_name: str) -> None: + # torq_rescale is a per-unit gravity-delivery calibration measured with the + # kp=0 float test / run/gravity_tune.sh on our arms: X5 base 0.803 (ENCOS + # reporting the factory +-30 codec, effective physical full scale + # ~37.5 Nm) and wrist 1.51 (DM4310, effective ~6.7 Nm). The calculated + # conservative values (0.714 / 1.316) ship as the active devices.toml + # override; these are the tuned fallbacks used when that line is + # commented out. + config = ArmConfig("follower", model_name, SocketCanConnection("test")) + model = json.loads(config.resolve_assets().model_config.read_text()) + + expected = [0.803] * 3 + [1.51] * 3 if model_name == "ARX_X5" else [1.4] * 6 + assert [joint["torq_rescale"] for joint in model["joints"]] == expected + + +@pytest.mark.parametrize("model_name", ["ARX_X5", "ARX_L5"]) +def test_arx_default_gains_are_the_gravity_assisted_baseline(model_name: str) -> None: + # 00.00.94: the model config ships the gravity-assisted baseline gains as + # the default (position gains correct tracking error; gravity feed-forward + # holds the arm). The stiff profile lives in _high_gain_01.json. + config = ArmConfig("follower", model_name, SocketCanConnection("test")) + model = json.loads(config.resolve_assets().model_config.read_text()) + + gains = [ + (joint["servos"][0]["pos_kp"], joint["servos"][0]["pos_kd"]) + for joint in model["joints"] + ] + if model_name == "ARX_X5": + assert gains == [(40, 1.2), (40, 1.2), (32, 1.0), (10, 0.8), (10, 0.8), (10, 1)] + else: + assert gains == [(150, 5), (150, 5), (150, 5), (30, 0.8), (25, 0.8), (10, 1)] + + +@pytest.mark.parametrize("model_name", ["ARX_X5", "ARX_L5"]) +def test_arx_bundles_high_gain_calibration_variant(model_name: str) -> None: + # 00.00.94: the stiff vendor-style gains are a selectable instance-config + # variant (one-line devices.toml toggle, no wheel rebuild). + config = ArmConfig("follower", model_name, SocketCanConnection("test")) + arm_dir = config.resolve_assets().instance_config.parent + variant = json.loads((arm_dir / f"{model_name}_high_gain_01.json").read_text()) + + gains = [ + (joint["servos"][0]["pos_kp"], joint["servos"][0]["pos_kd"]) + for joint in variant["joints"] + ] + if model_name == "ARX_X5": + assert gains == [(150, 12), (150, 12), (150, 12), (30, 0.8), (25, 0.8), (10, 1)] + else: + assert gains == [(150, 12), (150, 12), (150, 12), (30, 0.8), (30, 0.8), (30, 0.8)] + + +def test_arm_config_torq_rescale_normalizes_to_floats() -> None: + config = ArmConfig( + "follower", + "ARX_X5", + SocketCanConnection("test"), + torq_rescale=[0.8, 0.8, 0.8, 1.5, 1.5, 1.5], + ) + assert config.torq_rescale == (0.8, 0.8, 0.8, 1.5, 1.5, 1.5) + + +@pytest.mark.parametrize("bad", [[], [-0.1], [float("nan")], [float("inf")]]) +def test_arm_config_rejects_non_physical_torq_rescale(bad: list[float]) -> None: + with pytest.raises(ConfigurationError, match="torq_rescale"): + ArmConfig("follower", "ARX_X5", SocketCanConnection("test"), torq_rescale=bad) + + +def test_arx_gripper_uses_reference_torque_spring() -> None: + # Reference gripper torque-spring values: X5 stiffness 6.67 Nm/rad with + # 0.2 rad spring offset, saturated at +-1.11 Nm; the bundled + # E_ARX_l5_01.json variant carries the softer L5 spring (4.44 / 0.1). + config = ArmConfig("follower", "ARX_X5", SocketCanConnection("test"), effector_model="E_ARX") + assets = config.resolve_assets() + assert assets.effector_model_config is not None + assert assets.effector_instance_config is not None + model = json.loads(assets.effector_model_config.read_text()) + instance = json.loads(assets.effector_instance_config.read_text()) + + assert model["joints"][0]["grip_torque_limit"] == 1.11 + assert instance["control_mode"] == "torque" + assert instance["dist_to_torque_const"] == 6.67 + assert instance["grip_spring_offset"] == 0.2 + + l5_variant = assets.effector_instance_config.parent / "E_ARX_l5_01.json" + l5 = json.loads(l5_variant.read_text()) + assert l5["dist_to_torque_const"] == 4.44 + assert l5["grip_spring_offset"] == 0.1 def test_arx_model_configs_define_servo_position_limits() -> None: diff --git a/tests/test_native_backend.py b/tests/test_native_backend.py index 2249729..d878bc3 100644 --- a/tests/test_native_backend.py +++ b/tests/test_native_backend.py @@ -1109,6 +1109,47 @@ def test_consume_status_mode_update_replaces_state_mode(): backend.close() +def test_consume_status_servo_param_populates_reports(): + # One DEVICE_INFO_SERVO_PARAM message per joint: effective codec ranges, + # the motor-reported firmware ranges (gravity_tune compares them across arms + # before assuming one torq_rescale calibration fits both), the applied + # torq_rescale, the position gains (milli-units in the int slots — the wire + # format caps a message at 10 floats) and the gravity feed-forward flag. + backend = make_consuming_backend() + # Exactly representable in the wire float32 so the assertions compare equal. + floats = (-21.0, 21.0, -30.0, 30.0, -20.5, 20.5, -42.0, 42.0, 0.75) + backend._consume_status( + STATUS_STRUCT.pack( + int(NativeStatus.SERVO_PARAM), len(floats), 6, + *(floats + (0.0,) * (10 - len(floats))), + *((1, 1, 1, 1, 40000, 1500) + (0,) * 4), + ) + ) + # A joint without a range query (DM wrist): valid flags 0 -> reported None. + backend._consume_status( + STATUS_STRUCT.pack( + int(NativeStatus.SERVO_PARAM), len(floats), 6, + *(floats + (0.0,) * (10 - len(floats))), + *((3, 0, 0, 0, 25500, 800) + (0,) * 4), + ) + ) + reports = backend.servo_reports() + assert reports[1].codec_vel_range == (-21.0, 21.0) + assert reports[1].codec_tor_range == (-30.0, 30.0) + assert reports[1].reported_spd_range == (-20.5, 20.5) + assert reports[1].reported_tor_range == (-42.0, 42.0) + assert reports[1].torq_rescale == 0.75 + assert reports[1].pos_kp == 40.0 + assert reports[1].pos_kd == 1.5 + assert reports[1].gravity_feed_forward is True + assert reports[3].reported_spd_range is None + assert reports[3].reported_tor_range is None + assert reports[3].pos_kp == 25.5 + assert reports[3].pos_kd == 0.8 + assert reports[3].gravity_feed_forward is False + backend.close() + + def test_state_packet_cannot_overwrite_a_concurrent_mode_transition(): class CoordinatedCondition: def __init__(self) -> None: diff --git a/uv.lock b/uv.lock index 6f319f7..78d68bd 100644 --- a/uv.lock +++ b/uv.lock @@ -333,7 +333,7 @@ wheels = [ [[package]] name = "openpi-control" -version = "0.1.1" +version = "0.1.3" source = { editable = "." } dependencies = [ { name = "numpy", version = "2.4.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.12'" },