feat: add SO-101 robot arm teleoperation plugin#279
Open
vickybot911 wants to merge 4 commits intoNVIDIA:mainfrom
Open
feat: add SO-101 robot arm teleoperation plugin#279vickybot911 wants to merge 4 commits intoNVIDIA:mainfrom
vickybot911 wants to merge 4 commits intoNVIDIA:mainfrom
Conversation
Add a new plugin for Haply Robotics Inverse3 and VerseGrip devices, bridging the Haply SDK WebSocket interface to the Isaac Teleop framework via OpenXR hand tracking injection. Components: - haply_plugin_core: shared library (libIsaacTeleopPluginsHaply.so) implementing WebSocket client, JSON parsing, OpenXR HandInjector integration, and DeviceIO controller tracking - haply_hand_plugin: plugin executable running at 90Hz - haply_hand_tracker_printer: standalone CLI tool for verifying Haply device data without OpenXR dependencies Architecture: - Background I/O thread communicates with Haply SDK via WebSocket (ws://localhost:10001, configurable via HAPLY_WEBSOCKET_HOST/PORT) - Maps Inverse3 cursor position and VerseGrip orientation to OpenXR XrHandJointLocationEXT format (26 joints) - WRIST and PALM joints receive real tracked data; finger joints are placed at wrist position with VALID but not TRACKED flags - Controller aim pose provides root positioning (same as Manus plugin) - Exponential backoff reconnection on WebSocket errors No build-time SDK dependency — Haply SDK is a runtime service. Vendored: nlohmann/json (MIT, single header) for JSON parsing. Signed-off-by: Vicky <vickybot911@gmail.com>
- Add WebSocket payload size limit (16 MB) to prevent OOM on malformed frames - Add SO_RCVTIMEO (5s) connect timeout to prevent indefinite blocking - Replace std::atoi with std::stoul + range validation for port parsing - Add signal handler for graceful shutdown in plugin executable - Add TODO comment for unused m_grip_interpolant (future finger synthesis) - Fix British spelling: Synthesised → Synthesized - Apply same payload limit and port validation fixes to printer tool Signed-off-by: Vicky <vickybot911@gmail.com>
- Rename HAPLY_WS_HOST/HAPLY_WS_PORT to HAPLY_WEBSOCKET_HOST/HAPLY_WEBSOCKET_PORT in core plugin to match README and printer tool - Add SO_RCVTIMEO (5s) to printer tool's MiniWebSocket for consistency Signed-off-by: Vicky <vickybot911@gmail.com>
Add a new plugin for the SO-101 6-DOF robot arm (leader arm) at src/plugins/so101/, enabling arm-to-arm teleoperation within the Isaac Teleop framework. Components: - FlatBuffers schema (so101_arm.fbs): 6 normalized joint positions (shoulder_pan, shoulder_lift, elbow_flex, wrist_flex, wrist_roll, gripper) pushed via OpenXR SchemaPusher - Feetech STS3215 serial driver: POSIX termios, 1Mbaud half-duplex protocol with instruction/status packet framing and checksum validation - Plugin executable (so101_arm_plugin): 90Hz update loop reading joint positions and pushing via SchemaPusher - Standalone printer (so101_printer): diagnostic tool for verifying servo communication without OpenXR dependencies Configuration via environment variables: SO101_SERIAL_PORT (default: /dev/ttyACM0) SO101_BAUDRATE (default: 1000000) SO101_COLLECTION_ID (default: so101_leader) Joint positions normalized to [-1.0, 1.0] from raw 12-bit servo values (0-4095), centered on configurable calibration midpoints. Handles serial disconnect/reconnect gracefully. No external SDK dependency — communicates directly with Feetech STS3215 servos over USB serial. Signed-off-by: Vicky <vickybot911@gmail.com>
📝 WalkthroughWalkthroughThis pull request introduces two complete teleoperation plugins to the Isaac system: the Haply hand-tracking plugin using WebSocket communication with OpenXR integration, and the SO-101 robotic arm plugin interfacing with Feetech servos. Both include build configurations, FlatBuffers schema definitions, documentation, and diagnostic tools. Changes
Sequence Diagram(s)sequenceDiagram
participant Client as Haply Client
participant WebSocket as HaplyWebSocket
participant Tracker as HaplyTracker
participant OpenXR as OpenXR Session
participant Injector as HandInjector
Client->>WebSocket: connect(host, port)
WebSocket-->>Client: connected
Client->>Tracker: update()
Tracker->>WebSocket: recv_text(json_message)
WebSocket-->>Tracker: {inverse3, verse_grip}
Tracker->>Tracker: parse JSON state<br/>(cursor, orientation, buttons)
Tracker->>OpenXR: query session, time
OpenXR-->>Tracker: session_time
Tracker->>Injector: create/reset for handedness
Injector->>OpenXR: synthesize finger joints<br/>from wrist/palm pose
Tracker->>OpenXR: inject hand data<br/>with DeviceIO
OpenXR-->>Tracker: data accepted
Tracker-->>Client: state updated
sequenceDiagram
participant App as SO101App
participant Plugin as SO101Plugin
participant Driver as FeetechDriver
participant UART as Serial UART
participant OpenXR as OpenXR Session
participant Pusher as SchemaPusher
App->>Plugin: new SO101Plugin(port, collection_id)
Plugin->>Driver: new FeetechDriver()
Plugin->>Driver: open(port, baudrate)
Driver->>UART: open serial, configure 8N1
UART-->>Driver: fd
Driver-->>Plugin: open success
App->>Plugin: update()
Plugin->>Driver: read_all_positions()
Driver->>UART: send packet(id, read_instruction)
UART-->>Driver: status packet(position)
Driver-->>Plugin: positions[6]
Plugin->>Plugin: normalize each position<br/>to [-1.0, 1.0]
Plugin->>Pusher: push FlatBuffer<br/>SO101ArmOutput
Pusher->>OpenXR: write to collection
OpenXR-->>Pusher: ack
Pusher-->>Plugin: pushed
Plugin-->>App: update complete
Estimated code review effort🎯 4 (Complex) | ⏱️ ~75 minutes The changes introduce two feature-complete plugins with substantial new code spanning network I/O (WebSocket with framing/masking), serial driver protocol implementation, OpenXR integration with hand/arm data injection, JSON parsing, FlatBuffers schema serialization, threading, and comprehensive error handling. While individual functions are relatively self-contained, the breadth of heterogeneous functionality across haply_hand_tracking_plugin.cpp, haply_hand_tracker_printer.cpp, feetech_driver.cpp, and so101_plugin.cpp, combined with protocol-specific logic and state management, demands careful review across multiple domains. Poem
🚥 Pre-merge checks | ✅ 2 | ❌ 1❌ Failed checks (1 warning)
✅ Passed checks (2 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches🧪 Generate unit tests (beta)
📝 Coding Plan
Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 17
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/core/schema/fbs/so101_arm.fbs`:
- Around line 11-18: The gripper comment on SO101ArmOutput (field "gripper") is
inaccurate because normalize_position() in so101_plugin.cpp produces values in
[-1.0, 1.0]; either update the comment to state the actual range "[-1.0 = fully
open, 1.0 = fully closed]" or implement explicit mapping for the gripper in
normalize_position() (e.g., map [-1,1] -> [0,1] and clamp) and add a brief
comment describing that mapping; reference the SO101ArmOutput table's gripper
field and the normalize_position() function when making the change.
In `@src/plugins/haply/core/haply_hand_tracking_plugin.cpp`:
- Around line 548-628: The code holds m_state_mutex while constructing and
sending a WebSocket message (ws.send_text), which can block; instead, keep the
mutex only for mutating/reading m_state and copy the needed value(s) (at minimum
m_state.inverse3_device_id) into a local variable while holding m_state_mutex,
then release the lock and build/send the JSON command using that local copy;
update the code around m_state_mutex, m_state.inverse3_device_id and
ws.send_text to perform only the minimal protected reads under the lock and move
cmd creation + ws.send_text outside the locked section (ensure
inject_hand_data()/any readers still use the mutex for state access).
- Around line 193-198: Replace the fragile substring check that looks for "101"
with a full RFC6455-compliant handshake validation: parse the HTTP status line
from the response (ensure it matches "HTTP/1.1 101 Switching Protocols"), parse
response headers into a map, verify headers "Upgrade" (case-insensitive ==
"websocket") and "Connection" (contains "Upgrade"), and validate the
"Sec-WebSocket-Accept" value by computing base64(sha1(SEC_WEBSOCKET_KEY +
"258EAFA5-E914-47DA-95CA-C5AB0DC85B11")) where SEC_WEBSOCKET_KEY is the
Sec-WebSocket-Key originally sent in the request (store it if not already); if
any check fails, call close() and log the response (as currently done) and
return false, otherwise proceed to mark the connection established.
- Around line 349-353: In recv_text(), do not append binary frames (opcode 0x02)
or unknown/reserved opcodes to the text buffer (variable out); instead detect
the opcode on receipt and immediately initiate a WebSocket close with the
appropriate code (use 1003 Unsupported Data for binary frames and 1002 Protocol
Error for unknown/reserved opcodes), stop further processing/return after
sending the close, and avoid calling json::parse() on any accumulated invalid
data; update the opcode handling branch that currently does
out.append(reinterpret_cast<char*>(data.data()), data.size()) to perform the
close/return logic for these cases.
In `@src/plugins/haply/tools/haply_hand_tracker_printer.cpp`:
- Around line 287-295: The RNG objects are being constructed on each WebSocket
frame send (the local std::random_device, std::mt19937 gen, and
std::uniform_int_distribution in the send_frame-like code block), which is
inefficient; move these into the class as persistent members (e.g.,
std::random_device rd; std::mt19937 gen; std::uniform_int_distribution<> dis;)
and initialize them once (constructor or member initializer) and then use the
class members inside the function that currently fills mask[] and pushes into f,
removing the per-call constructions to reuse the RNG across calls.
- Around line 81-88: Check the return values of the two setsockopt calls that
configure TCP_NODELAY and the SO_RCVTIMEO timeout on fd_ (the calls using
IPPROTO_TCP/TCP_NODELAY and SOL_SOCKET/SO_RCVTIMEO with struct timeval tv); if
either returns -1, log an error including errno/strerror details (or use the
existing logger) and handle it appropriately (e.g., continue but report the
failure or return an error code) so the diagnostic tool doesn't silently ignore
failed socket option changes.
- Line 20: Remove the unused `#include` <mutex> from
haply_hand_tracker_printer.cpp: search for any usage of std::mutex or related
types in functions or classes (e.g., any mutex variables or lock_guard) and if
none exist, delete the `#include` <mutex> line to avoid unnecessary compilation
overhead and warnings; if a mutex is actually needed, instead keep it and ensure
the declared mutex symbol is used where synchronization is required.
In `@src/plugins/so101/CMakeLists.txt`:
- Around line 11-28: The so101_arm_plugin target lacks RPATH properties, which
can prevent it from finding shared libraries at runtime; update the CMake for
target so101_arm_plugin by setting the same RPATH-related target properties used
for haply_hand_plugin (e.g. BUILD_RPATH, INSTALL_RPATH,
INSTALL_RPATH_USE_LINK_PATH, SKIP_BUILD_RPATH and visibility) so the installed
plugin can resolve its shared library dependencies (use the same
values/variables/hints as haply_hand_plugin, and apply them via
set_target_properties(so101_arm_plugin PROPERTIES ...)).
In `@src/plugins/so101/feetech_driver.cpp`:
- Around line 223-230: The range check on the assembled position is redundant
because position is formed from two uint8_t bytes (buf[5], buf[6]) and cannot be
negative; update the validation in the code that computes `position` (using
`buf[5]` and `buf[6]`) to remove the `position < 0` test and only check the
meaningful upper bound (e.g. `if (position > 4095) { return -1; }`), keeping the
rest of the logic and variable names (`position`, `buf`) unchanged.
In `@src/plugins/so101/inc/so101/so101_plugin.hpp`:
- Around line 79-82: Replace the raw C-style arrays positions_[6] and
centers_[6] with std::array<int, 6> to get safer bounds-checked access and
modern C++ semantics; add `#include` <array>, change the declarations to
std::array<int, 6> positions_ = {0,0,0,0,0,0} and std::array<int, 6> centers_ =
{2048,2048,2048,2048,2048,2048}, and update any code that assumes
decay-to-pointer or C-array usage (e.g., loops, std::memcpy, or passing to APIs)
to use positions_.data() or std::span/std::begin/end as appropriate.
- Around line 39-83: The SO101Plugin class manages non-copyable resources
(std::unique_ptr<FeetechDriver>, core::SchemaPusher) so explicitly delete copy
and move operations to make intent clear: add deleted declarations for
SO101Plugin(const SO101Plugin&) = delete, SO101Plugin& operator=(const
SO101Plugin&) = delete, SO101Plugin(SO101Plugin&&) = delete, and SO101Plugin&
operator=(SO101Plugin&&) = delete in the class definition (near the public
ctor/dtor) to prevent copying or moving of instances that own driver_, pusher_
and the internal positions_/centers_ arrays.
In `@src/plugins/so101/main.cpp`:
- Around line 23-25: The current baudrate parsing uses std::atoi which returns 0
on invalid input; update the parsing of baudrate (variable baudrate in
main/main.cpp) to use std::stoi inside a try/catch (or manually validate digits)
when parsing argv[2] or env_baud, ensure the parsed value is > 0, and if parsing
fails or value is non-positive fall back to the default 1000000 (or log and
exit) before passing baudrate into the plugin constructor; reference the
baudrate variable and the argv[2]/SO101_BAUDRATE sources when adding the
validation and error handling.
- Around line 38-45: The main loop currently runs forever and lacks
SIGINT/SIGTERM handling; introduce a process-wide atomic<bool> (e.g., running)
initialized true, register signal handlers for SIGINT and SIGTERM that set
running = false, change the loop to while(running) { plugin.update();
frame_count++; std::this_thread::sleep_until(program_start + frame_duration *
frame_count); }, and ensure you call any plugin cleanup method (e.g.,
plugin.shutdown() or close serial resources) after the loop exits so the serial
port is left in a defined state.
In `@src/plugins/so101/README.md`:
- Around line 173-181: The README is ambiguous about gripper normalization;
update the Data Format and Calibration documentation to explicitly state the
gripper mapping and make the comment in the SO101ArmOutput table consistent:
either state that gripper is normalized to [-1.0, 1.0] like other joints (with
0.0 as midpoint and additional downstream mapping to [0.0,1.0] if used), or
state that gripper is normalized to [0.0, 1.0] directly (0.0=open, 1.0=closed)
and adjust the Calibration description accordingly; reference and update the
SO101ArmOutput table's gripper comment and the Calibration section so they match
and include a short note describing any downstream mapping applied.
In `@src/plugins/so101/so101_plugin.cpp`:
- Around line 47-70: The update() method currently calls push_current_state()
even after read_all_positions() fails, so add explicit staleness tracking:
introduce a member like positions_valid_ (bool) or last_positions_timestamp_ and
set it true/now only when read_all_positions(positions_) returns true; set it
false/clear timestamp immediately when read_all_positions() fails or when
driver_->is_open() is false and open() fails; update push_current_state() (or
its callers) to include/emit this validity flag or timestamp so consumers can
distinguish stale vs fresh data, and ensure the flag/timestamp is reset
appropriately on successful reconnect in update().
In `@src/plugins/so101/tools/so101_printer.cpp`:
- Around line 26-27: The baudrate parsing silently yields 0 on invalid input;
update the logic that sets the baudrate variable (currently using std::atoi for
argv[2] and env_baud) to validate the string first and produce a clear error
when parsing fails: use a safe parser (std::from_chars or std::stoi in a
try/catch) or a digit-only check, verify the parsed value is >0 and one of the
supported baud rates, and if invalid print an explicit error like "Invalid
baudrate input: '<value>'" and exit instead of letting driver.open() receive 0;
refer to the baudrate variable and the port/argv handling in so101_printer.cpp
to locate where to add the check and error message.
- Around line 43-75: The loop in main never exits (while (true)) so return 0 is
unreachable and there is no graceful shutdown; add a signal handler (e.g., a
sig_atomic_t stop_flag or std::atomic<bool> stopRequested and a handler for
SIGINT/SIGTERM) and modify the loop condition to check that flag instead of
while (true), ensuring the handler sets the flag to break the loop, let the loop
finish cleanly (closing any resources) and then return 0; update references in
this file to use the new flag in place of the infinite loop and ensure
driver.read_all_positions/ read_errors logic remains unchanged inside the
controlled loop.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Path: .coderabbit.yaml
Review profile: ASSERTIVE
Plan: Pro
Run ID: da4b8ceb-374c-4e6f-a472-37422137f2d7
📒 Files selected for processing (26)
CMakeLists.txtsrc/core/schema/fbs/so101_arm.fbssrc/plugins/haply/.gitignoresrc/plugins/haply/CMakeLists.txtsrc/plugins/haply/CMakePresets.jsonsrc/plugins/haply/CMakePresets.json.licensesrc/plugins/haply/README.mdsrc/plugins/haply/app/CMakeLists.txtsrc/plugins/haply/app/main.cppsrc/plugins/haply/core/CMakeLists.txtsrc/plugins/haply/core/haply_hand_tracking_plugin.cppsrc/plugins/haply/inc/core/haply_hand_tracking_plugin.hppsrc/plugins/haply/plugin.yamlsrc/plugins/haply/third_party/nlohmann/json.hppsrc/plugins/haply/tools/CMakeLists.txtsrc/plugins/haply/tools/haply_hand_tracker_printer.cppsrc/plugins/so101/CMakeLists.txtsrc/plugins/so101/README.mdsrc/plugins/so101/feetech_driver.cppsrc/plugins/so101/feetech_driver.hppsrc/plugins/so101/inc/so101/so101_plugin.hppsrc/plugins/so101/main.cppsrc/plugins/so101/plugin.yamlsrc/plugins/so101/so101_plugin.cppsrc/plugins/so101/tools/CMakeLists.txtsrc/plugins/so101/tools/so101_printer.cpp
Comment on lines
+11
to
+18
| table SO101ArmOutput { | ||
| shoulder_pan: float (id: 0); | ||
| shoulder_lift: float (id: 1); | ||
| elbow_flex: float (id: 2); | ||
| wrist_flex: float (id: 3); | ||
| wrist_roll: float (id: 4); | ||
| gripper: float (id: 5); // 0.0 = open, 1.0 = closed | ||
| } |
There was a problem hiding this comment.
Gripper range comment may be misleading.
The comment on line 17 says // 0.0 = open, 1.0 = closed but the normalize_position() function in so101_plugin.cpp returns values in [-1.0, 1.0]. Either the gripper needs special handling (not currently implemented), or this comment should reflect the actual [-1.0, 1.0] range.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/core/schema/fbs/so101_arm.fbs` around lines 11 - 18, The gripper comment
on SO101ArmOutput (field "gripper") is inaccurate because normalize_position()
in so101_plugin.cpp produces values in [-1.0, 1.0]; either update the comment to
state the actual range "[-1.0 = fully open, 1.0 = fully closed]" or implement
explicit mapping for the gripper in normalize_position() (e.g., map [-1,1] ->
[0,1] and clamp) and add a brief comment describing that mapping; reference the
SO101ArmOutput table's gripper field and the normalize_position() function when
making the change.
Comment on lines
+193
to
+198
| if (response.find("101") == std::string::npos) | ||
| { | ||
| close(); | ||
| std::cerr << "[HaplyWebSocket] Upgrade failed, response: " << response.substr(0, 80) << std::endl; | ||
| return false; | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🏁 Script executed:
#!/bin/bash
# Verify whether handshake validation checks include status line and Sec-WebSocket-Accept
rg -n -C3 'find\("101"\)|Sec-WebSocket-Accept|Upgrade: websocket|Connection: Upgrade' src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 695
🏁 Script executed:
#!/bin/bash
# Get broader context around recv_response to see full validation logic
rg -n -B15 -A10 'response\.find\("101"\)' src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 900
🏁 Script executed:
#!/bin/bash
# Check if Sec-WebSocket-Accept is validated anywhere in the file
rg -n 'Sec-WebSocket-Accept|sha1|base64' src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 44
🏁 Script executed:
#!/bin/bash
# Look for the recv_response function definition to understand full handshake flow
ast-grep --pattern 'recv_response() {
$$$
}'Repository: NVIDIA/IsaacTeleop
Length of output: 44
Harden WebSocket upgrade validation to check HTTP status line and required headers.
Line 193 validates only by checking for substring "101", which is insufficient per RFC 6455. Validate the full HTTP status line (e.g., HTTP/1.1 101 Switching Protocols), verify required upgrade headers (Upgrade, Connection), and validate the Sec-WebSocket-Accept header using the HMAC-SHA1 challenge response before marking the connection as established.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/haply/core/haply_hand_tracking_plugin.cpp` around lines 193 -
198, Replace the fragile substring check that looks for "101" with a full
RFC6455-compliant handshake validation: parse the HTTP status line from the
response (ensure it matches "HTTP/1.1 101 Switching Protocols"), parse response
headers into a map, verify headers "Upgrade" (case-insensitive == "websocket")
and "Connection" (contains "Upgrade"), and validate the "Sec-WebSocket-Accept"
value by computing base64(sha1(SEC_WEBSOCKET_KEY +
"258EAFA5-E914-47DA-95CA-C5AB0DC85B11")) where SEC_WEBSOCKET_KEY is the
Sec-WebSocket-Key originally sent in the request (store it if not already); if
any check fails, call close() and log the response (as currently done) and
return false, otherwise proceed to mark the connection established.
Comment on lines
+349
to
+353
| else | ||
| { | ||
| // Binary or unknown | ||
| out.append(reinterpret_cast<char*>(data.data()), data.size()); | ||
| } |
There was a problem hiding this comment.
🧩 Analysis chain
🌐 Web query:
RFC 6455 client behavior for unsupported WebSocket opcodes and binary frames when the application protocol expects JSON text only.
💡 Result:
1) Received frame uses an unsupported / unknown opcode (incl. reserved 0x3–0x7, 0xB–0xF)
Per RFC 6455, if an unknown opcode is received, the endpoint MUST fail the WebSocket connection. Reserved opcodes are not valid unless defined by a negotiated extension/subprotocol, so in practice they’re treated as “unknown” by a base RFC 6455 endpoint. When failing an established connection, the endpoint SHOULD send a Close frame with an appropriate status code; for this case that is typically 1002 (Protocol Error). [1]
2) Received frame has RSV1/RSV2/RSV3 set but no negotiated extension defines it
RSV bits MUST be 0 unless an extension is negotiated. If a nonzero RSV bit is received and no negotiated extension defines it, the endpoint MUST fail the WebSocket connection (again, commonly closed with 1002). [1]
3) Received Binary frames (opcode 0x2) when the app protocol expects JSON text only
A Binary frame is not a WebSocket protocol error—it is a valid data-frame type in RFC 6455. Your application/subprotocol can reject it. If you choose to close because you “cannot accept” binary data, RFC 6455 defines close code 1003 (Unsupported Data) for “only understands text data … receives a binary message”. [1]
4) Received Text frames (opcode 0x1) that are not valid UTF‑8 / or not valid JSON
- Invalid UTF‑8 in a Text message: close with 1007 (Invalid frame payload data) (“non‑UTF‑8 … within a text message”). [1]
- Valid UTF‑8 but not valid JSON: that’s application-level, not a WebSocket framing violation; RFC 6455 doesn’t define “invalid JSON” handling. Common practice is to close with 1008 (Policy Violation) (or an app-defined 3xxx code) rather than 1002/1007. [1]
Source
[1] RFC 6455: The WebSocket Protocol (base framing rules for RSV/opcode and “fail the connection”; and close codes 1002/1003/1007/1008) https://datatracker.ietf.org/doc/html/rfc6455.html
🏁 Script executed:
# First, let's examine the broader context of lines 349-353
sed -n '300,380p' src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 2071
🏁 Script executed:
# Find the function containing this code and understand its signature/return
sed -n '250,300p' src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 1316
🏁 Script executed:
# Look at how recv_text is used and where JSON parsing occurs
rg -A 5 "recv_text" src/plugins/haply/core/haply_hand_tracking_plugin.cpp | head -60Repository: NVIDIA/IsaacTeleop
Length of output: 336
🏁 Script executed:
# Find where msg is used after recv_text
rg -B 3 -A 10 "recv_text\(msg\)" src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 457
🏁 Script executed:
# Check if there's error handling around json::parse and what happens on failure
rg -B 2 -A 15 "json::parse\(msg\)" src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 877
🏁 Script executed:
# Check for catch block around json::parse
rg -B 2 -A 25 "json j = json::parse\(msg\)" src/plugins/haply/core/haply_hand_tracking_plugin.cpp | tail -35Repository: NVIDIA/IsaacTeleop
Length of output: 1414
🏁 Script executed:
# Find the complete try-catch block for json parsing
rg -B 5 "catch.*json" src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 312
🏁 Script executed:
# Get the catch block content
rg -B 2 -A 10 "catch.*json::exception" src/plugins/haply/core/haply_hand_tracking_plugin.cppRepository: NVIDIA/IsaacTeleop
Length of output: 313
Binary frames and unknown opcodes must not be appended to the text stream.
The recv_text() function appends binary frames (opcode 0x02) and unknown/reserved opcodes to the text buffer without validation. Per RFC 6455, when an unknown opcode is received, the endpoint MUST fail the WebSocket connection with status code 1002 (Protocol Error). Binary frames can be rejected with status code 1003 (Unsupported Data) if the application only accepts text. Currently, appending binary/unknown data to the text stream will cause json::parse() to fail repeatedly, and the connection is never closed, allowing continued accumulation of invalid data.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/haply/core/haply_hand_tracking_plugin.cpp` around lines 349 -
353, In recv_text(), do not append binary frames (opcode 0x02) or
unknown/reserved opcodes to the text buffer (variable out); instead detect the
opcode on receipt and immediately initiate a WebSocket close with the
appropriate code (use 1003 Unsupported Data for binary frames and 1002 Protocol
Error for unknown/reserved opcodes), stop further processing/return after
sending the close, and avoid calling json::parse() on any accumulated invalid
data; update the opcode handling branch that currently does
out.append(reinterpret_cast<char*>(data.data()), data.size()) to perform the
close/return logic for these cases.
Comment on lines
+548
to
+628
| std::lock_guard<std::mutex> lock(m_state_mutex); | ||
|
|
||
| // Parse inverse3 array | ||
| if (j.contains("inverse3") && j["inverse3"].is_array() && !j["inverse3"].empty()) | ||
| { | ||
| const auto& inv3 = j["inverse3"][0]; | ||
| if (inv3.contains("device_id")) | ||
| { | ||
| m_state.inverse3_device_id = inv3["device_id"].get<std::string>(); | ||
| } | ||
| // Handedness from config (only in first message) | ||
| if (inv3.contains("config") && inv3["config"].contains("handedness")) | ||
| { | ||
| m_state.handedness = inv3["config"]["handedness"].get<std::string>(); | ||
| } | ||
| if (inv3.contains("state")) | ||
| { | ||
| const auto& st = inv3["state"]; | ||
| if (st.contains("cursor_position")) | ||
| { | ||
| const auto& pos = st["cursor_position"]; | ||
| m_state.cursor_position.x = pos.value("x", 0.0f); | ||
| m_state.cursor_position.y = pos.value("y", 0.0f); | ||
| m_state.cursor_position.z = pos.value("z", 0.0f); | ||
| } | ||
| if (st.contains("cursor_velocity")) | ||
| { | ||
| const auto& vel = st["cursor_velocity"]; | ||
| m_state.cursor_velocity.x = vel.value("x", 0.0f); | ||
| m_state.cursor_velocity.y = vel.value("y", 0.0f); | ||
| m_state.cursor_velocity.z = vel.value("z", 0.0f); | ||
| } | ||
| } | ||
| m_state.has_data = true; | ||
| } | ||
|
|
||
| // Parse wireless_verse_grip array | ||
| if (j.contains("wireless_verse_grip") && j["wireless_verse_grip"].is_array() && | ||
| !j["wireless_verse_grip"].empty()) | ||
| { | ||
| const auto& vg = j["wireless_verse_grip"][0]; | ||
| if (vg.contains("device_id")) | ||
| { | ||
| m_state.versegrip_device_id = vg["device_id"].get<std::string>(); | ||
| } | ||
| if (vg.contains("state")) | ||
| { | ||
| const auto& st = vg["state"]; | ||
| if (st.contains("orientation")) | ||
| { | ||
| const auto& ori = st["orientation"]; | ||
| m_state.orientation.w = ori.value("w", 1.0f); | ||
| m_state.orientation.x = ori.value("x", 0.0f); | ||
| m_state.orientation.y = ori.value("y", 0.0f); | ||
| m_state.orientation.z = ori.value("z", 0.0f); | ||
| } | ||
| if (st.contains("buttons")) | ||
| { | ||
| const auto& btn = st["buttons"]; | ||
| m_state.buttons.button_0 = btn.value("button_0", false); | ||
| m_state.buttons.button_1 = btn.value("button_1", false); | ||
| m_state.buttons.button_2 = btn.value("button_2", false); | ||
| m_state.buttons.button_3 = btn.value("button_3", false); | ||
| } | ||
| } | ||
| m_state.has_data = true; | ||
| } | ||
|
|
||
| // Send a command to keep receiving updates (set zero force) | ||
| if (!m_state.inverse3_device_id.empty()) | ||
| { | ||
| json cmd; | ||
| cmd["inverse3"] = json::array(); | ||
| json dev; | ||
| dev["device_id"] = m_state.inverse3_device_id; | ||
| dev["commands"]["set_cursor_force"]["values"]["x"] = 0; | ||
| dev["commands"]["set_cursor_force"]["values"]["y"] = 0; | ||
| dev["commands"]["set_cursor_force"]["values"]["z"] = 0; | ||
| cmd["inverse3"].push_back(dev); | ||
| ws.send_text(cmd.dump()); | ||
| } |
There was a problem hiding this comment.
Avoid socket writes while holding m_state_mutex.
The lock currently spans JSON state mutation and ws.send_text(...). If send blocks, state readers in inject_hand_data() stall and can introduce tracking jitter.
♻️ Proposed refactor
- std::lock_guard<std::mutex> lock(m_state_mutex);
+ std::string outbound_cmd;
+ bool has_outbound_cmd = false;
+ {
+ std::lock_guard<std::mutex> lock(m_state_mutex);
// Parse inverse3 array
if (j.contains("inverse3") && j["inverse3"].is_array() && !j["inverse3"].empty())
{
const auto& inv3 = j["inverse3"][0];
@@
// Send a command to keep receiving updates (set zero force)
if (!m_state.inverse3_device_id.empty())
{
json cmd;
cmd["inverse3"] = json::array();
json dev;
dev["device_id"] = m_state.inverse3_device_id;
dev["commands"]["set_cursor_force"]["values"]["x"] = 0;
dev["commands"]["set_cursor_force"]["values"]["y"] = 0;
dev["commands"]["set_cursor_force"]["values"]["z"] = 0;
cmd["inverse3"].push_back(dev);
- ws.send_text(cmd.dump());
+ outbound_cmd = cmd.dump();
+ has_outbound_cmd = true;
}
+ }
+ if (has_outbound_cmd)
+ {
+ ws.send_text(outbound_cmd);
+ }🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/haply/core/haply_hand_tracking_plugin.cpp` around lines 548 -
628, The code holds m_state_mutex while constructing and sending a WebSocket
message (ws.send_text), which can block; instead, keep the mutex only for
mutating/reading m_state and copy the needed value(s) (at minimum
m_state.inverse3_device_id) into a local variable while holding m_state_mutex,
then release the lock and build/send the JSON command using that local copy;
update the code around m_state_mutex, m_state.inverse3_device_id and
ws.send_text to perform only the minimal protected reads under the lock and move
cmd creation + ws.send_text outside the locked section (ensure
inject_hand_data()/any readers still use the mutex for state access).
| #include <cstring> | ||
| #include <iomanip> | ||
| #include <iostream> | ||
| #include <mutex> |
There was a problem hiding this comment.
🧹 Nitpick | 🔵 Trivial
Unused include: <mutex>.
The <mutex> header is included but not used anywhere in the file. Consider removing it to reduce compilation overhead.
♻️ Remove unused include
-#include <mutex>📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| #include <mutex> |
🧰 Tools
🪛 Cppcheck (2.20.0)
[information] 20-20: Include file
(missingIncludeSystem)
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/haply/tools/haply_hand_tracker_printer.cpp` at line 20, Remove
the unused `#include` <mutex> from haply_hand_tracker_printer.cpp: search for any
usage of std::mutex or related types in functions or classes (e.g., any mutex
variables or lock_guard) and if none exist, delete the `#include` <mutex> line to
avoid unnecessary compilation overhead and warnings; if a mutex is actually
needed, instead keep it and ensure the declared mutex symbol is used where
synchronization is required.
Comment on lines
+38
to
+45
| while (true) | ||
| { | ||
| plugin.update(); | ||
| frame_count++; | ||
| std::this_thread::sleep_until(program_start + frame_duration * frame_count); | ||
| } | ||
|
|
||
| return 0; |
There was a problem hiding this comment.
No signal handling for graceful shutdown.
The infinite loop has no mechanism to exit gracefully on SIGINT/SIGTERM. This means the plugin cannot be stopped cleanly, which may leave the serial port in an undefined state. The PR description mentions signal handling was added for other components; consider adding it here as well.
🛡️ Proposed fix to add signal handling
+#include <atomic>
+#include <csignal>
+
+namespace {
+std::atomic<bool> g_running{true};
+void signal_handler(int) { g_running.store(false); }
+}
+
int main(int argc, char** argv)
try
{
+ std::signal(SIGINT, signal_handler);
+ std::signal(SIGTERM, signal_handler);
+
// ... existing setup code ...
- while (true)
+ while (g_running.load())
{
plugin.update();
frame_count++;
std::this_thread::sleep_until(program_start + frame_duration * frame_count);
}
return 0;
}🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/so101/main.cpp` around lines 38 - 45, The main loop currently
runs forever and lacks SIGINT/SIGTERM handling; introduce a process-wide
atomic<bool> (e.g., running) initialized true, register signal handlers for
SIGINT and SIGTERM that set running = false, change the loop to while(running) {
plugin.update(); frame_count++; std::this_thread::sleep_until(program_start +
frame_duration * frame_count); }, and ensure you call any plugin cleanup method
(e.g., plugin.shutdown() or close serial resources) after the loop exits so the
serial port is left in a defined state.
Comment on lines
+173
to
+181
| ```fbs | ||
| table SO101ArmOutput { | ||
| shoulder_pan: float; // [-1.0, 1.0] | ||
| shoulder_lift: float; // [-1.0, 1.0] | ||
| elbow_flex: float; // [-1.0, 1.0] | ||
| wrist_flex: float; // [-1.0, 1.0] | ||
| wrist_roll: float; // [-1.0, 1.0] | ||
| gripper: float; // [-1.0, 1.0] (0.0 = open, 1.0 = closed after mapping) | ||
| } |
There was a problem hiding this comment.
Gripper normalization range inconsistency.
The Data Format section shows gripper: float; // [-1.0, 1.0] (0.0 = open, 1.0 = closed after mapping) but the Calibration section (lines 113-114) states all joints normalize to [-1.0, 1.0] centered on midpoint 2048. The "after mapping" comment suggests additional post-processing for the gripper, but this isn't documented elsewhere.
Clarify whether the gripper uses the same [-1.0, 1.0] normalization as other joints, or if there's a separate 0.0-1.0 mapping applied downstream.
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/so101/README.md` around lines 173 - 181, The README is ambiguous
about gripper normalization; update the Data Format and Calibration
documentation to explicitly state the gripper mapping and make the comment in
the SO101ArmOutput table consistent: either state that gripper is normalized to
[-1.0, 1.0] like other joints (with 0.0 as midpoint and additional downstream
mapping to [0.0,1.0] if used), or state that gripper is normalized to [0.0, 1.0]
directly (0.0=open, 1.0=closed) and adjust the Calibration description
accordingly; reference and update the SO101ArmOutput table's gripper comment and
the Calibration section so they match and include a short note describing any
downstream mapping applied.
Comment on lines
+47
to
+70
| void SO101Plugin::update() | ||
| { | ||
| if (!driver_->is_open()) | ||
| { | ||
| // Attempt to reconnect | ||
| std::cerr << "SO101Plugin: Serial port closed, attempting reconnect to " << device_port_ << std::endl; | ||
| if (!driver_->open(device_port_, baudrate_)) | ||
| { | ||
| // Push last known state even on failure | ||
| push_current_state(); | ||
| return; | ||
| } | ||
| std::cout << "SO101Plugin: Reconnected to " << device_port_ << std::endl; | ||
| } | ||
|
|
||
| if (!driver_->read_all_positions(positions_)) | ||
| { | ||
| // Read failed — connection may be lost. Close so next update() tries reconnect. | ||
| std::cerr << "SO101Plugin: Failed to read servo positions" << std::endl; | ||
| driver_->close(); | ||
| } | ||
|
|
||
| push_current_state(); | ||
| } |
There was a problem hiding this comment.
Reconnection logic silently pushes stale data on read failure.
When read_all_positions() fails (line 62-67), the driver is closed but push_current_state() is still called with the previous positions_[] values. While the header comment says "Always pushes the latest known state (even on read failure)", this could be problematic for safety-critical teleoperation if downstream consumers can't distinguish stale from fresh data.
Consider adding a mechanism to indicate data staleness (e.g., a timestamp comparison or an explicit validity flag).
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/so101/so101_plugin.cpp` around lines 47 - 70, The update() method
currently calls push_current_state() even after read_all_positions() fails, so
add explicit staleness tracking: introduce a member like positions_valid_ (bool)
or last_positions_timestamp_ and set it true/now only when
read_all_positions(positions_) returns true; set it false/clear timestamp
immediately when read_all_positions() fails or when driver_->is_open() is false
and open() fails; update push_current_state() (or its callers) to include/emit
this validity flag or timestamp so consumers can distinguish stale vs fresh
data, and ensure the flag/timestamp is reset appropriately on successful
reconnect in update().
Comment on lines
+26
to
+27
| const std::string port = (argc > 1) ? argv[1] : (env_port ? env_port : "/dev/ttyACM0"); | ||
| const int baudrate = (argc > 2) ? std::atoi(argv[2]) : (env_baud ? std::atoi(env_baud) : 1000000); |
There was a problem hiding this comment.
🧹 Nitpick | 🔵 Trivial
Invalid baudrate input silently defaults to failure.
When std::atoi(argv[2]) or std::atoi(env_baud) receives an invalid string (e.g., "abc"), it returns 0. While this will fail gracefully in driver.open() (since baudrate_to_speed(0) returns B0), the error message will be "Unsupported baudrate 0" which doesn't indicate the actual parsing issue.
♻️ Suggested input validation
- const int baudrate = (argc > 2) ? std::atoi(argv[2]) : (env_baud ? std::atoi(env_baud) : 1000000);
+ int baudrate = 1000000;
+ if (argc > 2)
+ {
+ baudrate = std::atoi(argv[2]);
+ if (baudrate <= 0)
+ {
+ std::cerr << "Invalid baudrate argument: " << argv[2] << std::endl;
+ return 1;
+ }
+ }
+ else if (env_baud)
+ {
+ baudrate = std::atoi(env_baud);
+ if (baudrate <= 0)
+ {
+ std::cerr << "Invalid SO101_BAUDRATE: " << env_baud << std::endl;
+ return 1;
+ }
+ }📝 Committable suggestion
‼️ IMPORTANT
Carefully review the code before committing. Ensure that it accurately replaces the highlighted code, contains no missing lines, and has no issues with indentation. Thoroughly test & benchmark the code to ensure it meets the requirements.
Suggested change
| const std::string port = (argc > 1) ? argv[1] : (env_port ? env_port : "/dev/ttyACM0"); | |
| const int baudrate = (argc > 2) ? std::atoi(argv[2]) : (env_baud ? std::atoi(env_baud) : 1000000); | |
| const std::string port = (argc > 1) ? argv[1] : (env_port ? env_port : "/dev/ttyACM0"); | |
| int baudrate = 1000000; | |
| if (argc > 2) | |
| { | |
| baudrate = std::atoi(argv[2]); | |
| if (baudrate <= 0) | |
| { | |
| std::cerr << "Invalid baudrate argument: " << argv[2] << std::endl; | |
| return 1; | |
| } | |
| } | |
| else if (env_baud) | |
| { | |
| baudrate = std::atoi(env_baud); | |
| if (baudrate <= 0) | |
| { | |
| std::cerr << "Invalid SO101_BAUDRATE: " << env_baud << std::endl; | |
| return 1; | |
| } | |
| } |
🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/so101/tools/so101_printer.cpp` around lines 26 - 27, The baudrate
parsing silently yields 0 on invalid input; update the logic that sets the
baudrate variable (currently using std::atoi for argv[2] and env_baud) to
validate the string first and produce a clear error when parsing fails: use a
safe parser (std::from_chars or std::stoi in a try/catch) or a digit-only check,
verify the parsed value is >0 and one of the supported baud rates, and if
invalid print an explicit error like "Invalid baudrate input: '<value>'" and
exit instead of letting driver.open() receive 0; refer to the baudrate variable
and the port/argv handling in so101_printer.cpp to locate where to add the check
and error message.
Comment on lines
+43
to
+75
| while (true) | ||
| { | ||
| if (driver.read_all_positions(positions)) | ||
| { | ||
| read_errors = 0; | ||
| std::cout << "\r"; | ||
| for (int i = 0; i < 6; ++i) | ||
| { | ||
| std::cout << std::setw(14) << std::left << kJointNames[i] << ": " << std::setw(4) << std::right | ||
| << positions[i]; | ||
| if (i < 5) | ||
| { | ||
| std::cout << " | "; | ||
| } | ||
| } | ||
| std::cout << std::flush; | ||
| } | ||
| else | ||
| { | ||
| read_errors++; | ||
| std::cerr << "\rRead error (" << read_errors << ")"; | ||
| if (read_errors > 10) | ||
| { | ||
| std::cerr << std::endl << "Too many consecutive errors, exiting." << std::endl; | ||
| return 1; | ||
| } | ||
| } | ||
|
|
||
| // Read at ~30 Hz (printer doesn't need 90 Hz) | ||
| std::this_thread::sleep_for(std::chrono::milliseconds(33)); | ||
| } | ||
|
|
||
| return 0; |
There was a problem hiding this comment.
🧹 Nitpick | 🔵 Trivial
Unreachable code and missing graceful shutdown.
Line 75 (return 0;) is unreachable due to the infinite while (true) loop. The tool relies solely on Ctrl+C for termination. Consider adding signal handling for graceful shutdown (similar to other plugins in the PR that use signal handlers), which would also make the return 0 reachable.
♻️ Suggested improvement with signal handling
+#include <csignal>
+
using namespace plugins::so101;
namespace
{
const char* kJointNames[6] = { "shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper" };
+
+volatile std::sig_atomic_t g_running = 1;
+
+void signal_handler(int /*signum*/)
+{
+ g_running = 0;
+}
}
int main(int argc, char** argv)
try
{
+ std::signal(SIGINT, signal_handler);
+ std::signal(SIGTERM, signal_handler);
+
// ... existing setup code ...
- while (true)
+ while (g_running)
{
// ... loop body ...
}
+ std::cout << std::endl << "Shutting down." << std::endl;
return 0;
}🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed.
In `@src/plugins/so101/tools/so101_printer.cpp` around lines 43 - 75, The loop in
main never exits (while (true)) so return 0 is unreachable and there is no
graceful shutdown; add a signal handler (e.g., a sig_atomic_t stop_flag or
std::atomic<bool> stopRequested and a handler for SIGINT/SIGTERM) and modify the
loop condition to check that flag instead of while (true), ensuring the handler
sets the flag to break the loop, let the loop finish cleanly (closing any
resources) and then return 0; update references in this file to use the new flag
in place of the infinite loop and ensure driver.read_all_positions/ read_errors
logic remains unchanged inside the controlled loop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Summary
Add a new plugin for the SO-101 6-DOF robot arm at
src/plugins/so101/, enabling leader-arm teleoperation within the Isaac Teleop framework via raw joint angle streaming.Components
so101_arm.fbs) — 6 normalized joint positions pushed via SchemaPusherso101_arm_plugin) — 90Hz update loopso101_printer) — diagnostic tool (no OpenXR dependency)Joint Data
shoulder_panshoulder_liftelbow_flexwrist_flexwrist_rollgripperPositions normalized to
[-1.0, 1.0]from raw 12-bit servo values (0–4095).Configuration
SO101_SERIAL_PORT/dev/ttyACM0SO101_BAUDRATE1000000SO101_COLLECTION_IDso101_leaderArchitecture
Follows the same SchemaPusher pattern as the
generic_3axis_pedalplugin:SO101ArmOutput)No external SDK dependency — communicates directly with STS3215 servos over half-duplex serial.
Hardware
Checklist
Summary by CodeRabbit
New Features
Build & Infrastructure
Documentation