diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c12c7c8e..f0345ee83 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -182,6 +182,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..5fbe3d3fc --- /dev/null +++ b/src/plugins/rebot_devarm_leader/CMakeLists.txt @@ -0,0 +1,17 @@ +# 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 +) + +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..078e31983 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/README.md @@ -0,0 +1,107 @@ + + +# 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`). + +The reBot DevArm is 7 Damiao 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. + +When no serial 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 + +# ... 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]`. The serial backend is +Linux/macOS only (POSIX `termios`); the adapter enumerates as a CDC-ACM device (nominal 921600 bps). + +## 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 +# 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. +- `model`: Damiao model name (`4310`, `4310P`, `4340`, `4340P`) selecting the feedback + fixed-point limits. +- `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..50f82061d --- /dev/null +++ b/src/plugins/rebot_devarm_leader/main.cpp @@ -0,0 +1,64 @@ +// 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] + // 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] + // Empty device_path selects 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..17fc21720 --- /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; 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..8bba9ddb4 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.cpp @@ -0,0 +1,497 @@ +// 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 +#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; +} + +//! 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 (!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; + } + const ModelLimits limits = 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; + } + } + + // 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 + { + 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 serial device path is required (e.g. /dev/ttyACM0)" << 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; + } + } + } + } + + 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)) + { + } + + const auto t_end = std::chrono::steady_clock::now() + std::chrono::seconds(seconds); + 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; + } + } + } + + if (cycle % 10 == 0) + { + 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; + } + ++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..6ddb8bbd8 --- /dev/null +++ b/src/plugins/rebot_devarm_leader/rebot_devarm_leader_plugin.hpp @@ -0,0 +1,111 @@ +// 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; + +//! 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. + * + * The reBot DevArm is 7 Damiao 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. When a + * serial @p device_path is given, the plugin talks to the motors directly via :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. + * + * 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 real Damiao dm-serial + * 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(); + //! Synthetic smooth trajectory used when no serial device is attached. + void read_synthetic(); + 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; + + std::unique_ptr bus_; // null => synthetic backend + + std::shared_ptr session_; + core::SchemaPusher pusher_; +}; + +//! Hardware probe helper: open @p device_path, 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