From fe15d6feb6ba5b8887342a95cc7245cfc67f42b2 Mon Sep 17 00:00:00 2001 From: justagist Date: Mon, 2 Nov 2026 09:00:20 +0000 Subject: [PATCH 1/5] Document the firmware-dependent unit of max joint speed --- .../include/piper_cpp/interface/piper_interface_v2.h | 10 +++++++--- piper_cpp/include/piper_cpp/types/common.h | 2 +- piper_cpp/src/interface/piper_interface_v2.cpp | 11 +++++------ 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h index 53c86e1..310061b 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h @@ -576,13 +576,17 @@ class PiperInterfaceV2 /// @param motor Joint motor (1-6). /// @param max_angle_01deg Max angle limit in 0.1 degrees. /// @param min_angle_01deg Min angle limit in 0.1 degrees. - /// @param max_spd_001rad Max joint speed in 0.001 rad/s. Range 0-3000. + /// @param max_spd Max joint speed. Per the Python SDK docs, the unit depends on + /// the firmware: 0.001 rad/s before V1.5-8 and 0.01 rad/s from + /// V1.5-8, so 0-3 rad/s is 0-3000 or 0-300. bool setMotorAngleLimitsMaxSpeed( - MotorIndex motor, int16_t max_angle_01deg, int16_t min_angle_01deg, int16_t max_spd_001rad + MotorIndex motor, int16_t max_angle_01deg, int16_t min_angle_01deg, int16_t max_spd ); /// Convenience: set just the max joint speed for a single motor. - bool setMotorMaxSpeed(MotorIndex motor, int16_t max_spd_001rad); + /// @param motor Joint motor (1-6). + /// @param max_spd Max joint speed, in the firmware-dependent unit described above. + bool setMotorMaxSpeed(MotorIndex motor, int16_t max_spd); // ---- End-effector velocity/acceleration (CAN ID 0x479) ---------------------------------- diff --git a/piper_cpp/include/piper_cpp/types/common.h b/piper_cpp/include/piper_cpp/types/common.h index 7e336c3..6ce6583 100644 --- a/piper_cpp/include/piper_cpp/types/common.h +++ b/piper_cpp/include/piper_cpp/types/common.h @@ -108,7 +108,7 @@ struct ArmMsgCurrentMotorAngleLimitMaxSpd uint8_t motor_num{0}; ///< Motor index 1-6. int16_t max_angle_limit{0}; ///< Max angle limit, 0.1 deg. 0x7FFF = leave unchanged (firmware V1.5-2+). int16_t min_angle_limit{0}; ///< Min angle limit, 0.1 deg. 0x7FFF = leave unchanged. - int16_t max_joint_spd{0}; ///< Max joint speed, 0.001 rad/s. Range 0-3000. 0x7FFF = leave unchanged. + int16_t max_joint_spd{0}; ///< Max joint speed; firmware-dependent unit, see `setMotorAngleLimitsMaxSpeed()`. std::string toString() const { diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index cf99c08..a1cf200 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/piper_cpp/src/interface/piper_interface_v2.cpp @@ -812,22 +812,21 @@ bool PiperInterfaceV2::clearJointError(MotorIndex motor) } bool PiperInterfaceV2::setMotorAngleLimitsMaxSpeed( - MotorIndex motor, int16_t max_angle_01deg, int16_t min_angle_01deg, int16_t max_spd_001rad + MotorIndex motor, int16_t max_angle_01deg, int16_t min_angle_01deg, int16_t max_spd ) { PiperMessage msg; msg.type = ArmMsgType::MotorAngleLimitMaxSpdSet; - msg.arm_motor_angle_limit_max_spd_set = ArmMsgCurrentMotorAngleLimitMaxSpd{ - static_cast(motor), max_angle_01deg, min_angle_01deg, max_spd_001rad - }; + msg.arm_motor_angle_limit_max_spd_set = + ArmMsgCurrentMotorAngleLimitMaxSpd{static_cast(motor), max_angle_01deg, min_angle_01deg, max_spd}; return sendPiperMsg(msg); } -bool PiperInterfaceV2::setMotorMaxSpeed(MotorIndex motor, int16_t max_spd_001rad) +bool PiperInterfaceV2::setMotorMaxSpeed(MotorIndex motor, int16_t max_spd) { // 0x7FFF on the angle limit fields tells firmware V1.5-2+ to leave them untouched. constexpr int16_t LEAVE = static_cast(0x7FFF); - return setMotorAngleLimitsMaxSpeed(motor, LEAVE, LEAVE, max_spd_001rad); + return setMotorAngleLimitsMaxSpeed(motor, LEAVE, LEAVE, max_spd); } bool PiperInterfaceV2::setEndVelAccLimits( From ddb1c8ba83bdd773aad56269ffbab04e1273b84c Mon Sep 17 00:00:00 2001 From: justagist Date: Mon, 2 Nov 2026 09:00:20 +0000 Subject: [PATCH 2/5] Add opt-in Piper protocol V3 support --- piper_cpp/CMakeLists.txt | 2 + piper_cpp/README.md | 109 +- .../piper_cpp/interface/piper_interface_v2.h | 68 +- .../piper_cpp/interface/piper_interface_v3.h | 63 ++ piper_cpp/include/piper_cpp/piper_params.h | 30 + .../piper_cpp/protocol/piper_parser_base.h | 4 + .../piper_cpp/protocol/piper_parser_v2.h | 10 + .../piper_cpp/protocol/piper_parser_v3.h | 28 + piper_cpp/include/piper_cpp/types/common.h | 9 + piper_cpp/include/piper_cpp/types/control.h | 37 +- piper_cpp/include/piper_cpp/types/core.h | 10 +- .../piper_cpp/types/feedback/arm_status.h | 5 +- .../piper_cpp/types/feedback/gripper.h | 11 +- .../include/piper_cpp/types/piper_message.h | 6 + .../src/interface/piper_interface_v2.cpp | 267 +++-- .../src/interface/piper_interface_v3.cpp | 34 + piper_cpp/src/protocol/piper_codec.h | 144 +++ piper_cpp/src/protocol/piper_parser_v2.cpp | 132 +-- piper_cpp/src/protocol/piper_parser_v3.cpp | 179 ++++ piper_cpp/src/types/core.cpp | 18 + piper_cpp/test/CMakeLists.txt | 3 + piper_cpp/test/piper_interface_v2_test.cpp | 125 ++- piper_cpp/test/piper_interface_v3_test.cpp | 210 ++++ piper_cpp/test/piper_parser_v2_test.cpp | 80 +- piper_cpp/test/piper_parser_v3_test.cpp | 269 +++++ piper_cpp/test/piper_sdk_reference_test.cpp | 255 +++++ .../sdk_reference/generate_sdk_reference.py | 323 ++++++ .../test/sdk_reference/sdk_reference_frames.h | 953 ++++++++++++++++++ piper_cpp/test/test_support.h | 99 ++ 29 files changed, 3137 insertions(+), 346 deletions(-) create mode 100644 piper_cpp/include/piper_cpp/interface/piper_interface_v3.h create mode 100644 piper_cpp/include/piper_cpp/protocol/piper_parser_v3.h create mode 100644 piper_cpp/src/interface/piper_interface_v3.cpp create mode 100644 piper_cpp/src/protocol/piper_codec.h create mode 100644 piper_cpp/src/protocol/piper_parser_v3.cpp create mode 100644 piper_cpp/test/piper_interface_v3_test.cpp create mode 100644 piper_cpp/test/piper_parser_v3_test.cpp create mode 100644 piper_cpp/test/piper_sdk_reference_test.cpp create mode 100644 piper_cpp/test/sdk_reference/generate_sdk_reference.py create mode 100644 piper_cpp/test/sdk_reference/sdk_reference_frames.h create mode 100644 piper_cpp/test/test_support.h diff --git a/piper_cpp/CMakeLists.txt b/piper_cpp/CMakeLists.txt index 636f763..465859d 100644 --- a/piper_cpp/CMakeLists.txt +++ b/piper_cpp/CMakeLists.txt @@ -14,7 +14,9 @@ add_library(piper_cpp STATIC src/can_utils.cpp src/types/core.cpp src/protocol/piper_parser_v2.cpp + src/protocol/piper_parser_v3.cpp src/interface/piper_interface_v2.cpp + src/interface/piper_interface_v3.cpp ) # Built as a static lib but needs to be linkable into shared libs (e.g. ROS plugins). set_target_properties(piper_cpp PROPERTIES POSITION_INDEPENDENT_CODE ON) diff --git a/piper_cpp/README.md b/piper_cpp/README.md index f917830..41823b6 100644 --- a/piper_cpp/README.md +++ b/piper_cpp/README.md @@ -15,6 +15,8 @@ bytes. - Full message coverage of the v2 Piper protocol: arm status, joint feedback (high- and low-speed), end pose, gripper feedback, motor angle/speed/acc limits, end vel/acc, crash protection, gripper teaching pendant, instruction-response config, firmware version. +- Opt-in protocol V3 for firmware V1.8-8 and newer (`PiperInterfaceV3`); V2 stays the + default. See [Protocol V3](#protocol-v3). - Control APIs: enable/disable, motion-mode (granular per-field setters with TX-shadow merging), joint control, end-pose control, gripper control, MIT mode (torque + position mix control with Kp/Kd gains, in the style of the MIT mini-cheetah motor driver), @@ -157,6 +159,106 @@ A few subtleties learned the hard way (and worth knowing before commanding the a power and clears all internal/error flags -- after that, `enableRobot()` and motion-mode setup brings it back. It's a softer alternative to a physical power cycle. +## Units and ranges + +Every value is an integer in the unit shown unless it says otherwise. + +| Quantity | Unit | Range | +| --- | --- | --- | +| Joint angles (targets, feedback, command echo, V3 IK) | 0.001 deg | int32 | +| End pose position X/Y/Z | 0.001 mm | int32 | +| End pose orientation RX/RY/RZ (Euler) | 0.001 deg | int32 | +| Speed percentage | % | 0-100, higher values clamped to 100 | +| Offline-trajectory residence time | s | 0-254; 255 terminates the trajectory | +| MIT position | rad (double) | [-12.5, 12.5], 16 bits | +| MIT velocity | -- (Python SDK gives none) | [-45, 45], 12 bits | +| MIT Kp / Kd | -- | [0, 500] / [-5, 5], 12 bits each | +| MIT torque | -- (Python SDK gives none) | V2 [-8, 8], 8 bits; V3 [-16, 16], 12 bits | +| Gripper width | 0.001 mm | Optional `sdk_gripper_limit` clamp, 0-70 mm | +| Gripper angle (V3) | 0.001 deg | Optional `sdk_gripper_limit` clamp, -180 to 180 deg | +| Gripper effort (command and feedback) | 0.001 N*m | 0-5000; V3 refuses more, V2 sends it unchanged | +| Gripper teaching-pendant travel limit | mm | Documented values 0, 70, 100 | +| Motor angle limits | 0.1 deg | int16; `0x7FFF` leaves the limit unchanged | +| Max joint speed | 0.001 rad/s before firmware V1.5-8; 0.01 rad/s from V1.5-8 (per the Python SDK docs) | 0-3 rad/s; `0x7FFF` leaves it unchanged | +| Max joint acceleration | 0.01 rad/s^2 | 0-500 (0-5 rad/s^2); the Python SDK documents the `0x47C` reply in 0.001 rad/s^2 | +| End velocity / acceleration limits | 0.001 m/s, 0.001 rad/s, 0.001 m/s^2, 0.001 rad/s^2 | int16 | +| Crash protection level | -- | 0-8; 0 disables detection | +| Motor speed (high-speed feedback) | 0.001 rad/s | int16 | +| Motor current (high-speed feedback) | 0.001 A | int16, signed | +| Joint effort (derived from current) | 0.001 N*m | float | +| Driver voltage (low-speed feedback) | 0.1 V | uint16 | +| Driver and motor temperature | deg C | int16 / int8 | +| Driver bus current | 0.001 A | uint16 | +| State timestamps | s since the Unix epoch | double | + +MIT values outside their range are clamped to it, and NaN or infinite values are rejected +(nothing is sent) under both protocols. V3 converts MIT values to codes by truncation, the +same arithmetic as the Python SDK; V2 rounds to the nearest code, so V2 MIT frames can differ +from the Python SDK's by one code. + +## Protocol V3 + +Firmware V1.8-8 introduced protocol V3. The protocol is chosen by the class you construct, +never detected from the arm: `PiperInterfaceV2` speaks V2 (the default, unchanged) and +`PiperInterfaceV3` speaks V3. Using the wrong one for the firmware misreads MOVE M and MIT +frames, so check the firmware first (`piper_read_firmware`). + +> [!WARNING] +> V3 has **not** been tested on a physical arm. It is tested byte for byte against the +> official Python SDK ([`piper_sdk`](https://github.com/agilexrobotics/piper_sdk) at commit +> `b847e5393a7ce6ff8508d713eca7bdd3a42421b4`) and against frames worked out by hand from its +> protocol tables. Those tests show the frames match the Python SDK's; they say nothing about +> how the arm moves, its timing, or its safety. + +```cpp +#include "piper_cpp/interface/piper_interface_v3.h" + +using namespace piper_cpp; + +PiperInterfaceV3 piper("can0"); +piper.connectPort(/*can_init=*/true, /*piper_init=*/true, /*start_threads=*/true); +piper.enableRobot(); + +// Every V2 call is available and means the same thing. MOVE M is still MoveMode::MoveM; +// V3 just sends it as 0x06 instead of 0x04. +piper.sendMotionCtrl2(CtrlMode::CanCmd, MoveMode::MoveM, 50, MitMode::MIT); +piper.controlJointMit(1, /*pos_rad=*/0.0, /*vel=*/0.0, /*kp=*/10.0, /*kd=*/0.8, /*torque=*/0.0); + +// V3-only: gripper by angle or by width, and IK joint feedback. +piper.controlGripperAngle(/*0.001 deg*/ 30000, /*0.001 N*m*/ 1000); +piper.controlGripperWidth(/*0.001 mm*/ 40000, 1000); +auto g = piper.getGripperStates(); // g.value.mode says what grippers_angle measures +auto ik = piper.getArmIKJointStates(); // after a MoveP end-pose command; 0.001 deg +``` + +In the ROS hardware interface, set the `protocol_version` hardware parameter to `3`. + +### What changes in V3 + +| Message | V2 | V3 | API in this library | +| --- | --- | --- | --- | +| `0x151` move mode | MOVE M = `0x04` | MOVE M = `0x06`; `0x04` undefined | `MoveMode::MoveM` either way | +| `0x151` installation | `0x00`-`0x03` | adds `0x04`, horizontal inverted | `InstallationPos::HorizontalInverted` (V2 refuses it) | +| `0x2A1` mode feedback | MOVE M = `0x04` | MOVE M = `0x06`; others undefined | `ModeFeed::MOVE_M`; undefined values read as `ModeFeed::UNKNOWN` | +| `0x15A`-`0x15F` MIT | torque 8 bits over [-8, 8], CRC nibble in byte 7 | torque 12 bits over [-16, 16], no CRC | `controlJointMit()`; the range follows the protocol | +| `0x159` gripper command | status `0`-`3` | bit 2 of the status selects angle mode | `controlGripperWidth()` / `controlGripperAngle()` | +| `0x2A8` gripper feedback | byte 7 unused | byte 7: `0` width, `1` angle | `ArmMsgFeedbackGripper::mode` (`GripperMode::Unknown` otherwise) | +| `0x2AA`-`0x2AC` | -- | IK joint angles, two int32 per frame | `getArmIKJointStates()` | + +Everything else is the V2 frame, handled by the same code. + +Modes in this API are protocol-independent: `MoveMode`, `ModeFeed`, `InstallationPos` and +`GripperMode` name what you mean, and the parser translates to and from each protocol's +bytes. `getCommandedMotionCtrlState()` and `getArmMotionCtrlCode151()` report modes in the +same terms. + +### Where V3 differs from the Python SDK + +- The V3 `0x151` encoder rejects the values the Python SDK's V3 message class rejects. The + interface still clamps the speed percentage to 100 before encoding, as it does for V2. +- `0x151` and `0x159` command echoes are read back as protocol-independent modes; the Python SDK + reports the raw bytes. + ## CLI binaries All scripts in [scripts/](scripts/) are built as standalone CLI binaries. The most useful @@ -186,8 +288,8 @@ prerequisites. ``` piper_cpp/ ├── include/piper_cpp/ # public headers -│ ├── interface/ # PiperInterfaceV2 (main entry point) -│ ├── protocol/ # PiperParserV2 (CAN frame decode) +│ ├── interface/ # PiperInterfaceV2 / PiperInterfaceV3 (main entry points) +│ ├── protocol/ # PiperParserV2 / PiperParserV3 (CAN frame encode/decode) │ ├── types/ # message structs + control enums │ ├── feedback/ # feedback message definitions │ ├── std_can_interface.h # SocketCAN wrapper @@ -195,7 +297,8 @@ piper_cpp/ │ ├── fk.h # forward kinematics │ └── piper_params.h # arm geometry + DH params ├── src/ # implementations -└── scripts/ # CLI binaries +├── scripts/ # CLI binaries +└── test/ # unit tests; sdk_reference/ holds the Python SDK comparison frames ``` ## License diff --git a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h index 310061b..1dd4e55 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h @@ -19,6 +19,8 @@ namespace piper_cpp { +/// Piper arm interface speaking protocol V2. For protocol V3 (firmware V1.8-8+), construct +/// `PiperInterfaceV3` instead: the protocol is chosen by the class, never detected. class PiperInterfaceV2 { public: @@ -54,7 +56,10 @@ class PiperInterfaceV2 ); /// Destructor; calls `disconnectPort()` if it hasn't been called already. - ~PiperInterfaceV2(); + virtual ~PiperInterfaceV2(); + + /// Protocol version this interface encodes and decodes: 2, or 3 for `PiperInterfaceV3`. + uint8_t getProtocolVersion() const { return parser_->getProtocolVersion(); } /// Open the CAN port, optionally fire the initial state-query commands, and optionally /// start the background read thread. Idempotent -- calling on an already-connected @@ -126,6 +131,8 @@ class PiperInterfaceV2 /// Latest gripper feedback: jaw position (0.001 mm), effort (0.001 N*m), and the per-bit /// status flags (enable, homed, error, etc.) in `value.foc_status`. Updates at ~200 Hz. + /// Under V3, `value.mode` says whether the position is a width (0.001 mm) or an angle + /// (0.001 deg); under V2 it is always `Width`. StateSnapshot getGripperStates() const { return StateSnapshot(arm_gripper_msgs_); } /// Per-joint high-speed feedback: motor speed, current, position, and computed effort for @@ -325,8 +332,9 @@ class PiperInterfaceV2 // Every send to the arm overwrites all six fields, so to make per-field updates safe // (so changing the movement type doesn't accidentally reset speed back to a default), // this class keeps a private cached copy of the last commanded state. The granular - // setters below mutate one field of the cache under a lock and re-send the whole - // message; `sendMotionCtrl2` is the all-at-once escape hatch. + // setters below change one field of a copy of the cache under a lock and send the whole + // message; `sendMotionCtrl2` is the all-at-once escape hatch. The cache only takes the + // new state once it has been sent, so a refused or failed send changes nothing. /// Set the top-level control source -- where the arm should be listening for commands /// (CAN bus, Ethernet, Wi-Fi, etc.). Use `CtrlMode::CanCmd` for normal SDK use. @@ -359,6 +367,7 @@ class PiperInterfaceV2 /// Tell the arm how it is physically mounted (upright, side-mount left, side-mount right). /// This calibrates gravity compensation, so getting it right matters for hold-against-gravity. /// Typically a one-time commissioning step. Requires firmware V1.5-2 or newer. + /// `HorizontalInverted` exists only in protocol V3; under V2 the send fails. bool setInstallationPos(InstallationPos installation_pos); /// Set all six motion-mode fields at once in a single CAN frame. Use this when you want @@ -431,7 +440,8 @@ class PiperInterfaceV2 /// @return true on successful CAN send. bool setAsSlaveArm() { return setMasterSlaveConfig(LinkageConfig::MotionOutputArm); } - /// Command the gripper to a target jaw position with a maximum effort. Send `Enable` + /// Command the gripper to a target jaw opening (width mode) with a maximum effort. Under V3 + /// this is the same frame as `PiperInterfaceV3::controlGripperWidth()`. Send `Enable` /// once to bring the position-control loop online; subsequent frames with /// `status_code = Enable` update the target without changing enable state. Send /// `Disable` explicitly to stop the loop. @@ -441,7 +451,8 @@ class PiperInterfaceV2 /// (check `getGripperStates().value.foc_status.homing_status`). /// /// @param grippers_angle Target jaw stroke in micrometres (0.001 mm units). e.g. 30000 = 30 mm. - /// @param grippers_effort Maximum gripper torque in 0.001 N*m. Range 0-5000 (0-5 N*m). + /// @param grippers_effort Maximum gripper torque in 0.001 N*m. Range 0-5000 (0-5 N*m). Under V3 + /// a larger value is refused; V2 sends it unchanged. /// @param status_code Gripper enable/disable + error-clear command. Default `Enable`. /// @return true on successful CAN send. bool controlGripper( @@ -494,16 +505,17 @@ class PiperInterfaceV2 /// MIT joint control for a single joint. Sends one of the per-joint MIT command frames. /// Pair with `setMitMode(MitMode::MIT)` and `setMoveMode(MoveMode::MoveM)`. All scalar - /// inputs are floats in physical units; the encoder converts to the protocol's packed - /// fixed-point representation internally. + /// inputs are floats in physical units; the parser converts them to the protocol's packed + /// fixed-point representation. Out-of-range values are clamped to the range. /// /// @param motor_num Joint index 1-6. /// @param pos_rad Target position in radians, range [-12.5, 12.5]. /// @param vel Velocity setpoint, range [-45.0, 45.0]. /// @param kp Position-error gain, reference 10.0, range [0.0, 500.0]. /// @param kd Velocity-error gain, reference 0.8, range [-5.0, 5.0]. - /// @param torque Feed-forward torque, range [-8.0, 8.0]. - /// @return true on successful CAN send. Returns false if motor_num is out of range. + /// @param torque Feed-forward torque, range [-8.0, 8.0] under V2 and [-16.0, 16.0] under V3. + /// @return true on successful CAN send. Returns false if motor_num is out of range or any + /// input is NaN or infinite. bool controlJointMit(uint8_t motor_num, double pos_rad, double vel, double kp, double kd, double torque); // ---- Safety / mode commands (CAN ID 0x150 -- "MotionCtrl_1") ---------------------------- @@ -633,6 +645,25 @@ class PiperInterfaceV2 /// setups -- on a single arm in slave mode this command has no effect. bool requestMasterArmMoveToHome(MasterArmHomeMode mode); +protected: + /// Construct with the parser for a specific protocol version. Otherwise as the public + /// constructors above. + PiperInterfaceV2( + std::unique_ptr parser, std::string can_name, bool judge_flag, bool auto_init, bool dh_offset, + bool sdk_joint_limit, bool sdk_gripper_limit + ); + PiperInterfaceV2( + std::unique_ptr parser, std::unique_ptr can, bool dh_offset, + bool sdk_joint_limit, bool sdk_gripper_limit + ); + + /// Send a gripper command in the given mode. The parser refuses modes its protocol lacks. + bool sendGripperCtrl(GripperMode mode, int32_t value, uint16_t effort, GripperStatusCode status_code); + + // IK joint feedback (0x2AA-0x2AC). Only a V3 parser produces these frames, so under V2 + // this state never becomes valid. + TimedFreqState arm_ik_joint_states_; + private: /// High‐resolution timestamp in seconds static double getCurrentTime() @@ -644,6 +675,17 @@ class PiperInterfaceV2 /// Parse a raw CAN frame and update all the TimedFreqState<> members. void parseCANFrame(const struct can_frame &, double timestamp); + /// Store the two joints of one part of a three-frame joint message (part 0: joints 1-2, + /// 1: joints 3-4, 2: joints 5-6) into `state`, clamping them to the SDK joint limits + /// when `clamp` is set. + void updateJointPair( + TimedFreqState &state, size_t part, const ArmMsgJointValues &src, double timestamp, + bool clamp + ); + + /// Clamp a gripper value to the SDK limit for its mode, if `sdk_gripper_limit` is enabled. + int32_t clampGripperValue(int32_t value, GripperMode mode) const; + bool searchAllMotorLimits(); bool sendPiperMsg(const PiperMessage &msg); @@ -684,8 +726,7 @@ class PiperInterfaceV2 std::vector firmware_data_; mutable std::mutex firmware_data_mutex_; - // TX shadow of the last commanded 0x151 state. Granular setters mutate one field under - // this mutex and re-send the whole message. Initialised to the struct's defaults + // TX shadow of the last 0x151 state actually sent. Initialised to the struct's defaults // (CanCmd / MoveJ / 50% / PositionVelocity / 0 / Invalid). mutable std::mutex ctrl_151_tx_mutex_; ArmMsgMotionCtrl_2 ctrl_151_tx_; @@ -693,7 +734,10 @@ class PiperInterfaceV2 // Held for the whole of a multi-frame command so two callers' frames cannot interleave // into a target the arm never received from either. std::mutex multipart_tx_mutex_; - bool sendCommandedMotionCtrlLocked(); + /// Send `next` as the 0x151 message and, only if that succeeds, make it the commanded + /// state. Otherwise a value the encoder refuses would stay in the shadow and make every + /// later setter fail with it. Caller must hold `ctrl_151_tx_mutex_`. + bool sendMotionCtrl2Locked(const ArmMsgMotionCtrl_2 &next); void resetStates(); diff --git a/piper_cpp/include/piper_cpp/interface/piper_interface_v3.h b/piper_cpp/include/piper_cpp/interface/piper_interface_v3.h new file mode 100644 index 0000000..1d615c9 --- /dev/null +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v3.h @@ -0,0 +1,63 @@ +#pragma once + +#include "piper_cpp/interface/piper_interface_v2.h" + +namespace piper_cpp +{ + +/// Piper arm interface speaking protocol V3, for firmware V1.8-8 and newer. Every +/// `PiperInterfaceV2` method is available and keeps its meaning; the protocol differences are +/// handled by the V3 parser: +/// - `MoveMode::MoveM` is sent as 0x06, and MOVE M feedback (0x06) reads back as +/// `ModeFeed::MOVE_M`. +/// - `InstallationPos::HorizontalInverted` can be sent. +/// - `controlJointMit()` accepts torque in [-16, 16] and packs it into 12 bits. +/// - The gripper can be driven by width or by angle; feedback reports which. +/// - IK joint feedback is available. +/// +/// @warning Tested against the official piper_sdk V3 encoder and decoder only, not on a +/// physical arm. +class PiperInterfaceV3 : public PiperInterfaceV2 +{ +public: + /// Same parameters as the matching `PiperInterfaceV2` constructor. + explicit PiperInterfaceV3( + std::string can_name, bool judge_flag = true, bool auto_init = true, bool dh_offset = true, + bool sdk_joint_limit = false, bool sdk_gripper_limit = false + ); + + /// Same parameters as the matching `PiperInterfaceV2` constructor. + explicit PiperInterfaceV3( + std::unique_ptr can, bool dh_offset = true, bool sdk_joint_limit = false, + bool sdk_gripper_limit = false + ); + + /// Joint angles from the arm's own inverse kinematics (joint_1..joint_6, 0.001 deg). + /// Reassembled from three CAN frames (0x2AA-0x2AC). The arm only starts publishing them + /// after it has received an end-pose command in `MoveMode::MoveP`; until then `is_valid` + /// stays false. + StateSnapshot getArmIKJointStates() const { return StateSnapshot(arm_ik_joint_states_); } + + /// Command the gripper to a jaw opening. Same frame as `controlGripper()`. + /// + /// @param width_um Target jaw opening in 0.001 mm. + /// @param effort Maximum gripper torque in 0.001 N*m, range 0-5000. + /// @param status_code Enable/disable + error-clear command. + /// @return true on successful CAN send; false if `effort` is above 5000 (nothing is sent). + bool controlGripperWidth( + int32_t width_um, uint16_t effort = 1000, GripperStatusCode status_code = GripperStatusCode::Enable + ); + + /// Command the gripper to a jaw angle. The firmware version that introduced angle mode is + /// not documented upstream. + /// + /// @param angle_millideg Target jaw angle in 0.001 deg. + /// @param effort Maximum gripper torque in 0.001 N*m, range 0-5000. + /// @param status_code Enable/disable + error-clear command. + /// @return true on successful CAN send; false if `effort` is above 5000 (nothing is sent). + bool controlGripperAngle( + int32_t angle_millideg, uint16_t effort = 1000, GripperStatusCode status_code = GripperStatusCode::Enable + ); +}; + +} // namespace piper_cpp diff --git a/piper_cpp/include/piper_cpp/piper_params.h b/piper_cpp/include/piper_cpp/piper_params.h index 4f51958..de10543 100644 --- a/piper_cpp/include/piper_cpp/piper_params.h +++ b/piper_cpp/include/piper_cpp/piper_params.h @@ -50,6 +50,7 @@ class PiperParamManager std::unique_lock lock(mutex_); joint_limits_rad_ = kDefaultsJointRad; gripper_range_m_ = kDefaultGripperRangeM; + gripper_angle_range_deg_ = kDefaultGripperAngleRangeDeg; } PiperParams getPiperParams() const @@ -100,6 +101,21 @@ class PiperParamManager gripper_range_m_ = {min_m, max_m}; } + // ----- Gripper angle range (degrees), for protocol V3's angle mode ----- + Range getGripperAngleRangeDeg() const + { + std::shared_lock lock(mutex_); + return gripper_angle_range_deg_; + } + + void setGripperAngleRangeDeg(double min_deg, double max_deg) + { + if (max_deg < min_deg) + throw std::invalid_argument("max_val must be >= min_val"); + std::unique_lock lock(mutex_); + gripper_angle_range_deg_ = {min_deg, max_deg}; + } + // ----- Helpers ----- /// Convert a joint name string ("j1".."j6", case-insensitive) to a `Joint` enum value. /// @throws std::invalid_argument if the name doesn't match. @@ -149,6 +165,18 @@ class PiperParamManager return static_cast(std::clamp(value_um, gmin_um, gmax_um)); } + /// Clamp a V3 angle-mode gripper value in milli-degrees (0.001 deg) to the configured angle + /// range. Pass `enabled = false` to bypass the clamp. + int clampGripperMilliDeg(int value_mdeg, bool enabled = true) const + { + if (!enabled) + return value_mdeg; + auto [mn_deg, mx_deg] = getGripperAngleRangeDeg(); + const long gmin_mdeg = std::lround(mn_deg * 1000.0); + const long gmax_mdeg = std::lround(mx_deg * 1000.0); + return static_cast(std::clamp(value_mdeg, gmin_mdeg, gmax_mdeg)); + } + /// Clamp gripper jaw opening in metres (native units) to the configured range. Pass /// `enabled = false` to bypass the clamp. double clampGripperMeters(double value_m, bool enabled = true) const @@ -173,11 +201,13 @@ class PiperParamManager } }; static constexpr Range kDefaultGripperRangeM{0.0, 0.07}; // 0–70mm + static constexpr Range kDefaultGripperAngleRangeDeg{-180.0, 180.0}; // piper_sdk's default // Storage mutable std::shared_mutex mutex_; JointLimits joint_limits_rad_{}; Range gripper_range_m_{}; + Range gripper_angle_range_deg_{}; const std::string sdk_version_ = PIPER_CPP_CURRENT_VERSION; }; diff --git a/piper_cpp/include/piper_cpp/protocol/piper_parser_base.h b/piper_cpp/include/piper_cpp/protocol/piper_parser_base.h index 621d252..ff4617b 100644 --- a/piper_cpp/include/piper_cpp/protocol/piper_parser_base.h +++ b/piper_cpp/include/piper_cpp/protocol/piper_parser_base.h @@ -13,6 +13,10 @@ class PiperParserBase virtual bool decodeMessage(const struct can_frame& rx_frame, double timestamp, PiperMessage& msg) = 0; virtual bool encodeMessage(const PiperMessage& msg, struct can_frame& tx_frame) = 0; virtual uint8_t getProtocolVersion() const = 0; + + /// Pack MIT targets given in physical units into this protocol's fixed-point fields. Each + /// input is clamped to the protocol's range first; all inputs must be finite. + virtual ArmMsgJointMitCtrl packJointMit(double pos_rad, double vel, double kp, double kd, double torque) const = 0; }; } // namespace piper_cpp diff --git a/piper_cpp/include/piper_cpp/protocol/piper_parser_v2.h b/piper_cpp/include/piper_cpp/protocol/piper_parser_v2.h index 575dd6f..156affc 100644 --- a/piper_cpp/include/piper_cpp/protocol/piper_parser_v2.h +++ b/piper_cpp/include/piper_cpp/protocol/piper_parser_v2.h @@ -10,6 +10,16 @@ class PiperParserV2 : public PiperParserBase uint8_t getProtocolVersion() const override { return 2; } bool decodeMessage(const struct can_frame& rx_frame, double timestamp, PiperMessage& msg) override; bool encodeMessage(const PiperMessage& msg, struct can_frame& tx_frame) override; + + /// V2 ranges: position [-12.5, 12.5] rad, velocity [-45, 45], Kp [0, 500], Kd [-5, 5] and + /// torque [-8, 8], the torque packed into 8 bits. Rounds to the nearest code. + ArmMsgJointMitCtrl packJointMit(double pos_rad, double vel, double kp, double kd, double torque) const override; + +protected: + /// Check that `rx_frame` is a standard data frame with a known ID and enough payload for + /// that ID, then fill in the ID, type, raw bytes and timestamp. On failure sets the type + /// to `Unknown` and returns false. + bool acceptFrame(const struct can_frame& rx_frame, double timestamp, PiperMessage& msg) const; }; } // namespace piper_cpp diff --git a/piper_cpp/include/piper_cpp/protocol/piper_parser_v3.h b/piper_cpp/include/piper_cpp/protocol/piper_parser_v3.h new file mode 100644 index 0000000..4346211 --- /dev/null +++ b/piper_cpp/include/piper_cpp/protocol/piper_parser_v3.h @@ -0,0 +1,28 @@ +#pragma once +#include "piper_cpp/protocol/piper_parser_v2.h" + +namespace piper_cpp +{ + +/// Piper protocol V3, for firmware V1.8-8 and newer. Same frames as V2 except: +/// - 0x151 and 0x2A1: MOVE M is 0x06 on the wire (V2: 0x04); 0x151 also accepts the +/// horizontal-inverted installation position (0x04). +/// - 0x159 and 0x2A8: the gripper value is a width or an angle; the command carries the mode +/// in bit 2 of the status byte, the feedback in byte 7. +/// - 0x15A-0x15F: MIT torque is 12 bits over [-16, 16] and byte 7 has no CRC nibble. +/// - 0x2AA-0x2AC: IK joint feedback. +/// Every other message is handled by the V2 parser unchanged. +class PiperParserV3 : public PiperParserV2 +{ +public: + uint8_t getProtocolVersion() const override { return 3; } + bool decodeMessage(const struct can_frame& rx_frame, double timestamp, PiperMessage& msg) override; + bool encodeMessage(const PiperMessage& msg, struct can_frame& tx_frame) override; + + /// V3 ranges: position [-12.5, 12.5] rad, velocity [-45, 45], Kp [0, 500], Kd [-5, 5] and + /// torque [-16, 16], the torque packed into 12 bits. Truncates to the code below, as the + /// official SDK does. + ArmMsgJointMitCtrl packJointMit(double pos_rad, double vel, double kp, double kd, double torque) const override; +}; + +} // namespace piper_cpp diff --git a/piper_cpp/include/piper_cpp/types/common.h b/piper_cpp/include/piper_cpp/types/common.h index 6ce6583..ef2225b 100644 --- a/piper_cpp/include/piper_cpp/types/common.h +++ b/piper_cpp/include/piper_cpp/types/common.h @@ -7,6 +7,15 @@ namespace piper_cpp { +/// What the gripper value means. Protocol V3 adds an angle mode; under V2 the gripper only +/// has the width mode. +enum class GripperMode : uint8_t +{ + Width = 0x00, ///< Jaw opening, 0.001 mm. The only mode in V2. + Angle = 0x01, ///< Jaw angle, 0.001 deg. V3 only. + Unknown = 0xFF, ///< A V3 frame carried a mode this library does not know. +}; + /// Six-element joint angle vector. Used both for measured joint feedback (wire ID 0x2A4-0x2A6) /// and joint targets (0x155-0x157). Values are in 0.001-degree units. struct ArmMsgJointValues diff --git a/piper_cpp/include/piper_cpp/types/control.h b/piper_cpp/include/piper_cpp/types/control.h index d2a893e..f7cfb06 100644 --- a/piper_cpp/include/piper_cpp/types/control.h +++ b/piper_cpp/include/piper_cpp/types/control.h @@ -4,6 +4,8 @@ #include #include +#include "piper_cpp/types/common.h" + namespace piper_cpp { @@ -21,13 +23,16 @@ enum class CtrlMode : uint8_t /// Interpolation type for subsequent motion commands. Pick `MoveJ` when sending joint targets, /// `MoveL`/`MoveP` when sending end-pose targets, etc. +/// +/// These are protocol-independent modes. Their values match the V2 wire bytes; the parser +/// translates them for protocols that number a mode differently (V3 sends `MoveM` as 0x06). enum class MoveMode : uint8_t { MoveP = 0x00, ///< Point-to-point in cartesian space. MoveJ = 0x01, ///< Joint-space interpolation -- pair with joint targets. MoveL = 0x02, ///< Linear cartesian -- pair with end-pose targets. MoveC = 0x03, ///< Circular cartesian. - MoveM = 0x04, ///< MIT torque/position-mix mode. Requires firmware V1.5-2 or newer. + MoveM = 0x04, ///< MIT torque/position-mix mode. V2: wire 0x04, firmware V1.5-2+. V3: wire 0x06, firmware V1.8-8+. MoveCPV = 0x05, ///< Continuous position + velocity. Requires firmware V1.8-1 or newer. }; @@ -45,10 +50,11 @@ enum class MitMode : uint8_t /// commissioning step. Requires firmware V1.5-2 or newer. enum class InstallationPos : uint8_t { - Invalid = 0x00, ///< Sentinel; firmware will reject. Useful as a "leave the current setting" placeholder. - HorizontalUpright = 0x01, ///< Mounted upright on a horizontal surface, cabling toward the rear. - SideMountLeft = 0x02, ///< Side-mounted on the left. - SideMountRight = 0x03, ///< Side-mounted on the right. + Invalid = 0x00, ///< Sentinel; firmware will reject. Useful as a "leave the current setting" placeholder. + HorizontalUpright = 0x01, ///< Mounted upright on a horizontal surface, cabling toward the rear. + SideMountLeft = 0x02, ///< Side-mounted on the left. + SideMountRight = 0x03, ///< Side-mounted on the right. + HorizontalInverted = 0x04, ///< Inverted on a horizontal surface. V3 only (firmware V1.8-8+). }; /// In a leader/follower (a.k.a. master/slave) setup, which role this arm should play. @@ -225,21 +231,27 @@ struct ArmMsgCircularPatternCoordNumUpdateCtrl }; /// Gripper command payload (CAN ID 0x159). Sent on every cycle of the gripper control loop. +/// +/// V3 carries the mode in bit 2 of the status byte; the V3 parser combines and splits it, so +/// `status_code` stays a `GripperStatusCode` and `mode` holds the mode. V2 only has `Width`. struct ArmMsgGripperCtrl { - int32_t grippers_angle = 0; ///< Target jaw stroke, 0.001 mm. + int32_t grippers_angle = 0; ///< Target: jaw stroke in 0.001 mm (`Width`) or jaw angle in 0.001 deg (`Angle`). uint16_t grippers_effort = 0; ///< Max gripper torque, 0.001 N*m. Range 0-5000. uint8_t status_code = 0; ///< Enable/disable + error-clear command; see `GripperStatusCode`. uint8_t set_zero = 0; ///< One-shot set-zero flag; see `GripperSetZero`. + piper_cpp::GripperMode mode = piper_cpp::GripperMode::Width; ///< What `grippers_angle` means. std::string toString() const { std::ostringstream oss; oss << "ArmMsgGripperCtrl(\n" - << " grippers_angle: " << grippers_angle << " (" << grippers_angle * 0.001 << " deg),\n" + << " grippers_angle: " << grippers_angle << " (" << grippers_angle * 0.001 + << (mode == piper_cpp::GripperMode::Angle ? " deg" : " mm") << "),\n" << " grippers_effort: " << grippers_effort << " (" << grippers_effort * 0.001 << " Nm),\n" << " status_code: " << int(status_code) << ",\n" - << " set_zero: " << int(set_zero) << "\n" + << " set_zero: " << int(set_zero) << ",\n" + << " mode: " << int(mode) << "\n" << ")"; return oss.str(); } @@ -274,15 +286,15 @@ struct ArmMsgJointConfig /// Per-joint MIT control payload (CAN IDs 0x15A-0x15F). Fields are pre-packed integers in the /// protocol's fixed-point representation; use `PiperInterfaceV2::controlJointMit()` to send -/// natural floats and let the helper handle the float-to-uint mapping. +/// natural floats and let the parser handle the float-to-uint mapping. struct ArmMsgJointMitCtrl { uint16_t pos_ref = 0; ///< Position reference (16-bit packed; unsigned mapping over [-12.5, 12.5] rad). uint16_t vel_ref = 0; ///< Velocity reference (12-bit packed; mapped over [-45, 45]). uint16_t kp = 10; ///< Position-error gain (12-bit packed; mapped over [0, 500]). uint16_t kd = 0; ///< Velocity-error gain (12-bit packed; mapped over [-5, 5]). - uint16_t t_ref = 0; ///< Feed-forward torque (8-bit packed; mapped over [-8, 8]). - uint8_t crc = 0; ///< Optional CRC byte (firmware-dependent; 0 if unused). + uint16_t t_ref = 0; ///< Feed-forward torque. V2: 8-bit over [-8, 8]. V3: 12-bit over [-16, 16]. + uint8_t crc = 0; ///< Unused: the V2 encoder computes the CRC nibble itself and V3 has none. std::string toString() const { @@ -368,7 +380,8 @@ struct ArmMsgMotionCtrl_1 struct ArmMsgMotionCtrl_2 { uint8_t ctrl_mode = 0x01; ///< Top-level control source; see `CtrlMode`. Default `CanCmd`. - uint8_t move_mode = 0x01; ///< Interpolation type; see `MoveMode`. Default `MoveJ`. + uint8_t move_mode = 0x01; ///< Interpolation type as a `MoveMode`, not a wire byte. Default `MoveJ`. A V3 echo of a + ///< mode V3 does not define decodes as 0xFF, which no encoder accepts back. uint8_t move_spd_rate_ctrl = 50; ///< Speed cap percentage in [0, 100]. Default 50. uint8_t mit_mode = 0x00; ///< MIT mode toggle; see `MitMode`. Default `PositionVelocity`. uint8_t residence_time = 0; ///< Hold time at offline-trajectory waypoints (s). 0-254; 255 terminates. diff --git a/piper_cpp/include/piper_cpp/types/core.h b/piper_cpp/include/piper_cpp/types/core.h index e39e4e7..a4e16c8 100644 --- a/piper_cpp/include/piper_cpp/types/core.h +++ b/piper_cpp/include/piper_cpp/types/core.h @@ -82,7 +82,12 @@ enum class ArmMsgType : uint16_t LightCtrl, CanUpdateSilentModeConfig, - FirmwareRead + FirmwareRead, + + // ---- Protocol V3 feedback (appended so existing values keep their numbers) ---- + IKJointFeedback12, + IKJointFeedback34, + IKJointFeedback56, }; /// Wire-level CAN identifiers used by the Piper protocol. Each value is the actual numeric ID @@ -97,6 +102,9 @@ enum class CanIDPiper : uint16_t ARM_JOINT_FEEDBACK_34 = 0x2A6, ARM_JOINT_FEEDBACK_56 = 0x2A7, ARM_GRIPPER_FEEDBACK = 0x2A8, + ARM_IK_JOINT_FEEDBACK_12 = 0x2AA, ///< V3 only: joint angles from the arm's own IK, joints 1-2. + ARM_IK_JOINT_FEEDBACK_34 = 0x2AB, + ARM_IK_JOINT_FEEDBACK_56 = 0x2AC, ARM_MOTION_CTRL_1 = 0x150, ARM_MOTION_CTRL_2 = 0x151, diff --git a/piper_cpp/include/piper_cpp/types/feedback/arm_status.h b/piper_cpp/include/piper_cpp/types/feedback/arm_status.h index 5e9ba59..8c660a9 100644 --- a/piper_cpp/include/piper_cpp/types/feedback/arm_status.h +++ b/piper_cpp/include/piper_cpp/types/feedback/arm_status.h @@ -113,8 +113,9 @@ enum class ModeFeed : uint8_t MOVE_J = 0x01, MOVE_L = 0x02, MOVE_C = 0x03, - MOVE_M = 0x04, - MOVE_CPV = 0x05 + MOVE_M = 0x04, ///< Protocol-independent; V3 reports MOVE M as 0x06 on the wire and the parser maps it here. + MOVE_CPV = 0x05, + UNKNOWN = 0xFF ///< V3 only: a mode byte V3 does not define. V2 passes every byte through unchanged. }; inline std::string toString(ModeFeed m) { diff --git a/piper_cpp/include/piper_cpp/types/feedback/gripper.h b/piper_cpp/include/piper_cpp/types/feedback/gripper.h index 7017d82..3c07e9d 100644 --- a/piper_cpp/include/piper_cpp/types/feedback/gripper.h +++ b/piper_cpp/include/piper_cpp/types/feedback/gripper.h @@ -4,14 +4,17 @@ #include #include +#include "piper_cpp/types/common.h" + namespace piper_cpp { /// Gripper feedback payload (CAN ID 0x2A8). Carries the measured jaw position, applied effort, -/// and a packed status byte that's decoded into per-bit flags below. +/// and a packed status byte that's decoded into per-bit flags below. V3 adds the control mode +/// in byte 7; under V2 `mode` is always `Width`. struct ArmMsgFeedbackGripper { - int32_t grippers_angle{0}; ///< Measured jaw stroke, 0.001 mm. + int32_t grippers_angle{0}; ///< Measured jaw stroke in 0.001 mm (`Width`) or jaw angle in 0.001 deg (`Angle`). int16_t grippers_effort{0}; ///< Measured gripper torque, 0.001 N*m. uint8_t status_code{0}; ///< Raw status byte; decoded into `foc_status` by `updateFocStatus()`. @@ -57,6 +60,7 @@ struct ArmMsgFeedbackGripper }; FOCStatus foc_status; ///< Decoded view of `status_code`; populated by `updateFocStatus()`. + GripperMode mode{GripperMode::Width}; ///< What `grippers_angle` measures. /// Decode `status_code` into the per-bit flags in `foc_status`. The parser calls this /// automatically after assigning `status_code`; only invoke directly if you've manually @@ -67,7 +71,8 @@ struct ArmMsgFeedbackGripper { std::ostringstream oss; oss << "ArmMsgFeedbackGripper(\n" - << " grippers_angle: " << grippers_angle << " (" << grippers_angle * 0.001 << " mm)\n" + << " grippers_angle: " << grippers_angle << " (" << grippers_angle * 0.001 + << (mode == GripperMode::Angle ? " deg" : " mm") << ")\n" << " grippers_effort: " << grippers_effort << " (" << grippers_effort * 0.001 << " N*m)\n" << " status_code: 0x" << std::hex << int(status_code) << std::dec << "\n" << " FOCStatus:\n" diff --git a/piper_cpp/include/piper_cpp/types/piper_message.h b/piper_cpp/include/piper_cpp/types/piper_message.h index b258b3a..d5f7771 100644 --- a/piper_cpp/include/piper_cpp/types/piper_message.h +++ b/piper_cpp/include/piper_cpp/types/piper_message.h @@ -32,6 +32,7 @@ struct PiperMessage ArmMsgFeedbackStatus arm_status_msgs; ArmMsgEndPose arm_end_pose; ArmMsgJointValues arm_joint_feedback; + ArmMsgJointValues arm_ik_joint_feedback; ///< V3 only: joint angles from the arm's IK, 0.001 deg. ArmMsgFeedbackAllJointVelAcc all_joint_vel_acc; ArmMsgFeedbackGripper gripper_feedback; ArmMsgGripperTeachingPendantParam arm_gripper_teaching_param_feedback; @@ -86,6 +87,11 @@ struct PiperMessage case ArmMsgType::JointFeedback56: oss << "Joint Feed: " << arm_joint_feedback.toString() << "\n"; break; + case ArmMsgType::IKJointFeedback12: + case ArmMsgType::IKJointFeedback34: + case ArmMsgType::IKJointFeedback56: + oss << "IK Joint Feed: " << arm_ik_joint_feedback.toString() << "\n"; + break; case ArmMsgType::GripperFeedback: oss << "Gripper Feed: " << gripper_feedback.toString() << "\n"; break; diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index a1cf200..909b7e5 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/piper_cpp/src/interface/piper_interface_v2.cpp @@ -12,10 +12,27 @@ static constexpr double MILLIDEG2RAD = 0.001 * M_PI / 180; PiperInterfaceV2::PiperInterfaceV2( std::string can_name, bool judge_flag, bool auto_init, bool dh_offset, bool sdk_joint_limit, bool sdk_gripper_limit ) - : can_name_(std::move(can_name)), sdk_joint_limit_(sdk_joint_limit), sdk_gripper_limit_(sdk_gripper_limit) + : PiperInterfaceV2( + std::make_unique(), std::move(can_name), judge_flag, auto_init, dh_offset, sdk_joint_limit, + sdk_gripper_limit + ) +{ +} + +PiperInterfaceV2::PiperInterfaceV2( + std::unique_ptr can, bool dh_offset, bool sdk_joint_limit, bool sdk_gripper_limit +) + : PiperInterfaceV2(std::make_unique(), std::move(can), dh_offset, sdk_joint_limit, sdk_gripper_limit) +{ +} + +PiperInterfaceV2::PiperInterfaceV2( + std::unique_ptr parser, std::string can_name, bool judge_flag, bool auto_init, bool dh_offset, + bool sdk_joint_limit, bool sdk_gripper_limit +) + : can_name_(std::move(can_name)), parser_(std::move(parser)), sdk_joint_limit_(sdk_joint_limit), + sdk_gripper_limit_(sdk_gripper_limit) { - // Initialize parser and other members here - parser_ = std::make_unique(); fk_ = std::make_unique(dh_offset); arm_can_ = std::make_unique( can_name_, 1000000, judge_flag, auto_init, @@ -39,6 +56,7 @@ PiperInterfaceV2::PiperInterfaceV2( arm_gripper_ctrl_.setNumCallers(1); // gripper control messages arm_motion_ctrl_code_151_.setNumCallers(1); arm_instruction_response_.setNumCallers(1); + arm_ik_joint_states_.setNumCallers(3); // IK joints are updated in 3 parts // Query responses and command echoes arrive once, not periodically, so their age says // nothing about the link; only the periodic feedback streams keep the default timeout. @@ -57,9 +75,13 @@ PiperInterfaceV2::PiperInterfaceV2( } PiperInterfaceV2::PiperInterfaceV2( - std::unique_ptr can, bool dh_offset, bool sdk_joint_limit, bool sdk_gripper_limit + std::unique_ptr parser, std::unique_ptr can, bool dh_offset, bool sdk_joint_limit, + bool sdk_gripper_limit ) - : PiperInterfaceV2("", /*judge_flag=*/false, /*auto_init=*/false, dh_offset, sdk_joint_limit, sdk_gripper_limit) + : PiperInterfaceV2( + std::move(parser), "", /*judge_flag=*/false, /*auto_init=*/false, dh_offset, sdk_joint_limit, + sdk_gripper_limit + ) { arm_can_ = std::move(can); arm_can_->setCallback([this](const struct can_frame &frame, double timestamp) { parseCANFrame(frame, timestamp); }); @@ -153,6 +175,7 @@ void PiperInterfaceV2::resetStates() arm_gripper_ctrl_.reset(); arm_motion_ctrl_code_151_.reset(); arm_instruction_response_.reset(); + arm_ik_joint_states_.reset(); arm_health_.reset(); std::lock_guard lk(firmware_data_mutex_); firmware_data_.clear(); @@ -164,7 +187,6 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times PiperMessage msg; if (parser_->decodeMessage(frame, timestamp, msg)) { - auto &pm = getParameterManager(); auto health = arm_health_.getValue(); switch (msg.type) { @@ -197,67 +219,28 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times break; } case ArmMsgType::JointFeedback12: - { - auto joint_values = arm_joint_states_.getValue(); - if (sdk_joint_limit_) - { - joint_values.joint_1 = - pm.clampJointMilliDeg(PiperParamManager::Joint::J1, msg.arm_joint_feedback.joint_1); - joint_values.joint_2 = - pm.clampJointMilliDeg(PiperParamManager::Joint::J2, msg.arm_joint_feedback.joint_2); - } - else - { - joint_values.joint_1 = msg.arm_joint_feedback.joint_1; - joint_values.joint_2 = msg.arm_joint_feedback.joint_2; - } - arm_joint_states_.set(joint_values, msg.timestamp, 0); - break; - } case ArmMsgType::JointFeedback34: + case ArmMsgType::JointFeedback56: { - auto joint_values = arm_joint_states_.getValue(); - if (sdk_joint_limit_) - { - joint_values.joint_3 = - pm.clampJointMilliDeg(PiperParamManager::Joint::J3, msg.arm_joint_feedback.joint_3); - joint_values.joint_4 = - pm.clampJointMilliDeg(PiperParamManager::Joint::J4, msg.arm_joint_feedback.joint_4); - } - else - { - joint_values.joint_3 = msg.arm_joint_feedback.joint_3; - joint_values.joint_4 = msg.arm_joint_feedback.joint_4; - } - arm_joint_states_.set(joint_values, msg.timestamp, 1); + const size_t part = static_cast(msg.type) - static_cast(ArmMsgType::JointFeedback12); + updateJointPair(arm_joint_states_, part, msg.arm_joint_feedback, msg.timestamp, sdk_joint_limit_); break; } - case ArmMsgType::JointFeedback56: + case ArmMsgType::IKJointFeedback12: + case ArmMsgType::IKJointFeedback34: + case ArmMsgType::IKJointFeedback56: { - auto joint_values = arm_joint_states_.getValue(); - if (sdk_joint_limit_) - { - joint_values.joint_5 = - pm.clampJointMilliDeg(PiperParamManager::Joint::J5, msg.arm_joint_feedback.joint_5); - joint_values.joint_6 = - pm.clampJointMilliDeg(PiperParamManager::Joint::J6, msg.arm_joint_feedback.joint_6); - } - else - { - joint_values.joint_5 = msg.arm_joint_feedback.joint_5; - joint_values.joint_6 = msg.arm_joint_feedback.joint_6; - } - arm_joint_states_.set(joint_values, msg.timestamp, 2); + // IK solutions are the arm's own targets, not measurements, so the SDK limits + // that guard feedback are not applied. + const size_t part = static_cast(msg.type) - static_cast(ArmMsgType::IKJointFeedback12); + updateJointPair(arm_ik_joint_states_, part, msg.arm_ik_joint_feedback, msg.timestamp, false); break; } case ArmMsgType::GripperFeedback: { // Update gripper messages auto gripper_fb = msg.gripper_feedback; - if (sdk_gripper_limit_) - { - gripper_fb.grippers_angle = pm.clampGripperMicrometers(gripper_fb.grippers_angle); - } + gripper_fb.grippers_angle = clampGripperValue(gripper_fb.grippers_angle, gripper_fb.mode); arm_gripper_msgs_.set(gripper_fb, msg.timestamp); break; } @@ -372,63 +355,19 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times break; } case ArmMsgType::JointCtrl12: - { - auto joint_ctrl = arm_joint_ctrl_msgs_.getValue(); - if (sdk_joint_limit_) - { - joint_ctrl.joint_1 = pm.clampJointMilliDeg(PiperParamManager::Joint::J1, msg.arm_joint_ctrl.joint_1); - joint_ctrl.joint_2 = pm.clampJointMilliDeg(PiperParamManager::Joint::J2, msg.arm_joint_ctrl.joint_2); - } - else - { - joint_ctrl.joint_1 = msg.arm_joint_ctrl.joint_1; - joint_ctrl.joint_2 = msg.arm_joint_ctrl.joint_2; - } - arm_joint_ctrl_msgs_.set(joint_ctrl, msg.timestamp, 0); - break; - } case ArmMsgType::JointCtrl34: - { - auto joint_ctrl = arm_joint_ctrl_msgs_.getValue(); - if (sdk_joint_limit_) - { - joint_ctrl.joint_3 = pm.clampJointMilliDeg(PiperParamManager::Joint::J3, msg.arm_joint_ctrl.joint_3); - joint_ctrl.joint_4 = pm.clampJointMilliDeg(PiperParamManager::Joint::J4, msg.arm_joint_ctrl.joint_4); - } - else - { - joint_ctrl.joint_3 = msg.arm_joint_ctrl.joint_3; - joint_ctrl.joint_4 = msg.arm_joint_ctrl.joint_4; - } - arm_joint_ctrl_msgs_.set(joint_ctrl, msg.timestamp, 1); - break; - } case ArmMsgType::JointCtrl56: { - auto joint_ctrl = arm_joint_ctrl_msgs_.getValue(); - if (sdk_joint_limit_) - { - joint_ctrl.joint_5 = pm.clampJointMilliDeg(PiperParamManager::Joint::J5, msg.arm_joint_ctrl.joint_5); - joint_ctrl.joint_6 = pm.clampJointMilliDeg(PiperParamManager::Joint::J6, msg.arm_joint_ctrl.joint_6); - } - else - { - joint_ctrl.joint_5 = msg.arm_joint_ctrl.joint_5; - joint_ctrl.joint_6 = msg.arm_joint_ctrl.joint_6; - } - arm_joint_ctrl_msgs_.set(joint_ctrl, msg.timestamp, 2); + const size_t part = static_cast(msg.type) - static_cast(ArmMsgType::JointCtrl12); + updateJointPair(arm_joint_ctrl_msgs_, part, msg.arm_joint_ctrl, msg.timestamp, sdk_joint_limit_); break; } case ArmMsgType::GripperCtrl: { auto gripper_ctrl = msg.gripper_ctrl; - if (sdk_gripper_limit_) - { - /// WARNING: TODO: potential bug here, python docs say this ctrl feedback is in millideg, but is clamped - /// using the same micrometer clamping done for the gripper state feedback (which is documented to be in - /// micrometer) - gripper_ctrl.grippers_angle = pm.clampGripperMicrometers(gripper_ctrl.grippers_angle); - } + // A width frame carries 0.001 mm and a V3 angle frame 0.001 deg, so clamp with the + // limit for the frame's own mode. + gripper_ctrl.grippers_angle = clampGripperValue(gripper_ctrl.grippers_angle, gripper_ctrl.mode); arm_gripper_ctrl_.set(gripper_ctrl, msg.timestamp); break; } @@ -462,6 +401,39 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times } } +void PiperInterfaceV2::updateJointPair( + TimedFreqState &state, size_t part, const ArmMsgJointValues &src, double timestamp, bool clamp +) +{ + static constexpr int32_t ArmMsgJointValues::*kJoints[] = { + &ArmMsgJointValues::joint_1, &ArmMsgJointValues::joint_2, &ArmMsgJointValues::joint_3, + &ArmMsgJointValues::joint_4, &ArmMsgJointValues::joint_5, &ArmMsgJointValues::joint_6, + }; + auto &pm = getParameterManager(); + auto values = state.getValue(); + for (size_t j = 2 * part; j < 2 * part + 2; ++j) + { + const int32_t v = src.*kJoints[j]; + values.*kJoints[j] = clamp ? pm.clampJointMilliDeg(static_cast(j), v) : v; + } + state.set(values, timestamp, part); +} + +int32_t PiperInterfaceV2::clampGripperValue(int32_t value, GripperMode mode) const +{ + if (!sdk_gripper_limit_) + return value; + switch (mode) + { + case GripperMode::Width: + return getParameterManager().clampGripperMicrometers(value); + case GripperMode::Angle: + return getParameterManager().clampGripperMilliDeg(value); + default: + return value; // no limit is known for a mode this library cannot interpret + } +} + std::vector> PiperInterfaceV2::getCalculatedFeedbackFK() const { auto joint_fb = getArmJointStates(); @@ -536,13 +508,15 @@ bool PiperInterfaceV2::disableRobot() return sendPiperMsg(msg); } -bool PiperInterfaceV2::sendCommandedMotionCtrlLocked() +bool PiperInterfaceV2::sendMotionCtrl2Locked(const ArmMsgMotionCtrl_2 &next) { - // Caller must hold ctrl_151_tx_mutex_. PiperMessage msg; msg.type = ArmMsgType::MotionCtrl2; - msg.arm_motion_ctrl_2 = ctrl_151_tx_; - return sendPiperMsg(msg); + msg.arm_motion_ctrl_2 = next; + if (!sendPiperMsg(msg)) + return false; + ctrl_151_tx_ = next; + return true; } bool PiperInterfaceV2::sendMotionCtrl2( @@ -553,29 +527,30 @@ bool PiperInterfaceV2::sendMotionCtrl2( if (move_spd_rate_ctrl > 100) move_spd_rate_ctrl = 100; std::lock_guard lock(ctrl_151_tx_mutex_); - ctrl_151_tx_ = ArmMsgMotionCtrl_2{ + return sendMotionCtrl2Locked(ArmMsgMotionCtrl_2{ static_cast(ctrl_mode), static_cast(move_mode), move_spd_rate_ctrl, static_cast(mit_mode), residence_time, static_cast(installation_pos), - }; - return sendCommandedMotionCtrlLocked(); + }); } bool PiperInterfaceV2::setCtrlMode(CtrlMode ctrl_mode) { std::lock_guard lock(ctrl_151_tx_mutex_); - ctrl_151_tx_.ctrl_mode = static_cast(ctrl_mode); - return sendCommandedMotionCtrlLocked(); + auto next = ctrl_151_tx_; + next.ctrl_mode = static_cast(ctrl_mode); + return sendMotionCtrl2Locked(next); } bool PiperInterfaceV2::setMoveMode(MoveMode move_mode) { std::lock_guard lock(ctrl_151_tx_mutex_); - ctrl_151_tx_.move_mode = static_cast(move_mode); - return sendCommandedMotionCtrlLocked(); + auto next = ctrl_151_tx_; + next.move_mode = static_cast(move_mode); + return sendMotionCtrl2Locked(next); } bool PiperInterfaceV2::setSpeedPercentage(uint8_t percentage) @@ -583,29 +558,33 @@ bool PiperInterfaceV2::setSpeedPercentage(uint8_t percentage) if (percentage > 100) percentage = 100; std::lock_guard lock(ctrl_151_tx_mutex_); - ctrl_151_tx_.move_spd_rate_ctrl = percentage; - return sendCommandedMotionCtrlLocked(); + auto next = ctrl_151_tx_; + next.move_spd_rate_ctrl = percentage; + return sendMotionCtrl2Locked(next); } bool PiperInterfaceV2::setMitMode(MitMode mit_mode) { std::lock_guard lock(ctrl_151_tx_mutex_); - ctrl_151_tx_.mit_mode = static_cast(mit_mode); - return sendCommandedMotionCtrlLocked(); + auto next = ctrl_151_tx_; + next.mit_mode = static_cast(mit_mode); + return sendMotionCtrl2Locked(next); } bool PiperInterfaceV2::setResidenceTime(uint8_t seconds) { std::lock_guard lock(ctrl_151_tx_mutex_); - ctrl_151_tx_.residence_time = seconds; - return sendCommandedMotionCtrlLocked(); + auto next = ctrl_151_tx_; + next.residence_time = seconds; + return sendMotionCtrl2Locked(next); } bool PiperInterfaceV2::setInstallationPos(InstallationPos installation_pos) { std::lock_guard lock(ctrl_151_tx_mutex_); - ctrl_151_tx_.installation_pos = static_cast(installation_pos); - return sendCommandedMotionCtrlLocked(); + auto next = ctrl_151_tx_; + next.installation_pos = static_cast(installation_pos); + return sendMotionCtrl2Locked(next); } ArmMsgMotionCtrl_2 PiperInterfaceV2::getCommandedMotionCtrlState() const @@ -630,14 +609,16 @@ bool PiperInterfaceV2::setMasterSlaveConfig( } bool PiperInterfaceV2::controlGripper(int32_t grippers_angle, uint16_t grippers_effort, GripperStatusCode status_code) +{ + return sendGripperCtrl(GripperMode::Width, grippers_angle, grippers_effort, status_code); +} + +bool PiperInterfaceV2::sendGripperCtrl(GripperMode mode, int32_t value, uint16_t effort, GripperStatusCode status_code) { PiperMessage msg; msg.type = ArmMsgType::GripperCtrl; msg.gripper_ctrl = ArmMsgGripperCtrl{ - grippers_angle, - grippers_effort, - static_cast(status_code), - static_cast(GripperSetZero::NoOp), + value, effort, static_cast(status_code), static_cast(GripperSetZero::NoOp), mode, }; return sendPiperMsg(msg); } @@ -697,20 +678,6 @@ bool PiperInterfaceV2::controlEndPose(int32_t x_um, int32_t y_um, int32_t z_um, return sendPiperMsg(msg); } -// Float -> N-bit unsigned mapping used by the MIT joint commands. Mirrors piper_sdk's -// FloatToUint(value, min, max, bits): clamps to [min,max], then linearly maps onto -// [0, 2^bits - 1]. -static uint32_t floatToUintRange(double value, double min, double max, int bits) -{ - if (value < min) - value = min; - if (value > max) - value = max; - const double span = max - min; - const uint32_t max_int = (uint32_t{1} << bits) - 1u; - return static_cast(std::lround((value - min) / span * max_int)); -} - bool PiperInterfaceV2::controlJointMit( uint8_t motor_num, double pos_rad, double vel, double kp, double kd, double torque ) @@ -720,16 +687,18 @@ bool PiperInterfaceV2::controlJointMit( std::cerr << "controlJointMit: motor_num " << int(motor_num) << " out of range [1,6]\n"; return false; } - // Fixed scaling ranges per the Piper protocol; do not change without firmware reference. - const uint16_t pos_packed = static_cast(floatToUintRange(pos_rad, -12.5, 12.5, 16)); - const uint16_t vel_packed = static_cast(floatToUintRange(vel, -45.0, 45.0, 12)); - const uint16_t kp_packed = static_cast(floatToUintRange(kp, 0.0, 500.0, 12)); - const uint16_t kd_packed = static_cast(floatToUintRange(kd, -5.0, 5.0, 12)); - const uint16_t t_packed = static_cast(floatToUintRange(torque, -8.0, 8.0, 8)); + // A NaN passes every range check and has no defined integer conversion, so it would put an + // arbitrary target on the bus. + if (!std::isfinite(pos_rad) || !std::isfinite(vel) || !std::isfinite(kp) || !std::isfinite(kd) || + !std::isfinite(torque)) + { + std::cerr << "controlJointMit: non-finite input for motor " << int(motor_num) << "\n"; + return false; + } PiperMessage msg; const size_t idx = motor_num - 1; - msg.arm_joint_mit_ctrl.motors[idx] = ArmMsgJointMitCtrl{pos_packed, vel_packed, kp_packed, kd_packed, t_packed, 0}; + msg.arm_joint_mit_ctrl.motors[idx] = parser_->packJointMit(pos_rad, vel, kp, kd, torque); msg.type = static_cast(static_cast(ArmMsgType::JointMitCtrl1) + idx); return sendPiperMsg(msg); } diff --git a/piper_cpp/src/interface/piper_interface_v3.cpp b/piper_cpp/src/interface/piper_interface_v3.cpp new file mode 100644 index 0000000..f31dfdd --- /dev/null +++ b/piper_cpp/src/interface/piper_interface_v3.cpp @@ -0,0 +1,34 @@ +#include "piper_cpp/interface/piper_interface_v3.h" +#include "piper_cpp/protocol/piper_parser_v3.h" + +namespace piper_cpp +{ + +PiperInterfaceV3::PiperInterfaceV3( + std::string can_name, bool judge_flag, bool auto_init, bool dh_offset, bool sdk_joint_limit, bool sdk_gripper_limit +) + : PiperInterfaceV2( + std::make_unique(), std::move(can_name), judge_flag, auto_init, dh_offset, sdk_joint_limit, + sdk_gripper_limit + ) +{ +} + +PiperInterfaceV3::PiperInterfaceV3( + std::unique_ptr can, bool dh_offset, bool sdk_joint_limit, bool sdk_gripper_limit +) + : PiperInterfaceV2(std::make_unique(), std::move(can), dh_offset, sdk_joint_limit, sdk_gripper_limit) +{ +} + +bool PiperInterfaceV3::controlGripperWidth(int32_t width_um, uint16_t effort, GripperStatusCode status_code) +{ + return sendGripperCtrl(GripperMode::Width, width_um, effort, status_code); +} + +bool PiperInterfaceV3::controlGripperAngle(int32_t angle_millideg, uint16_t effort, GripperStatusCode status_code) +{ + return sendGripperCtrl(GripperMode::Angle, angle_millideg, effort, status_code); +} + +} // namespace piper_cpp diff --git a/piper_cpp/src/protocol/piper_codec.h b/piper_cpp/src/protocol/piper_codec.h new file mode 100644 index 0000000..67e746b --- /dev/null +++ b/piper_cpp/src/protocol/piper_codec.h @@ -0,0 +1,144 @@ +#pragma once + +// Byte-level helpers shared by the protocol parsers. Every multi-byte Piper field is big-endian. + +#include "piper_cpp/types/types.h" + +#include +#include +#include + +namespace piper_cpp +{ + +// Utility for endian conversion, assumes network (big-endian) order. +inline int32_t bytesToInt32(const uint8_t *data) +{ + uint32_t val = (uint32_t(data[0]) << 24) | (uint32_t(data[1]) << 16) | (uint32_t(data[2]) << 8) | uint32_t(data[3]); + return static_cast(val); // This cast will sign-extend if MSB is set +} + +inline int16_t bytesToInt16(const uint8_t *data) +{ + uint16_t val = (uint16_t(data[0]) << 8) | uint16_t(data[1]); + return static_cast(val); // This cast will sign-extend if MSB is set +} + +// For unsigned 16-bit +inline uint16_t bytesToUint16(const uint8_t *data) { return (uint16_t(data[0]) << 8) | uint16_t(data[1]); } + +// For signed 8-bit (this is safe, but explicit) +inline int8_t bytesToInt8(const uint8_t *data) { return static_cast(data[0]); } + +// For unsigned 8-bit +inline uint8_t bytesToUint8(const uint8_t *data) { return data[0]; } + +// Split a value into unsigned bytes (big-endian) +inline void int16_to_bytes(uint16_t val, uint8_t *out) +{ + out[0] = (val >> 8) & 0xFF; + out[1] = (val) & 0xFF; +} + +inline void uint16_to_bytes(uint16_t value, uint8_t *out) +{ + out[0] = (value >> 8) & 0xFF; + out[1] = value & 0xFF; +} + +inline void int32_to_bytes(int32_t val, uint8_t *out) +{ + out[0] = (val >> 24) & 0xFF; + out[1] = (val >> 16) & 0xFF; + out[2] = (val >> 8) & 0xFF; + out[3] = (val) & 0xFF; +} +// For uint32_t +inline void uint32_to_bytes(uint32_t val, uint8_t *out) +{ + out[0] = (val >> 24) & 0xFF; + out[1] = (val >> 16) & 0xFF; + out[2] = (val >> 8) & 0xFF; + out[3] = (val) & 0xFF; +} +// For 8-bit +inline void int8_to_bytes(uint8_t val, uint8_t *out) { out[0] = val & 0xFF; } + +/// How a fixed-point mapping resolves a value that falls between two codes. +enum class Quantisation +{ + Nearest, ///< Round to the nearest code, halves away from zero. + Truncate, ///< Drop the fraction, as the official Python SDK does. +}; + +/// Float -> N-bit unsigned mapping used by the MIT joint commands. Mirrors piper_sdk's +/// FloatToUint(value, min, max, bits), but clamps to [min, max] first so an out-of-range value +/// saturates instead of wrapping into the neighbouring bit fields. The value must be finite. +inline uint32_t floatToUintRange(double value, double min, double max, int bits, Quantisation quantisation) +{ + if (value < min) + value = min; + if (value > max) + value = max; + const double span = max - min; + const uint32_t max_int = (uint32_t{1} << bits) - 1u; + if (quantisation == Quantisation::Nearest) + return static_cast(std::lround((value - min) / span * max_int)); + // Same operation order as the SDK, so a value on a code boundary truncates to the same code. + return static_cast(std::trunc((value - min) * max_int / span)); +} + +// ---- Frame layouts shared by every protocol version ---- +// +// Where a version changes a message, it changes the values in the fields, not where they sit, +// so each layout is written once here and the parsers only differ in what they put into it. + +/// Write the 0x151 payload. `wire` holds wire bytes, already mapped for the target protocol. +inline void writeMotionCtrl2(const ArmMsgMotionCtrl_2 &wire, uint8_t *data) +{ + data[0] = wire.ctrl_mode; + data[1] = wire.move_mode; + data[2] = wire.move_spd_rate_ctrl; + data[3] = wire.mit_mode; + data[4] = wire.residence_time; + data[5] = wire.installation_pos; + // [6..7] zero +} + +/// Write the 0x159 payload with `status` as the wire status byte. +inline void writeGripperCtrl(const ArmMsgGripperCtrl &ctrl, uint8_t status, uint8_t *data) +{ + int32_to_bytes(ctrl.grippers_angle, &data[0]); + uint16_to_bytes(ctrl.grippers_effort, &data[4]); + data[6] = status; + data[7] = ctrl.set_zero; +} + +/// Write the MIT fields both protocols place identically: position, velocity, Kp, Kd and the +/// high nibble of byte 6. The torque bits after them differ per protocol. +inline void writeMitGains(const ArmMsgJointMitCtrl &mit, uint8_t *data) +{ + data[0] = (mit.pos_ref >> 8) & 0xFF; + data[1] = mit.pos_ref & 0xFF; + data[2] = (mit.vel_ref >> 4) & 0xFF; + data[3] = ((mit.vel_ref & 0xF) << 4) | ((mit.kp >> 8) & 0x0F); + data[4] = mit.kp & 0xFF; + data[5] = (mit.kd >> 4) & 0xFF; + data[6] = (mit.kd & 0xF) << 4; +} + +/// True for the six per-joint MIT command types. +inline bool isJointMitCtrl(ArmMsgType type) +{ + return type >= ArmMsgType::JointMitCtrl1 && type <= ArmMsgType::JointMitCtrl6; +} + +/// Every Piper command is an 8-byte standard frame whose ID follows from the type, so callers +/// only fill in the payload fields. +inline void setCommandHeader(ArmMsgType type, struct can_frame &tx_frame) +{ + tx_frame.can_id = static_cast(msgTypeToCanId(type)); + tx_frame.can_dlc = 8; +} + +} // namespace piper_cpp diff --git a/piper_cpp/src/protocol/piper_parser_v2.cpp b/piper_cpp/src/protocol/piper_parser_v2.cpp index a3709dd..53317ee 100644 --- a/piper_cpp/src/protocol/piper_parser_v2.cpp +++ b/piper_cpp/src/protocol/piper_parser_v2.cpp @@ -1,32 +1,11 @@ #include "piper_cpp/protocol/piper_parser_v2.h" +#include "piper_codec.h" #include #include namespace piper_cpp { -// Utility for endian conversion, assumes network (big-endian) order. -static int32_t bytesToInt32(const uint8_t *data) -{ - uint32_t val = (uint32_t(data[0]) << 24) | (uint32_t(data[1]) << 16) | (uint32_t(data[2]) << 8) | uint32_t(data[3]); - return static_cast(val); // This cast will sign-extend if MSB is set -} - -static int16_t bytesToInt16(const uint8_t *data) -{ - uint16_t val = (uint16_t(data[0]) << 8) | uint16_t(data[1]); - return static_cast(val); // This cast will sign-extend if MSB is set -} - -// For unsigned 16-bit -static uint16_t bytesToUint16(const uint8_t *data) { return (uint16_t(data[0]) << 8) | uint16_t(data[1]); } - -// For signed 8-bit (this is safe, but explicit) -static int8_t bytesToInt8(const uint8_t *data) { return static_cast(data[0]); } - -// For unsigned 8-bit -static uint8_t bytesToUint8(const uint8_t *data) { return data[0]; } - // Smallest payload each decoded ID needs: one past the last byte the decoder reads. A shorter // frame would otherwise be decoded from bytes that were never on the bus. static uint8_t requiredDlc(CanIDPiper id) @@ -51,38 +30,7 @@ static uint8_t requiredDlc(CanIDPiper id) } } -// Split a value into unsigned bytes (big-endian) -inline void int16_to_bytes(uint16_t val, uint8_t *out) -{ - out[0] = (val >> 8) & 0xFF; - out[1] = (val) & 0xFF; -} - -inline void uint16_to_bytes(uint16_t value, uint8_t *out) -{ - out[0] = (value >> 8) & 0xFF; - out[1] = value & 0xFF; -} - -inline void int32_to_bytes(int32_t val, uint8_t *out) -{ - out[0] = (val >> 24) & 0xFF; - out[1] = (val >> 16) & 0xFF; - out[2] = (val >> 8) & 0xFF; - out[3] = (val) & 0xFF; -} -// For uint32_t -inline void uint32_to_bytes(uint32_t val, uint8_t *out) -{ - out[0] = (val >> 24) & 0xFF; - out[1] = (val >> 16) & 0xFF; - out[2] = (val >> 8) & 0xFF; - out[3] = (val) & 0xFF; -} -// For 8-bit -inline void int8_to_bytes(uint8_t val, uint8_t *out) { out[0] = val & 0xFF; } - -bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double timestamp, PiperMessage &msg) +bool PiperParserV2::acceptFrame(const struct can_frame &rx_frame, double timestamp, PiperMessage &msg) const { // The Piper protocol uses standard 11-bit data frames only. Anything above those bits is // a remote-request, error or extended flag, or a reserved bit no valid frame sets. @@ -91,10 +39,8 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times msg.type = ArmMsgType::Unknown; return false; } - uint32_t can_id = rx_frame.can_id; - const uint8_t *data = rx_frame.data; - auto can_enum_id = static_cast(can_id); - msg.can_id = can_id; + auto can_enum_id = static_cast(rx_frame.can_id); + msg.can_id = rx_frame.can_id; try { msg.type = canIdToMsgType(can_enum_id); @@ -110,9 +56,19 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times return false; } // Only the received bytes are real; the rest of the buffer is whatever the frame carried. - std::copy(data, data + rx_frame.can_dlc, std::begin(msg.raw_data)); + std::copy(rx_frame.data, rx_frame.data + rx_frame.can_dlc, std::begin(msg.raw_data)); std::fill(std::begin(msg.raw_data) + rx_frame.can_dlc, std::end(msg.raw_data), 0); msg.timestamp = timestamp; + return true; +} + +bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double timestamp, PiperMessage &msg) +{ + if (!acceptFrame(rx_frame, timestamp, msg)) + return false; + uint32_t can_id = rx_frame.can_id; + const uint8_t *data = rx_frame.data; + auto can_enum_id = static_cast(can_id); // ----------------- Main switch ----------------- switch (can_enum_id) @@ -173,6 +129,7 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times msg.gripper_feedback.grippers_effort = bytesToInt16(&data[4]); msg.gripper_feedback.status_code = bytesToUint8(&data[6]); msg.gripper_feedback.updateFocStatus(); + msg.gripper_feedback.mode = GripperMode::Width; break; // ----- High-speed driver feedback (per joint, 1~6) ----- @@ -311,6 +268,7 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times msg.gripper_ctrl.grippers_effort = bytesToInt16(&data[4]); msg.gripper_ctrl.status_code = bytesToUint8(&data[6]); msg.gripper_ctrl.set_zero = bytesToUint8(&data[7]); + msg.gripper_ctrl.mode = GripperMode::Width; break; } @@ -333,6 +291,7 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times } default: + msg.type = ArmMsgType::Unknown; // e.g. V3's IK feedback, which V2 does not define return false; } return true; @@ -357,13 +316,13 @@ bool PiperParserV2::encodeMessage(const PiperMessage &msg, struct can_frame &tx_ // --- Motion Ctrl 2 --- case ArmMsgType::MotionCtrl2: - tx_frame.data[0] = msg.arm_motion_ctrl_2.ctrl_mode; - tx_frame.data[1] = msg.arm_motion_ctrl_2.move_mode; - tx_frame.data[2] = msg.arm_motion_ctrl_2.move_spd_rate_ctrl; - tx_frame.data[3] = msg.arm_motion_ctrl_2.mit_mode; - tx_frame.data[4] = msg.arm_motion_ctrl_2.residence_time; - tx_frame.data[5] = msg.arm_motion_ctrl_2.installation_pos; - // [6..7] zero + // 0x04 is V3's horizontal-inverted mounting; V2 defines it as invalid, so never send it. + if (msg.arm_motion_ctrl_2.installation_pos == static_cast(InstallationPos::HorizontalInverted)) + { + ret = false; + break; + } + writeMotionCtrl2(msg.arm_motion_ctrl_2, tx_frame.data); break; // --- Cartesian Control (1/2/3) --- @@ -402,10 +361,13 @@ bool PiperParserV2::encodeMessage(const PiperMessage &msg, struct can_frame &tx_ // --- Gripper Control --- case ArmMsgType::GripperCtrl: - int32_to_bytes(msg.gripper_ctrl.grippers_angle, &tx_frame.data[0]); - uint16_to_bytes(msg.gripper_ctrl.grippers_effort, &tx_frame.data[4]); - tx_frame.data[6] = msg.gripper_ctrl.status_code; - tx_frame.data[7] = msg.gripper_ctrl.set_zero; + // V2 has no gripper modes: a non-width target would be read as a width. + if (msg.gripper_ctrl.mode != GripperMode::Width) + { + ret = false; + break; + } + writeGripperCtrl(msg.gripper_ctrl, msg.gripper_ctrl.status_code, tx_frame.data); break; // --- Master-Slave Mode Config --- @@ -505,13 +467,8 @@ bool PiperParserV2::encodeMessage(const PiperMessage &msg, struct can_frame &tx_ size_t joint_idx = static_cast(msg.type) - static_cast(ArmMsgType::JointMitCtrl1); const ArmMsgJointMitCtrl &mit = msg.arm_joint_mit_ctrl.motors[joint_idx]; - tx_frame.data[0] = (mit.pos_ref >> 8) & 0xFF; - tx_frame.data[1] = mit.pos_ref & 0xFF; - tx_frame.data[2] = (mit.vel_ref >> 4) & 0xFF; - tx_frame.data[3] = ((mit.vel_ref & 0xF) << 4) | ((mit.kp >> 8) & 0x0F); - tx_frame.data[4] = mit.kp & 0xFF; - tx_frame.data[5] = (mit.kd >> 4) & 0xFF; - tx_frame.data[6] = ((mit.kd & 0xF) << 4) | ((mit.t_ref >> 4) & 0x0F); + writeMitGains(mit, tx_frame.data); + tx_frame.data[6] |= (mit.t_ref >> 4) & 0x0F; uint8_t crc = (tx_frame.data[0] ^ tx_frame.data[1] ^ tx_frame.data[2] ^ tx_frame.data[3] ^ tx_frame.data[4] ^ tx_frame.data[5] ^ tx_frame.data[6]) & @@ -535,13 +492,22 @@ bool PiperParserV2::encodeMessage(const PiperMessage &msg, struct can_frame &tx_ } if (ret) - { - // Every Piper command is an 8-byte standard frame whose ID follows from the type, so - // callers only fill in the payload fields. - tx_frame.can_id = static_cast(msgTypeToCanId(msg.type)); - tx_frame.can_dlc = 8; - } + setCommandHeader(msg.type, tx_frame); return ret; } +ArmMsgJointMitCtrl PiperParserV2::packJointMit(double pos_rad, double vel, double kp, double kd, double torque) const +{ + // Fixed scaling ranges per the Piper protocol; do not change without firmware reference. + constexpr auto q = Quantisation::Nearest; + ArmMsgJointMitCtrl mit; + mit.pos_ref = static_cast(floatToUintRange(pos_rad, -12.5, 12.5, 16, q)); + mit.vel_ref = static_cast(floatToUintRange(vel, -45.0, 45.0, 12, q)); + mit.kp = static_cast(floatToUintRange(kp, 0.0, 500.0, 12, q)); + mit.kd = static_cast(floatToUintRange(kd, -5.0, 5.0, 12, q)); + mit.t_ref = static_cast(floatToUintRange(torque, -8.0, 8.0, 8, q)); + mit.crc = 0; + return mit; +} + } // namespace piper_cpp diff --git a/piper_cpp/src/protocol/piper_parser_v3.cpp b/piper_cpp/src/protocol/piper_parser_v3.cpp new file mode 100644 index 0000000..8301852 --- /dev/null +++ b/piper_cpp/src/protocol/piper_parser_v3.cpp @@ -0,0 +1,179 @@ +#include "piper_cpp/protocol/piper_parser_v3.h" +#include "piper_codec.h" +#include +#include +#include + +namespace piper_cpp +{ + +namespace +{ + +constexpr uint8_t kMoveMWire = 0x06; +constexpr uint8_t kUnknownMode = 0xFF; +// Bit 2 of the 0x159 status byte selects angle mode; bits 0-1 keep their V2 meaning. +constexpr uint8_t kGripperAngleBit = 0x04; +// The official SDK's V3 gripper command rejects efforts above 5 N*m. +constexpr uint16_t kMaxGripperEffort = 5000; + +bool isOneOf(uint8_t value, std::initializer_list allowed) +{ + for (uint8_t a : allowed) + if (value == a) + return true; + return false; +} + +/// V3 moved MOVE M from 0x04 to 0x06 and left 0x04 undefined; every other mode keeps its V2 +/// number. Used for both the 0x151 echo and the 0x2A1 mode feedback. +uint8_t moveModeFromWire(uint8_t wire) +{ + if (wire == kMoveMWire) + return static_cast(MoveMode::MoveM); + if (wire == static_cast(MoveMode::MoveM) || wire > static_cast(MoveMode::MoveCPV)) + return kUnknownMode; + return wire; +} + +/// Map the semantic 0x151 fields onto V3 wire bytes, rejecting what the official V3 message +/// class rejects. Its changelog also lists ctrl_mode 0x02, 0x05, 0x06 and 0xFF, but the class +/// refuses them, so this does too. +bool motionCtrl2ToWire(ArmMsgMotionCtrl_2 &ctrl) +{ + if (!isOneOf(ctrl.ctrl_mode, {0x00, 0x01, 0x03, 0x04, 0x07}) || ctrl.move_spd_rate_ctrl > 100 || + !isOneOf(ctrl.mit_mode, {0x00, 0xAD, 0xFF}) || + ctrl.installation_pos > static_cast(InstallationPos::HorizontalInverted) || + ctrl.move_mode > static_cast(MoveMode::MoveCPV)) + return false; + if (ctrl.move_mode == static_cast(MoveMode::MoveM)) + ctrl.move_mode = kMoveMWire; + return true; +} + +/// The V3 status byte for `ctrl`, or nothing for a value the official `GripperCtrl_V3()` +/// refuses. Its message class has these range checks commented out; the interface method +/// enforces them, and so does this. +std::optional gripperStatusToWire(const ArmMsgGripperCtrl &ctrl) +{ + if (ctrl.status_code > static_cast(GripperStatusCode::EnableAndClearError) || + ctrl.grippers_effort > kMaxGripperEffort || !isOneOf(ctrl.set_zero, {0x00, 0xAE})) + return std::nullopt; + switch (ctrl.mode) + { + case GripperMode::Width: + return ctrl.status_code; + case GripperMode::Angle: + return ctrl.status_code | kGripperAngleBit; + default: + return std::nullopt; + } +} + +} // namespace + +bool PiperParserV3::decodeMessage(const struct can_frame &rx_frame, double timestamp, PiperMessage &msg) +{ + switch (rx_frame.can_id) + { + case static_cast(CanIDPiper::ARM_IK_JOINT_FEEDBACK_12): + case static_cast(CanIDPiper::ARM_IK_JOINT_FEEDBACK_34): + case static_cast(CanIDPiper::ARM_IK_JOINT_FEEDBACK_56): + { + if (!acceptFrame(rx_frame, timestamp, msg)) + return false; + auto &ik = msg.arm_ik_joint_feedback; + int32_t *pair[] = {&ik.joint_1, &ik.joint_3, &ik.joint_5}; + const size_t part = static_cast(msg.type) - static_cast(ArmMsgType::IKJointFeedback12); + pair[part][0] = bytesToInt32(&rx_frame.data[0]); + pair[part][1] = bytesToInt32(&rx_frame.data[4]); + return true; + } + default: + break; + } + + if (!PiperParserV2::decodeMessage(rx_frame, timestamp, msg)) + return false; + const uint8_t *data = rx_frame.data; + switch (msg.type) + { + case ArmMsgType::StatusFeedback: + msg.arm_status_msgs.mode_feed = static_cast(moveModeFromWire(data[2])); + break; + case ArmMsgType::MotionCtrl2: + msg.arm_motion_ctrl_2.move_mode = moveModeFromWire(data[1]); + break; + case ArmMsgType::GripperFeedback: + // A 7-byte frame carries no mode byte; read it as width, as the SDK does. + if (rx_frame.can_dlc < 8) + msg.gripper_feedback.mode = GripperMode::Width; + else + msg.gripper_feedback.mode = data[7] == 0x00 ? GripperMode::Width + : data[7] == 0x01 ? GripperMode::Angle + : GripperMode::Unknown; + break; + case ArmMsgType::GripperCtrl: + if (data[6] <= (static_cast(GripperStatusCode::EnableAndClearError) | kGripperAngleBit)) + { + msg.gripper_ctrl.status_code = data[6] & ~kGripperAngleBit; + msg.gripper_ctrl.mode = (data[6] & kGripperAngleBit) ? GripperMode::Angle : GripperMode::Width; + } + else + { + msg.gripper_ctrl.mode = GripperMode::Unknown; // status_code keeps the raw byte + } + break; + default: + break; + } + return true; +} + +bool PiperParserV3::encodeMessage(const PiperMessage &msg, struct can_frame &tx_frame) +{ + if (msg.type == ArmMsgType::MotionCtrl2) + { + ArmMsgMotionCtrl_2 wire = msg.arm_motion_ctrl_2; + if (!motionCtrl2ToWire(wire)) + return false; + std::memset(tx_frame.data, 0, 8); + writeMotionCtrl2(wire, tx_frame.data); + } + else if (msg.type == ArmMsgType::GripperCtrl) + { + const auto status = gripperStatusToWire(msg.gripper_ctrl); + if (!status) + return false; + writeGripperCtrl(msg.gripper_ctrl, *status, tx_frame.data); + } + else if (isJointMitCtrl(msg.type)) + { + const size_t joint_idx = static_cast(msg.type) - static_cast(ArmMsgType::JointMitCtrl1); + const ArmMsgJointMitCtrl &mit = msg.arm_joint_mit_ctrl.motors[joint_idx]; + writeMitGains(mit, tx_frame.data); + tx_frame.data[6] |= (mit.t_ref >> 8) & 0x0F; + tx_frame.data[7] = mit.t_ref & 0xFF; + } + else + { + return PiperParserV2::encodeMessage(msg, tx_frame); + } + setCommandHeader(msg.type, tx_frame); + return true; +} + +ArmMsgJointMitCtrl PiperParserV3::packJointMit(double pos_rad, double vel, double kp, double kd, double torque) const +{ + constexpr auto q = Quantisation::Truncate; + ArmMsgJointMitCtrl mit; + mit.pos_ref = static_cast(floatToUintRange(pos_rad, -12.5, 12.5, 16, q)); + mit.vel_ref = static_cast(floatToUintRange(vel, -45.0, 45.0, 12, q)); + mit.kp = static_cast(floatToUintRange(kp, 0.0, 500.0, 12, q)); + mit.kd = static_cast(floatToUintRange(kd, -5.0, 5.0, 12, q)); + mit.t_ref = static_cast(floatToUintRange(torque, -16.0, 16.0, 12, q)); + mit.crc = 0; + return mit; +} + +} // namespace piper_cpp diff --git a/piper_cpp/src/types/core.cpp b/piper_cpp/src/types/core.cpp index a3c0820..71f7b87 100644 --- a/piper_cpp/src/types/core.cpp +++ b/piper_cpp/src/types/core.cpp @@ -128,6 +128,12 @@ CanIDPiper msgTypeToCanId(ArmMsgType type) return CanIDPiper::ARM_CAN_UPDATE_SILENT_MODE_CONFIG; case ArmMsgType::FirmwareRead: return CanIDPiper::ARM_FIRMWARE_READ; + case ArmMsgType::IKJointFeedback12: + return CanIDPiper::ARM_IK_JOINT_FEEDBACK_12; + case ArmMsgType::IKJointFeedback34: + return CanIDPiper::ARM_IK_JOINT_FEEDBACK_34; + case ArmMsgType::IKJointFeedback56: + return CanIDPiper::ARM_IK_JOINT_FEEDBACK_56; case ArmMsgType::Unknown: throw std::invalid_argument("Unknown ArmMsgType has no valid CAN ID"); } @@ -258,6 +264,12 @@ ArmMsgType canIdToMsgType(CanIDPiper id) return ArmMsgType::CanUpdateSilentModeConfig; case CanIDPiper::ARM_FIRMWARE_READ: return ArmMsgType::FirmwareRead; + case CanIDPiper::ARM_IK_JOINT_FEEDBACK_12: + return ArmMsgType::IKJointFeedback12; + case CanIDPiper::ARM_IK_JOINT_FEEDBACK_34: + return ArmMsgType::IKJointFeedback34; + case CanIDPiper::ARM_IK_JOINT_FEEDBACK_56: + return ArmMsgType::IKJointFeedback56; } throw std::invalid_argument("Unhandled CanIDPiper in canIdToMsgType"); } @@ -330,6 +342,9 @@ std::string toString(ArmMsgType type) CASE(LightCtrl); CASE(CanUpdateSilentModeConfig); CASE(FirmwareRead); + CASE(IKJointFeedback12); + CASE(IKJointFeedback34); + CASE(IKJointFeedback56); #undef CASE } return "Unknown"; @@ -402,6 +417,9 @@ std::string toString(CanIDPiper id) CASE(ARM_INFO_LOW_SPD_FEEDBACK_6); CASE(ARM_CAN_UPDATE_SILENT_MODE_CONFIG); CASE(ARM_FIRMWARE_READ); + CASE(ARM_IK_JOINT_FEEDBACK_12); + CASE(ARM_IK_JOINT_FEEDBACK_34); + CASE(ARM_IK_JOINT_FEEDBACK_56); #undef CASE } return "Unknown"; diff --git a/piper_cpp/test/CMakeLists.txt b/piper_cpp/test/CMakeLists.txt index b83087b..f20f711 100644 --- a/piper_cpp/test/CMakeLists.txt +++ b/piper_cpp/test/CMakeLists.txt @@ -13,5 +13,8 @@ endfunction() add_piper_test(std_can_interface_test) add_piper_test(piper_interface_v2_test) add_piper_test(piper_parser_v2_test) +add_piper_test(piper_parser_v3_test) +add_piper_test(piper_interface_v3_test) +add_piper_test(piper_sdk_reference_test) add_piper_test(timed_freq_state_test) add_piper_test(fk_test) diff --git a/piper_cpp/test/piper_interface_v2_test.cpp b/piper_cpp/test/piper_interface_v2_test.cpp index 6016718..6c4e1b2 100644 --- a/piper_cpp/test/piper_interface_v2_test.cpp +++ b/piper_cpp/test/piper_interface_v2_test.cpp @@ -2,15 +2,16 @@ // peer end reach the interface's read thread exactly as they would from a raw CAN socket. #include "piper_cpp/interface/piper_interface_v2.h" +#include "test_support.h" #include #include #include #include +#include #include #include -#include #include #include #include @@ -20,52 +21,16 @@ using namespace std::chrono_literals; using piper_cpp::PiperInterfaceV2; -using piper_cpp::StdCanInterface; using std::chrono::steady_clock; +using test_support::frame; +using test_support::jointPairFrame; +using test_support::makeBus; +using test_support::sendFrame; +using test_support::waitFor; namespace { -struct Bus -{ - int peer; - std::unique_ptr piper; // destroyed first, so the read thread is gone before peer closes - ~Bus() { ::close(peer); } -}; - -Bus makeBus() -{ - int sv[2]; - if (socketpair(AF_UNIX, SOCK_SEQPACKET, 0, sv) != 0) - throw std::runtime_error("socketpair failed"); - return {sv[1], std::make_unique(std::make_unique(sv[0]))}; -} - -// Joint feedback frames carry two big-endian int32 angles in 0.001 deg. -can_frame jointFeedbackFrame(uint32_t id, int32_t a, int32_t b) -{ - can_frame f{}; - f.can_id = id; - f.can_dlc = 8; - const auto ua = static_cast(a); - const auto ub = static_cast(b); - for (int i = 0; i < 4; ++i) - { - f.data[i] = static_cast(ua >> (24 - 8 * i)); - f.data[4 + i] = static_cast(ub >> (24 - 8 * i)); - } - return f; -} - -can_frame frame(uint32_t id, std::initializer_list bytes) -{ - can_frame f{}; - f.can_id = id; - f.can_dlc = static_cast(bytes.size()); - std::copy(bytes.begin(), bytes.end(), f.data); - return f; -} - // 0x473 reply: motor index, then max angle, min angle and max speed as big-endian int16. can_frame angleLimitReply(uint8_t motor, int16_t max_angle) { @@ -74,24 +39,6 @@ can_frame angleLimitReply(uint8_t motor, int16_t max_angle) return frame(0x473, {motor, hi, lo, 0, 0, 0, 0}); } -template bool waitFor(Pred pred, std::chrono::milliseconds timeout = 2s) -{ - const auto deadline = steady_clock::now() + timeout; - while (!pred()) - { - if (steady_clock::now() > deadline) - return false; - std::this_thread::sleep_for(1ms); - } - return true; -} - -void sendFrame(int fd, const can_frame& f, steady_clock::time_point deadline) -{ - while (::send(fd, &f, sizeof(f), MSG_DONTWAIT) < 0 && errno == EAGAIN && steady_clock::now() < deadline) - std::this_thread::yield(); -} - } // namespace TEST(PiperInterfaceV2, DisconnectReturnsPromptlyOnSilentBus) @@ -118,9 +65,9 @@ TEST(PiperInterfaceV2, ReadLoopKeepsUpWithBurst) const auto t0 = steady_clock::now(); for (int n = 1; n <= kSets; ++n) { - sendFrame(bus.peer, jointFeedbackFrame(0x2A5, n, -n), deadline); - sendFrame(bus.peer, jointFeedbackFrame(0x2A6, 2 * n, -2 * n), deadline); - sendFrame(bus.peer, jointFeedbackFrame(0x2A7, 3 * n, -3 * n), deadline); + sendFrame(bus.peer, jointPairFrame(0x2A5, n, -n), deadline); + sendFrame(bus.peer, jointPairFrame(0x2A6, 2 * n, -2 * n), deadline); + sendFrame(bus.peer, jointPairFrame(0x2A7, 3 * n, -3 * n), deadline); } const auto send_ms = std::chrono::duration_cast(steady_clock::now() - t0).count(); EXPECT_LT(send_ms, 1000); @@ -197,9 +144,9 @@ TEST(PiperInterfaceV2, DisconnectClearsCachedState) auto bus = makeBus(); ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); const auto deadline = steady_clock::now() + 5s; - sendFrame(bus.peer, jointFeedbackFrame(0x2A5, 1, 2), deadline); - sendFrame(bus.peer, jointFeedbackFrame(0x2A6, 3, 4), deadline); - sendFrame(bus.peer, jointFeedbackFrame(0x2A7, 5, 6), deadline); + sendFrame(bus.peer, jointPairFrame(0x2A5, 1, 2), deadline); + sendFrame(bus.peer, jointPairFrame(0x2A6, 3, 4), deadline); + sendFrame(bus.peer, jointPairFrame(0x2A7, 5, 6), deadline); ASSERT_TRUE(waitFor([&] { return bus.piper->getArmJointStates().is_valid; })); bus.piper->disconnectPort(); @@ -378,3 +325,49 @@ TEST(PiperInterfaceV2, WaitForMotorLimitsGivesUpAtTheTimeout) EXPECT_LT(waited, 1000); EXPECT_FALSE(bus.piper->getArmAllMotorAngleLimitMaxSpd().is_valid); } + +TEST(PiperInterfaceV2, MitCommandFramesAreUnchangedOnEveryJoint) +{ + // Worked by hand in piper_parser_v2_test: pos 0, vel 0, kp 10, kd 0.8, torque 0 packs to + // 80 00 80 00 52 94 78 0E under V2 (round to nearest, 8-bit torque, CRC nibble). + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/false)); + for (uint8_t joint = 1; joint <= 6; ++joint) + { + ASSERT_TRUE(bus.piper->controlJointMit(joint, 0.0, 0.0, 10.0, 0.8, 0.0)); + const auto tx = test_support::receiveFrame(bus.peer); + ASSERT_TRUE(tx); + EXPECT_EQ(tx->can_id, 0x159u + joint); + const uint8_t expected[8] = {0x80, 0x00, 0x80, 0x00, 0x52, 0x94, 0x78, 0x0E}; + EXPECT_TRUE(std::equal(expected, expected + 8, tx->data)) << "joint " << int(joint); + } +} + +TEST(PiperInterfaceV2, SdkJointLimitClampsEachJointToItsOwnRange) +{ + // Every joint gets the same out-of-range value; each must end up at its own upper limit, + // in both the measured feedback and the command echo. The default limits differ per joint, + // so a joint read through another joint's limit shows up here. + auto bus = makeBus(/*dh_offset=*/true, /*sdk_joint_limit=*/true); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + const auto deadline = steady_clock::now() + 5s; + constexpr int32_t kFar = 1'000'000'000; + for (uint32_t id : {0x2A5u, 0x2A6u, 0x2A7u, 0x155u, 0x156u, 0x157u}) + sendFrame(bus.peer, jointPairFrame(id, kFar, kFar), deadline); + ASSERT_TRUE( + waitFor([&] { return bus.piper->getArmJointStates().is_valid && bus.piper->getArmJointCtrlMsgs().is_valid; }) + ); + + const auto& pm = piper_cpp::PiperParamManager::instance(); + using Joint = piper_cpp::PiperParamManager::Joint; + for (const auto& values : {bus.piper->getArmJointStates().value, bus.piper->getArmJointCtrlMsgs().value}) + { + const int32_t got[] = {values.joint_1, values.joint_2, values.joint_3, + values.joint_4, values.joint_5, values.joint_6}; + for (int j = 0; j < 6; ++j) + { + const double max_mdeg = pm.getJointLimitRad(static_cast(j))[1] * 180.0 / M_PI * 1000.0; + EXPECT_NEAR(got[j], max_mdeg, 1.0) << "joint " << j + 1; + } + } +} diff --git a/piper_cpp/test/piper_interface_v3_test.cpp b/piper_cpp/test/piper_interface_v3_test.cpp new file mode 100644 index 0000000..13b01b2 --- /dev/null +++ b/piper_cpp/test/piper_interface_v3_test.cpp @@ -0,0 +1,210 @@ +// Drives PiperInterfaceV3 (and PiperInterfaceV2 where the two must differ) through a +// socketpair in place of the CAN bus, checking what the public API puts on the wire and how +// V3 feedback reaches the state getters. + +#include "piper_cpp/interface/piper_interface_v3.h" +#include "test_support.h" + +#include + +#include +#include + +using namespace std::chrono_literals; +using piper_cpp::GripperMode; +using piper_cpp::GripperStatusCode; +using piper_cpp::InstallationPos; +using piper_cpp::MoveMode; +using piper_cpp::PiperInterfaceV2; +using piper_cpp::PiperInterfaceV3; +using std::chrono::steady_clock; +using test_support::frame; +using test_support::jointPairFrame; +using test_support::makeBus; +using test_support::receiveFrame; +using test_support::sendFrame; +using test_support::waitFor; +using ModeFeed = piper_cpp::ArmMsgFeedbackStatusEnum::ModeFeed; + +namespace +{ + +constexpr auto kQuiet = 50ms; // long enough for a frame that was sent to arrive + +template void connectWithoutReadThread(test_support::Bus& bus) +{ + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/false)); +} + +} // namespace + +TEST(PiperInterfaceV3, ReportsItsProtocol) +{ + EXPECT_EQ(makeBus().piper->getProtocolVersion(), 3); + EXPECT_EQ(makeBus().piper->getProtocolVersion(), 2); +} + +TEST(PiperInterfaceV3, MoveMIsSentAndReadBackAsTheSameMode) +{ + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + + ASSERT_TRUE(bus.piper->setMoveMode(MoveMode::MoveM)); + const auto tx = receiveFrame(bus.peer); + ASSERT_TRUE(tx); + EXPECT_EQ(tx->can_id, 0x151u); + EXPECT_EQ(tx->data[1], 0x06); + EXPECT_EQ(bus.piper->getCommandedMotionCtrlState().move_mode, static_cast(MoveMode::MoveM)); + + const auto deadline = steady_clock::now() + 2s; + sendFrame(bus.peer, frame(0x2A1, {0x01, 0x00, 0x06, 0, 0, 0, 0, 0}), deadline); + sendFrame(bus.peer, frame(0x151, {0x01, 0x06, 0x32, 0xAD, 0x00, 0x00, 0, 0}), deadline); + ASSERT_TRUE( + waitFor([&] { return bus.piper->getArmStatus().is_valid && bus.piper->getArmMotionCtrlCode151().is_valid; }) + ); + EXPECT_EQ(bus.piper->getArmStatus().value.mode_feed, ModeFeed::MOVE_M); + EXPECT_EQ(bus.piper->getArmMotionCtrlCode151().value.move_mode, static_cast(MoveMode::MoveM)); +} + +TEST(PiperInterfaceV3, OnlyV3SendsHorizontalInvertedInstallation) +{ + auto v3 = makeBus(); + connectWithoutReadThread(v3); + ASSERT_TRUE(v3.piper->setInstallationPos(InstallationPos::HorizontalInverted)); + const auto tx = receiveFrame(v3.peer); + ASSERT_TRUE(tx); + EXPECT_EQ(tx->data[5], 0x04); + + auto v2 = makeBus(); + connectWithoutReadThread(v2); + EXPECT_FALSE(v2.piper->setInstallationPos(InstallationPos::HorizontalInverted)); + EXPECT_FALSE(receiveFrame(v2.peer, kQuiet)) << "V2 must not send a value it defines as invalid"; + + // The refused value must not stay in the commanded state, or every later 0x151 setter + // would resend it and fail too. + EXPECT_EQ(v2.piper->getCommandedMotionCtrlState().installation_pos, 0x00); + ASSERT_TRUE(v2.piper->setMoveMode(MoveMode::MoveP)); + const auto next = receiveFrame(v2.peer); + ASSERT_TRUE(next); + EXPECT_EQ(next->data[1], 0x00); + EXPECT_EQ(next->data[5], 0x00); +} + +TEST(PiperInterfaceV3, GripperCommandsSelectTheMode) +{ + auto bus = makeBus(); + connectWithoutReadThread(bus); + + ASSERT_TRUE(bus.piper->controlGripperAngle(-45000, 1000)); + auto tx = receiveFrame(bus.peer); + ASSERT_TRUE(tx); + EXPECT_EQ(tx->data[6], 0x05) << "enable | angle bit"; + + ASSERT_TRUE(bus.piper->controlGripperWidth(30000, 1000, GripperStatusCode::EnableAndClearError)); + tx = receiveFrame(bus.peer); + ASSERT_TRUE(tx); + EXPECT_EQ(tx->data[6], 0x03); + + // The V2 call keeps meaning width mode under V3. + ASSERT_TRUE(bus.piper->controlGripper(30000, 1000)); + tx = receiveFrame(bus.peer); + ASSERT_TRUE(tx); + EXPECT_EQ(tx->data[6], 0x01); + + EXPECT_FALSE(bus.piper->controlGripperAngle(0, 5001)); + EXPECT_FALSE(bus.piper->controlGripperWidth(0, 5001)); + EXPECT_FALSE(bus.piper->controlGripper(0, 5001)) << "the same width frame, so the same limit"; + EXPECT_FALSE(receiveFrame(bus.peer, kQuiet)) << "a rejected effort must not reach the bus"; + + // V2 keeps sending what it always sent. + auto v2 = makeBus(); + connectWithoutReadThread(v2); + EXPECT_TRUE(v2.piper->controlGripper(0, 5001)); +} + +TEST(PiperInterfaceV3, GripperFeedbackLimitFollowsTheMode) +{ + // With SDK gripper limits on, a width is clamped to the width range (default 0-70 mm) and + // an angle to the angle range (default +-180 deg). + auto bus = makeBus(/*dh_offset=*/true, /*sdk_joint_limit=*/false, /*sdk_gripper_limit=*/true); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + const auto deadline = steady_clock::now() + 2s; + + // 200 deg = 200000 = 0x00030D40, angle mode. + sendFrame(bus.peer, frame(0x2A8, {0x00, 0x03, 0x0D, 0x40, 0, 0, 0x40, 0x01}), deadline); + ASSERT_TRUE(waitFor([&] { return bus.piper->getGripperStates().is_valid; })); + EXPECT_EQ(bus.piper->getGripperStates().value.mode, GripperMode::Angle); + EXPECT_EQ(bus.piper->getGripperStates().value.grippers_angle, 180000); + + // The same value in width mode is 200 mm, clamped to 70 mm. + sendFrame(bus.peer, frame(0x2A8, {0x00, 0x03, 0x0D, 0x40, 0, 0, 0x40, 0x00}), deadline); + ASSERT_TRUE(waitFor([&] { return bus.piper->getGripperStates().value.mode == GripperMode::Width; })); + EXPECT_EQ(bus.piper->getGripperStates().value.grippers_angle, 70000); +} + +TEST(PiperInterfaceV3, IkJointStatesNeedAllThreeFramesAndGoStale) +{ + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + const auto deadline = steady_clock::now() + 5s; + + sendFrame(bus.peer, jointPairFrame(0x2AA, 1000, -2000), deadline); + sendFrame(bus.peer, jointPairFrame(0x2AB, 3000, -4000), deadline); + ASSERT_TRUE(waitFor([&] { return bus.piper->getArmIKJointStates().value.joint_4 == -4000; })); + EXPECT_FALSE(bus.piper->getArmIKJointStates().is_valid) << "joints 5-6 have not arrived"; + + sendFrame(bus.peer, jointPairFrame(0x2AC, 5000, -6000), deadline); + ASSERT_TRUE(waitFor([&] { return bus.piper->getArmIKJointStates().is_valid; })); + const auto ik = bus.piper->getArmIKJointStates(); + EXPECT_TRUE(ik.is_healthy); + EXPECT_EQ(ik.value.joint_1, 1000); + EXPECT_EQ(ik.value.joint_6, -6000); + EXPECT_FALSE(bus.piper->getArmJointStates().is_valid) << "IK solutions are not measured joint feedback"; + + // A stream that stops is reported as unhealthy once the 1 s timeout passes. + ASSERT_TRUE(waitFor([&] { return !bus.piper->getArmIKJointStates().is_healthy; }, 3s)); + + bus.piper->disconnectPort(); + EXPECT_FALSE(bus.piper->getArmIKJointStates().is_valid) << "a closed session's IK feedback must not look current"; +} + +TEST(PiperInterfaceV3, MitRejectsNonFiniteInputsWithoutSending) +{ + constexpr double nan = std::numeric_limits::quiet_NaN(); + constexpr double inf = std::numeric_limits::infinity(); + auto v3 = makeBus(); + auto v2 = makeBus(); + connectWithoutReadThread(v3); + connectWithoutReadThread(v2); + for (PiperInterfaceV2* piper : {static_cast(v3.piper.get()), v2.piper.get()}) + { + EXPECT_FALSE(piper->controlJointMit(1, nan, 0, 10, 0.8, 0)); + EXPECT_FALSE(piper->controlJointMit(2, 0, inf, 10, 0.8, 0)); + EXPECT_FALSE(piper->controlJointMit(3, 0, 0, -inf, 0.8, 0)); + EXPECT_FALSE(piper->controlJointMit(4, 0, 0, 10, nan, 0)); + EXPECT_FALSE(piper->controlJointMit(6, 0, 0, 10, 0.8, nan)); + EXPECT_FALSE(piper->controlJointMit(7, 0, 0, 10, 0.8, 0)) << "joint index"; + } + EXPECT_FALSE(receiveFrame(v3.peer, kQuiet)); + EXPECT_FALSE(receiveFrame(v2.peer, kQuiet)); +} + +TEST(PiperInterfaceV3, MitTorqueRangeDependsOnProtocol) +{ + // 12 N*m is inside V3's [-16, 16] but beyond V2's [-8, 8], where it saturates. + auto v3 = makeBus(); + auto v2 = makeBus(); + connectWithoutReadThread(v3); + connectWithoutReadThread(v2); + ASSERT_TRUE(v3.piper->controlJointMit(2, 0, 0, 10, 0.8, 12.0)); + ASSERT_TRUE(v2.piper->controlJointMit(2, 0, 0, 10, 0.8, 12.0)); + const auto a = receiveFrame(v3.peer); + const auto b = receiveFrame(v2.peer); + ASSERT_TRUE(a && b); + // V3: 28 * 4095 / 32 = 3583.125 -> 0xDFF. V2: saturated at 0xFF. + EXPECT_EQ(a->can_id, 0x15Bu); + EXPECT_EQ(a->data[6] & 0x0F, 0x0D); + EXPECT_EQ(a->data[7], 0xFF); + EXPECT_EQ(b->data[6] & 0x0F, 0x0F); + EXPECT_EQ(b->data[7] >> 4, 0x0F); +} diff --git a/piper_cpp/test/piper_parser_v2_test.cpp b/piper_cpp/test/piper_parser_v2_test.cpp index 1b59440..759ee03 100644 --- a/piper_cpp/test/piper_parser_v2_test.cpp +++ b/piper_cpp/test/piper_parser_v2_test.cpp @@ -2,30 +2,18 @@ // signed fields, and that the encoder produces complete frames on its own. #include "piper_cpp/protocol/piper_parser_v2.h" +#include "test_support.h" #include #include -#include +#include #include using piper_cpp::ArmMsgType; using piper_cpp::PiperMessage; using piper_cpp::PiperParserV2; - -namespace -{ - -can_frame frame(uint32_t id, std::initializer_list bytes) -{ - can_frame f{}; - f.can_id = id; - f.can_dlc = static_cast(bytes.size()); - std::copy(bytes.begin(), bytes.end(), f.data); - return f; -} - -} // namespace +using test_support::frame; TEST(PiperParserV2, RejectsFramesThatAreNotStandardData) { @@ -121,3 +109,65 @@ TEST(PiperParserV2, MotionCtrl2RoundTripsInstallationPos) EXPECT_EQ(back.arm_motion_ctrl_2.move_spd_rate_ctrl, 30); EXPECT_EQ(back.arm_motion_ctrl_2.installation_pos, 3); } + +// V2 MIT packing as it was before V3 existed: round to nearest, 8-bit torque over [-8, 8] and a +// CRC nibble. Code = round((x - min) * (2^bits - 1) / (max - min)), worked per field: +// pos 0 -> 32767.5 -> 0x8000 vel 0 -> 2047.5 -> 0x800 kp 10 -> 81.9 -> 0x052 +// kd 0.8 -> 2375.1 -> 0x947 torque 0 -> 127.5 -> 0x80 torque -4 -> 63.75 -> 0x40 +// Byte 6 is kd[3:0]|t[7:4], byte 7 is t[3:0]|crc, crc = (byte0 ^ ... ^ byte6) & 0xF. +TEST(PiperParserV2, MitPackingIsUnchangedByV3) +{ + PiperParserV2 parser; + struct Case + { + double torque; + std::array expected; + }; + const Case cases[] = { + {0.0, {0x80, 0x00, 0x80, 0x00, 0x52, 0x94, 0x78, 0x0E}}, // crc 80^00^80^00^52^94^78 = BE + {-4.0, {0x80, 0x00, 0x80, 0x00, 0x52, 0x94, 0x74, 0x02}}, // crc ... ^74 = B2 + }; + for (int joint = 1; joint <= 6; ++joint) + { + for (const auto& c : cases) + { + PiperMessage msg; + msg.type = static_cast(static_cast(ArmMsgType::JointMitCtrl1) + joint - 1); + msg.arm_joint_mit_ctrl.motors[joint - 1] = parser.packJointMit(0.0, 0.0, 10.0, 0.8, c.torque); + can_frame tx{}; + ASSERT_TRUE(parser.encodeMessage(msg, tx)); + EXPECT_EQ(tx.can_id, 0x159u + joint); + EXPECT_TRUE(std::equal(c.expected.begin(), c.expected.end(), tx.data)) << "joint " << joint; + } + } + // V2 saturates torque at +-8, so 16 encodes as 8 would. + EXPECT_EQ(parser.packJointMit(0, 0, 10, 0.8, 16.0).t_ref, 0xFF); +} + +TEST(PiperParserV2, KeepsV2ModeNumbersAndRefusesV3OnlyValues) +{ + PiperParserV2 parser; + PiperMessage msg; + can_frame tx{}; + msg.type = ArmMsgType::MotionCtrl2; + msg.arm_motion_ctrl_2.move_mode = static_cast(piper_cpp::MoveMode::MoveM); + ASSERT_TRUE(parser.encodeMessage(msg, tx)); + EXPECT_EQ(tx.data[1], 0x04) << "V2 MOVE M stays 0x04"; + + msg.arm_motion_ctrl_2.installation_pos = static_cast(piper_cpp::InstallationPos::HorizontalInverted); + EXPECT_FALSE(parser.encodeMessage(msg, tx)) << "0x04 is an invalid installation position in V2"; + + msg.type = ArmMsgType::GripperCtrl; + msg.gripper_ctrl.mode = piper_cpp::GripperMode::Angle; + EXPECT_FALSE(parser.encodeMessage(msg, tx)) << "V2 would read an angle as a width"; + + // Byte 7 is not a mode under V2, whatever it holds. + ASSERT_TRUE(parser.decodeMessage(frame(0x2A8, {0, 0, 0x75, 0x30, 0, 0, 0, 0x01}), 0.0, msg)); + EXPECT_EQ(msg.gripper_feedback.mode, piper_cpp::GripperMode::Width); + + // Feedback bytes stay as received: V2 has no MOVE M at 0x06 and no IK feedback. + ASSERT_TRUE(parser.decodeMessage(frame(0x2A1, {0x01, 0x00, 0x06, 0, 0, 0, 0, 0}), 0.0, msg)); + EXPECT_EQ(static_cast(msg.arm_status_msgs.mode_feed), 0x06); + EXPECT_FALSE(parser.decodeMessage(frame(0x2AA, {0, 0, 0, 1, 0, 0, 0, 2}), 0.0, msg)); + EXPECT_EQ(msg.type, ArmMsgType::Unknown); +} diff --git a/piper_cpp/test/piper_parser_v3_test.cpp b/piper_cpp/test/piper_parser_v3_test.cpp new file mode 100644 index 0000000..169ce1f --- /dev/null +++ b/piper_cpp/test/piper_parser_v3_test.cpp @@ -0,0 +1,269 @@ +// Golden-frame tests for the V3 wire protocol. Expected bytes are worked out by hand from the +// V3 frame tables (assets/V3/CHANGELOG-V3.MD in piper_sdk), independently of both this +// library's encoder and the Python SDK, so a mistake shared by the two cannot hide here. + +#include "piper_cpp/protocol/piper_parser_v3.h" +#include "test_support.h" + +#include + +#include +#include + +using piper_cpp::ArmMsgType; +using piper_cpp::GripperMode; +using piper_cpp::PiperMessage; +using piper_cpp::PiperParserV2; +using piper_cpp::PiperParserV3; +using test_support::frame; +using test_support::jointPairFrame; +using ModeFeed = piper_cpp::ArmMsgFeedbackStatusEnum::ModeFeed; +using Bytes = std::array; + +namespace +{ + +Bytes payload(const can_frame& f) +{ + Bytes b{}; + std::copy(f.data, f.data + 8, b.begin()); + return b; +} + +can_frame encodeMit(PiperParserV3& parser, int joint, double pos, double vel, double kp, double kd, double torque) +{ + PiperMessage msg; + msg.type = static_cast(static_cast(ArmMsgType::JointMitCtrl1) + joint - 1); + msg.arm_joint_mit_ctrl.motors[joint - 1] = parser.packJointMit(pos, vel, kp, kd, torque); + can_frame tx{}; + EXPECT_TRUE(parser.encodeMessage(msg, tx)); + return tx; +} + +can_frame encodeMotionCtrl2(PiperParserV3& parser, uint8_t ctrl, uint8_t move, uint8_t spd, uint8_t install, bool& ok) +{ + PiperMessage msg; + msg.type = ArmMsgType::MotionCtrl2; + msg.arm_motion_ctrl_2 = ArmMsgMotionCtrl_2{ctrl, move, spd, 0x00, 0, install}; + can_frame tx{}; + ok = parser.encodeMessage(msg, tx); + return tx; +} + +} // namespace + +// Code = trunc((x - min) * (2^bits - 1) / (max - min)). Worked per field: +// pos 0 -> 12.5 * 65535 / 25 = 32767.5 -> 0x7FFF pos -1 -> 11.5 * 65535 / 25 = 30146.1 -> 0x75C2 +// vel 0 -> 45 * 4095 / 90 = 2047.5 -> 0x7FF vel 22.5 -> 67.5 * 4095 / 90 = 3071.25 -> 0xBFF +// kp 10 -> 10 * 4095 / 500 = 81.9 -> 0x051 kd 0.8 -> 5.8 * 4095 / 10 = 2375.1 -> 0x947 +// torque 0 -> 16 * 4095 / 32 = 2047.5 -> 0x7FF torque -8 -> 8 * 4095 / 32 = 1023.75 -> 0x3FF +// torque 1 -> 17 * 4095 / 32 = 2175.47 -> 0x87F +// Layout: pos[15:8] pos[7:0] vel[11:4] vel[3:0]|kp[11:8] kp[7:0] kd[11:4] kd[3:0]|t[11:8] t[7:0]. +TEST(PiperParserV3, MitPacksHandComputedCodesOnEveryJoint) +{ + PiperParserV3 parser; + struct Case + { + double pos, vel, kp, kd, torque; + Bytes expected; + }; + const Case cases[] = { + {0.0, 0.0, 10.0, 0.8, 0.0, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}, + {0.0, 0.0, 10.0, 0.8, -8.0, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x73, 0xFF}}, + {0.0, 0.0, 10.0, 0.8, 1.0, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x7F}}, + {-1.0, 22.5, 10.0, 0.8, 0.0, {0x75, 0xC2, 0xBF, 0xF0, 0x51, 0x94, 0x77, 0xFF}}, + {12.5, 45.0, 500.0, 5.0, 16.0, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}, + {-12.5, -45.0, 0.0, -5.0, -16.0, {0, 0, 0, 0, 0, 0, 0, 0} }, + // Out of range saturates at the ends instead of wrapping into the next field. + {13.0, 50.0, 600.0, 6.0, 17.0, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}, + {-13.0, -50.0, -1.0, -6.0, -17.0, {0, 0, 0, 0, 0, 0, 0, 0} }, + }; + for (int joint = 1; joint <= 6; ++joint) + { + for (const auto& c : cases) + { + const auto tx = encodeMit(parser, joint, c.pos, c.vel, c.kp, c.kd, c.torque); + EXPECT_EQ(tx.can_id, 0x159u + joint); + EXPECT_EQ(tx.can_dlc, 8); + EXPECT_EQ(payload(tx), c.expected) << "joint " << joint << " torque " << c.torque; + } + } +} + +TEST(PiperParserV3, MitTorqueUsesTwelveBitsAndNoCrc) +{ + PiperParserV3 parser; + PiperMessage msg; + msg.type = ArmMsgType::JointMitCtrl1; + msg.arm_joint_mit_ctrl.motors[0] = {0x1234, 0xABC, 0xDEF, 0x345, 0x678, 0}; + can_frame tx{}; + ASSERT_TRUE(parser.encodeMessage(msg, tx)); + EXPECT_EQ(payload(tx), (Bytes{0x12, 0x34, 0xAB, 0xCD, 0xEF, 0x34, 0x56, 0x78})); +} + +TEST(PiperParserV3, MotionCtrl2SendsV3ModeNumbers) +{ + PiperParserV3 parser; + bool ok = false; + // MOVE M is 0x04 as a MoveMode but 0x06 on the V3 wire; the other modes keep their numbers. + auto tx = encodeMotionCtrl2(parser, 0x01, 0x04, 100, 0x00, ok); + ASSERT_TRUE(ok); + EXPECT_EQ(tx.can_id, 0x151u); + EXPECT_EQ(payload(tx), (Bytes{0x01, 0x06, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00})); + for (uint8_t mode : {0x00, 0x01, 0x02, 0x03, 0x05}) + { + tx = encodeMotionCtrl2(parser, 0x01, mode, 50, 0x00, ok); + ASSERT_TRUE(ok); + EXPECT_EQ(tx.data[1], mode); + } + tx = encodeMotionCtrl2(parser, 0x01, 0x01, 50, 0x04, ok); + ASSERT_TRUE(ok) << "horizontal inverted is a V3 installation position"; + EXPECT_EQ(tx.data[5], 0x04); +} + +TEST(PiperParserV3, MotionCtrl2RejectsValuesV3DoesNotDefine) +{ + PiperParserV3 parser; + bool ok = true; + encodeMotionCtrl2(parser, 0x01, 0x06, 50, 0x00, ok); + EXPECT_FALSE(ok) << "0x06 is a wire byte, not a MoveMode"; + encodeMotionCtrl2(parser, 0x01, 0x01, 50, 0x05, ok); + EXPECT_FALSE(ok) << "installation position"; + encodeMotionCtrl2(parser, 0x02, 0x01, 50, 0x00, ok); + EXPECT_FALSE(ok) << "control mode"; + encodeMotionCtrl2(parser, 0x01, 0x01, 101, 0x00, ok); + EXPECT_FALSE(ok) << "speed percentage"; +} + +TEST(PiperParserV3, GripperCommandCarriesModeInStatusBit2) +{ + PiperParserV3 parser; + PiperMessage msg; + msg.type = ArmMsgType::GripperCtrl; + // -45 deg = -45000 = 0xFFFF5038; effort 1000 = 0x03E8; Enable (0x01) | angle bit (0x04). + msg.gripper_ctrl = ArmMsgGripperCtrl{-45000, 1000, 0x01, 0x00, GripperMode::Angle}; + can_frame tx{}; + ASSERT_TRUE(parser.encodeMessage(msg, tx)); + EXPECT_EQ(tx.can_id, 0x159u); + EXPECT_EQ(payload(tx), (Bytes{0xFF, 0xFF, 0x50, 0x38, 0x03, 0xE8, 0x05, 0x00})); + + msg.gripper_ctrl = ArmMsgGripperCtrl{30000, 1000, 0x03, 0x00, GripperMode::Width}; + ASSERT_TRUE(parser.encodeMessage(msg, tx)); + EXPECT_EQ(payload(tx), (Bytes{0x00, 0x00, 0x75, 0x30, 0x03, 0xE8, 0x03, 0x00})); + + msg.gripper_ctrl = ArmMsgGripperCtrl{0, 0, 0x04, 0x00, GripperMode::Width}; + EXPECT_FALSE(parser.encodeMessage(msg, tx)) << "the mode belongs in `mode`, not the status code"; + msg.gripper_ctrl = ArmMsgGripperCtrl{0, 0, 0x01, 0x00, GripperMode::Unknown}; + EXPECT_FALSE(parser.encodeMessage(msg, tx)); +} + +TEST(PiperParserV3, StatusModeFeedbackMapsToProtocolIndependentModes) +{ + PiperParserV3 parser; + PiperMessage msg; + auto modeFeed = [&](uint8_t wire) + { + EXPECT_TRUE(parser.decodeMessage(frame(0x2A1, {0x01, 0x00, wire, 0, 0, 0, 0, 0}), 0.0, msg)); + return msg.arm_status_msgs.mode_feed; + }; + EXPECT_EQ(modeFeed(0x06), ModeFeed::MOVE_M); + EXPECT_EQ(modeFeed(0x05), ModeFeed::MOVE_CPV); + EXPECT_EQ(modeFeed(0x00), ModeFeed::MOVE_P); + EXPECT_EQ(modeFeed(0x04), ModeFeed::UNKNOWN) << "0x04 is not MOVE M under V3"; + EXPECT_EQ(modeFeed(0x07), ModeFeed::UNKNOWN); +} + +TEST(PiperParserV3, MotionCtrl2EchoReadsBackAsMoveMode) +{ + PiperParserV3 parser; + PiperMessage msg; + ASSERT_TRUE(parser.decodeMessage(frame(0x151, {0x01, 0x06, 0x32, 0xAD, 0x00, 0x04, 0, 0}), 0.0, msg)); + EXPECT_EQ(msg.arm_motion_ctrl_2.move_mode, static_cast(piper_cpp::MoveMode::MoveM)); + EXPECT_EQ(msg.arm_motion_ctrl_2.installation_pos, 0x04); +} + +TEST(PiperParserV3, GripperFeedbackReportsMode) +{ + PiperParserV3 parser; + PiperMessage msg; + // 30000 (0x7530), effort -1000 (0xFC18), status 0xC1: enabled, homed, low voltage. + ASSERT_TRUE(parser.decodeMessage(frame(0x2A8, {0, 0, 0x75, 0x30, 0xFC, 0x18, 0xC1, 0x01}), 0.0, msg)); + EXPECT_EQ(msg.gripper_feedback.grippers_angle, 30000); + EXPECT_EQ(msg.gripper_feedback.grippers_effort, -1000); + EXPECT_TRUE(msg.gripper_feedback.foc_status.homing_status); + EXPECT_EQ(msg.gripper_feedback.mode, GripperMode::Angle); + + ASSERT_TRUE(parser.decodeMessage(frame(0x2A8, {0, 0, 0x75, 0x30, 0, 0, 0, 0x00}), 0.0, msg)); + EXPECT_EQ(msg.gripper_feedback.mode, GripperMode::Width); + ASSERT_TRUE(parser.decodeMessage(frame(0x2A8, {0, 0, 0x75, 0x30, 0, 0, 0, 0x02}), 0.0, msg)); + EXPECT_EQ(msg.gripper_feedback.mode, GripperMode::Unknown); + + // A seven-byte frame has no mode byte; the SDK reads it as width, and so does this parser. + msg.gripper_feedback.mode = GripperMode::Angle; + ASSERT_TRUE(parser.decodeMessage(frame(0x2A8, {0, 0, 0x75, 0x30, 0, 0, 0}), 0.0, msg)); + EXPECT_EQ(msg.gripper_feedback.mode, GripperMode::Width); +} + +TEST(PiperParserV3, GripperCommandEchoSplitsStatusAndMode) +{ + PiperParserV3 parser; + PiperMessage msg; + ASSERT_TRUE(parser.decodeMessage(frame(0x159, {0, 0, 0x30, 0x39, 0, 0, 0x07, 0x00}), 0.0, msg)); + EXPECT_EQ(msg.gripper_ctrl.status_code, 0x03); + EXPECT_EQ(msg.gripper_ctrl.mode, GripperMode::Angle); + + ASSERT_TRUE(parser.decodeMessage(frame(0x159, {0, 0, 0x30, 0x39, 0, 0, 0x08, 0x00}), 0.0, msg)); + EXPECT_EQ(msg.gripper_ctrl.mode, GripperMode::Unknown); + EXPECT_EQ(msg.gripper_ctrl.status_code, 0x08) << "an undefined status byte is kept as received"; +} + +TEST(PiperParserV3, IkJointFeedbackDecodesSignedPairs) +{ + PiperParserV3 parser; + PiperMessage msg; + ASSERT_TRUE(parser.decodeMessage(jointPairFrame(0x2AA, 10000, -20000), 1.5, msg)); + EXPECT_EQ(msg.type, ArmMsgType::IKJointFeedback12); + EXPECT_EQ(msg.arm_ik_joint_feedback.joint_1, 10000); + EXPECT_EQ(msg.arm_ik_joint_feedback.joint_2, -20000); + EXPECT_DOUBLE_EQ(msg.timestamp, 1.5); + ASSERT_TRUE(parser.decodeMessage(jointPairFrame(0x2AB, INT32_MIN, INT32_MAX), 0.0, msg)); + EXPECT_EQ(msg.arm_ik_joint_feedback.joint_3, INT32_MIN); + EXPECT_EQ(msg.arm_ik_joint_feedback.joint_4, INT32_MAX); + ASSERT_TRUE(parser.decodeMessage(jointPairFrame(0x2AC, 0, -1), 0.0, msg)); + EXPECT_EQ(msg.type, ArmMsgType::IKJointFeedback56); + EXPECT_EQ(msg.arm_ik_joint_feedback.joint_6, -1); +} + +TEST(PiperParserV3, RejectsMalformedIkFrames) +{ + PiperParserV3 parser; + PiperMessage msg; + auto shortFrame = jointPairFrame(0x2AA, 1, 2); + shortFrame.can_dlc = 7; + EXPECT_FALSE(parser.decodeMessage(shortFrame, 0.0, msg)); + auto extended = jointPairFrame(0x2AA, 1, 2); + extended.can_id |= CAN_EFF_FLAG; + EXPECT_FALSE(parser.decodeMessage(extended, 0.0, msg)); + EXPECT_EQ(msg.type, ArmMsgType::Unknown); +} + +TEST(PiperParserV3, UnchangedMessagesMatchV2) +{ + PiperParserV2 v2; + PiperParserV3 v3; + PiperMessage msg; + msg.type = ArmMsgType::JointCtrl34; + msg.arm_joint_ctrl = {1, 2, -30000, 40000, 5, 6}; + can_frame a{}, b{}; + ASSERT_TRUE(v2.encodeMessage(msg, a)); + ASSERT_TRUE(v3.encodeMessage(msg, b)); + EXPECT_EQ(a.can_id, b.can_id); + EXPECT_EQ(payload(a), payload(b)); + + PiperMessage fa, fb; + const auto feedback = frame(0x251, {0, 0x10, 0xFF, 0x9C, 0, 0, 0x01, 0x00}); + ASSERT_TRUE(v2.decodeMessage(feedback, 0.0, fa)); + ASSERT_TRUE(v3.decodeMessage(feedback, 0.0, fb)); + EXPECT_EQ(fa.high_spd_feedbacks[0].current, fb.high_spd_feedbacks[0].current); + EXPECT_EQ(fa.high_spd_feedbacks[0].pos, fb.high_spd_feedbacks[0].pos); +} diff --git a/piper_cpp/test/piper_sdk_reference_test.cpp b/piper_cpp/test/piper_sdk_reference_test.cpp new file mode 100644 index 0000000..2467bb9 --- /dev/null +++ b/piper_cpp/test/piper_sdk_reference_test.cpp @@ -0,0 +1,255 @@ +// Differential tests against the official Python piper_sdk. sdk_reference/sdk_reference_frames.h +// holds the frames the SDK (pinned commit, see kSdkCommit) sends for a set of public command +// calls and what it decodes from a set of feedback frames; these tests make the same calls +// through this library's public API and compare CAN ID, payload length and payload bytes. +// +// Not compared, on purpose: V2 MIT through the interface (V2 here rounds to nearest where the +// SDK truncates, and clamps where the SDK does not), and V2 0x151 values the SDK rejects but +// V2 here has always passed through. Both keep V2's existing wire behaviour. + +#include "piper_cpp/interface/piper_interface_v3.h" +#include "piper_cpp/protocol/piper_parser_v3.h" +#include "sdk_reference/sdk_reference_frames.h" +#include "test_support.h" + +#include + +#include +#include +#include + +using namespace piper_cpp; +using test_support::makeBus; +using test_support::receiveFrame; + +namespace +{ + +constexpr auto kQuiet = std::chrono::milliseconds(20); + +std::string describe(const sdk_reference::CommandCase& c) +{ + std::ostringstream oss; + oss << "V" << c.protocol << " " << c.api << "("; + for (double a : c.args) + oss << a << ","; + oss << ")"; + return oss.str(); +} + +/// The SDK's V3 move modes (0x151 arguments and 0x2A1 feedback) use V3 wire numbers; this +/// library uses protocol-independent modes, where MOVE M is 0x04 in both versions. +uint8_t modeFromSdk(int protocol, int64_t sdk_value) +{ + if (protocol == 3 && sdk_value == 0x06) + return static_cast(MoveMode::MoveM); + return static_cast(sdk_value); +} + +/// Make the call `c` names through the public API of `piper`. +bool call(PiperInterfaceV2& piper, const sdk_reference::CommandCase& c) +{ + const double* a = c.args; + auto u8 = [&](int i) { return static_cast(a[i]); }; + auto i16 = [&](int i) { return static_cast(a[i]); }; + auto i32 = [&](int i) { return static_cast(a[i]); }; + const std::string api = c.api; + + if (api == "controlJointMit") + return piper.controlJointMit(u8(0), a[1], a[2], a[3], a[4], a[5]); + if (api == "sendMotionCtrl2") + return piper.sendMotionCtrl2( + static_cast(u8(0)), static_cast(modeFromSdk(c.protocol, u8(1))), u8(2), + static_cast(u8(3)), u8(4), static_cast(u8(5)) + ); + if (api == "controlGripper") + return piper.controlGripper(i32(0), static_cast(a[1]), static_cast(u8(2))); + if (api == "setGripperZero") + return piper.setGripperZero(); + if (api == "controlGripperV3") + { + // The SDK packs the mode into the status code; here they are separate. + auto& v3 = dynamic_cast(piper); + const auto status = static_cast(u8(2) & 0x03); + return (u8(2) & 0x04) ? v3.controlGripperAngle(i32(0), static_cast(a[1]), status) + : v3.controlGripperWidth(i32(0), static_cast(a[1]), status); + } + if (api == "controlJoints") + return piper.controlJoints(i32(0), i32(1), i32(2), i32(3), i32(4), i32(5)); + if (api == "controlEndPose") + return piper.controlEndPose(i32(0), i32(1), i32(2), i32(3), i32(4), i32(5)); + if (api == "enableMotor") + return piper.enableMotor(static_cast(u8(0))); + if (api == "disableMotor") + return piper.disableMotor(static_cast(u8(0))); + if (api == "emergencyStop") + return piper.emergencyStop(static_cast(u8(0))); + if (api == "moveCAxisUpdate") + return piper.moveCAxisUpdate(static_cast(u8(0))); + if (api == "sendJointConfig") + return piper.sendJointConfig( + static_cast(u8(0)), static_cast(u8(1)), static_cast(u8(2)), + static_cast(a[3]), static_cast(u8(4)) + ); + if (api == "setMotorAngleLimitsMaxSpeed") + return piper.setMotorAngleLimitsMaxSpeed(static_cast(u8(0)), i16(1), i16(2), i16(3)); + if (api == "setEndVelAccLimits") + return piper.setEndVelAccLimits(i16(0), i16(1), i16(2), i16(3)); + if (api == "setCrashProtectionLevels") + return piper.setCrashProtectionLevels(u8(0), u8(1), u8(2), u8(3), u8(4), u8(5)); + if (api == "sendArmParamEnquiryAndConfig") + return piper.sendArmParamEnquiryAndConfig( + static_cast(u8(0)), static_cast(u8(1)), static_cast(u8(2)), + static_cast(u8(3)), static_cast(u8(4)) + ); + if (api == "setGripperTeachingPendantParam") + return piper.setGripperTeachingPendantParam(u8(0), u8(1), u8(2)); + if (api == "setMasterSlaveConfig") + return piper.setMasterSlaveConfig( + static_cast(u8(0)), static_cast(u8(1)), static_cast(u8(2)), + static_cast(u8(3)) + ); + ADD_FAILURE() << "no C++ call for " << c.api; + return false; +} + +void expectFrame(const can_frame& actual, const sdk_reference::Frame& expected, const std::string& what) +{ + EXPECT_EQ(actual.can_id, expected.can_id) << what; + ASSERT_EQ(actual.can_dlc, expected.dlc) << what; + for (int i = 0; i < expected.dlc; ++i) + EXPECT_EQ(int(actual.data[i]), int(expected.data[i])) << what << " byte " << i; +} + +} // namespace + +TEST(PiperSdkReference, PublicCommandsSendTheSdkFrames) +{ + for (const auto& c : sdk_reference::kCommandCases) + { + if (std::string(c.api) == "encodeJointMit") + continue; + const auto what = describe(c); + auto run = [&](auto bus) + { + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/false)); + const bool sent = call(*bus.piper, c); + if (c.rejected) + { + EXPECT_FALSE(sent) << what << ": the SDK rejects this call"; + EXPECT_FALSE(receiveFrame(bus.peer, kQuiet)) << what; + return; + } + EXPECT_TRUE(sent) << what; + for (int i = 0; i < c.num_frames; ++i) + { + const auto tx = receiveFrame(bus.peer); + ASSERT_TRUE(tx) << what << " frame " << i; + expectFrame(*tx, c.frames[i], what); + } + EXPECT_FALSE(receiveFrame(bus.peer, kQuiet)) << what << ": more frames than the SDK sends"; + }; + if (c.protocol == 3) + run(makeBus()); + else + run(makeBus()); + } +} + +TEST(PiperSdkReference, MitFieldsPackLikeTheSdk) +{ + for (const auto& c : sdk_reference::kCommandCases) + { + if (std::string(c.api) != "encodeJointMit") + continue; + std::unique_ptr parser; + if (c.protocol == 3) + parser = std::make_unique(); + else + parser = std::make_unique(); + const int joint = static_cast(c.args[0]); + PiperMessage msg; + msg.type = static_cast(static_cast(ArmMsgType::JointMitCtrl1) + joint - 1); + auto& mit = msg.arm_joint_mit_ctrl.motors[joint - 1]; + mit.pos_ref = static_cast(c.args[1]); + mit.vel_ref = static_cast(c.args[2]); + mit.kp = static_cast(c.args[3]); + mit.kd = static_cast(c.args[4]); + mit.t_ref = static_cast(c.args[5]); + can_frame tx{}; + ASSERT_TRUE(parser->encodeMessage(msg, tx)); + expectFrame(tx, c.frames[0], describe(c)); + } +} + +TEST(PiperSdkReference, FeedbackDecodesLikeTheSdk) +{ + PiperParserV2 v2; + PiperParserV3 v3; + for (const auto& c : sdk_reference::kDecodeCases) + { + can_frame f{}; + f.can_id = c.frame.can_id; + f.can_dlc = c.frame.dlc; + std::copy(c.frame.data, c.frame.data + 8, f.data); + PiperMessage msg; + const std::string kind = c.kind; + const std::string what = "V" + std::to_string(c.protocol) + " " + kind; + ASSERT_TRUE(c.protocol == 3 ? v3.decodeMessage(f, 0.0, msg) : v2.decodeMessage(f, 0.0, msg)) << what; + const int64_t* x = c.fields; + if (kind == "status") + { + const auto& s = msg.arm_status_msgs; + EXPECT_EQ(int(s.ctrl_mode), x[0]) << what; + EXPECT_EQ(int(s.arm_status), x[1]) << what; + EXPECT_EQ(int(s.mode_feed), modeFromSdk(c.protocol, x[2])) << what; + EXPECT_EQ(int(s.teach_status), x[3]) << what; + EXPECT_EQ(int(s.motion_status), x[4]) << what; + EXPECT_EQ(int(s.trajectory_num), x[5]) << what; + EXPECT_EQ(int(s.err_code), x[6]) << what; + } + else if (kind == "gripper") + { + const auto& g = msg.gripper_feedback; + EXPECT_EQ(g.grippers_angle, x[0]) << what; + EXPECT_EQ(g.grippers_effort, x[1]) << what; + EXPECT_EQ(g.status_code, x[2]) << what; + if (x[3] >= 0) // the V2 SDK reports no mode + { + EXPECT_EQ(int(g.mode), x[3]) << what; + } + } + else if (kind == "ik") + { + const auto& ik = msg.arm_ik_joint_feedback; + const int32_t* pair = f.can_id == 0x2AA ? &ik.joint_1 : f.can_id == 0x2AB ? &ik.joint_3 : &ik.joint_5; + EXPECT_EQ(pair[0], x[0]) << what; + EXPECT_EQ(pair[1], x[1]) << what; + } + else if (kind == "joint") + { + EXPECT_EQ(msg.arm_joint_feedback.joint_3, x[0]) << what; + EXPECT_EQ(msg.arm_joint_feedback.joint_4, x[1]) << what; + } + else if (kind == "high_spd") + { + const auto& h = msg.high_spd_feedbacks[2]; + EXPECT_EQ(h.motor_speed, x[0]) << what; + EXPECT_EQ(h.current, x[1]) << what; + EXPECT_EQ(h.pos, x[2]) << what; + } + else if (kind == "low_spd") + { + const auto& lo = msg.low_spd_feedbacks[4]; + EXPECT_EQ(lo.vol, x[0]) << what; + EXPECT_EQ(lo.foc_temp, x[1]) << what; + EXPECT_EQ(lo.motor_temp, x[2]) << what; + EXPECT_EQ(lo.foc_status_code, x[3]) << what; + EXPECT_EQ(lo.bus_current, x[4]) << what; + } + else + { + ADD_FAILURE() << "unknown kind " << kind; + } + } +} diff --git a/piper_cpp/test/sdk_reference/generate_sdk_reference.py b/piper_cpp/test/sdk_reference/generate_sdk_reference.py new file mode 100644 index 0000000..1d47688 --- /dev/null +++ b/piper_cpp/test/sdk_reference/generate_sdk_reference.py @@ -0,0 +1,323 @@ +#!/usr/bin/env python3 +"""Generate sdk_reference_frames.h from the official Python piper_sdk. + +Test tooling only: the library never runs Python, and the generated header is committed, so +the tests need neither this script nor network access. Regenerate only when moving the pin. + + git clone https://github.com/agilexrobotics/piper_sdk.git /tmp/piper_sdk + git -C /tmp/piper_sdk checkout b847e5393a7ce6ff8508d713eca7bdd3a42421b4 + python3 -m venv /tmp/sdk_venv && /tmp/sdk_venv/bin/pip install python-can + /tmp/sdk_venv/bin/python generate_sdk_reference.py --sdk /tmp/piper_sdk + +Commands go through the SDK's public interface methods with the CAN bus replaced by a +recorder, so the frames include its unit conversion, range handling and packing. A call the +SDK rejects with ValueError/TypeError is recorded as rejected. +""" + +import argparse +import logging +import math +import subprocess +import sys +from pathlib import Path + +PINNED_COMMIT = "b847e5393a7ce6ff8508d713eca7bdd3a42421b4" +OUTPUT = Path(__file__).with_name("sdk_reference_frames.h") + + +class RecordingCan: + """Stands in for the SDK's CAN wrapper and keeps every frame it is asked to send.""" + + class CAN_STATUS: + SEND_MESSAGE_SUCCESS = object() + + def __init__(self): + self.sent = [] + + def SendCanMessage(self, arbitration_id, data): + self.sent.append((arbitration_id, bytes(data))) + return self.CAN_STATUS.SEND_MESSAGE_SUCCESS + + +def mit_values(): + """Physical MIT inputs: zero, both ends, negatives, mid-range, clamping and code boundaries.""" + values = [ + (0.0, 0.0, 10.0, 0.8, 0.0), + (-12.5, -45.0, 0.0, -5.0, -16.0), + (12.5, 45.0, 500.0, 5.0, 16.0), + (-1.0, -3.0, 5.0, -0.5, -2.5), + (0.7, 1.3, 33.3, 1.1, 3.3), + (-6.25, 22.5, 250.0, 2.5, -8.0), + (-1e-9, -1e-9, 1e-9, -1e-9, -1e-9), + (13.0, 50.0, 600.0, 6.0, 17.0), + (-13.0, -50.0, -1.0, -6.0, -17.0), + ] + # Inputs exactly on a code boundary and one ulp below it, where truncation changes code. + for k in (1, 2047, 2048, 4094): + t = -16.0 + k * 32.0 / 4095 + values += [(0.0, 0.0, 10.0, 0.8, t), (0.0, 0.0, 10.0, 0.8, math.nextafter(t, -math.inf))] + for k in (1, 32767, 32768, 65534): + p = -12.5 + k * 25.0 / 65535 + values += [(p, 0.0, 10.0, 0.8, 0.0), (math.nextafter(p, -math.inf), 0.0, 10.0, 0.8, 0.0)] + return values + + +def command_cases(): + """(protocols, api, args, sdk_call). `api` names the C++ entry point the test calls.""" + both = (2, 3) + cases = [] + for motor in range(1, 7): + for v in mit_values(): + cases.append(((3,), "controlJointMit", (motor, *v), lambda p, *a: p.JointMitCtrl(*a))) + for bad in (math.nan, math.inf, -math.inf): + cases.append(((3,), "controlJointMit", (1, bad, 0.0, 10.0, 0.8, 0.0), lambda p, *a: p.JointMitCtrl(*a))) + cases.append(((3,), "controlJointMit", (6, 0.0, 0.0, 10.0, 0.8, bad), lambda p, *a: p.JointMitCtrl(*a))) + + # Raw packed fields straight into the parser: isolates the bit layout from the scaling. + for protocol, t_bits in ((2, 8), (3, 12)): + t_max = (1 << t_bits) - 1 + for motor, packed in ( + (1, (0x1234, 0xABC, 0xDEF, 0x345, 0x678 & t_max)), + (2, (0, 0, 0, 0, 0)), + (3, (0xFFFF, 0xFFF, 0xFFF, 0xFFF, t_max)), + (6, (0x8000, 0x800, 0x052, 0x947, (t_max + 1) // 2)), + ): + cases.append(((protocol,), "encodeJointMit", (motor, *packed), None)) + + # 0x151, args as the SDK takes them (wire move_mode). V2's own rejections are not compared: + # the C++ V2 encoder keeps its existing permissive behaviour. + for args in ((1, 1, 50, 0x00, 0, 0), (1, 4, 100, 0xAD, 0, 0), (1, 0, 30, 0x00, 0, 3), (7, 5, 10, 0x00, 255, 1), + (1, 2, 0, 0x00, 0, 2), (1, 1, 50, 0x00, 0, 4)): + cases.append(((2,), "sendMotionCtrl2", args, lambda p, *a: p.MotionCtrl_2(*a))) + for args in ((1, 6, 100, 0xAD, 0, 0), (1, 1, 50, 0x00, 0, 4), (1, 0, 30, 0x00, 0, 3), (7, 5, 10, 0x00, 255, 1), + (1, 2, 0, 0xFF, 0, 2), (1, 1, 50, 0x00, 0, 5), (2, 1, 50, 0x00, 0, 0)): + cases.append(((3,), "sendMotionCtrl2", args, lambda p, *a: p.MotionCtrl_2(*a))) + + for args in ((30000, 1000, 0x01), (0, 0, 0x00), (-5000, 5000, 0x03), (70000, 2000, 0x02)): + cases.append((both, "controlGripper", args, lambda p, *a: p.GripperCtrl(*a, 0x00))) + # The SDK rejects efforts above 5000 under both protocols; only V3 here does the same. + cases.append(((3,), "controlGripper", (1000, 5001, 0x01), lambda p, *a: p.GripperCtrl(*a, 0x00))) + cases.append((both, "setGripperZero", (), lambda p: p.GripperCtrl(0, 0, 0x00, 0xAE))) + for args in ((30000, 1000, 0x01), (25000, 500, 0x03), (-45000, 1000, 0x05), (90000, 5000, 0x07), + (0, 0, 0x04), (12345, 0, 0x06), (1000, 5001, 0x01), (1000, 5001, 0x05)): + cases.append(((3,), "controlGripperV3", args, lambda p, *a: p.GripperCtrl_V3(*a, 0x00))) + + unchanged = [ + ("controlJoints", (10000, -20000, 30000, -40000, 50000, -60000), lambda p, *a: p.JointCtrl(*a)), + ("controlEndPose", (150000, -20000, 300000, -179000, 85000, -90000), lambda p, *a: p.EndPoseCtrl(*a)), + ("enableMotor", (7,), lambda p, m: p.EnableArm(m)), + ("enableMotor", (3,), lambda p, m: p.EnableArm(m)), + ("disableMotor", (7,), lambda p, m: p.DisableArm(m)), + ("emergencyStop", (0x01,), lambda p, s: p.EmergencyStop(s)), + ("emergencyStop", (0x02,), lambda p, s: p.EmergencyStop(s)), + ("moveCAxisUpdate", (0x02,), lambda p, n: p.MoveCAxisUpdateCtrl(n)), + ("sendJointConfig", (3, 0xAE, 0xAE, 300, 0xAE), lambda p, *a: p.JointConfig(*a)), + ("setMotorAngleLimitsMaxSpeed", (2, 1500, -1500, 3000), lambda p, *a: p.MotorAngleLimitMaxSpdSet(*a)), + ("setEndVelAccLimits", (1000, 2000, 3000, 4000), lambda p, *a: p.EndSpdAndAccParamSet(*a)), + ("setCrashProtectionLevels", (1, 2, 3, 4, 5, 8), lambda p, *a: p.CrashProtectionConfig(*a)), + ("sendArmParamEnquiryAndConfig", (0x01, 0x00, 0x02, 0xAE, 0x01), lambda p, *a: p.ArmParamEnquiryAndConfig(*a)), + ("setGripperTeachingPendantParam", (150, 100, 5), lambda p, *a: p.GripperTeachingPendantParamConfig(*a)), + ("setMasterSlaveConfig", (0xFC, 0x00, 0x00, 0x00), lambda p, *a: p.MasterSlaveConfig(*a)), + ] + cases += [(both, api, args, call) for api, args, call in unchanged] + return cases + + +def encode_packed_mit(sdk, protocol, motor, pos, vel, kp, kd, t): + import can + + if protocol == 2: + msgs, parser = sdk["msg_v2"], sdk["parser_v2"] + mit = msgs.ArmMsgJointMitCtrl(pos, vel, kp, kd, t) + else: + msgs, parser = sdk["msg_v3"], sdk["parser_v3"] + mit = msgs.ArmMsgJointMitCtrl(pos, vel, kp, kd, t) + msg = msgs.PiperMessage(type_=getattr(msgs.ArmMsgType, f"PiperMsgJointMitCtrl_{motor}"), arm_joint_mit_ctrl=mit) + frame = can.Message(arbitration_id=0, data=[], is_extended_id=False) + assert parser.EncodeMessage(msg, frame) + return [(frame.arbitration_id, bytes(frame.data))] + + +def decode_cases(): + """(protocols, kind, can_id, data). Every frame is a full 8-byte frame.""" + both = (2, 3) + cases = [] + for mode in (0x00, 0x01, 0x02, 0x03, 0x05): + cases.append((both, "status", 0x2A1, [0x01, 0x00, mode, 0x00, 0x01, 0x02, 0x01, 0x02])) + for mode in (0x04, 0x06, 0x07, 0xFF): + cases.append(((3,), "status", 0x2A1, [0x01, 0x00, mode, 0x00, 0x01, 0x02, 0x01, 0x02])) + for mode in (0x00, 0x01, 0x02): + cases.append(((3,), "gripper", 0x2A8, [0x00, 0x00, 0x75, 0x30, 0xFC, 0x18, 0xC1, mode])) + cases.append((both, "gripper", 0x2A8, [0xFF, 0xFF, 0xEC, 0x78, 0x03, 0xE8, 0x40, 0x00])) + cases.append(((3,), "ik", 0x2AA, [0x00, 0x00, 0x27, 0x10, 0xFF, 0xFF, 0xB1, 0xE0])) + cases.append(((3,), "ik", 0x2AB, [0x80, 0x00, 0x00, 0x00, 0x7F, 0xFF, 0xFF, 0xFF])) + cases.append(((3,), "ik", 0x2AC, [0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF])) + cases.append((both, "joint", 0x2A6, [0x00, 0x00, 0x27, 0x10, 0xFF, 0xFF, 0xB1, 0xE0])) + cases.append((both, "high_spd", 0x253, [0xFF, 0x38, 0xFF, 0x9C, 0x00, 0x01, 0x86, 0xA0])) + cases.append((both, "low_spd", 0x265, [0x00, 0xF0, 0xFF, 0xF6, 0xE2, 0x41, 0x01, 0x2C])) + return cases + + +def decode(sdk, protocol, kind, can_id, data): + import can + + msgs = sdk["msg_v2"] if protocol == 2 else sdk["msg_v3"] + parser = sdk["parser_v2"] if protocol == 2 else sdk["parser_v3"] + msg = msgs.PiperMessage() + frame = can.Message(arbitration_id=can_id, data=data, is_extended_id=False) + assert parser.DecodeMessage(frame, msg) + if kind == "status": + s = msg.arm_status_msgs + return [int(s.ctrl_mode), int(s.arm_status), int(s.mode_feed), int(s.teach_status), int(s.motion_status), + s.trajectory_num, s.err_code] + if kind == "gripper": + g = msg.gripper_feedback + # The V2 SDK has no gripper mode; -1 tells the test there is nothing to compare. + mode = int(msg.gripper_feedback_v3.mode) if protocol == 3 else -1 + return [g.grippers_angle, g.grippers_effort, g.status_code, mode] + if kind == "ik": + ik = msg.arm_ik_joint_feedback + return {0x2AA: [ik.ik_joint_1, ik.ik_joint_2], 0x2AB: [ik.ik_joint_3, ik.ik_joint_4], + 0x2AC: [ik.ik_joint_5, ik.ik_joint_6]}[can_id] + if kind == "joint": + return [msg.arm_joint_feedback.joint_3, msg.arm_joint_feedback.joint_4] + if kind == "high_spd": + h = msg.arm_high_spd_feedback_3 + return [h.motor_speed, h.current, h.pos] + if kind == "low_spd": + lo = msg.arm_low_spd_feedback_5 + return [lo.vol, lo.foc_temp, lo.motor_temp, lo.foc_status_code, lo.bus_current] + raise ValueError(kind) + + +def cpp_double(x): + if math.isnan(x): + return "kNaN" + if math.isinf(x): + return "kInf" if x > 0 else "-kInf" + return repr(float(x)) # shortest form that reads back as the same double + + +def cpp_frames(frames): + out = [] + for can_id, data in frames: + out.append("{0x%03X, %d, {%s}}" % (can_id, len(data), ", ".join("0x%02X" % b for b in data))) + return "{" + ", ".join(out) + "}" + + +def load_sdk(sdk_path): + head = subprocess.run(["git", "-C", str(sdk_path), "rev-parse", "HEAD"], check=True, capture_output=True, + text=True).stdout.strip() + if head != PINNED_COMMIT: + sys.exit(f"piper_sdk checkout is at {head}, expected {PINNED_COMMIT}") + sys.path.insert(0, str(sdk_path)) + from piper_sdk.interface.piper_interface_v2 import C_PiperInterface_V2 + from piper_sdk.interface.piper_interface_v3 import C_PiperInterface_V3 + from piper_sdk.piper_msgs import msg_v2, msg_v3 + from piper_sdk.protocol.protocol_v2 import C_PiperParserV2 + from piper_sdk.protocol.protocol_v3 import C_PiperParserV3 + + # The SDK logs a warning for every clamped MIT input; the frames are what matter here. + logging.disable(logging.WARNING) + return { + "interface_v2": C_PiperInterface_V2("sdk-reference-v2", can_auto_init=False), + "interface_v3": C_PiperInterface_V3("sdk-reference-v3", can_auto_init=False), + "msg_v2": msg_v2, "msg_v3": msg_v3, + "parser_v2": C_PiperParserV2(), "parser_v3": C_PiperParserV3(), + } + + +def main(): + ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + ap.add_argument("--sdk", type=Path, required=True, help="piper_sdk checkout at the pinned commit") + sdk = load_sdk(ap.parse_args().sdk) + + command_rows = [] + for protocols, api, args, call in command_cases(): + for protocol in protocols: + if call is None: + frames = encode_packed_mit(sdk, protocol, *args) + else: + piper = sdk[f"interface_v{protocol}"] + piper._arm_can = RecordingCan() + try: + call(piper, *args) + frames = piper._arm_can.sent + except (ValueError, TypeError): + frames = None + rejected = frames is None + assert len(args) <= 6 and (rejected or 1 <= len(frames) <= 3) + padded = list(args) + [0] * (6 - len(args)) + command_rows.append( + " {%d, \"%s\", {%s}, %s, %d, %s}," + % (protocol, api, ", ".join(cpp_double(a) for a in padded), "true" if rejected else "false", + 0 if rejected else len(frames), cpp_frames([] if rejected else frames)) + ) + + decode_rows = [] + for protocols, kind, can_id, data in decode_cases(): + for protocol in protocols: + fields = decode(sdk, protocol, kind, can_id, data) + fields += [0] * (7 - len(fields)) + decode_rows.append( + " {%d, \"%s\", {0x%03X, 8, {%s}}, {%s}}," + % (protocol, kind, can_id, ", ".join("0x%02X" % b for b in data), ", ".join(str(f) for f in fields)) + ) + + OUTPUT.write_text(f"""// Generated by generate_sdk_reference.py from the official piper_sdk; do not edit. +// https://github.com/agilexrobotics/piper_sdk at commit {PINNED_COMMIT}. +#pragma once + +#include +#include + +namespace sdk_reference +{{ + +inline constexpr const char *kSdkCommit = "{PINNED_COMMIT}"; +inline constexpr double kNaN = std::numeric_limits::quiet_NaN(); +inline constexpr double kInf = std::numeric_limits::infinity(); + +struct Frame +{{ + uint32_t can_id; + uint8_t dlc; + uint8_t data[8]; +}}; + +/// One SDK command call: `api` names the C++ entry point, `args` its inputs as the SDK took +/// them (zero-padded). `rejected` means the SDK raised instead of sending. +struct CommandCase +{{ + int protocol; + const char *api; + double args[6]; + bool rejected; + int num_frames; + Frame frames[3]; +}}; + +/// One decoded feedback frame; the meaning of `fields` depends on `kind`. +struct DecodeCase +{{ + int protocol; + const char *kind; + Frame frame; + int64_t fields[7]; +}}; + +inline constexpr CommandCase kCommandCases[] = {{ +{chr(10).join(command_rows)} +}}; + +inline constexpr DecodeCase kDecodeCases[] = {{ +{chr(10).join(decode_rows)} +}}; + +}} // namespace sdk_reference +""") + print(f"wrote {OUTPUT}: {len(command_rows)} command cases, {len(decode_rows)} decode cases") + + +if __name__ == "__main__": + main() diff --git a/piper_cpp/test/sdk_reference/sdk_reference_frames.h b/piper_cpp/test/sdk_reference/sdk_reference_frames.h new file mode 100644 index 0000000..5bda91e --- /dev/null +++ b/piper_cpp/test/sdk_reference/sdk_reference_frames.h @@ -0,0 +1,953 @@ +// Generated by generate_sdk_reference.py from the official piper_sdk; do not edit. +// https://github.com/agilexrobotics/piper_sdk at commit b847e5393a7ce6ff8508d713eca7bdd3a42421b4. +#pragma once + +#include +#include + +namespace sdk_reference +{ + +inline constexpr const char *kSdkCommit = "b847e5393a7ce6ff8508d713eca7bdd3a42421b4"; +inline constexpr double kNaN = std::numeric_limits::quiet_NaN(); +inline constexpr double kInf = std::numeric_limits::infinity(); + +struct Frame +{ + uint32_t can_id; + uint8_t dlc; + uint8_t data[8]; +}; + +/// One SDK command call: `api` names the C++ entry point, `args` its inputs as the SDK took +/// them (zero-padded). `rejected` means the SDK raised instead of sending. +struct CommandCase +{ + int protocol; + const char *api; + double args[6]; + bool rejected; + int num_frames; + Frame frames[3]; +}; + +/// One decoded feedback frame; the meaning of `fields` depends on `kind`. +struct DecodeCase +{ + int protocol; + const char *kind; + Frame frame; + int64_t fields[7]; +}; + +inline constexpr CommandCase kCommandCases[] = { + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, -12.5, -45.0, 0.0, -5.0, -16.0}, + false, 1, + {{0x15A, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {1.0, 12.5, 45.0, 500.0, 5.0, 16.0}, + false, 1, + {{0x15A, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {1.0, -1.0, -3.0, 5.0, -0.5, -2.5}, + false, 1, + {{0x15A, 8, {0x75, 0xC2, 0x77, 0x70, 0x28, 0x73, 0x26, 0xBF}}} }, + {3, + "controlJointMit", {1.0, 0.7, 1.3, 33.3, 1.1, 3.3}, + false, 1, + {{0x15A, 8, {0x87, 0x2A, 0x83, 0xA1, 0x10, 0x9C, 0x19, 0xA5}}} }, + {3, + "controlJointMit", {1.0, -6.25, 22.5, 250.0, 2.5, -8.0}, + false, 1, + {{0x15A, 8, {0x3F, 0xFF, 0xBF, 0xF7, 0xFF, 0xBF, 0xF3, 0xFF}}} }, + {3, + "controlJointMit", {1.0, -1e-09, -1e-09, 1e-09, -1e-09, -1e-09}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x00, 0x7F, 0xF7, 0xFF}}} }, + {3, + "controlJointMit", {1.0, 13.0, 50.0, 600.0, 6.0, 17.0}, + false, 1, + {{0x15A, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {1.0, -13.0, -50.0, -1.0, -6.0, -17.0}, + false, 1, + {{0x15A, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185592}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185594}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203893}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203894}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, 0.003907203907203893}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, 0.0039072039072038925}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, 15.992185592185592}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {1.0, 0.0, 0.0, 10.0, 0.8, 15.99218559218559}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {1.0, -12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, -12.499618524452584, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, -0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, -0.00019073777370870462, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, 0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, 0.00019073777370870457, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, 12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0xFF, 0xFE, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {1.0, 12.49961852445258, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15A, 8, {0xFF, 0xFD, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, -12.5, -45.0, 0.0, -5.0, -16.0}, + false, 1, + {{0x15B, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {2.0, 12.5, 45.0, 500.0, 5.0, 16.0}, + false, 1, + {{0x15B, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {2.0, -1.0, -3.0, 5.0, -0.5, -2.5}, + false, 1, + {{0x15B, 8, {0x75, 0xC2, 0x77, 0x70, 0x28, 0x73, 0x26, 0xBF}}} }, + {3, + "controlJointMit", {2.0, 0.7, 1.3, 33.3, 1.1, 3.3}, + false, 1, + {{0x15B, 8, {0x87, 0x2A, 0x83, 0xA1, 0x10, 0x9C, 0x19, 0xA5}}} }, + {3, + "controlJointMit", {2.0, -6.25, 22.5, 250.0, 2.5, -8.0}, + false, 1, + {{0x15B, 8, {0x3F, 0xFF, 0xBF, 0xF7, 0xFF, 0xBF, 0xF3, 0xFF}}} }, + {3, + "controlJointMit", {2.0, -1e-09, -1e-09, 1e-09, -1e-09, -1e-09}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x00, 0x7F, 0xF7, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 13.0, 50.0, 600.0, 6.0, 17.0}, + false, 1, + {{0x15B, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {2.0, -13.0, -50.0, -1.0, -6.0, -17.0}, + false, 1, + {{0x15B, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185592}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185594}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203893}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203894}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, 0.003907203907203893}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, 0.0039072039072038925}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, 15.992185592185592}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {2.0, 0.0, 0.0, 10.0, 0.8, 15.99218559218559}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {2.0, -12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, -12.499618524452584, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, -0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, -0.00019073777370870462, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 0.00019073777370870457, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0xFF, 0xFE, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {2.0, 12.49961852445258, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15B, 8, {0xFF, 0xFD, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, -12.5, -45.0, 0.0, -5.0, -16.0}, + false, 1, + {{0x15C, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {3.0, 12.5, 45.0, 500.0, 5.0, 16.0}, + false, 1, + {{0x15C, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {3.0, -1.0, -3.0, 5.0, -0.5, -2.5}, + false, 1, + {{0x15C, 8, {0x75, 0xC2, 0x77, 0x70, 0x28, 0x73, 0x26, 0xBF}}} }, + {3, + "controlJointMit", {3.0, 0.7, 1.3, 33.3, 1.1, 3.3}, + false, 1, + {{0x15C, 8, {0x87, 0x2A, 0x83, 0xA1, 0x10, 0x9C, 0x19, 0xA5}}} }, + {3, + "controlJointMit", {3.0, -6.25, 22.5, 250.0, 2.5, -8.0}, + false, 1, + {{0x15C, 8, {0x3F, 0xFF, 0xBF, 0xF7, 0xFF, 0xBF, 0xF3, 0xFF}}} }, + {3, + "controlJointMit", {3.0, -1e-09, -1e-09, 1e-09, -1e-09, -1e-09}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x00, 0x7F, 0xF7, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 13.0, 50.0, 600.0, 6.0, 17.0}, + false, 1, + {{0x15C, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {3.0, -13.0, -50.0, -1.0, -6.0, -17.0}, + false, 1, + {{0x15C, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185592}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185594}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203893}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203894}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, 0.003907203907203893}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, 0.0039072039072038925}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, 15.992185592185592}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {3.0, 0.0, 0.0, 10.0, 0.8, 15.99218559218559}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {3.0, -12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, -12.499618524452584, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, -0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, -0.00019073777370870462, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 0.00019073777370870457, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0xFF, 0xFE, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {3.0, 12.49961852445258, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15C, 8, {0xFF, 0xFD, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, -12.5, -45.0, 0.0, -5.0, -16.0}, + false, 1, + {{0x15D, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {4.0, 12.5, 45.0, 500.0, 5.0, 16.0}, + false, 1, + {{0x15D, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {4.0, -1.0, -3.0, 5.0, -0.5, -2.5}, + false, 1, + {{0x15D, 8, {0x75, 0xC2, 0x77, 0x70, 0x28, 0x73, 0x26, 0xBF}}} }, + {3, + "controlJointMit", {4.0, 0.7, 1.3, 33.3, 1.1, 3.3}, + false, 1, + {{0x15D, 8, {0x87, 0x2A, 0x83, 0xA1, 0x10, 0x9C, 0x19, 0xA5}}} }, + {3, + "controlJointMit", {4.0, -6.25, 22.5, 250.0, 2.5, -8.0}, + false, 1, + {{0x15D, 8, {0x3F, 0xFF, 0xBF, 0xF7, 0xFF, 0xBF, 0xF3, 0xFF}}} }, + {3, + "controlJointMit", {4.0, -1e-09, -1e-09, 1e-09, -1e-09, -1e-09}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x00, 0x7F, 0xF7, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 13.0, 50.0, 600.0, 6.0, 17.0}, + false, 1, + {{0x15D, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {4.0, -13.0, -50.0, -1.0, -6.0, -17.0}, + false, 1, + {{0x15D, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185592}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185594}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203893}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203894}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, 0.003907203907203893}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, 0.0039072039072038925}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, 15.992185592185592}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {4.0, 0.0, 0.0, 10.0, 0.8, 15.99218559218559}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {4.0, -12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, -12.499618524452584, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, -0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, -0.00019073777370870462, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 0.00019073777370870457, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0xFF, 0xFE, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {4.0, 12.49961852445258, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15D, 8, {0xFF, 0xFD, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, -12.5, -45.0, 0.0, -5.0, -16.0}, + false, 1, + {{0x15E, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {5.0, 12.5, 45.0, 500.0, 5.0, 16.0}, + false, 1, + {{0x15E, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {5.0, -1.0, -3.0, 5.0, -0.5, -2.5}, + false, 1, + {{0x15E, 8, {0x75, 0xC2, 0x77, 0x70, 0x28, 0x73, 0x26, 0xBF}}} }, + {3, + "controlJointMit", {5.0, 0.7, 1.3, 33.3, 1.1, 3.3}, + false, 1, + {{0x15E, 8, {0x87, 0x2A, 0x83, 0xA1, 0x10, 0x9C, 0x19, 0xA5}}} }, + {3, + "controlJointMit", {5.0, -6.25, 22.5, 250.0, 2.5, -8.0}, + false, 1, + {{0x15E, 8, {0x3F, 0xFF, 0xBF, 0xF7, 0xFF, 0xBF, 0xF3, 0xFF}}} }, + {3, + "controlJointMit", {5.0, -1e-09, -1e-09, 1e-09, -1e-09, -1e-09}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x00, 0x7F, 0xF7, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 13.0, 50.0, 600.0, 6.0, 17.0}, + false, 1, + {{0x15E, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {5.0, -13.0, -50.0, -1.0, -6.0, -17.0}, + false, 1, + {{0x15E, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185592}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185594}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203893}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203894}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, 0.003907203907203893}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, 0.0039072039072038925}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, 15.992185592185592}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {5.0, 0.0, 0.0, 10.0, 0.8, 15.99218559218559}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {5.0, -12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, -12.499618524452584, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, -0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, -0.00019073777370870462, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 0.00019073777370870457, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0xFF, 0xFE, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {5.0, 12.49961852445258, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15E, 8, {0xFF, 0xFD, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, -12.5, -45.0, 0.0, -5.0, -16.0}, + false, 1, + {{0x15F, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {6.0, 12.5, 45.0, 500.0, 5.0, 16.0}, + false, 1, + {{0x15F, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {6.0, -1.0, -3.0, 5.0, -0.5, -2.5}, + false, 1, + {{0x15F, 8, {0x75, 0xC2, 0x77, 0x70, 0x28, 0x73, 0x26, 0xBF}}} }, + {3, + "controlJointMit", {6.0, 0.7, 1.3, 33.3, 1.1, 3.3}, + false, 1, + {{0x15F, 8, {0x87, 0x2A, 0x83, 0xA1, 0x10, 0x9C, 0x19, 0xA5}}} }, + {3, + "controlJointMit", {6.0, -6.25, 22.5, 250.0, 2.5, -8.0}, + false, 1, + {{0x15F, 8, {0x3F, 0xFF, 0xBF, 0xF7, 0xFF, 0xBF, 0xF3, 0xFF}}} }, + {3, + "controlJointMit", {6.0, -1e-09, -1e-09, 1e-09, -1e-09, -1e-09}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x00, 0x7F, 0xF7, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 13.0, 50.0, 600.0, 6.0, 17.0}, + false, 1, + {{0x15F, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "controlJointMit", {6.0, -13.0, -50.0, -1.0, -6.0, -17.0}, + false, 1, + {{0x15F, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185592}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, -15.992185592185594}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x70, 0x00}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203893}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, -0.003907203907203894}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, 0.003907203907203893}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, 0.0039072039072038925}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x78, 0x00}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, 15.992185592185592}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, 15.99218559218559}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x7F, 0xFE}}} }, + {3, + "controlJointMit", {6.0, -12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, -12.499618524452584, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0x00, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, -0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, -0.00019073777370870462, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0x7F, 0xFF, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 0.0001907377737087046, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 0.00019073777370870457, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0x80, 0x00, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 12.499618524452583, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0xFF, 0xFE, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, + "controlJointMit", {6.0, 12.49961852445258, 0.0, 10.0, 0.8, 0.0}, + false, 1, + {{0x15F, 8, {0xFF, 0xFD, 0x7F, 0xF0, 0x51, 0x94, 0x77, 0xFF}}} }, + {3, "controlJointMit", {1.0, kNaN, 0.0, 10.0, 0.8, 0.0}, true, 0, {}}, + {3, "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, kNaN}, true, 0, {}}, + {3, "controlJointMit", {1.0, kInf, 0.0, 10.0, 0.8, 0.0}, true, 0, {}}, + {3, "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, kInf}, true, 0, {}}, + {3, "controlJointMit", {1.0, -kInf, 0.0, 10.0, 0.8, 0.0}, true, 0, {}}, + {3, "controlJointMit", {6.0, 0.0, 0.0, 10.0, 0.8, -kInf}, true, 0, {}}, + {2, + "encodeJointMit", {1.0, 4660.0, 2748.0, 3567.0, 837.0, 120.0}, + false, 1, + {{0x15A, 8, {0x12, 0x34, 0xAB, 0xCD, 0xEF, 0x34, 0x57, 0x8C}}} }, + {2, + "encodeJointMit", {2.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x15B, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "encodeJointMit", {3.0, 65535.0, 4095.0, 4095.0, 4095.0, 255.0}, + false, 1, + {{0x15C, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {2, + "encodeJointMit", {6.0, 32768.0, 2048.0, 82.0, 2375.0, 128.0}, + false, 1, + {{0x15F, 8, {0x80, 0x00, 0x80, 0x00, 0x52, 0x94, 0x78, 0x0E}}} }, + {3, + "encodeJointMit", {1.0, 4660.0, 2748.0, 3567.0, 837.0, 1656.0}, + false, 1, + {{0x15A, 8, {0x12, 0x34, 0xAB, 0xCD, 0xEF, 0x34, 0x56, 0x78}}} }, + {3, + "encodeJointMit", {2.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x15B, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "encodeJointMit", {3.0, 65535.0, 4095.0, 4095.0, 4095.0, 4095.0}, + false, 1, + {{0x15C, 8, {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}}} }, + {3, + "encodeJointMit", {6.0, 32768.0, 2048.0, 82.0, 2375.0, 2048.0}, + false, 1, + {{0x15F, 8, {0x80, 0x00, 0x80, 0x00, 0x52, 0x94, 0x78, 0x00}}} }, + {2, + "sendMotionCtrl2", {1.0, 1.0, 50.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x151, 8, {0x01, 0x01, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "sendMotionCtrl2", {1.0, 4.0, 100.0, 173.0, 0.0, 0.0}, + false, 1, + {{0x151, 8, {0x01, 0x04, 0x64, 0xAD, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "sendMotionCtrl2", {1.0, 0.0, 30.0, 0.0, 0.0, 3.0}, + false, 1, + {{0x151, 8, {0x01, 0x00, 0x1E, 0x00, 0x00, 0x03, 0x00, 0x00}}} }, + {2, + "sendMotionCtrl2", {7.0, 5.0, 10.0, 0.0, 255.0, 1.0}, + false, 1, + {{0x151, 8, {0x07, 0x05, 0x0A, 0x00, 0xFF, 0x01, 0x00, 0x00}}} }, + {2, + "sendMotionCtrl2", {1.0, 2.0, 0.0, 0.0, 0.0, 2.0}, + false, 1, + {{0x151, 8, {0x01, 0x02, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00}}} }, + {2, "sendMotionCtrl2", {1.0, 1.0, 50.0, 0.0, 0.0, 4.0}, true, 0, {}}, + {3, + "sendMotionCtrl2", {1.0, 6.0, 100.0, 173.0, 0.0, 0.0}, + false, 1, + {{0x151, 8, {0x01, 0x06, 0x64, 0xAD, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "sendMotionCtrl2", {1.0, 1.0, 50.0, 0.0, 0.0, 4.0}, + false, 1, + {{0x151, 8, {0x01, 0x01, 0x32, 0x00, 0x00, 0x04, 0x00, 0x00}}} }, + {3, + "sendMotionCtrl2", {1.0, 0.0, 30.0, 0.0, 0.0, 3.0}, + false, 1, + {{0x151, 8, {0x01, 0x00, 0x1E, 0x00, 0x00, 0x03, 0x00, 0x00}}} }, + {3, + "sendMotionCtrl2", {7.0, 5.0, 10.0, 0.0, 255.0, 1.0}, + false, 1, + {{0x151, 8, {0x07, 0x05, 0x0A, 0x00, 0xFF, 0x01, 0x00, 0x00}}} }, + {3, + "sendMotionCtrl2", {1.0, 2.0, 0.0, 255.0, 0.0, 2.0}, + false, 1, + {{0x151, 8, {0x01, 0x02, 0x00, 0xFF, 0x00, 0x02, 0x00, 0x00}}} }, + {3, "sendMotionCtrl2", {1.0, 1.0, 50.0, 0.0, 0.0, 5.0}, true, 0, {}}, + {3, "sendMotionCtrl2", {2.0, 1.0, 50.0, 0.0, 0.0, 0.0}, true, 0, {}}, + {2, + "controlGripper", {30000.0, 1000.0, 1.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x75, 0x30, 0x03, 0xE8, 0x01, 0x00}}} }, + {3, + "controlGripper", {30000.0, 1000.0, 1.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x75, 0x30, 0x03, 0xE8, 0x01, 0x00}}} }, + {2, + "controlGripper", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "controlGripper", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "controlGripper", {-5000.0, 5000.0, 3.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0xFF, 0xFF, 0xEC, 0x78, 0x13, 0x88, 0x03, 0x00}}} }, + {3, + "controlGripper", {-5000.0, 5000.0, 3.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0xFF, 0xFF, 0xEC, 0x78, 0x13, 0x88, 0x03, 0x00}}} }, + {2, + "controlGripper", {70000.0, 2000.0, 2.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x01, 0x11, 0x70, 0x07, 0xD0, 0x02, 0x00}}} }, + {3, + "controlGripper", {70000.0, 2000.0, 2.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x01, 0x11, 0x70, 0x07, 0xD0, 0x02, 0x00}}} }, + {3, "controlGripper", {1000.0, 5001.0, 1.0, 0.0, 0.0, 0.0}, true, 0, {}}, + {2, + "setGripperZero", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAE}}} }, + {3, + "setGripperZero", {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xAE}}} }, + {3, + "controlGripperV3", {30000.0, 1000.0, 1.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x75, 0x30, 0x03, 0xE8, 0x01, 0x00}}} }, + {3, + "controlGripperV3", {25000.0, 500.0, 3.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x61, 0xA8, 0x01, 0xF4, 0x03, 0x00}}} }, + {3, + "controlGripperV3", {-45000.0, 1000.0, 5.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0xFF, 0xFF, 0x50, 0x38, 0x03, 0xE8, 0x05, 0x00}}} }, + {3, + "controlGripperV3", {90000.0, 5000.0, 7.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x01, 0x5F, 0x90, 0x13, 0x88, 0x07, 0x00}}} }, + {3, + "controlGripperV3", {0.0, 0.0, 4.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00}}} }, + {3, + "controlGripperV3", {12345.0, 0.0, 6.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x159, 8, {0x00, 0x00, 0x30, 0x39, 0x00, 0x00, 0x06, 0x00}}} }, + {3, "controlGripperV3", {1000.0, 5001.0, 1.0, 0.0, 0.0, 0.0}, true, 0, {}}, + {3, "controlGripperV3", {1000.0, 5001.0, 5.0, 0.0, 0.0, 0.0}, true, 0, {}}, + {2, + "controlJoints", {10000.0, -20000.0, 30000.0, -40000.0, 50000.0, -60000.0}, + false, 3, + {{0x155, 8, {0x00, 0x00, 0x27, 0x10, 0xFF, 0xFF, 0xB1, 0xE0}}, + {0x156, 8, {0x00, 0x00, 0x75, 0x30, 0xFF, 0xFF, 0x63, 0xC0}}, + {0x157, 8, {0x00, 0x00, 0xC3, 0x50, 0xFF, 0xFF, 0x15, 0xA0}}} }, + {3, + "controlJoints", {10000.0, -20000.0, 30000.0, -40000.0, 50000.0, -60000.0}, + false, 3, + {{0x155, 8, {0x00, 0x00, 0x27, 0x10, 0xFF, 0xFF, 0xB1, 0xE0}}, + {0x156, 8, {0x00, 0x00, 0x75, 0x30, 0xFF, 0xFF, 0x63, 0xC0}}, + {0x157, 8, {0x00, 0x00, 0xC3, 0x50, 0xFF, 0xFF, 0x15, 0xA0}}} }, + {2, + "controlEndPose", {150000.0, -20000.0, 300000.0, -179000.0, 85000.0, -90000.0}, + false, 3, + {{0x152, 8, {0x00, 0x02, 0x49, 0xF0, 0xFF, 0xFF, 0xB1, 0xE0}}, + {0x153, 8, {0x00, 0x04, 0x93, 0xE0, 0xFF, 0xFD, 0x44, 0xC8}}, + {0x154, 8, {0x00, 0x01, 0x4C, 0x08, 0xFF, 0xFE, 0xA0, 0x70}}} }, + {3, + "controlEndPose", {150000.0, -20000.0, 300000.0, -179000.0, 85000.0, -90000.0}, + false, 3, + {{0x152, 8, {0x00, 0x02, 0x49, 0xF0, 0xFF, 0xFF, 0xB1, 0xE0}}, + {0x153, 8, {0x00, 0x04, 0x93, 0xE0, 0xFF, 0xFD, 0x44, 0xC8}}, + {0x154, 8, {0x00, 0x01, 0x4C, 0x08, 0xFF, 0xFE, 0xA0, 0x70}}} }, + {2, + "enableMotor", {7.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x471, 8, {0x07, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "enableMotor", {7.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x471, 8, {0x07, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "enableMotor", {3.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x471, 8, {0x03, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "enableMotor", {3.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x471, 8, {0x03, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "disableMotor", {7.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x471, 8, {0x07, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "disableMotor", {7.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x471, 8, {0x07, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "emergencyStop", {1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x150, 8, {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "emergencyStop", {1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x150, 8, {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "emergencyStop", {2.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x150, 8, {0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "emergencyStop", {2.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x150, 8, {0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "moveCAxisUpdate", {2.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x158, 8, {0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "moveCAxisUpdate", {2.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x158, 8, {0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "sendJointConfig", {3.0, 174.0, 174.0, 300.0, 174.0, 0.0}, + false, 1, + {{0x475, 8, {0x03, 0xAE, 0xAE, 0x01, 0x2C, 0xAE, 0x00, 0x00}}} }, + {3, + "sendJointConfig", {3.0, 174.0, 174.0, 300.0, 174.0, 0.0}, + false, 1, + {{0x475, 8, {0x03, 0xAE, 0xAE, 0x01, 0x2C, 0xAE, 0x00, 0x00}}} }, + {2, + "setMotorAngleLimitsMaxSpeed", {2.0, 1500.0, -1500.0, 3000.0, 0.0, 0.0}, + false, 1, + {{0x474, 8, {0x02, 0x05, 0xDC, 0xFA, 0x24, 0x0B, 0xB8, 0x00}}} }, + {3, + "setMotorAngleLimitsMaxSpeed", {2.0, 1500.0, -1500.0, 3000.0, 0.0, 0.0}, + false, 1, + {{0x474, 8, {0x02, 0x05, 0xDC, 0xFA, 0x24, 0x0B, 0xB8, 0x00}}} }, + {2, + "setEndVelAccLimits", {1000.0, 2000.0, 3000.0, 4000.0, 0.0, 0.0}, + false, 1, + {{0x479, 8, {0x03, 0xE8, 0x07, 0xD0, 0x0B, 0xB8, 0x0F, 0xA0}}} }, + {3, + "setEndVelAccLimits", {1000.0, 2000.0, 3000.0, 4000.0, 0.0, 0.0}, + false, 1, + {{0x479, 8, {0x03, 0xE8, 0x07, 0xD0, 0x0B, 0xB8, 0x0F, 0xA0}}} }, + {2, + "setCrashProtectionLevels", {1.0, 2.0, 3.0, 4.0, 5.0, 8.0}, + false, 1, + {{0x47A, 8, {0x01, 0x02, 0x03, 0x04, 0x05, 0x08, 0x00, 0x00}}} }, + {3, + "setCrashProtectionLevels", {1.0, 2.0, 3.0, 4.0, 5.0, 8.0}, + false, 1, + {{0x47A, 8, {0x01, 0x02, 0x03, 0x04, 0x05, 0x08, 0x00, 0x00}}} }, + {2, + "sendArmParamEnquiryAndConfig", {1.0, 0.0, 2.0, 174.0, 1.0, 0.0}, + false, 1, + {{0x477, 8, {0x01, 0x00, 0x02, 0xAE, 0x01, 0x00, 0x00, 0x00}}} }, + {3, + "sendArmParamEnquiryAndConfig", {1.0, 0.0, 2.0, 174.0, 1.0, 0.0}, + false, 1, + {{0x477, 8, {0x01, 0x00, 0x02, 0xAE, 0x01, 0x00, 0x00, 0x00}}} }, + {2, + "setGripperTeachingPendantParam", {150.0, 100.0, 5.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x47D, 8, {0x96, 0x64, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "setGripperTeachingPendantParam", {150.0, 100.0, 5.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x47D, 8, {0x96, 0x64, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {2, + "setMasterSlaveConfig", {252.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x470, 8, {0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, + {3, + "setMasterSlaveConfig", {252.0, 0.0, 0.0, 0.0, 0.0, 0.0}, + false, 1, + {{0x470, 8, {0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}}} }, +}; + +inline constexpr DecodeCase kDecodeCases[] = { + {2, "status", {0x2A1, 8, {0x01, 0x00, 0x00, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 0, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x00, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 0, 0, 1, 2, 258} }, + {2, "status", {0x2A1, 8, {0x01, 0x00, 0x01, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 1, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x01, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 1, 0, 1, 2, 258} }, + {2, "status", {0x2A1, 8, {0x01, 0x00, 0x02, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 2, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x02, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 2, 0, 1, 2, 258} }, + {2, "status", {0x2A1, 8, {0x01, 0x00, 0x03, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 3, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x03, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 3, 0, 1, 2, 258} }, + {2, "status", {0x2A1, 8, {0x01, 0x00, 0x05, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 5, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x05, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 5, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x04, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 255, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x06, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 6, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0x07, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 255, 0, 1, 2, 258} }, + {3, "status", {0x2A1, 8, {0x01, 0x00, 0xFF, 0x00, 0x01, 0x02, 0x01, 0x02}}, {1, 0, 255, 0, 1, 2, 258} }, + {3, "gripper", {0x2A8, 8, {0x00, 0x00, 0x75, 0x30, 0xFC, 0x18, 0xC1, 0x00}}, {30000, -1000, 193, 0, 0, 0, 0} }, + {3, "gripper", {0x2A8, 8, {0x00, 0x00, 0x75, 0x30, 0xFC, 0x18, 0xC1, 0x01}}, {30000, -1000, 193, 1, 0, 0, 0} }, + {3, "gripper", {0x2A8, 8, {0x00, 0x00, 0x75, 0x30, 0xFC, 0x18, 0xC1, 0x02}}, {30000, -1000, 193, 255, 0, 0, 0} }, + {2, "gripper", {0x2A8, 8, {0xFF, 0xFF, 0xEC, 0x78, 0x03, 0xE8, 0x40, 0x00}}, {-5000, 1000, 64, -1, 0, 0, 0} }, + {3, "gripper", {0x2A8, 8, {0xFF, 0xFF, 0xEC, 0x78, 0x03, 0xE8, 0x40, 0x00}}, {-5000, 1000, 64, 0, 0, 0, 0} }, + {3, "ik", {0x2AA, 8, {0x00, 0x00, 0x27, 0x10, 0xFF, 0xFF, 0xB1, 0xE0}}, {10000, -20000, 0, 0, 0, 0, 0} }, + {3, "ik", {0x2AB, 8, {0x80, 0x00, 0x00, 0x00, 0x7F, 0xFF, 0xFF, 0xFF}}, {-2147483648, 2147483647, 0, 0, 0, 0, 0}}, + {3, "ik", {0x2AC, 8, {0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF}}, {0, -1, 0, 0, 0, 0, 0} }, + {2, "joint", {0x2A6, 8, {0x00, 0x00, 0x27, 0x10, 0xFF, 0xFF, 0xB1, 0xE0}}, {10000, -20000, 0, 0, 0, 0, 0} }, + {3, "joint", {0x2A6, 8, {0x00, 0x00, 0x27, 0x10, 0xFF, 0xFF, 0xB1, 0xE0}}, {10000, -20000, 0, 0, 0, 0, 0} }, + {2, "high_spd", {0x253, 8, {0xFF, 0x38, 0xFF, 0x9C, 0x00, 0x01, 0x86, 0xA0}}, {-200, -100, 100000, 0, 0, 0, 0} }, + {3, "high_spd", {0x253, 8, {0xFF, 0x38, 0xFF, 0x9C, 0x00, 0x01, 0x86, 0xA0}}, {-200, -100, 100000, 0, 0, 0, 0} }, + {2, "low_spd", {0x265, 8, {0x00, 0xF0, 0xFF, 0xF6, 0xE2, 0x41, 0x01, 0x2C}}, {240, -10, -30, 65, 300, 0, 0} }, + {3, "low_spd", {0x265, 8, {0x00, 0xF0, 0xFF, 0xF6, 0xE2, 0x41, 0x01, 0x2C}}, {240, -10, -30, 65, 300, 0, 0} }, +}; + +} // namespace sdk_reference diff --git a/piper_cpp/test/test_support.h b/piper_cpp/test/test_support.h new file mode 100644 index 0000000..3b7b610 --- /dev/null +++ b/piper_cpp/test/test_support.h @@ -0,0 +1,99 @@ +// Shared helpers for the parser and interface tests. The interface tests drive an interface +// through a socketpair in place of the CAN bus: frames written to the peer end reach the +// interface's read thread exactly as they would from a raw CAN socket, and frames the +// interface sends can be read back from the peer end. +#pragma once + +#include "piper_cpp/interface/piper_interface_v2.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace test_support +{ + +using namespace std::chrono_literals; +using std::chrono::steady_clock; + +template struct Bus +{ + int peer; + std::unique_ptr piper; // destroyed first, so the read thread is gone before peer closes + ~Bus() { ::close(peer); } +}; + +/// An interface of the given protocol class wired to the returned peer socket. `args` are the +/// constructor parameters after the transport (dh_offset, sdk_joint_limit, sdk_gripper_limit). +template Bus makeBus(Args... args) +{ + int sv[2]; + if (socketpair(AF_UNIX, SOCK_SEQPACKET, 0, sv) != 0) + throw std::runtime_error("socketpair failed"); + return {sv[1], std::make_unique(std::make_unique(sv[0]), args...)}; +} + +inline can_frame frame(uint32_t id, std::initializer_list bytes) +{ + can_frame f{}; + f.can_id = id; + f.can_dlc = static_cast(bytes.size()); + std::copy(bytes.begin(), bytes.end(), f.data); + return f; +} + +/// Joint-pair frames (feedback, command echo, IK) carry two big-endian int32 angles in 0.001 deg. +inline can_frame jointPairFrame(uint32_t id, int32_t a, int32_t b) +{ + can_frame f{}; + f.can_id = id; + f.can_dlc = 8; + const auto ua = static_cast(a); + const auto ub = static_cast(b); + for (int i = 0; i < 4; ++i) + { + f.data[i] = static_cast(ua >> (24 - 8 * i)); + f.data[4 + i] = static_cast(ub >> (24 - 8 * i)); + } + return f; +} + +template bool waitFor(Pred pred, std::chrono::milliseconds timeout = 2s) +{ + const auto deadline = steady_clock::now() + timeout; + while (!pred()) + { + if (steady_clock::now() > deadline) + return false; + std::this_thread::sleep_for(1ms); + } + return true; +} + +inline void sendFrame(int fd, const can_frame& f, steady_clock::time_point deadline) +{ + while (::send(fd, &f, sizeof(f), MSG_DONTWAIT) < 0 && errno == EAGAIN && steady_clock::now() < deadline) + std::this_thread::yield(); +} + +/// The next frame the interface sent, or nothing if none arrives within `timeout`. +inline std::optional receiveFrame(int fd, std::chrono::milliseconds timeout = 1s) +{ + pollfd pfd{fd, POLLIN, 0}; + can_frame f{}; + if (poll(&pfd, 1, static_cast(timeout.count())) <= 0 || + ::recv(fd, &f, sizeof(f), 0) != static_cast(sizeof(f))) + return std::nullopt; + return f; +} + +} // namespace test_support From d65fb6ec5f6a1aa144b1d3470934a45a3c12447f Mon Sep 17 00:00:00 2001 From: justagist Date: Mon, 2 Nov 2026 09:00:21 +0000 Subject: [PATCH 3/5] Select the Piper protocol in the ROS hardware interface and MoveIt launch --- piper_cpp_moveit/README.md | 1 + .../launch/piper_moveit.launch.py | 9 ++++ piper_cpp_ros/README.md | 7 ++++ .../piper_hardware_interface.hpp | 8 ++-- piper_cpp_ros/launch/piper_control.launch.py | 8 ++++ .../src/piper_hardware_interface.cpp | 42 ++++++++++++++++--- piper_cpp_ros/urdf/piper.ros2_control.xacro | 6 ++- ..._with_gripper_with_ros2_control.urdf.xacro | 2 + .../urdf/piper_with_ros2_control.urdf.xacro | 2 + 9 files changed, 76 insertions(+), 9 deletions(-) diff --git a/piper_cpp_moveit/README.md b/piper_cpp_moveit/README.md index e92f653..517c87e 100644 --- a/piper_cpp_moveit/README.md +++ b/piper_cpp_moveit/README.md @@ -87,6 +87,7 @@ they're what selects the URDF, SRDF, and controllers config. | `with_gripper` | `false` | URDF, SRDF, MoveIt controllers, `piper_cpp_ros` launch. | | `use_real_hardware` | `true` | URDF (`mock_components/GenericSystem` when `false`). | | `can_interface` | `can0` | `piper_cpp_ros` launch (real hardware only). | +| `protocol_version` | `2` | `piper_cpp_ros` launch (real hardware only). `3` for firmware V1.8-8+. | | `speed_pct` | `30` | `piper_cpp_ros` launch (real hardware only). | | `go_to_zero_on_activate` | `true` | `piper_cpp_ros` launch (real hardware only). | | `gripper_max_effort` | `1.0` | `piper_cpp_ros` launch (real hardware + with_gripper only). | diff --git a/piper_cpp_moveit/launch/piper_moveit.launch.py b/piper_cpp_moveit/launch/piper_moveit.launch.py index 526af42..bca1f36 100644 --- a/piper_cpp_moveit/launch/piper_moveit.launch.py +++ b/piper_cpp_moveit/launch/piper_moveit.launch.py @@ -32,6 +32,7 @@ def _build_actions(context, *args, **kwargs): use_real_hardware = LaunchConfiguration("use_real_hardware").perform(context) can_interface = LaunchConfiguration("can_interface").perform(context) + protocol_version = LaunchConfiguration("protocol_version").perform(context) speed_pct = LaunchConfiguration("speed_pct").perform(context) go_to_zero_on_activate = LaunchConfiguration("go_to_zero_on_activate").perform(context) with_gripper_str = LaunchConfiguration("with_gripper").perform(context) @@ -57,6 +58,7 @@ def _build_actions(context, *args, **kwargs): urdf_mappings = { "use_real_hardware": use_real_hardware, "can_interface": can_interface, + "protocol_version": protocol_version, "speed_pct": speed_pct, "go_to_zero_on_activate": go_to_zero_on_activate, "gripper_max_effort": gripper_max_effort, @@ -70,6 +72,7 @@ def _build_actions(context, *args, **kwargs): urdf_mappings = { "use_real_hardware": use_real_hardware, "can_interface": can_interface, + "protocol_version": protocol_version, "speed_pct": speed_pct, "go_to_zero_on_activate": go_to_zero_on_activate, } @@ -98,6 +101,7 @@ def _build_actions(context, *args, **kwargs): launch_arguments={ "use_real_hardware": use_real_hardware, "can_interface": can_interface, + "protocol_version": protocol_version, "speed_pct": speed_pct, "go_to_zero_on_activate": go_to_zero_on_activate, "with_gripper": with_gripper_str, @@ -143,6 +147,11 @@ def generate_launch_description(): "a connected Piper.", ), DeclareLaunchArgument("can_interface", default_value="can0"), + DeclareLaunchArgument( + "protocol_version", + default_value="2", + description="Piper CAN protocol: 2, or 3 for firmware V1.8-8 and newer.", + ), DeclareLaunchArgument("speed_pct", default_value="30"), DeclareLaunchArgument("go_to_zero_on_activate", default_value="true"), DeclareLaunchArgument( diff --git a/piper_cpp_ros/README.md b/piper_cpp_ros/README.md index cf2df58..92bc5a3 100644 --- a/piper_cpp_ros/README.md +++ b/piper_cpp_ros/README.md @@ -65,6 +65,7 @@ pose automatically without needing a driver. | Param | Default | Meaning | | -------------------------- | ------- | -------------------------------------------------------------------------------------------------------------------------- | | `can_interface` | `can0` | SocketCAN interface to open. | +| `protocol_version` | `2` | Piper CAN protocol, `2` or `3`. Use `3` only for firmware V1.8-8 or newer; anything else fails `on_init`. | | `speed_pct` | `30` | Speed-cap percentage forwarded to `MoveJ`. | | `go_to_zero_on_activate` | `true` | Drive arm joints to zero before handing over to the controller. | | `with_gripper` | `false` | Expose a 7th prismatic joint for the parallel-jaw gripper. | @@ -246,6 +247,12 @@ shows the pattern. a second, and `write()` returns an error if any command is not a usable position or a CAN send fails; nothing is sent when any part of the command set is invalid. The controller manager then stops the controllers and the arm holds its last target, as on deactivation. +- `protocol_version:=3` selects `piper_cpp`'s protocol V3 (firmware V1.8-8+). The interface + only uses joint position (MoveJ) and gripper width commands, whose frames are the same in + both protocols. The gripper joint models a jaw opening, so under V3 a gripper that reports + angle mode fails `on_activate`, makes `read()` return an error while active, and leaves the + gripper state unchanged while inactive. Send it a width-mode command first. V3 has not been + validated on a physical arm; see the `piper_cpp` README. - `update_rate` in [piper_controllers.yaml](config/piper_controllers.yaml) sets the read/write rate. 200 Hz is used, this is what the robot seem to expect; the CAN bus and arm firmware comfortably handle this and higher. diff --git a/piper_cpp_ros/include/piper_cpp_ros/piper_hardware_interface.hpp b/piper_cpp_ros/include/piper_cpp_ros/piper_hardware_interface.hpp index 425ad1f..aa46421 100644 --- a/piper_cpp_ros/include/piper_cpp_ros/piper_hardware_interface.hpp +++ b/piper_cpp_ros/include/piper_cpp_ros/piper_hardware_interface.hpp @@ -27,10 +27,11 @@ namespace piper_cpp_ros /// parallel-jaw gripper. /// /// Lifecycle: -/// - on_init : parse joint list + hardware params (`can_interface`, `speed_pct`, -/// `go_to_zero_on_activate`, `with_gripper`, `gripper_max_effort`, +/// - on_init : parse joint list + hardware params (`can_interface`, `protocol_version`, +/// `speed_pct`, `go_to_zero_on_activate`, `with_gripper`, `gripper_max_effort`, /// `home_gripper_on_activate`); allocate state/command buffers. -/// - on_configure : open the CAN port and start the piper_cpp read thread. +/// - on_configure : open the CAN port with the selected protocol (V2 by default, V3 for +/// firmware V1.8-8+) and start the piper_cpp read thread. /// - on_activate : enable joint motors, set motion mode (CanCmd/MoveJ/PositionVelocity), /// optionally drive all joints to zero, then seed the command buffer /// from the live feedback so the first write does not jump the arm. @@ -83,6 +84,7 @@ class PiperHardware : public hardware_interface::SystemInterface // Configurable via in the URDF's block. std::string can_interface_ = "can0"; + int protocol_version_ = 2; ///< 2 or 3; chosen explicitly, never detected. int speed_pct_ = 30; bool go_to_zero_on_activate_ = true; bool with_gripper_ = false; diff --git a/piper_cpp_ros/launch/piper_control.launch.py b/piper_cpp_ros/launch/piper_control.launch.py index df94684..907b2f3 100644 --- a/piper_cpp_ros/launch/piper_control.launch.py +++ b/piper_cpp_ros/launch/piper_control.launch.py @@ -36,6 +36,7 @@ def _build_actions(context, *args, **kwargs): controllers_file_arg = LaunchConfiguration("controllers_file").perform(context) use_real_hardware = LaunchConfiguration("use_real_hardware").perform(context) can_interface = LaunchConfiguration("can_interface").perform(context) + protocol_version = LaunchConfiguration("protocol_version").perform(context) speed_pct = LaunchConfiguration("speed_pct").perform(context) go_to_zero_on_activate = LaunchConfiguration("go_to_zero_on_activate").perform(context) with_gripper = LaunchConfiguration("with_gripper").perform(context).lower() in ( @@ -67,6 +68,8 @@ def _build_actions(context, *args, **kwargs): " ", f"can_interface:={can_interface}", " ", + f"protocol_version:={protocol_version}", + " ", f"speed_pct:={speed_pct}", " ", f"go_to_zero_on_activate:={go_to_zero_on_activate}", @@ -173,6 +176,11 @@ def generate_launch_description(): "the real CAN-driven PiperHardware.", ), DeclareLaunchArgument("can_interface", default_value="can0"), + DeclareLaunchArgument( + "protocol_version", + default_value="2", + description="Piper CAN protocol: 2, or 3 for firmware V1.8-8 and newer.", + ), DeclareLaunchArgument("speed_pct", default_value="30"), DeclareLaunchArgument("go_to_zero_on_activate", default_value="true"), DeclareLaunchArgument( diff --git a/piper_cpp_ros/src/piper_hardware_interface.cpp b/piper_cpp_ros/src/piper_hardware_interface.cpp index dc76c6e..4583e6f 100644 --- a/piper_cpp_ros/src/piper_hardware_interface.cpp +++ b/piper_cpp_ros/src/piper_hardware_interface.cpp @@ -11,7 +11,7 @@ #include "pluginlib/class_list_macros.hpp" #include "rclcpp/logging.hpp" -#include "piper_cpp/interface/piper_interface_v2.h" +#include "piper_cpp/interface/piper_interface_v3.h" #include "piper_cpp/types/control.h" #include "piper_cpp_ros/command_conversion.hpp" @@ -21,6 +21,11 @@ namespace piper_cpp_ros namespace { constexpr auto kLogger = "PiperHardware"; +// Under protocol V3 the gripper can be in angle mode, whose value this interface would misread +// as a jaw opening. +constexpr auto kGripperNotWidthMode = + "Gripper reports angle mode (or an unknown mode); this interface only drives width mode. Send " + "the gripper a width-mode command (e.g. piper_cpp's controlGripperWidth()) before activating."; } // namespace hardware_interface::CallbackReturn @@ -46,6 +51,15 @@ hardware_interface::CallbackReturn }; can_interface_ = get_param("can_interface", "can0"); + // The wrong protocol misreads MOVE M and MIT frames, so an unknown value is an error, not + // a fallback. + const std::string protocol = get_param("protocol_version", "2"); + if (protocol != "2" && protocol != "3") + { + RCLCPP_ERROR(rclcpp::get_logger(kLogger), "protocol_version must be 2 or 3, got '%s'", protocol.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + protocol_version_ = protocol == "3" ? 3 : 2; try { speed_pct_ = std::stoi(get_param("speed_pct", "30")); @@ -173,8 +187,10 @@ hardware_interface::CallbackReturn RCLCPP_INFO( rclcpp::get_logger(kLogger), - "Configured for can_interface='%s', speed_pct=%d, go_to_zero_on_activate=%s, with_gripper=%s", - can_interface_.c_str(), speed_pct_, go_to_zero_on_activate_ ? "true" : "false", with_gripper_ ? "true" : "false" + "Configured for can_interface='%s', protocol_version=%d, speed_pct=%d, go_to_zero_on_activate=%s, " + "with_gripper=%s", + can_interface_.c_str(), protocol_version_, speed_pct_, go_to_zero_on_activate_ ? "true" : "false", + with_gripper_ ? "true" : "false" ); if (with_gripper_) { @@ -189,7 +205,10 @@ hardware_interface::CallbackReturn hardware_interface::CallbackReturn PiperHardware::on_configure(const rclcpp_lifecycle::State&) { - piper_ = std::make_unique(can_interface_); + if (protocol_version_ == 3) + piper_ = std::make_unique(can_interface_); + else + piper_ = std::make_unique(can_interface_); if (!piper_->connectPort(/*can_init=*/true, /*piper_init=*/true, /*start_threads=*/true)) { RCLCPP_ERROR(rclcpp::get_logger(kLogger), "Failed to open CAN port '%s'", can_interface_.c_str()); @@ -319,6 +338,11 @@ hardware_interface::CallbackReturn PiperHardware::on_activate(const rclcpp_lifec gs = piper_->getGripperStates(); // refresh post-zero } + if (gs.value.mode != piper_cpp::GripperMode::Width) + { + RCLCPP_ERROR(rclcpp::get_logger(kLogger), "%s", kGripperNotWidthMode); + return hardware_interface::CallbackReturn::ERROR; + } hw_state_gripper_position_ = static_cast(gs.value.grippers_angle) * kUmToM; hw_state_gripper_effort_ = static_cast(gs.value.grippers_effort) * kMilliToUnit; hw_command_gripper_position_ = hw_state_gripper_position_; @@ -459,7 +483,15 @@ hardware_interface::return_type PiperHardware::read(const rclcpp::Time&, const r RCLCPP_ERROR(rclcpp::get_logger(kLogger), "Gripper feedback has gone stale."); return hardware_interface::return_type::ERROR; } - if (gs.is_valid) + // Like staleness, a non-width mode is only an error while active; inactive, the gripper + // state is just left as it was rather than reporting an angle as a jaw opening. + const bool width_mode = gs.value.mode == piper_cpp::GripperMode::Width; + if (active && gs.is_valid && !width_mode) + { + RCLCPP_ERROR(rclcpp::get_logger(kLogger), "%s", kGripperNotWidthMode); + return hardware_interface::return_type::ERROR; + } + if (gs.is_valid && width_mode) { hw_state_gripper_position_ = static_cast(gs.value.grippers_angle) * kUmToM; hw_state_gripper_effort_ = static_cast(gs.value.grippers_effort) * kMilliToUnit; diff --git a/piper_cpp_ros/urdf/piper.ros2_control.xacro b/piper_cpp_ros/urdf/piper.ros2_control.xacro index 1247b00..8e6a35c 100644 --- a/piper_cpp_ros/urdf/piper.ros2_control.xacro +++ b/piper_cpp_ros/urdf/piper.ros2_control.xacro @@ -15,10 +15,12 @@ simulate position-following with no hardware required. Useful for MoveIt planning + RViz development without a connected Piper. The hardware-only params below - (can_interface, speed_pct, go_to_zero_on_activate, + (can_interface, protocol_version, speed_pct, go_to_zero_on_activate, gripper_max_effort, home_gripper_on_activate) are ignored under mock. can_interface SocketCAN interface (default 'can0'). + protocol_version Piper CAN protocol, 2 (default) or 3. Use 3 only for + firmware V1.8-8 or newer. speed_pct Speed-cap percentage forwarded to MoveJ (default 30). go_to_zero_on_activate Drive joints to zero on activation (default true). with_gripper Expose a 7th prismatic joint for the parallel-jaw gripper @@ -36,6 +38,7 @@ params="name:=piper use_real_hardware:=true can_interface:=can0 + protocol_version:=2 speed_pct:=30 go_to_zero_on_activate:=true with_gripper:=false @@ -47,6 +50,7 @@ piper_cpp_ros/PiperHardware ${can_interface} + ${protocol_version} ${speed_pct} ${go_to_zero_on_activate} ${with_gripper} diff --git a/piper_cpp_ros/urdf/piper_with_gripper_with_ros2_control.urdf.xacro b/piper_cpp_ros/urdf/piper_with_gripper_with_ros2_control.urdf.xacro index ea0310d..e6ffe23 100644 --- a/piper_cpp_ros/urdf/piper_with_gripper_with_ros2_control.urdf.xacro +++ b/piper_cpp_ros/urdf/piper_with_gripper_with_ros2_control.urdf.xacro @@ -33,6 +33,7 @@ + @@ -41,6 +42,7 @@ + From ba21680f453d52eaa4642c1187fe61ce0093cfb4 Mon Sep 17 00:00:00 2001 From: justagist Date: Mon, 2 Nov 2026 09:00:21 +0000 Subject: [PATCH 4/5] List the supported Piper protocol versions --- README.md | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 8c3386a..d8fab61 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,27 @@ alone or, with `with_gripper:=true`, on the arm + gripper together. [![piper_cpp.gif](https://media.githubusercontent.com/media/justagist/_assets/refs/heads/main/piper_cpp/piper_cpp.gif)](https://media.githubusercontent.com/media/justagist/_assets/refs/heads/main/piper_cpp/piper_cpp.gif) +## Supported protocol versions + +The CAN protocol follows the official [`piper_sdk`](https://github.com/agilexrobotics/piper_sdk) +protocol definitions at commit +[`b847e539`](https://github.com/agilexrobotics/piper_sdk/tree/b847e5393a7ce6ff8508d713eca7bdd3a42421b4). +The protocol is chosen by the interface class you construct, never detected from the arm. + +| Protocol | Firmware (per `piper_sdk`) | Upstream definition | `piper_cpp` class | `piper_cpp_ros` | +| --- | --- | --- | --- | --- | +| V2 (default) | V1.5-2 up to, not including, V1.8-8 | [`INTERFACE_V2.MD`](https://github.com/agilexrobotics/piper_sdk/blob/b847e5393a7ce6ff8508d713eca7bdd3a42421b4/assets/V2/INTERFACE_V2.MD) | `PiperInterfaceV2` | `protocol_version:=2` | +| V3 | V1.8-8 and later | [`INTERFACE_V3.MD`](https://github.com/agilexrobotics/piper_sdk/blob/b847e5393a7ce6ff8508d713eca7bdd3a42421b4/assets/V3/INTERFACE_V3.MD), [`CHANGELOG-V3.MD`](https://github.com/agilexrobotics/piper_sdk/blob/b847e5393a7ce6ff8508d713eca7bdd3a42421b4/assets/V3/CHANGELOG-V3.MD) | `PiperInterfaceV3` | `protocol_version:=3` | + +`INTERFACE_V2.MD` itself only says "V1.5-2 and later"; the upper bound comes from the +`piper_sdk` README, which says to use the V3 interface from firmware V1.8-8. On that +firmware V2 misreads at least MOVE M and the MIT commands. + +Firmware older than V1.5-2 (the `piper_sdk` 0.0.x line) is not supported. Some V2 features +need newer firmware than V1.5-2 (for example MOVE CPV needs V1.8-1); the method docs say +where. V3 is tested against the Python SDK's encoder and decoder only, not on a physical arm. See +[Protocol V3](piper_cpp/README.md#protocol-v3) for what changes. + ## License -MIT -- see [LICENSE](LICENSE). \ No newline at end of file +MIT -- see [LICENSE](LICENSE). From 5b1e25537e5af0f2a6313363dd55d6fa03bf0ac0 Mon Sep 17 00:00:00 2001 From: justagist Date: Mon, 2 Nov 2026 09:21:58 +0000 Subject: [PATCH 5/5] Fix IK decode indexing, test bus teardown order and NaN angle range check --- piper_cpp/include/piper_cpp/piper_params.h | 2 +- piper_cpp/src/protocol/piper_parser_v3.cpp | 21 +++++++++++++++++---- piper_cpp/test/piper_sdk_reference_test.cpp | 7 ++++--- piper_cpp/test/test_support.h | 10 ++++++++-- 4 files changed, 30 insertions(+), 10 deletions(-) diff --git a/piper_cpp/include/piper_cpp/piper_params.h b/piper_cpp/include/piper_cpp/piper_params.h index de10543..b571f21 100644 --- a/piper_cpp/include/piper_cpp/piper_params.h +++ b/piper_cpp/include/piper_cpp/piper_params.h @@ -110,7 +110,7 @@ class PiperParamManager void setGripperAngleRangeDeg(double min_deg, double max_deg) { - if (max_deg < min_deg) + if (!(max_deg >= min_deg)) // also refuses NaN throw std::invalid_argument("max_val must be >= min_val"); std::unique_lock lock(mutex_); gripper_angle_range_deg_ = {min_deg, max_deg}; diff --git a/piper_cpp/src/protocol/piper_parser_v3.cpp b/piper_cpp/src/protocol/piper_parser_v3.cpp index 8301852..74b26d5 100644 --- a/piper_cpp/src/protocol/piper_parser_v3.cpp +++ b/piper_cpp/src/protocol/piper_parser_v3.cpp @@ -83,10 +83,23 @@ bool PiperParserV3::decodeMessage(const struct can_frame &rx_frame, double times if (!acceptFrame(rx_frame, timestamp, msg)) return false; auto &ik = msg.arm_ik_joint_feedback; - int32_t *pair[] = {&ik.joint_1, &ik.joint_3, &ik.joint_5}; - const size_t part = static_cast(msg.type) - static_cast(ArmMsgType::IKJointFeedback12); - pair[part][0] = bytesToInt32(&rx_frame.data[0]); - pair[part][1] = bytesToInt32(&rx_frame.data[4]); + const int32_t first = bytesToInt32(&rx_frame.data[0]); + const int32_t second = bytesToInt32(&rx_frame.data[4]); + if (msg.type == ArmMsgType::IKJointFeedback12) + { + ik.joint_1 = first; + ik.joint_2 = second; + } + else if (msg.type == ArmMsgType::IKJointFeedback34) + { + ik.joint_3 = first; + ik.joint_4 = second; + } + else + { + ik.joint_5 = first; + ik.joint_6 = second; + } return true; } default: diff --git a/piper_cpp/test/piper_sdk_reference_test.cpp b/piper_cpp/test/piper_sdk_reference_test.cpp index 2467bb9..55c58b3 100644 --- a/piper_cpp/test/piper_sdk_reference_test.cpp +++ b/piper_cpp/test/piper_sdk_reference_test.cpp @@ -222,9 +222,10 @@ TEST(PiperSdkReference, FeedbackDecodesLikeTheSdk) else if (kind == "ik") { const auto& ik = msg.arm_ik_joint_feedback; - const int32_t* pair = f.can_id == 0x2AA ? &ik.joint_1 : f.can_id == 0x2AB ? &ik.joint_3 : &ik.joint_5; - EXPECT_EQ(pair[0], x[0]) << what; - EXPECT_EQ(pair[1], x[1]) << what; + const int32_t first = f.can_id == 0x2AA ? ik.joint_1 : f.can_id == 0x2AB ? ik.joint_3 : ik.joint_5; + const int32_t second = f.can_id == 0x2AA ? ik.joint_2 : f.can_id == 0x2AB ? ik.joint_4 : ik.joint_6; + EXPECT_EQ(first, x[0]) << what; + EXPECT_EQ(second, x[1]) << what; } else if (kind == "joint") { diff --git a/piper_cpp/test/test_support.h b/piper_cpp/test/test_support.h index 3b7b610..8ca02f6 100644 --- a/piper_cpp/test/test_support.h +++ b/piper_cpp/test/test_support.h @@ -28,8 +28,14 @@ using std::chrono::steady_clock; template struct Bus { int peer; - std::unique_ptr piper; // destroyed first, so the read thread is gone before peer closes - ~Bus() { ::close(peer); } + std::unique_ptr piper; + // The interface and its read thread must be gone before the peer end closes, and members + // are only destroyed after this body runs. + ~Bus() + { + piper.reset(); + ::close(peer); + } }; /// An interface of the given protocol class wired to the returned peer socket. `args` are the