diff --git a/CMakeLists.txt b/CMakeLists.txt index 2e36622f6..abc44029e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -183,6 +183,7 @@ if(BUILD_PLUGINS) add_subdirectory(src/plugins/controller_synthetic_hands) add_subdirectory(src/plugins/generic_3axis_pedal) add_subdirectory(src/plugins/so101_leader) + add_subdirectory(src/plugins/rebot_devarm_leader) add_subdirectory(src/plugins/manus) add_subdirectory(src/plugins/haptikos) if(BUILD_PLUGIN_OAK_CAMERA) diff --git a/src/plugins/rebot_devarm_leader/CMakeLists.txt b/src/plugins/rebot_devarm_leader/CMakeLists.txt new file mode 100644 index 000000000..16ae76127 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/CMakeLists.txt @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(rebot_devarm_leader_plugin + main.cpp + rebot_devarm_leader_plugin.cpp + damiao_bus.cpp + robstride_bus.cpp +) + +target_link_libraries(rebot_devarm_leader_plugin PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema +) + +install(TARGETS rebot_devarm_leader_plugin RUNTIME DESTINATION plugins/rebot_devarm_leader) +install(FILES plugin.yaml README.md DESTINATION plugins/rebot_devarm_leader) diff --git a/src/plugins/rebot_devarm_leader/README.md b/src/plugins/rebot_devarm_leader/README.md new file mode 100644 index 000000000..533c8d4c8 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/README.md @@ -0,0 +1,132 @@ + + +# reBot DevArm Leader Arm plugin + +Streams the [Seeed reBot DevArm](https://github.com/Seeed-Projects/reBot-DevArm) (6-DOF arm + +gripper) leader joint angles as a `JointStateOutput` FlatBuffer over the OpenXR tensor transport, +using the generic **joint-space device** path +(`JointStateTracker` / `JointStateSource` / `JointStateRetargeter`). + +Two motor builds of the arm are supported, selected by the shape of the device argument — a +serial path (`/dev/ttyACM0`) picks the **Damiao** backend, a bare SocketCAN interface name +(`can0`) picks the **RobStride** backend: + +- **Damiao build**: 7 DM-series MIT-protocol motors — DM4340P on joints 1–3, DM4310 on joints + 4–6 and the gripper — on a CAN bus behind a Damiao USB-to-CAN serial adapter (USB CDC-ACM, the + `dm-serial` transport also used by the vendor's DM_Control stack and the + [reBotArm_control_py](https://github.com/Seeed-Projects/reBotArm_control_py) reference code). + `DamiaoBus` (`damiao_bus.{hpp,cpp}`) speaks the adapter's fixed-size binary framing directly — + no SDK dependency — and implements just what a *leader* needs: send the **disable** control + frame so the arm can be back-driven by hand (Damiao motors keep answering feedback requests + while disabled), then request one feedback frame per motor per cycle (command `0xCC` addressed + via CAN id `0x7FF`) and decode position/velocity from the replies. Damiao feedback is + fixed-point over the model's `[-p_max, p_max]` / `[-v_max, v_max]` limits and lands directly + in radians — no tick conversion, only an optional per-joint sign and zero offset from a + calibration file. + +- **RobStride build**: 7 RS-series motors on classic CAN at 1 Mbps behind any SocketCAN + interface (e.g. a PCAN/candleLight USB adapter as `can0`). `RobStrideBus` + (`robstride_bus.{hpp,cpp}`) implements the leader subset of the RobStride private 29-bit + extended-id protocol directly — no SDK dependency: send the **stop** frame (comm type `0x04`) + for back-drive, then alternate single-parameter reads (comm type `0x11`) of `mechPos` + (`0x7019`) and `mechVel` (`0x701A`) per motor per cycle. Replies carry exact little-endian + IEEE f32 radians / rad/s, so the decode is **model-independent** — no fixed-point limit + tables — and each channel refreshes at 45 Hz with the plugin's 90 Hz loop, well under 20 % of + the 1 Mbps bus for 7 motors. + +When no device is given, the plugin falls back to a **synthetic** trajectory so the +device → tracker → retargeter pipeline runs with no hardware (CI and headless bring-up). + +## Run + +```bash +# Synthetic backend (no hardware): +./install/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin + +# Real reBot DevArm on the Damiao USB-to-CAN adapter (Linux), default collection id +# "rebot_devarm_leader": +./install/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin /dev/ttyACM0 + +# Real reBot DevArm (RobStride build) on a SocketCAN interface (Linux): +sudo ip link set can0 up type can bitrate 1000000 # once, after plugging the CAN adapter +./install/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin can0 + +# ... with a custom collection id and a calibration file: +./install/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin /dev/ttyACM0 rebot_devarm_leader rebot_devarm.calib +``` + +Args are positional: `[device_path] [collection_id] [calibration_file]`. A `device_path` +containing `/` is a serial port (Damiao); a bare name is a SocketCAN interface (RobStride). The +serial backend is Linux/macOS only (POSIX `termios`; the adapter enumerates as a CDC-ACM device, +nominal 921600 bps); the SocketCAN backend is Linux only. + +## Probe the hardware (no OpenXR runtime) + +The `probe` subcommand opens the bus, sends disable (back-drive mode), and streams decoded joint +positions to stdout — use it to verify wiring, motor ids, and the decode path before running the +full plugin: + +```bash +./install/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin probe /dev/ttyACM0 # Damiao build +./install/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin probe can0 # RobStride build +# probe: joint1=0.0012 joint2=-0.0034 ... gripper=0.0001 +``` + +Exit code 0 means every motor replied at least once. Exit code 3 means every motor replied but +the gripper reads outside its physical travel: the Damiao multi-turn counter is volatile across +power cycles, so the gripper (whose geared travel exceeds one turn) can wake up reading +`physical + 2*pi*k`. Re-home it (close against the mechanical stop and re-zero) before +teleoperating — a wrapped reading would slam the follower's gripper into its soft-limit clip on +the first frame. While wrapped, the running plugin streams the gripper joint with +`valid = false` so consumers can hold it instead of executing garbage. + +## Calibration file + +Plain text, one joint per line (`#` comments allowed): + +``` +# name motor_id feedback_id model sign offset_rad +joint1 1 17 4340P 1 0.0 +joint2 2 18 4340P 1 0.0 +joint3 3 19 4340P 1 0.0 +joint4 4 20 4310 1 0.0 +joint5 5 21 4310 1 0.0 +joint6 6 22 4310 1 0.0 +gripper 7 23 4310 1 0.0 +``` + +- `motor_id` / `feedback_id`: the motor's command (ESC) and feedback (MST) CAN ids, decimal. + Factory reBot DevArm ids are `1..7` / `0x11..0x17` (17..23) — the defaults, so a calibration + file is only needed for a sign flip, a zero offset, or a re-flashed id layout. On the + RobStride backend only `motor_id` (the RobStride device id, factory `1..7`) is used; replies + are matched by device id, so the `feedback_id` column is ignored. +- `model`: Damiao model name (`4310`, `4310P`, `4340`, `4340P`) selecting the feedback + fixed-point limits. RobStride motors report exact f32 values, so `rs-*` names (e.g. `rs-00`) + are accepted and ignored. +- `sign`: `-1` for any joint that moves opposite the URDF convention. +- `offset_rad`: feedback position at the joint's URDF zero pose. With the vendor's zeroing + procedure (motors zeroed at the reference pose) this stays `0.0`. + +## Joint order and names + +The DOF order matches the `reBot-DevArm_fixend` URDF: `joint1 … joint6`, then `gripper` (the 7th +Damiao motor). Downstream consumers read joints **by name** from the `JointStateOutput`, so wire +order does not matter. + +## Consume in Python + +```python +from isaacteleop.retargeting_engine.deviceio_source_nodes import JointStateSource + +source = JointStateSource( + name="leader", + collection_id="rebot_devarm_leader", + joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "gripper"], +) +``` + +Pair it with a `JointStateRetargeter` for joint-space or EE-space teleoperation, exactly like the +SO-101 leader (`src/plugins/so101_leader`). diff --git a/src/plugins/rebot_devarm_leader/damiao_bus.cpp b/src/plugins/rebot_devarm_leader/damiao_bus.cpp new file mode 100644 index 000000000..d8d195c5c --- /dev/null +++ b/src/plugins/rebot_devarm_leader/damiao_bus.cpp @@ -0,0 +1,307 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "damiao_bus.hpp" + +#include +#include + +#ifndef _WIN32 + +# include + +# include +# include +# include +# include +# include + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +namespace +{ + +// Adapter framing constants (see the class docs in damiao_bus.hpp). +constexpr int kTxFrameLen = 30; +constexpr int kRxFrameLen = 16; +constexpr uint8_t kTxHeader0 = 0x55; +constexpr uint8_t kTxHeader1 = 0xAA; +constexpr uint8_t kTxLength = 0x1E; +constexpr uint8_t kTxCmdCanForward = 0x03; // non-feedback CAN forwarding +constexpr uint8_t kRxHeader = 0xAA; +constexpr uint8_t kRxCmd = 0x11; +constexpr uint8_t kRxTrailer = 0x55; + +// Damiao motor protocol: register/control commands are addressed via arbitration id 0x7FF with +// the target motor id in the first two payload bytes. +constexpr uint32_t kRegisterArbitrationId = 0x7FF; +constexpr uint8_t kCmdRequestFeedback = 0xCC; + +// Map a numeric baud rate to the matching termios speed constant. The Damiao USB-CDC adapter +// enumerates as a CDC-ACM device (the rate is nominal), but set it anyway; anything unsupported +// throws rather than silently mis-configuring. +speed_t to_speed(int baud) +{ + switch (baud) + { +# ifdef B921600 + case 921600: + return B921600; +# endif +# ifdef B1000000 + case 1000000: + return B1000000; +# endif +# ifdef B460800 + case 460800: + return B460800; +# endif + case 115200: + return B115200; + default: + throw std::runtime_error("DamiaoBus: unsupported baud rate " + std::to_string(baud) + + " (the dm-serial adapter default is 921600)"); + } +} + +} // namespace + +DamiaoBus::DamiaoBus(const std::string& port, int baud) +{ + fd_ = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd_ < 0) + { + throw std::runtime_error("DamiaoBus: cannot open '" + port + "': " + std::strerror(errno)); + } + + termios tty{}; + if (::tcgetattr(fd_, &tty) != 0) + { + const std::string msg = std::strerror(errno); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("DamiaoBus: tcgetattr failed on '" + port + "': " + msg); + } + + ::cfmakeraw(&tty); + const speed_t spd = to_speed(baud); + ::cfsetispeed(&tty, spd); + ::cfsetospeed(&tty, spd); + + // 8N1, local, receiver enabled, no flow control. select()-driven reads (VMIN/VTIME = 0). + tty.c_cflag |= (CLOCAL | CREAD); + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; +# ifdef CRTSCTS + tty.c_cflag &= ~CRTSCTS; +# endif + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 0; + + if (::tcsetattr(fd_, TCSANOW, &tty) != 0) + { + const std::string msg = std::strerror(errno); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("DamiaoBus: tcsetattr failed on '" + port + "': " + msg); + } + + ::tcflush(fd_, TCIOFLUSH); +} + +DamiaoBus::~DamiaoBus() +{ + if (fd_ >= 0) + { + ::close(fd_); + } +} + +bool DamiaoBus::send_frame(uint32_t arbitration_id, const uint8_t data[8], uint8_t dlc) +{ + if (dlc > 8) + { + return false; + } + + uint8_t pkt[kTxFrameLen] = { 0 }; + pkt[0] = kTxHeader0; + pkt[1] = kTxHeader1; + pkt[2] = kTxLength; + pkt[3] = kTxCmdCanForward; + pkt[4] = 1; // sendTimes = 1 (u32 LE) + pkt[8] = 10; // timeInterval = 10 (u32 LE) + pkt[12] = 0; // idType: standard 11-bit + pkt[13] = static_cast(arbitration_id & 0xFF); + pkt[14] = static_cast((arbitration_id >> 8) & 0xFF); + pkt[15] = static_cast((arbitration_id >> 16) & 0xFF); + pkt[16] = static_cast((arbitration_id >> 24) & 0xFF); + pkt[17] = 0; // frameType: data frame + pkt[18] = dlc; + pkt[19] = 0; // idAcc + pkt[20] = 0; // dataAcc + for (int i = 0; i < dlc; ++i) + { + pkt[21 + i] = data[i]; + } + pkt[29] = 0; // crc (ignored by the adapter, matching the reference implementation) + + return ::write(fd_, pkt, sizeof(pkt)) == static_cast(sizeof(pkt)); +} + +bool DamiaoBus::request_feedback(uint16_t motor_id) +{ + const uint8_t data[8] = { + static_cast(motor_id & 0xFF), static_cast(motor_id >> 8), kCmdRequestFeedback, 0, 0, 0, 0, 0 + }; + return send_frame(kRegisterArbitrationId, data); +} + +bool DamiaoBus::disable(uint16_t motor_id) +{ + const uint8_t data[8] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD }; + return send_frame(motor_id, data); +} + +bool DamiaoBus::fill_rx_buffer(int timeout_ms) +{ + if (rx_len_ >= static_cast(sizeof(rx_buf_))) + { + // Should not happen (parse_frame always consumes); resync defensively. + rx_len_ = 0; + } + + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(fd_, &readfds); + timeval tv{}; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + if (::select(fd_ + 1, &readfds, nullptr, nullptr, &tv) <= 0) + { + return false; + } + + const ssize_t n = ::read(fd_, rx_buf_ + rx_len_, sizeof(rx_buf_) - static_cast(rx_len_)); + if (n <= 0) + { + return false; + } + rx_len_ += static_cast(n); + return true; +} + +bool DamiaoBus::parse_frame(CanFrame& out) +{ + int start = 0; + while (start + kRxFrameLen <= rx_len_) + { + const uint8_t* raw = rx_buf_ + start; + if (raw[0] != kRxHeader || raw[1] != kRxCmd || raw[15] != kRxTrailer) + { + ++start; // resync: skip one byte and retry + continue; + } + + const uint8_t flags = raw[2]; + const uint8_t dlc = flags & 0x3F; + const bool is_rtr = (flags & 0x80) != 0; + out.arbitration_id = static_cast(raw[3]) | (static_cast(raw[4]) << 8) | + (static_cast(raw[5]) << 16) | (static_cast(raw[6]) << 24); + for (int i = 0; i < 8; ++i) + { + out.data[i] = raw[7 + i]; + } + out.dlc = dlc > 8 ? 8 : dlc; + + // Consume this frame (and any skipped garbage before it). + const int consumed = start + kRxFrameLen; + std::memmove(rx_buf_, rx_buf_ + consumed, static_cast(rx_len_ - consumed)); + rx_len_ -= consumed; + + if (is_rtr) + { + start = 0; + continue; // remote frames carry no data; keep scanning + } + return true; + } + + // No complete frame: drop leading garbage so the buffer cannot fill with junk. + if (start > 0) + { + std::memmove(rx_buf_, rx_buf_ + start, static_cast(rx_len_ - start)); + rx_len_ -= start; + } + return false; +} + +bool DamiaoBus::read_frame(CanFrame& out, int timeout_ms) +{ + if (parse_frame(out)) + { + return true; + } + if (!fill_rx_buffer(timeout_ms)) + { + return false; + } + return parse_frame(out); +} + +} // namespace rebot_devarm_leader +} // namespace plugins + +#else // _WIN32 + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +DamiaoBus::DamiaoBus(const std::string& port, int /*baud*/) +{ + throw std::runtime_error("DamiaoBus: the serial backend is POSIX-only (cannot open '" + port + "' on Windows)"); +} + +DamiaoBus::~DamiaoBus() = default; + +bool DamiaoBus::send_frame(uint32_t, const uint8_t*, uint8_t) +{ + return false; +} + +bool DamiaoBus::request_feedback(uint16_t) +{ + return false; +} + +bool DamiaoBus::disable(uint16_t) +{ + return false; +} + +bool DamiaoBus::read_frame(CanFrame&, int) +{ + return false; +} + +bool DamiaoBus::fill_rx_buffer(int) +{ + return false; +} + +bool DamiaoBus::parse_frame(CanFrame&) +{ + return false; +} + +} // namespace rebot_devarm_leader +} // namespace plugins + +#endif // _WIN32 diff --git a/src/plugins/rebot_devarm_leader/damiao_bus.hpp b/src/plugins/rebot_devarm_leader/damiao_bus.hpp new file mode 100644 index 000000000..d2398c917 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/damiao_bus.hpp @@ -0,0 +1,83 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +//! One CAN frame as carried over the Damiao USB-CDC serial adapter (classic CAN, DLC <= 8). +struct CanFrame +{ + uint32_t arbitration_id = 0; + uint8_t data[8] = { 0 }; + uint8_t dlc = 0; +}; + +/*! + * @brief Minimal serial client for the Damiao USB-to-CAN adapter (dm-serial) driving DM-series + * MIT-protocol motors (e.g. the DM4340P/DM4310 in the Seeed reBot DevArm). + * + * Implements the adapter's USB-CDC framing and the subset of the Damiao motor CAN protocol a + * *leader* arm needs: + * - request a feedback frame (command ``0xCC`` addressed via arbitration id ``0x7FF``), and + * - disable torque (control frame ``FF .. FF FD``) so the arm can be back-driven by hand. + * + * Wire format (adapter, both directions are fixed-size binary frames): + * - TX (30 bytes): ``55 AA 1E 03 | sendTimes(u32 LE)=1 | timeInterval(u32 LE)=10 | idType(u8) + * | canId(u32 LE) | frameType(u8)=0 | dlc(u8) | idAcc(u8)=0 | dataAcc(u8)=0 | data[8] | crc(u8)=0`` + * - RX (16 bytes): ``AA 11 | flags(u8: dlc & 0x3F, ext 0x40, rtr 0x80) | canId(u32 LE) | data[8] | 55`` + * + * Feedback frames arrive on the motor's **MST (feedback) id** with the payload + * ``[status<<4 | canId_low, pos_hi, pos_lo, vel_hi, vel_lo<<4 | torq_hi, torq_lo, t_mos, t_rotor]`` + * where position is 16-bit and velocity/torque are 12-bit fixed-point over the model's limits. + * + * POSIX only (Linux/macOS); constructing on Windows throws. + */ +class DamiaoBus +{ +public: + //! Open and configure @p port (e.g. ``/dev/ttyACM0``) at @p baud. Throws ``std::runtime_error`` + //! on failure (or always, on Windows). + explicit DamiaoBus(const std::string& port, int baud = 921600); + ~DamiaoBus(); + + DamiaoBus(const DamiaoBus&) = delete; + DamiaoBus& operator=(const DamiaoBus&) = delete; + DamiaoBus(DamiaoBus&&) = delete; + DamiaoBus& operator=(DamiaoBus&&) = delete; + + //! Send one classic CAN frame through the adapter. Returns false on a short/failed write. + bool send_frame(uint32_t arbitration_id, const uint8_t data[8], uint8_t dlc = 8); + + //! Ask motor @p motor_id for one feedback frame (Damiao command ``0xCC`` via id ``0x7FF``). + //! The reply arrives asynchronously on the motor's MST id; collect it with read_frame(). + bool request_feedback(uint16_t motor_id); + + //! Send the Damiao disable control frame (``FF .. FF FD``) to @p motor_id so the joint goes + //! limp and can be moved by hand. Safe to send when the motor is already disabled. + bool disable(uint16_t motor_id); + + //! Read the next CAN frame from the adapter into @p out, waiting up to @p timeout_ms. + //! Returns false on timeout (no frame available). + bool read_frame(CanFrame& out, int timeout_ms); + +private: + //! Pull whatever bytes the adapter has ready into rx_buf_ (select()-bounded by @p timeout_ms). + bool fill_rx_buffer(int timeout_ms); + //! Try to parse one 16-byte adapter frame from rx_buf_; consumes garbage bytes on resync. + bool parse_frame(CanFrame& out); + + int fd_ = -1; + // Reassembly buffer for the fixed 16-byte RX frames (USB CDC reads can split them). + uint8_t rx_buf_[512]; + int rx_len_ = 0; +}; + +} // namespace rebot_devarm_leader +} // namespace plugins diff --git a/src/plugins/rebot_devarm_leader/main.cpp b/src/plugins/rebot_devarm_leader/main.cpp new file mode 100644 index 000000000..f762436c6 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/main.cpp @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "rebot_devarm_leader_plugin.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace plugins::rebot_devarm_leader; + +int main(int argc, char** argv) +try +{ + // Probe mode: rebot_devarm_leader_plugin probe [calibration_file] [seconds] + // is a serial path (/dev/ttyACM0, Damiao build) or a SocketCAN interface + // name (can0, RobStride build). Streams decoded joint positions to stdout to verify the + // bus / ids / decoding. No OpenXR runtime required. + if (argc > 1 && std::string(argv[1]) == "probe") + { + const std::string device_path = (argc > 2) ? argv[2] : ""; + const std::string calibration_path = (argc > 3) ? argv[3] : ""; + const int seconds = (argc > 4) ? std::atoi(argv[4]) : 3; + return run_probe(device_path, calibration_path, seconds); + } + + // Usage: rebot_devarm_leader_plugin [device_path] [collection_id] [calibration_file] + // device_path: serial path (Damiao), SocketCAN interface name (RobStride), or empty for + // the synthetic backend (no hardware required). + const std::string device_path = (argc > 1) ? argv[1] : ""; + const std::string collection_id = (argc > 2) ? argv[2] : "rebot_devarm_leader"; + const std::string calibration_path = (argc > 3) ? argv[3] : ""; + + std::cout << "reBot DevArm Leader (device: " << (device_path.empty() ? "" : device_path) + << ", collection: " << collection_id + << (calibration_path.empty() ? "" : ", calibration: " + calibration_path) << ")" << std::endl; + + RebotDevarmLeaderPlugin plugin(device_path, collection_id, calibration_path); + + // Push joint state at 90 Hz. + const auto frame_duration = std::chrono::nanoseconds(1000000000 / 90); + const auto program_start = std::chrono::steady_clock::now(); + std::size_t frame_count = 0; + + while (true) + { + plugin.update(); + frame_count++; + std::this_thread::sleep_until(program_start + frame_duration * frame_count); + } + + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error" << std::endl; + return 1; +} diff --git a/src/plugins/rebot_devarm_leader/plugin.yaml b/src/plugins/rebot_devarm_leader/plugin.yaml new file mode 100644 index 000000000..abb447b0b --- /dev/null +++ b/src/plugins/rebot_devarm_leader/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: rebot_devarm_leader +description: "Seeed reBot DevArm (6-DOF + gripper) leader arm joint angles as JointStateOutput" +command: "./rebot_devarm_leader_plugin" +version: "1.0.0" +devices: + - path: "/arm/rebot_devarm_leader" + type: "joint_state" + description: "reBot DevArm leader arm (7 Damiao DM-series motors over a USB-to-CAN serial adapter, or 7 RobStride RS-series motors over SocketCAN; synthetic fallback when no device path)" diff --git a/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.cpp b/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.cpp new file mode 100644 index 000000000..1da10a937 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.cpp @@ -0,0 +1,635 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "rebot_devarm_leader_plugin.hpp" + +#include "damiao_bus.hpp" +#include "robstride_bus.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +namespace +{ + +// Must agree with JointStateTracker::DEFAULT_MAX_FLATBUFFER_SIZE on the consumer side; sizes the +// fixed tensor buffer (7 named joints + velocity fit comfortably). +constexpr size_t kMaxFlatbufferSize = 4096; + +// reBot DevArm DOF order (matches the reBot-DevArm_fixend URDF joint names; the gripper is the +// extra 7th Damiao motor). +constexpr std::array kJointNames = { "joint1", "joint2", "joint3", "joint4", + "joint5", "joint6", "gripper" }; + +// Damiao model feedback limits (pmax [rad], vmax [rad/s]) -- the 16-bit position / 12-bit +// velocity fixed-point in a feedback frame spans [-limit, limit]. Values match the vendor's +// DM_Control tables (and motorbridge's damiao catalog). +struct ModelLimits +{ + const char* model; + double p_max; + double v_max; +}; +constexpr std::array kModelLimits = { + { { "4310", 12.5, 30.0 }, { "4310P", 12.5, 50.0 }, { "4340", 12.5, 10.0 }, { "4340P", 12.5, 10.0 } } +}; + +// Factory reBot DevArm layout: DM4340P on joints 1-3, DM4310 on joints 4-6 and the gripper. +constexpr std::array kDefaultModels = { "4340P", "4340P", "4340P", "4310", + "4310", "4310", "4310" }; + +// Default CAN ids (factory-flashed): command ids 0x01..0x07, MST feedback ids 0x11..0x17. +constexpr uint16_t kDefaultMotorIdBase = 0x01; +constexpr uint16_t kDefaultFeedbackIdBase = 0x11; + +// Feedback velocity/torque are 12-bit; position is 16-bit. +constexpr int kPositionBits = 16; +constexpr int kVelocityBits = 12; + +// Per-cycle reply collection window. One 0xCC round for 7 motors completes in a few ms on the +// 921600-baud CDC link; 5 ms keeps the 90 Hz frame budget comfortable. +constexpr int kCollectTimeoutMs = 5; + +constexpr double kSynthAmplitude = 0.6; // [rad] arm-joint motion amplitude for the synthetic signal +constexpr double kSynthPeriodFrames = 90.0; // one cycle per ~1 s at 90 Hz + +// Physical gripper travel in the calibrated frame (fully closed = 0, opening negative, +// ~6.8 rad of multi-turn geared travel) plus margin. The Damiao multi-turn counter is +// volatile across power cycles: the single-turn absolute zero survives but the turn count +// does not, so a gripper whose travel exceeds one turn can wake up reading +// physical + 2*pi*k (verified on a B601-DM: physically closed gripper read +6.227 rad +// = -0.056 + 2*pi). A reading outside this window is a wrapped encoder, not a pose — +// streaming it as-is would slam the follower's gripper into a soft-limit clip at t=0. +constexpr double kGripperTravelMinRad = -7.5; +constexpr double kGripperTravelMaxRad = 0.7; +constexpr int kGripperJointIndex = kNumJoints - 1; + +//! Damiao fixed-point decode: an unsigned @p bits -bit integer spanning [-limit, limit]. +double uint_to_float(uint32_t value, double limit, int bits) +{ + const double span = 2.0 * limit; + return static_cast(value) * span / static_cast((1u << bits) - 1) - limit; +} + +//! A device path with a '/' is a serial port (Damiao dm-serial adapter); a bare name is a +//! SocketCAN interface (RobStride backend). Empty selects the synthetic backend. +bool is_robstride_path(const std::string& device_path) +{ + return !device_path.empty() && device_path.find('/') == std::string::npos; +} + +//! Look up a Damiao model's feedback limits; falls back to the DM4310 values with a warning. +ModelLimits model_limits(const std::string& model) +{ + for (const auto& entry : kModelLimits) + { + if (model == entry.model) + { + return entry; + } + } + std::cerr << "RebotDevarmLeaderPlugin: warning: unknown Damiao model '" << model << "'; assuming 4310 limits" + << std::endl; + return kModelLimits[0]; +} + +} // namespace + +RebotDevarmLeaderPlugin::RebotDevarmLeaderPlugin(const std::string& device_path, + const std::string& collection_id, + const std::string& calibration_path) + : device_path_(device_path), + collection_id_(collection_id), + session_(std::make_shared( + "RebotDevarmLeaderPlugin", core::SchemaPusher::get_required_extensions())), + pusher_(session_->get_handles(), + core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = "joint_state", + .localized_name = "reBot DevArm Leader", + .app_name = "RebotDevarmLeaderPlugin" }) +{ + // Defaults: factory ids (1..7 / 0x11..0x17), factory models, no sign flip, zero offset. + for (int i = 0; i < kNumJoints; ++i) + { + const ModelLimits limits = model_limits(kDefaultModels[i]); + calibration_[i] = JointCalibration{ static_cast(kDefaultMotorIdBase + i), + static_cast(kDefaultFeedbackIdBase + i), + limits.p_max, + limits.v_max, + 1.0, + 0.0 }; + } + if (!calibration_path.empty()) + { + load_calibration(calibration_path); + } + + if (is_robstride_path(device_path_)) + { + // Throws if the SocketCAN interface doesn't exist; throws unconditionally off Linux. + rs_bus_ = std::make_unique(device_path_); + std::cout << "RebotDevarmLeaderPlugin: RobStride SocketCAN backend on " << device_path_ << std::endl; + + // Leader arm: stop the motors so the operator can back-drive them by hand. RobStride + // motors keep answering parameter reads while stopped (verified on RS-series hardware). + for (int i = 0; i < kNumJoints; ++i) + { + if (!rs_bus_->disable(static_cast(calibration_[i].motor_id))) + { + std::cerr << "RebotDevarmLeaderPlugin: warning: failed to send stop to motor 0x" << std::hex + << calibration_[i].motor_id << std::dec << " (is " << device_path_ << " up?)" << std::endl; + } + } + // Drain any stop-command replies so the first feedback cycle starts clean. + RobStrideSample scratch; + while (rs_bus_->read_sample(scratch, kCollectTimeoutMs)) + { + } + } + else if (!device_path_.empty()) + { + // Throws on POSIX if the port can't be opened; throws unconditionally on Windows. + bus_ = std::make_unique(device_path_); + std::cout << "RebotDevarmLeaderPlugin: Damiao dm-serial backend on " << device_path_ << std::endl; + + // Leader arm: disable torque so the operator can back-drive it by hand. Damiao motors + // keep replying to feedback requests while disabled (verified on the B601-DM hardware). + for (int i = 0; i < kNumJoints; ++i) + { + if (!bus_->disable(calibration_[i].motor_id)) + { + std::cerr << "RebotDevarmLeaderPlugin: warning: failed to send disable to motor 0x" << std::hex + << calibration_[i].motor_id << std::dec << " (is the adapter connected?)" << std::endl; + } + } + // Drain the disable-command status replies so the first feedback cycle starts clean. + CanFrame scratch; + while (bus_->read_frame(scratch, kCollectTimeoutMs)) + { + } + } + else + { + std::cout << "RebotDevarmLeaderPlugin: using synthetic joint backend (no device path)" << std::endl; + } +} + +RebotDevarmLeaderPlugin::~RebotDevarmLeaderPlugin() = default; + +void RebotDevarmLeaderPlugin::load_calibration(const std::string& path) +{ + std::ifstream file(path); + if (!file) + { + std::cerr << "RebotDevarmLeaderPlugin: warning: cannot open calibration file '" << path << "'; using defaults" + << std::endl; + return; + } + + std::string line; + int line_no = 0; + while (std::getline(file, line)) + { + ++line_no; + if (const auto hash = line.find('#'); hash != std::string::npos) + { + line.erase(hash); + } + + std::istringstream iss(line); + std::string name; + int motor_id = 0; + int feedback_id = 0; + std::string model; + double sign = 1.0; + double offset_rad = 0.0; + if (!(iss >> name >> motor_id >> feedback_id >> model >> sign >> offset_rad)) + { + continue; // blank / comment-only / malformed line + } + + int idx = -1; + for (int i = 0; i < kNumJoints; ++i) + { + if (name == kJointNames[i]) + { + idx = i; + break; + } + } + if (idx < 0) + { + std::cerr << "RebotDevarmLeaderPlugin: warning: unknown joint '" << name << "' at " << path << ":" + << line_no << std::endl; + continue; + } + // The RobStride backend reads exact f32 values, so the model column only selects + // fixed-point limits on the Damiao backend; rs-* names keep the (unused) defaults. + const bool is_robstride_model = model.rfind("rs", 0) == 0 || model.rfind("RS", 0) == 0; + const ModelLimits limits = is_robstride_model ? + ModelLimits{ model.c_str(), calibration_[idx].p_max, calibration_[idx].v_max } : + model_limits(model); + calibration_[idx] = JointCalibration{ static_cast(motor_id), + static_cast(feedback_id), + limits.p_max, + limits.v_max, + (sign < 0.0 ? -1.0 : 1.0), + offset_rad }; + } +} + +void RebotDevarmLeaderPlugin::read_synthetic() +{ + // Smooth, phase-shifted trajectory so the full device -> tracker -> retargeter path can run + // with no hardware. + const double phase = 2.0 * std::numbers::pi * static_cast(frame_) / kSynthPeriodFrames; + const double omega = 2.0 * std::numbers::pi / (kSynthPeriodFrames / 90.0); // [rad/s] at 90 Hz + for (int i = 0; i < kNumJoints - 1; ++i) + { + positions_[i] = kSynthAmplitude * std::sin(phase + 0.5 * static_cast(i)); + velocities_[i] = kSynthAmplitude * omega * std::cos(phase + 0.5 * static_cast(i)); + } + // Gripper: normalized open/close oscillation in [0, 1]. + positions_[kNumJoints - 1] = 0.5 * (1.0 + std::sin(phase)); + velocities_[kNumJoints - 1] = 0.5 * omega * std::cos(phase); +} + +void RebotDevarmLeaderPlugin::read_hardware() +{ + // Request one feedback frame per motor (command 0xCC via id 0x7FF), then collect the replies + // that arrive on each motor's MST id. A motor that doesn't reply this cycle holds its last + // value so a transient bus hiccup never faults. + for (int i = 0; i < kNumJoints; ++i) + { + bus_->request_feedback(calibration_[i].motor_id); + } + + int replies = 0; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(kCollectTimeoutMs); + CanFrame frame; + while (replies < kNumJoints && std::chrono::steady_clock::now() < deadline) + { + if (!bus_->read_frame(frame, 1)) + { + continue; + } + for (int i = 0; i < kNumJoints; ++i) + { + if (frame.arbitration_id != calibration_[i].feedback_id || frame.dlc < 8) + { + continue; + } + // Feedback payload: [status<<4 | id_low, pos_hi, pos_lo, vel_hi, vel_lo<<4 | torq_hi, + // torq_lo, t_mos, t_rotor]; pos is 16-bit and vel 12-bit over the model limits. + const uint32_t pos_u = (static_cast(frame.data[1]) << 8) | frame.data[2]; + const uint32_t vel_u = (static_cast(frame.data[3]) << 4) | (frame.data[4] >> 4); + const double raw_pos = uint_to_float(pos_u, calibration_[i].p_max, kPositionBits); + const double raw_vel = uint_to_float(vel_u, calibration_[i].v_max, kVelocityBits); + positions_[i] = calibration_[i].sign * (raw_pos - calibration_[i].offset_rad); + velocities_[i] = calibration_[i].sign * raw_vel; + ++replies; + break; + } + } + + check_gripper_travel(); +} + +void RebotDevarmLeaderPlugin::read_hardware_robstride() +{ + // Request one parameter read per motor (alternating mechPos / mechVel inside the bus), then + // collect the exact-f32 replies. A motor that doesn't reply this cycle holds its last value + // so a transient bus hiccup never faults. + for (int i = 0; i < kNumJoints; ++i) + { + rs_bus_->request_feedback(static_cast(calibration_[i].motor_id)); + } + + int replies = 0; + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(kCollectTimeoutMs); + RobStrideSample sample; + while (replies < kNumJoints && std::chrono::steady_clock::now() < deadline) + { + if (!rs_bus_->read_sample(sample, 1)) + { + continue; + } + for (int i = 0; i < kNumJoints; ++i) + { + if (sample.motor_id != calibration_[i].motor_id) + { + continue; + } + if (sample.is_velocity) + { + velocities_[i] = calibration_[i].sign * sample.value; + } + else + { + positions_[i] = calibration_[i].sign * (sample.value - calibration_[i].offset_rad); + } + ++replies; + break; + } + } + + check_gripper_travel(); +} + +void RebotDevarmLeaderPlugin::check_gripper_travel() +{ + // Wrapped multi-turn gripper detection (see kGripperTravelMinRad). Latch the flag on a + // rising edge only, so the warning prints once instead of at 90 Hz. + const double gripper_pos = positions_[kGripperJointIndex]; + const bool out_of_travel = gripper_pos < kGripperTravelMinRad || gripper_pos > kGripperTravelMaxRad; + if (out_of_travel && !gripper_out_of_travel_) + { + std::cerr << "RebotDevarmLeaderPlugin: warning: gripper reads " << gripper_pos + << " rad, outside its physical travel [" << kGripperTravelMinRad << ", " << kGripperTravelMaxRad + << "]. The multi-turn encoder most likely wrapped by 2*pi " + << "after a power cycle; the gripper joint is streamed as invalid until it reads " + << "in-travel again. Re-home the gripper (close against the stop and re-zero)." << std::endl; + } + gripper_out_of_travel_ = out_of_travel; +} + +void RebotDevarmLeaderPlugin::push_current_state() +{ + core::JointStateOutputT out; + out.device_id = collection_id_; + out.has_velocity = true; + out.has_effort = false; + out.ee_pose_valid = false; + for (size_t i = 0; i < kJointNames.size(); ++i) + { + auto joint = std::make_shared(); + joint->name = kJointNames[i]; + joint->position = static_cast(positions_[i]); + joint->velocity = static_cast(velocities_[i]); + // A 2*pi-wrapped multi-turn gripper reading is not a pose: mark it invalid so + // consumers (retargeters, lerobot leaders) can hold/ignore it instead of + // commanding a follower through its soft-limit clip. + joint->valid = !(static_cast(i) == kGripperJointIndex && gripper_out_of_travel_); + out.joints.push_back(std::move(joint)); + } + + const auto sample_time_ns = core::os_monotonic_now_ns(); + + flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); + auto offset = core::JointStateOutput::Pack(builder, &out); + builder.Finish(offset); + pusher_.push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_ns, sample_time_ns); +} + +void RebotDevarmLeaderPlugin::update() +{ + if (bus_) + { + read_hardware(); + } + else if (rs_bus_) + { + read_hardware_robstride(); + } + else + { + read_synthetic(); + } + push_current_state(); + ++frame_; +} + +int run_probe(const std::string& device_path, const std::string& calibration_path, int seconds) +{ + if (device_path.empty()) + { + std::cerr << "probe: a device is required: a serial path (e.g. /dev/ttyACM0, Damiao) or a " + "SocketCAN interface name (e.g. can0, RobStride)" + << std::endl; + return 2; + } + + // Mirror the plugin's defaults / calibration handling without an OpenXR session. + struct ProbeJoint + { + uint16_t motor_id; + uint16_t feedback_id; + double p_max; + double sign; + double offset_rad; + bool seen; + double pos; + }; + std::array joints; + for (int i = 0; i < kNumJoints; ++i) + { + const ModelLimits limits = model_limits(kDefaultModels[i]); + joints[i] = ProbeJoint{ static_cast(kDefaultMotorIdBase + i), + static_cast(kDefaultFeedbackIdBase + i), + limits.p_max, + 1.0, + 0.0, + false, + 0.0 }; + } + if (!calibration_path.empty()) + { + // Reuse the plugin's parser via a throwaway file re-read to keep one format. Plain + // duplication here would drift; instead parse with the same rules inline. + std::ifstream file(calibration_path); + std::string line; + while (std::getline(file, line)) + { + if (const auto hash = line.find('#'); hash != std::string::npos) + { + line.erase(hash); + } + std::istringstream iss(line); + std::string name, model; + int motor_id = 0, feedback_id = 0; + double sign = 1.0, offset_rad = 0.0; + if (!(iss >> name >> motor_id >> feedback_id >> model >> sign >> offset_rad)) + { + continue; + } + for (int i = 0; i < kNumJoints; ++i) + { + if (name == kJointNames[i]) + { + joints[i].motor_id = static_cast(motor_id); + joints[i].feedback_id = static_cast(feedback_id); + joints[i].p_max = model_limits(model).p_max; + joints[i].sign = sign < 0.0 ? -1.0 : 1.0; + joints[i].offset_rad = offset_rad; + break; + } + } + } + } + + // Per-cycle status line, shared by both backends. + const auto print_cycle = [&joints](int cycle) + { + if (cycle % 10 != 0) + { + return; + } + std::cout << "probe:"; + for (int i = 0; i < kNumJoints; ++i) + { + std::cout << " " << kJointNames[i] << "="; + if (joints[i].seen) + { + std::cout << joints[i].pos; + } + else + { + std::cout << "---"; + } + } + std::cout << std::endl; + }; + + const auto t_end = std::chrono::steady_clock::now() + std::chrono::seconds(seconds); + if (is_robstride_path(device_path)) + { + RobStrideBus bus(device_path); + for (const auto& j : joints) + { + bus.disable(static_cast(j.motor_id)); // back-drive; RS motors answer reads while stopped + } + RobStrideSample scratch; + while (bus.read_sample(scratch, kCollectTimeoutMs)) + { + } + + int cycle = 0; + while (std::chrono::steady_clock::now() < t_end) + { + for (const auto& j : joints) + { + bus.request_feedback(static_cast(j.motor_id)); + } + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(kCollectTimeoutMs); + RobStrideSample sample; + while (std::chrono::steady_clock::now() < deadline) + { + if (!bus.read_sample(sample, 1)) + { + continue; + } + for (auto& j : joints) + { + if (sample.motor_id == j.motor_id && !sample.is_velocity) + { + j.pos = j.sign * (sample.value - j.offset_rad); + j.seen = true; + break; + } + } + } + print_cycle(cycle); + ++cycle; + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } + else + { + DamiaoBus bus(device_path); + for (const auto& j : joints) + { + bus.disable(j.motor_id); // back-drive mode; Damiao motors reply to 0xCC while disabled + } + CanFrame scratch; + while (bus.read_frame(scratch, kCollectTimeoutMs)) + { + } + + int cycle = 0; + while (std::chrono::steady_clock::now() < t_end) + { + for (const auto& j : joints) + { + bus.request_feedback(j.motor_id); + } + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(kCollectTimeoutMs); + CanFrame frame; + while (std::chrono::steady_clock::now() < deadline) + { + if (!bus.read_frame(frame, 1)) + { + continue; + } + for (auto& j : joints) + { + if (frame.arbitration_id == j.feedback_id && frame.dlc >= 8) + { + const uint32_t pos_u = (static_cast(frame.data[1]) << 8) | frame.data[2]; + j.pos = j.sign * (uint_to_float(pos_u, j.p_max, kPositionBits) - j.offset_rad); + j.seen = true; + break; + } + } + } + print_cycle(cycle); + ++cycle; + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } + + bool all_ok = true; + for (int i = 0; i < kNumJoints; ++i) + { + if (!joints[i].seen) + { + all_ok = false; + std::cerr << "probe: no feedback from motor 0x" << std::hex << joints[i].motor_id << std::dec << " (" + << kJointNames[i] << ")" << std::endl; + } + } + if (!all_ok) + { + std::cout << "probe: some motors missing" << std::endl; + return 1; + } + + // Sanity-check the gripper against its physical travel: a reading outside the window + // means the multi-turn encoder wrapped by 2*pi after a power cycle (turn count is + // volatile; only the single-turn zero survives). Teleoperating in this state slams the + // follower gripper at t=0 and grinds the mechanism into its stop. + const ProbeJoint& gripper = joints[kGripperJointIndex]; + if (gripper.pos < kGripperTravelMinRad || gripper.pos > kGripperTravelMaxRad) + { + std::cerr << "probe: WARNING: gripper reads " << gripper.pos << " rad, outside its physical travel [" + << kGripperTravelMinRad << ", " << kGripperTravelMaxRad + << "]: the multi-turn encoder wrapped after a power cycle. Re-home the gripper " + << "(close against the stop and re-zero) before teleoperating." << std::endl; + std::cout << "probe: all motors replied (gripper out of travel)" << std::endl; + return 3; + } + + std::cout << "probe: all motors replied" << std::endl; + return 0; +} + +} // namespace rebot_devarm_leader +} // namespace plugins diff --git a/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.hpp b/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.hpp new file mode 100644 index 000000000..b858f3c99 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.hpp @@ -0,0 +1,128 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +class DamiaoBus; +class RobStrideBus; + +//! Number of reBot DevArm DOFs: 6-DOF arm + gripper. +inline constexpr int kNumJoints = 7; + +/*! + * @brief Streams Seeed reBot DevArm (6-DOF + gripper) leader-arm joint angles as + * ``JointStateOutput`` via OpenXR ``SchemaPusher``, on the generic joint-space device path. + * + * Two motor builds of the arm are supported, selected by the shape of @p device_path: + * + * - **Damiao** (7 DM-series MIT-protocol motors: DM4340P on joints 1-3, DM4310 on joints 4-6 + * and the gripper) behind a Damiao USB-to-CAN serial adapter. A path containing ``/`` (e.g. + * ``/dev/ttyACM0``) selects this backend (:class:`DamiaoBus`, the same dm-serial wire protocol + * the vendor's DM_Control / motorbridge stacks use): it sends the disable control frame so the + * arm can be back-driven by hand, then requests one feedback frame per motor per cycle + * (command ``0xCC``) and decodes position/velocity from the replies. Damiao feedback is + * already in SI units (fixed-point over the model's ``[-pmax, pmax]`` / ``[-vmax, vmax]`` + * limits), so no tick conversion is needed -- only an optional per-joint sign and zero-offset + * from a calibration file. + * + * - **RobStride** (7 RS-series motors) on a SocketCAN interface. A bare interface name (e.g. + * ``can0``) selects this backend (:class:`RobStrideBus`): it sends the RobStride stop frame + * (back-drive), then alternates parameter reads of ``mechPos`` / ``mechVel`` per motor per + * cycle, whose replies carry exact IEEE f32 radians / rad/s -- model-independent, so the + * calibration file's model column is ignored on this backend. + * + * With no device path it falls back to a **synthetic** trajectory so the device -> tracker -> + * retargeter pipeline can run with no hardware (used by CI and headless bring-up). + */ +class RebotDevarmLeaderPlugin +{ +public: + /*! + * @param device_path Serial device path (e.g. /dev/ttyACM0) for the Damiao dm-serial + * backend, or a SocketCAN interface name (e.g. can0) for the RobStride backend. + * Empty selects the synthetic backend. + * @param collection_id Tensor collection id; must match the consumer's JointStateTracker. + * Also used as the JointStateOutput.device_id. + * @param calibration_path Optional calibration file (see load_calibration()); empty uses + * defaults (motor ids 1..7, feedback ids 0x11..0x17, sign +1, zero offset 0). + */ + RebotDevarmLeaderPlugin(const std::string& device_path, + const std::string& collection_id, + const std::string& calibration_path = ""); + ~RebotDevarmLeaderPlugin(); + + void update(); + +private: + //! Per-joint mapping from a Damiao motor to a joint angle: + //! ``angle [rad] = sign * (feedback_pos - offset_rad)`` (feedback is already in radians). + struct JointCalibration + { + uint16_t motor_id; // command CAN id (ESC id) + uint16_t feedback_id; // MST id the motor replies on + double p_max; // model position limit [rad]; feedback pos is 16-bit over [-p_max, p_max] + double v_max; // model velocity limit [rad/s]; feedback vel is 12-bit over [-v_max, v_max] + double sign; // +1 / -1 if the joint moves opposite the URDF convention + double offset_rad; // feedback position at the joint's URDF zero pose + }; + + //! Request + collect one feedback frame per motor (held last on a missed reply). SEAM for + //! other backends. + void read_hardware(); + //! RobStride flavor of read_hardware(): parameter-read replies instead of feedback frames. + void read_hardware_robstride(); + //! Synthetic smooth trajectory used when no serial device is attached. + void read_synthetic(); + //! Latch gripper_out_of_travel_ from the current gripper position (warns on a rising edge). + void check_gripper_travel(); + void push_current_state(); + //! Load calibration from @p path: ``name motor_id feedback_id model sign offset_rad`` per + //! line (``#`` comments allowed). Unknown joint names are ignored; missing joints keep + //! defaults. ``model`` is a Damiao model name (4310, 4310P, 4340, 4340P) selecting the + //! feedback fixed-point limits. + void load_calibration(const std::string& path); + + std::string device_path_; + std::string collection_id_; + int64_t frame_ = 0; + double positions_[kNumJoints] = { 0.0 }; + double velocities_[kNumJoints] = { 0.0 }; + JointCalibration calibration_[kNumJoints]; + //! True while the gripper reads outside its physical travel (2*pi-wrapped multi-turn + //! encoder after a power cycle); the gripper joint is streamed with ``valid = false``. + bool gripper_out_of_travel_ = false; + + // At most one backend is non-null; both null => synthetic backend. + std::unique_ptr bus_; + std::unique_ptr rs_bus_; + + std::shared_ptr session_; + core::SchemaPusher pusher_; +}; + +//! Hardware probe helper: open @p device_path (serial path => Damiao, SocketCAN interface name +//! => RobStride), send disable (back-drive) to the default motor ids, then stream decoded joint +//! positions to stdout for @p seconds. Verifies the bus, motor ids, and feedback decoding with +//! no OpenXR runtime. Returns a process exit code (0 = every motor replied at least once, 3 = +//! motors replied but the gripper reads outside its physical travel — 2*pi-wrapped multi-turn +//! encoder, re-home before teleoperating). +int run_probe(const std::string& device_path, const std::string& calibration_path, int seconds); + +} // namespace rebot_devarm_leader +} // namespace plugins diff --git a/src/plugins/rebot_devarm_leader/robstride_bus.cpp b/src/plugins/rebot_devarm_leader/robstride_bus.cpp new file mode 100644 index 000000000..77fdf8c95 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/robstride_bus.cpp @@ -0,0 +1,213 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "robstride_bus.hpp" + +#include +#include + +#ifdef __linux__ + +# include +# include +# include +# include +# include + +# include +# include +# include +# include +# include + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +namespace +{ + +// RobStride private-protocol communication types (bits [28:24] of the extended id). +constexpr uint8_t kTypeStop = 0x04; // host -> motor: stop / torque off (back-drive) +constexpr uint8_t kTypeParamRead = 0x11; // both directions: single parameter read / its reply + +// Parameter indices (index goes little-endian in payload bytes 0..1; the reply echoes it and +// carries the value as little-endian IEEE f32 in bytes 4..7). +constexpr uint16_t kParamMechPos = 0x7019; // mechanical position [rad] +constexpr uint16_t kParamMechVel = 0x701A; // mechanical velocity [rad/s] + +//! Pack the RobStride 29-bit extended id: comm_type[28:24] | data16[23:8] | target_id[7:0]. +constexpr uint32_t pack_id(uint8_t comm_type, uint16_t data16, uint8_t target_id) +{ + return (static_cast(comm_type) << 24) | (static_cast(data16) << 8) | target_id; +} + +} // namespace + +RobStrideBus::RobStrideBus(const std::string& interface, uint8_t host_id) : host_id_(host_id) +{ + fd_ = ::socket(PF_CAN, SOCK_RAW | SOCK_NONBLOCK, CAN_RAW); + if (fd_ < 0) + { + throw std::runtime_error("RobStrideBus: cannot open a SocketCAN socket: " + std::string(std::strerror(errno))); + } + + ifreq ifr{}; + std::strncpy(ifr.ifr_name, interface.c_str(), sizeof(ifr.ifr_name) - 1); + if (::ioctl(fd_, SIOCGIFINDEX, &ifr) < 0) + { + const std::string msg = std::strerror(errno); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("RobStrideBus: no CAN interface '" + interface + "': " + msg + + " (bring it up with: ip link set " + interface + " up type can bitrate 1000000)"); + } + + sockaddr_can addr{}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + if (::bind(fd_, reinterpret_cast(&addr), sizeof(addr)) < 0) + { + const std::string msg = std::strerror(errno); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("RobStrideBus: cannot bind to '" + interface + "': " + msg); + } +} + +RobStrideBus::~RobStrideBus() +{ + if (fd_ >= 0) + { + ::close(fd_); + } +} + +bool RobStrideBus::send_frame(uint8_t comm_type, uint16_t data16, uint8_t target_id, const uint8_t data[8]) +{ + can_frame frame{}; + frame.can_id = pack_id(comm_type, data16, target_id) | CAN_EFF_FLAG; + frame.can_dlc = 8; + std::memcpy(frame.data, data, 8); + return ::write(fd_, &frame, sizeof(frame)) == static_cast(sizeof(frame)); +} + +bool RobStrideBus::disable(uint8_t motor_id) +{ + const uint8_t zeros[8] = { 0 }; + return send_frame(kTypeStop, host_id_, motor_id, zeros); +} + +bool RobStrideBus::request_feedback(uint8_t motor_id) +{ + const uint16_t index = velocity_next_[motor_id] ? kParamMechVel : kParamMechPos; + velocity_next_[motor_id] = !velocity_next_[motor_id]; + + uint8_t payload[8] = { 0 }; + payload[0] = static_cast(index & 0xFF); + payload[1] = static_cast(index >> 8); + return send_frame(kTypeParamRead, host_id_, motor_id, payload); +} + +bool RobStrideBus::read_sample(RobStrideSample& out, int timeout_ms) +{ + // Absolute deadline: skipped (unrelated) frames must consume the budget too, or a busy bus + // full of active-report traffic would keep this call spinning past its timeout. + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (true) + { + const auto remaining = + std::chrono::duration_cast(deadline - std::chrono::steady_clock::now()); + const int remaining_ms = static_cast(remaining.count()); + if (remaining_ms < 0) + { + return false; + } + pollfd pfd{ fd_, POLLIN, 0 }; + const int ready = ::poll(&pfd, 1, remaining_ms); + if (ready <= 0) + { + return false; // timeout or poll error + } + + can_frame frame{}; + const ssize_t n = ::read(fd_, &frame, sizeof(frame)); + if (n != static_cast(sizeof(frame))) + { + return false; + } + + // Only extended-id parameter-read replies addressed to us are samples; everything else + // on the bus (active-report feedback, other hosts' traffic) is skipped, not an error. + if (!(frame.can_id & CAN_EFF_FLAG) || frame.can_dlc < 8) + { + continue; + } + const uint32_t id = frame.can_id & CAN_EFF_MASK; + const uint8_t comm_type = static_cast(id >> 24); + const uint8_t target = static_cast(id & 0xFF); + if (comm_type != kTypeParamRead || target != host_id_) + { + continue; + } + + const uint16_t index = static_cast(frame.data[0]) | (static_cast(frame.data[1]) << 8); + if (index != kParamMechPos && index != kParamMechVel) + { + continue; // some other host's parameter transaction + } + + float value = 0.0f; + static_assert(sizeof(value) == 4, "RobStride parameter replies carry IEEE f32"); + std::memcpy(&value, &frame.data[4], sizeof(value)); // little-endian payload, LE hosts only + + out.motor_id = static_cast((id >> 8) & 0xFF); + out.is_velocity = (index == kParamMechVel); + out.value = static_cast(value); + return true; + } +} + +} // namespace rebot_devarm_leader +} // namespace plugins + +#else // !__linux__ + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +RobStrideBus::RobStrideBus(const std::string& interface, uint8_t host_id) : host_id_(host_id) +{ + (void)interface; + throw std::runtime_error("RobStrideBus: the RobStride SocketCAN backend is Linux-only"); +} + +RobStrideBus::~RobStrideBus() = default; + +bool RobStrideBus::send_frame(uint8_t, uint16_t, uint8_t, const uint8_t[8]) +{ + return false; +} + +bool RobStrideBus::disable(uint8_t) +{ + return false; +} + +bool RobStrideBus::request_feedback(uint8_t) +{ + return false; +} + +bool RobStrideBus::read_sample(RobStrideSample&, int) +{ + return false; +} + +} // namespace rebot_devarm_leader +} // namespace plugins + +#endif // __linux__ diff --git a/src/plugins/rebot_devarm_leader/robstride_bus.hpp b/src/plugins/rebot_devarm_leader/robstride_bus.hpp new file mode 100644 index 000000000..e4bf57535 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/robstride_bus.hpp @@ -0,0 +1,78 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace rebot_devarm_leader +{ + +//! One decoded RobStride feedback value (a type-0x11 parameter-read reply, already in SI units). +struct RobStrideSample +{ + uint8_t motor_id = 0; + bool is_velocity = false; //!< false: position [rad] (mechPos), true: velocity [rad/s] (mechVel) + double value = 0.0; +}; + +/*! + * @brief Minimal SocketCAN client for RobStride RS-series motors (the RobStride build of the + * reBot DevArm), implementing the leader-arm subset of the RobStride private CAN protocol. + * + * RobStride motors talk classic CAN at 1 Mbps with 29-bit extended ids laid out as + * ``comm_type[28:24] | data16[23:8] | target_id[7:0]``. Host -> motor frames carry the host id in + * ``data16`` and the motor id in ``target_id``; replies swap them. The subset a *leader* arm + * needs: + * - **stop/disable** (comm type ``0x04``) so the arm can be back-driven by hand, and + * - **parameter read** (comm type ``0x11``) of ``mechPos`` (``0x7019``) and ``mechVel`` + * (``0x701A``), whose replies carry the value as little-endian IEEE f32 in payload bytes + * 4..7 -- already radians / rad/s, with no per-model fixed-point scaling to configure. + * + * request_feedback() alternates between the position and velocity read per motor, so each + * channel refreshes at half the caller's cycle rate (45 Hz at the plugin's 90 Hz loop) while + * keeping bus load under ~20% of a 1 Mbps bus for 7 motors. + * + * The interface name is a SocketCAN device (e.g. ``can0``); Linux only -- constructing on any + * other platform throws. + */ +class RobStrideBus +{ +public: + //! Open a RAW SocketCAN socket bound to @p interface (e.g. ``can0``). @p host_id is this + //! host's id in the RobStride id scheme (replies are addressed to it; ``0xFD`` is the + //! vendor-tools convention). Throws ``std::runtime_error`` on failure (or always, off Linux). + explicit RobStrideBus(const std::string& interface, uint8_t host_id = 0xFD); + ~RobStrideBus(); + + RobStrideBus(const RobStrideBus&) = delete; + RobStrideBus& operator=(const RobStrideBus&) = delete; + RobStrideBus(RobStrideBus&&) = delete; + RobStrideBus& operator=(RobStrideBus&&) = delete; + + //! Send the RobStride stop frame (comm type 0x04) to @p motor_id so the joint goes limp and + //! can be moved by hand. Safe to send when the motor is already stopped. + bool disable(uint8_t motor_id); + + //! Ask motor @p motor_id for one feedback value, alternating mechPos / mechVel reads per + //! call. The reply arrives asynchronously; collect it with read_sample(). + bool request_feedback(uint8_t motor_id); + + //! Read the next parameter-read reply into @p out, waiting up to @p timeout_ms. Skips + //! unrelated bus traffic (e.g. active-report feedback frames). Returns false on timeout. + bool read_sample(RobStrideSample& out, int timeout_ms); + +private: + //! Send one extended-id frame ``comm_type | data16 | target_id`` with an 8-byte payload. + bool send_frame(uint8_t comm_type, uint16_t data16, uint8_t target_id, const uint8_t data[8]); + + int fd_ = -1; + uint8_t host_id_; + bool velocity_next_[256] = { false }; // per-motor mechPos/mechVel alternation state +}; + +} // namespace rebot_devarm_leader +} // namespace plugins