From d2ebd1fe681b88ff6c023118db3e707d96719162 Mon Sep 17 00:00:00 2001 From: Krishna_Hundekari Date: Thu, 2 Jul 2026 19:13:03 -0700 Subject: [PATCH] add r1lite bringup test suite: hardware-validated scaffolding, config, and lab log - r1lite_config.py: verified topology (domain 2, 6+6+4 DOF, 0-100 grippers, topic map) with remaining TODO(recon) markers - test_00 recon + test_01 topic assertion (PASS on hardware 2026-07-03) - test_02/04: feedback + wrist-roll arm test (PASS; tabletop-safe defaults) - test_03: chassis with all gates streamed (node validated; wheel-level refusal unresolved -- see BRINGUP_LOG) - test_06: torso joint path hard-guarded after linkage-coupling incident - BRINGUP_LOG.md: full dated evidence trail; RUNBOOK.md: session commands - domain_scan.py: ROS_DOMAIN_ID sweep utility --- scripts/r1lite_test/BRINGUP_LOG.md | 480 +++++ scripts/r1lite_test/README.md | 62 + scripts/r1lite_test/RUNBOOK.md | 115 + scripts/r1lite_test/__init__.py | 0 scripts/r1lite_test/domain_scan.py | 48 + scripts/r1lite_test/r1lite_config.py | 85 + scripts/r1lite_test/r1lite_from_robot.urdf | 1842 +++++++++++++++++ scripts/r1lite_test/run_all_tests.py | 115 + scripts/r1lite_test/test_00_recon.py | 72 + .../r1lite_test/test_01_topic_discovery.py | 63 + .../r1lite_test/test_02_read_arm_feedback.py | 74 + .../r1lite_test/test_03_chassis_command.py | 168 ++ .../r1lite_test/test_04_arm_joint_command.py | 134 ++ scripts/r1lite_test/test_06_torso_command.py | 151 ++ 14 files changed, 3409 insertions(+) create mode 100644 scripts/r1lite_test/BRINGUP_LOG.md create mode 100644 scripts/r1lite_test/README.md create mode 100644 scripts/r1lite_test/RUNBOOK.md create mode 100644 scripts/r1lite_test/__init__.py create mode 100644 scripts/r1lite_test/domain_scan.py create mode 100644 scripts/r1lite_test/r1lite_config.py create mode 100644 scripts/r1lite_test/r1lite_from_robot.urdf create mode 100644 scripts/r1lite_test/run_all_tests.py create mode 100644 scripts/r1lite_test/test_00_recon.py create mode 100644 scripts/r1lite_test/test_01_topic_discovery.py create mode 100644 scripts/r1lite_test/test_02_read_arm_feedback.py create mode 100644 scripts/r1lite_test/test_03_chassis_command.py create mode 100644 scripts/r1lite_test/test_04_arm_joint_command.py create mode 100644 scripts/r1lite_test/test_06_torso_command.py diff --git a/scripts/r1lite_test/BRINGUP_LOG.md b/scripts/r1lite_test/BRINGUP_LOG.md new file mode 100644 index 0000000000..2016053daf --- /dev/null +++ b/scripts/r1lite_test/BRINGUP_LOG.md @@ -0,0 +1,480 @@ +# R1 Lite Bring-up Log (living lab notebook) + +Verbose, dated evidence trail of the R1 Lite dimos integration. Append-only: +new findings get new dated entries; earlier observations are never rewritten +(if one turns out wrong, a later entry says so and why). At the end of the +integration this gets distilled into a polished README in the style of +[`scripts/r1pro_test/README.md`](../r1pro_test/README.md). + +Branch: `krishna/task/r1lite-integration` (stacked on +`mustafa/task/r1pro-coordinator-integration`, tip `9ab0ab318`; main was 204 +commits ahead of the shared base at branch time — Mustafa nudged to rebase). + +--- + +## 2026-07-02 — Day 1: discovery, environment, credentials, first recon + +### 1. Finding the robot (the hard way, then the surprise) + +**Hypothesis:** R1 Lite ships like the R1 Pro — ethernet at `192.168.2.150` +(factory) or no IPv4 at all. + +**Tests & findings:** +- Laptop `enp130s0` was `NO-CARRIER` until cable plugged (check: + `cat /sys/class/net/enp130s0/carrier`). +- Added runtime IPs to laptop without touching the saved "lidar" profile: + `nmcli device modify enp130s0 +ipv4.addresses 192.168.2.100/24 +ipv4.addresses 192.168.123.100/24` + (runtime-only — reverts on reconnect; saved profile keeps only 192.168.1.5). +- Pings to `.2.150` / `.123.150`: nothing. +- **IPv6 all-nodes multicast ping** (`ping -6 ff02::1%enp130s0`): one foreign + responder, `fe80::c68f:8fbd:376b:a2`, MAC `e0:51:d8:1f:04:75` — works + because every UP interface always has a link-local IPv6 even with zero + IPv4 config. This is the no-sudo substitute for Mustafa's tcpdump trick. +- Ping sweep of 192.168.{1,2,123}.0/24 + `ip neigh`: `192.168.1.85` + answered with the SAME MAC → device found. `nmap -sV`: hostname + **`r1lite`**, OpenSSH 8.9p1 (Ubuntu 22.04), only port 22 open. +- **Gotcha (ARP flux):** laptop had TWO interfaces on 192.168.1.0/24 (wifi + .110 + eth .5), and `.85` later turned out reachable only via WIFI. The + robot answered ARP for its *wifi* address out its *ethernet* port. + Resolved identity definitively by comparing SSH host keys over both paths + (`ssh-keyscan 192.168.1.85` vs `ssh-keyscan fe80::...%enp130s0`) — + **identical ed25519 key → same machine on both paths.** + +**Conclusion:** direct cable to robot confirmed; robot also on office wifi +at `192.168.1.85` (someone provisioned it — non-factory state); ethernet +port *appeared* to have no IPv4 (see §5 for the correction). + +### 2. Laptop environment (docker + py3.10) + +- ghcr pulls needed `read:packages` on the gh token + (`gh auth refresh -h github.com -s read:packages`). +- **Wrong-image trap:** compose file `docker/dev/docker-compose.yaml` uses + `ghcr.io/dimensionalos/dev:latest` — that's the NON-ROS track (no + `/opt/ros`). The ROS-track image is **`ghcr.io/dimensionalos/ros-dev:dev`** + (what `bin/dev` uses). Launched manually: + `docker run -d --name dimos-dev-r1lite --network host -v :/app ... ros-dev:dev` +- `/app/.venv` was the host's py3.12 venv (bind mount!) → interpreter + doesn't exist in container. Rebuilt per Mustafa §12.A: + `rm -rf .venv && UV_PYTHON=3.10 uv sync --all-extras --no-extra dds --no-extra unitree-dds` + → `rclpy OK`, `dimos OK`. `.envrc → .envrc.r1pro` symlinked. +- ROS 2 domain sweep 0–101 from container (scanner now at + `scripts/r1lite_test/domain_scan.py`): **zero foreign participants** — + robot stack simply not running (confirmed later, §5). + +### 3. Credentials saga + +**Hypothesis:** Galaxea default `nvidia`/`nvidia` (all their docs say so). + +**Tests:** 3 manual + scripted attempts as `nvidia@192.168.1.85` → all +`Permission denied`. Web docs (R1/R1 Lite/R1 Pro) all confirm nvidia/nvidia +as factory default → concluded unit was re-provisioned. + +**Resolution:** actual login is user **`r1lite`** (robot's own hostname as +username), password obtained from [fill in: who/where]. Factory docs were +wrong for this unit because it's NOT the documented Orin config (see §5). +Key auth installed via `ssh-copy-id`; laptop `~/.ssh/config` has alias +`r1lite` → `r1lite@192.168.1.85`. Robot's `last` showed a login +2026-05-22 from `192.168.1.105` (the provisioner's machine, presumably). + +### 4. Branch & scaffolding + +- `krishna/task/r1lite-integration` branched off Mustafa's branch tip, + tracking it (plain `git pull` follows his pushes). Strategy: stay + additive, sync often, never edit his r1pro files here. +- `scripts/r1lite_test/` scaffolding ported from `r1pro_test/`: + `r1lite_config.py` (single source of truth, `TODO(recon)` markers), + `test_00_recon.py` (new — graph dump), `test_01..04,06` (parameterized, + arm tests adapt DOF from live feedback), `run_all_tests.py`, + `domain_scan.py`, `README.md`. All syntax-checked in container. + Uncommitted pending hardware reconciliation. + +### 5. First SSH recon — most prior assumptions overturned + +Ran read-only identity/env recon over SSH. Findings: + +| Assumed (from R1 Pro) | Actual (R1 Lite unit) | +|---|---| +| Jetson Orin, aarch64 | **x86_64 PC**, Ubuntu 22.04.5 | +| user `nvidia` | user **`r1lite`** | +| eth has no IPv4 | **eth `enp2s0` = `10.42.0.2/24`** (we only probed 192.168.x!) | +| ROS_DOMAIN_ID=41 | **`ROS_DOMAIN_ID=02`** (bashrc; note the string "02") | +| maybe ROS 1 | **ROS 2 Humble installed** — no upgrade needed | + +More robot-side facts (from `~/.bashrc`, `ls ~`): +- `ROS_LOCALHOST_ONLY=0`, `ROS_IP=10.42.0.2` → robot pre-configured to + talk ROS over the ethernet cable; laptop just needs a `10.42.0.x` addr. + Mustafa's netplan step is NOT needed on this unit. +- Home dir: `can.sh`, `setup_can.sh`, `can.txt`, `first_start`, `lite`, + `galaxea/install/`, `GalaxeaDataset`, `realtime_tools`, `test`, + `findrobot_{server,client}.sh`, **`fastdds_unicast.xml`**, + **`super_client_configuration_file.xml`** (DDS discovery-server/unicast + configs — check whether launches depend on them before assuming + multicast discovery works). +- `can0` interface DOWN; no ros/hdas/mobiman processes → stack fully idle. + Explains the empty domain sweep in §2. + +**Open questions carried forward:** +1. What do `first_start`, `lite`, `can.txt`, the findrobot scripts do? +2. Does the stack launch depend on the FastDDS XML configs (unicast / + discovery server), and do we need matching client config in the container? +3. Topic names/types vs R1 Pro `/hdas/*` + `/motion_target/*` (test_00). +4. Arm/torso DOF (expected 6/4), torso safe home pose. +5. Chassis 3-gate problem present on R1 Lite's chassis node? +6. Camera/lidar inventory. + +**Next session plan (agreed division of labor):** Krishna drives all +state-changing robot commands from a Cursor Remote-SSH window ("Console A"); +Claude runs read-only SSH recon + laptop/container work. Steps: laptop +`10.42.0.100/24` → read startup scripts → Krishna starts CAN+stack → +`test_00_recon` from container on domain 2 → reconcile `r1lite_config.py`. + +### 6. Startup-machinery recon complete (read-only pass over SSH) + +**The password mystery, solved for real:** Galaxea's own `findrobot_server.sh` +hardcodes per-model credentials: `R1-LITE) passwd="1"`, `R1-PRO) passwd="nvidia"`. +And `can.sh`/`sessions.sh` do `echo "1" | sudo -S ...` throughout. So **R1 Lite +factory login = user `r1lite` / password `1`** — the unit was never +re-provisioned; the public docs just only cover the Orin models. (The office +wifi join was still done by someone — login 2026-05-22 from 192.168.1.105.) + +**Hardware identity** (`/opt/galaxea/body/hardware.json`): R1-LITE with +ARM=A1X, TORSO=T0, ECU=E03, CHASSIS=C1 (+CS1 steering sw), ARM_END=EE0, +HEAD_CAMERA=HC0, WRIST_CAMERA=WC1. Compare R1 Pro: chassis W1 3-wheel swerve. + +**DDS mode — multicast confirmed, three ways:** +- `.bashrc`: `ROS_DISCOVERY_SERVER` and `FASTRTPS_DEFAULT_PROFILES_FILE` lines + are COMMENTED OUT; active: `ROS_DOMAIN_ID=02`, `ROS_LOCALHOST_ONLY=0`, + `ROS_IP=10.42.0.2`. +- No listener on port 11811 while idle. +- `prestart_prepare.sh` / `r1lite_prestart_prepare.sh` (sourced by every + module start script) set only paths/setup.bash — no DDS env. +- Caveat: the boot profile DOES start `fastdds discovery -p 11811` + (ros2_discovery.yaml) but nothing points at it — vestigial. The + `findrobot_server.sh` script would switch the robot INTO discovery-server + mode (rewrites bashrc + reboots) — do NOT run it. + +**Boot machinery decoded:** +- Entry point: `robot_startup.sh boot ` → tmuxp loads + every `*.yaml` in the profile dir; `robot_startup.sh kill` = tmux + kill-server + ros2 daemon stop. Writes profile name to /tmp/start_type.log + (currently absent → nothing booted since reboot). +- Profiles under `sessions.d/`: ATCStandard + ATCSystem both have + **`R1LITEBody.d`** (mobiman.yaml identical; teleop/system differ only + cosmetically). Also R1LITEVRTeleop.d, R1LITEVRATC.d, ATCHostStandard/R1LITET.d. +- **`R1LITEBody.d` contents** (tmux sessions → commands): + - `hdas`: `start_hdas_r1lite.sh` = CAN bringup (down/up can0, CAN-FD 1M/5M) + + `ros2 launch HDAS r1lite.py`; second window: head camera + realsense + wrist cameras. + - `mobiman`: chassis (`r1_lite_chassis_control_w_o_eepose_launch.py`), + torso speed control (R1LITE arg), gripper controller (R1LITE), + ee_pose, jointTrackerdemo_fast, relaxed_ik left+right. + - `ros_discovery`: vestigial fastdds discovery server. + - `system`: system_manager + robot_monitor. + - `teleop`: **`start_r1lite_tabletop_gello_teleop.sh`** — GELLO leader-arm + teleop; will try to own the arms. Kill this session after boot unless + the GELLO rig is attached (likely fails w/o hardware, but don't risk it). + - `tools`: data_collection + monitors. +- `can.txt` = old candump debug capture (the 175KB "dump of shit"); `~/lite/` + = stereo calibration artifacts (Chinese dirnames); both ignorable. +- Workspace `~/galaxea/install/` includes full OCS2 MPC stack + (ocs2_mobile_manipulator etc.), livox_ros_driver2, greenwave_monitor — + richer than expected; relaxed_ik + jointTracker are the arm command paths. +- Quirk: prestart scripts hardcode `/home/nvidia/galaxea/maps` for MAP_DIR — + copied from Orin models, harmless for us. + +**Agreed boot command for step 3 (Krishna, Console A):** +```bash +cd ~/galaxea/install/startup_config/share/startup_config/script +./robot_startup.sh boot ../sessions.d/ATCStandard/R1LITEBody.d +# wait ~30-60s, then: tmux ls (expect: hdas, mobiman, ros_discovery, system, r1lite_teleop, tools) +tmux kill-session -t r1lite_teleop # keep GELLO teleop off the arms +# stop everything later with: ./robot_startup.sh kill +``` + +## 2026-07-03 — Day 2: stack boot + full topic recon (FIRST CONTACT) + +**Boot:** Krishna ran `robot_startup.sh boot ../sessions.d/ATCStandard/R1LITEBody.d` +(Console A). Arms/grippers twitched on HDAS init (expected health sign). +6 tmux sessions up (hdas, mobiman, r1lite_teleop, ros_discovery, system, +tools), 31 ROS processes. GELLO teleop session killed after boot. + +**Laptop wiring:** `nmcli device modify enp130s0 +ipv4.addresses 10.42.0.100/24` +→ ping 10.42.0.2 at ~0.4ms over cable. Container on ROS_DOMAIN_ID=2 sees +the full graph via plain multicast — NO XML/discovery-server config needed. + +**test_00_recon results (132 topics, 29 nodes):** + +| Segment | Feedback topic | DOF | Rate | +|---|---|---|---| +| arm left/right | /hdas/feedback_arm_* | **6 each** | 200 Hz | +| torso | /hdas/feedback_torso | **4** | **488 Hz** | +| chassis | /hdas/feedback_chassis | 3 | 200 Hz | +| gripper l/r | /hdas/feedback_gripper_* | **1 each** | 200 Hz | +| aggregate | /joint_states | 25 (?) | 500 Hz | +| hands | /hdas/feedback_hand_* | silent | 0 Hz (not fitted) | + +Upper body = 4+6+6 = **16 joints** (R1 Pro: 18). MotorCommandArray slicing +for the connection module: torso 0-3, left 4-9, right 10-15 (+ grippers +handled separately via target_position_gripper_*). + +**Command topics confirmed subs>0:** target_joint_state_{arm_left,arm_right, +torso}, target_position_gripper_*, target_speed_chassis, target_speed_torso +(TwistStamped — new vs R1 Pro), brake_mode, chassis_acc_limit. The +`target_joint_state_arm_*` pubs=1 scare resolved: jointTracker holds idle +publishers (0 Hz) — no contention. + +**Chassis gate suspects all present:** /controller (hdas_msg/ +ControllerSignalStamped, pubs=1 subs=1), brake_mode + chassis_acc_limit +(subs=1 each), /motion_control/chassis_speed (pubs=1). NOTE: R1 Pro's +gatekeeper /cmd_vel topic does NOT exist — command path is +target_speed_chassis directly. Whether the 3-gate unlock ritual is needed +is test_03's job. + +**Perception inventory (differs a lot from R1 Pro):** wrist L/R = full +RealSense stacks (color + depth + aligned + pointcloud + compressed). +Head = STEREO RGB pair (left_raw/right_raw compressed) with NO depth topic +(calib/head_left+right camera_info suggest stereo depth is computed +elsewhere/offline). NO chassis cameras. NO lidar topic (livox_ros_driver2 +installed but not in R1LITEBody profile). /robot_description published +(pubs=2) → URDF obtainable straight off the topic. TF + tf_static live. + +**Extras noticed:** /hdas/feedback_*_arm_wrench (force/torque!), /hdas/bms, +per-ECU version topics, relaxed_ik EE-pose command path, system_manager +manipulation/navi action servers, /vla/prompt_echo (VLA hooks), OCS2 stack +in workspace. R1 Lite is teleop/data-collection oriented out of the box. + +**Config reconciled:** r1lite_config.py updated with all verified values; +EXPECTED_TOPICS now 12 topics incl. grippers + IMUs. Remaining TODO(recon): +joint names per topic, torso home pose, chassis gating behavior, head-depth +story, lidar presence. + +### Day 2 continued — fill-ins + test_01 PASS + +- **Joint names:** HDAS feedback topics carry placeholder names + (`['left_arm']`, `['torso']`, ...) — ordering by convention, like R1 Pro. + `/joint_states` (25 joints) gives the real naming: steer_motor_joint1-3 + + wheel_motor_joint1-3 (**3-wheel swerve**, C1 ≈ W1 family), torso_joint1-3, + {left,right}_arm_joint1-6, {left,right}_gripper_finger_joint1-2. +- **URDF captured** → `scripts/r1lite_test/r1lite_from_robot.urdf` (44KB, + 33 joint tags). Meshes referenced as + `package://mobiman/urdf/R1_Lite/meshes/*.STL` → live on robot at + `~/galaxea/install/mobiman/share/mobiman/urdf/R1_Lite/meshes/` — scp later + for data/r1_lite_description. Wrist cams = **D405** (same as R1 Pro); + link naming mirrors R1 Pro conventions. +- **Torso 3-vs-4 discrepancy:** URDF has only torso_joint1-3, but + /hdas/feedback_torso streams 4 values @ 488 Hz. Suspect coupled/parallel + lift motors. Command-side expectation TBD from torso start script / + test_06 — treat torso command DOF as OPEN until verified. +- **"A message was lost!!!" spam** during `ros2 topic echo` on 200-488 Hz + topics = Python CLI subscriber too slow, benign. Watch for the same + symptom in the dimos connection later though (Mustafa's sensor-drop + class of problem). +- **test_01_topic_discovery: PASS** (132 topics visible, all 12 expected + present). First formal green check — laptop/container/cable/DDS/config + verified end-to-end. (Restart hygiene notes: container `docker start` + + 3 setup lines; nmcli runtime IP re-add after suspend; robot tmux + sessions survive laptop reboots.) + +### Day 2 continued — tabletop config discovered, resting pose captured + +- **Krishna flagged before any motion test: R1 Lite is a TABLETOP bimanual** + (photo taken) — arms mounted base-up on a flat platform, folded inward, + grippers facing each other; camera tower between them; torso = parallel- + linkage lift column; robot TETHERED (charger + ethernet). R1-Pro-style + "nudge joint 1" is unsafe (horizontal sweep into tower/other arm). + Motion tests re-ordered: gripper → wrist joint → torso (blocked) → + chassis (blocked until untethered). +- **Torso launch decoded** (`torso_control_r1_lite.launch.py`): MPC task-space + controller — v_{x,z,pitch,yaw} limits (0.15/0.15/0.3/0.3) and bounding box + x∈[±0.45], z∈[0.175,0.72], pitch∈[±1.25], yaw∈[±0.05] (yaw ~locked). + Internally loads R1's r1_v2_1_0.urdf with robot_model=R1_LITE switch. + DANGER: if target_joint_state_torso takes task-space values, a naive + [0,0,0,0] "zero pose" is BELOW z_min → test_06 LOCKED until semantics + verified. No home pose constant found in the launch file. +- **Resting pose captured (read-only):** arm_left ≈ all-zero, arm_right ≈ + all-zero → **folded tabletop pose IS joint zero**. Torso [−0.002, −0.002, + 0.004, 0.0] → feedback is 4 JOINT angles (not task-space; task z would be + ≥0.175). Grippers: left 101.8, right 96.0 → **0–100-ish units (percent + or mm), NOT radians/meters** — differs from R1 Pro catalog (meters). +- Planned first actuation (pending discussion + subscriber probes): + gripper no-op (target = current) → small close (−15) → back. One DOF, + cm-scale, clear path. + +### Day 2 — FIRST COMMANDED MOTION (gripper) ✅ + +Command-path probes: target_position_gripper_left consumed by +r1_gripper_controller (+greenwave_monitor watcher); target_joint_state_torso +consumed by **r1_lite_jointTracker_demo_node** (joint-space!) — NOT the MPC. +So two torso paths: joint targets → jointTracker, task-space speeds +(target_speed_torso) → MPC node. Torso joint targets = 4 joint angles +(matching feedback); resting ≈ zeros. control_torso has 2 publishers +(jointTracker + MPC) — command via ONE path only, watch for contention. +All /motion_target subscribers are BEST_EFFORT/VOLATILE QoS. + +Gripper sequence (left, empty, hands clear): +1. `--once` publish target=101.8 (no-op): accepted, zero movement. ✓ +2. `--once` publish target=85: **NO effect** — controller ignores one-shots. +3. `ros2 topic pub -r 10` streaming target=85: gripper closed smoothly + 101.84 → 87.0, and **stopped the moment the stream stopped** (Ctrl-C at + ~0.6s, before reaching target). → r1_gripper_controller is a + follow-the-stream tracker: commands must be CONTINUOUS, dead-man style. + Same design consequence as R1 Pro: the dimos connection module must + publish gripper/joint targets every tick, not fire-and-forget. + +Chain validated end-to-end: laptop container → DDS multicast over cable → +mobiman gripper controller → HDAS → CAN → motor. First dimos-side actuation +of the R1 Lite. + +### Day 2 — ARM COMMAND PATH VALIDATED ✅ (wrist-roll test) + +Gripper reopen settled at 100.84 (~1 unit deadband short of 101.8 target — +normal). Then left arm wrist roll (joint 6): streamed target_joint_state_ +arm_left at 10 Hz holding joints 1-5 at rest, joint 6 → 0.2 rad. Tracked to +exactly 0.200, other joints unmoved, physical rotation ~11° in place +confirmed; streamed back to 0.001 — home. Same dead-man stream semantics +as the gripper (jointTracker follows while stream lives). + +**Hardware validation status:** feedback ✓ (all segments), gripper ✓, +arm joints ✓ — via streamed /motion_target targets, exactly the R1 Pro +pattern minus DOF/unit differences. Remaining: torso (joint path via +jointTracker, tiny delta — planned, not yet run), chassis (BLOCKED until +untethered + clear floor; gating behavior unknown). Every fact needed for +R1LiteConnection is now in hand: 16 upper-body joints (4+6+6), separate +0-100-unit grippers, stream-required command paths, BEST_EFFORT QoS, +domain 2, multicast, topic map reconciled in r1lite_config.py. + +### Day 2 — TORSO JOINT-PATH TEST: FAILED SAFE ⚠️ (important negative result) + +Precheck: control_torso had NO active publisher (verified on robot via +ros2 topic hz — container can't deserialize hdas_msg custom types; note: +all dimos-facing topics are standard msgs, hdas_msg only on internal +/motion_control layer). + +Streamed target_joint_state_torso holding joints 1,2,4 at rest and joint 3 +→ +0.05 rad @ 0.3 rad/s. **Robot SHOOK** (motor oscillation), feedback never +tracked (stayed ~0.001). Ctrl-C → shaking stopped instantly (dead-man +confirmed again), torso settled at ~zeros. No damage. + +**Diagnosis:** torso is a PARALLELOGRAM lift — joints 1-3 mechanically +coupled. "Move joint 3 alone, hold 1-2" is kinematically unreachable; the +tracker drove motor 3 against the linkage while 1-2 held → oscillation. +Arm test worked because arm joints are serial/independent; torso joints +are not. This is exactly why Galaxea's torso controller is task-space +(x/z/pitch MPC with linkage-aware solver). + +**Rules going forward:** +- DO NOT send single-joint deltas to target_joint_state_torso. Any joint- + space torso command must be a known linkage-consistent tuple (e.g. a + pose previously read from feedback). +- v1 dimos integration: torso FIXED (mirrors Mustafa deferring R1 Pro + torso planning). Torso motion later via target_speed_torso task-space + path (MPC, bounded, linkage-aware) as a separate designed experiment. +- test_06_torso_command.py marked DO-NOT-RUN as written (R1-Pro-style + joint targets) — needs rewrite to task-space or consistent-tuple form. + +Hardware validation final for the session: feedback ✓, gripper ✓, arm ✓, +torso joint-path ✗ (by design of the mechanism, not a bug in our chain), +chassis untested (tethered). + +### Day 2 — Official R1 Lite documentation research (web) + +Sources: galaxea-dynamics.com product/spec pages, docs.galaxea-ai/dynamics +R1Lite software-introduction + VR-teleop pages (site flaky — content +recovered via search snippets), aifitlab listing. + +**Platform identity:** R1 Lite = data-collection/teleop-oriented dual-arm +mobile manipulator, $39,999 base. 23 total DOF = 2×6 (arms) + 3 (torso, +kinematic) + 6 (chassis) + 2 (grippers). 1280×670mm, vertical workspace +0–1.7m. Onboard PC: **Intel i9-12900HK 14-core / 32GB** (matches our +x86_64 finding; the aifitlab "Jetson AGX Orin" claim is wrong for this +model — that's R1/R1 Pro). + +**Arms:** Galaxea A1X, 6-DOF, 600mm reach, 4.2 kg each, payload 3 kg +rated @600mm / 5 kg max, ±0.1mm repeatability, max EE speed 5 m/s (!), +spherical wrist. + +**Grippers:** Galaxea G1 force-controlled parallel gripper. +**Position range 0–100 CONFIRMED** (VR-teleop config: opening_threshold +80.0/max 100.0, closing 0.0/min 0.0 → 0=closed, 100=open). Our 101.8 +reading = fully open w/ slight overtravel. Units resolved. + +**Torso (matches our negative result):** official software guide splits +torso control into TWO nodes and warns: torso velocity signals and torso +joint signals "cannot be used together — joint conflicts, safety risk"; +**JointTracker starts with disable_torso=true BY DEFAULT** ("users will +not be able to control the torso via target_joint_state_torso"). +Our unit shook on joint targets → its boot config evidently enables it +(or partially) — worth checking start_r1_lite_jointTrackerdemo_fast.sh +args someday, but v1 plan stands: torso FIXED, task-space path later +(v_x ±0.1 m/s, v_z ±0.1 m/s, w_pitch/w_yaw 0.3 rad/s of torso_link3 rel +base_link, right-hand rule). + +**Chassis:** 3 self-developed steering-wheel (swerve) modules = 6 DOF, +360° omni, modes translation/spin/Ackermann, max 1.5 m/s; control node +takes vector commands x, y, w simultaneously. → our chassis test = +target_speed_chassis TwistStamped with tiny vx, no gatekeeper needed +per docs (R1 Pro's 3-gate saga may be W1/R1Pro-specific — still verify). + +**Head:** pure binocular RGB stereo (no depth) ✓. Wrist: D405 depth cams ✓ +(WC1 option fitted on our unit). + +### Day 2 — CHASSIS MYSTERY SOLVED: RC mode map (no bypass needed!) + +Chassis node (`chassis_type: new_W1` — same W1 family as R1 Pro) subscribes +/controller and vetoes software unless the RC grants mode 5. Krishna mapped +the RC empirically (4 switches: sw1/sw4 two-position, sw2/sw3 three-position): + +| Switch combo | mode | Meaning | +|---|---|---| +| all switches position 1 | **5** | SOFTWARE/AUTO — external cmds may drive (the value Mustafa's gatekeeper faked!) | +| sw1@2 + sw2@3 | 2 | brake/lock (matches R1 Pro mode-2 braking) | +| sw1@2 + sw2@3 + sw3@mid | **3** | RC MANUAL — sticks drive (RC-driving validated live!) | +| other single flips | 255 | invalid combo | +| RC OFF | 3 | receiver failsafe = "manual, sticks centered" → chassis obediently parked, software vetoed | + +Explains everything: all "chassis dead" symptoms (0.3mm/s creep with gates +streamed, RC "not working") were the RC being off/in manual with no stick +input. The R1 Pro bypass (launch remap + fake mode=5) is NOT needed on this +unit — RC ON + all-switches-position-1 = legitimate mode 5, and the RC +remains a live takeover/brake layer (flip sw1 = revoke software control). +NO robot files were modified. Procedure for software chassis control: +RC ON, mode 5 verified via /controller, then stream gates+speed (test_03). + +### Day 2 (late) — THE CHASSIS SAGA: unresolved, fully mapped ⏸️ + +Chronology of the wheels-won't-turn hunt (all hypotheses tested and killed): +1. Software cmds produced 0.3mm/s creep → suspected R1 Pro 3-gate problem. +2. Streamed acc_limit + brake=false + Gate-1 subscriber (4-shell test, then + test_03 rewritten to do all gates itself): still creep. +3. Discovered RC mode map (see earlier entry); RC manual (mode 3) DROVE the + robot — motors/VCU proven working at that moment. +4. Mode 5 verified + all gates streamed: STILL creep. Chassis pane logs + (tmux capture) showed `target_speed_command 0.05` AND + `last_send_speed_command 0.05` — node accepts targets, ramp reaches + full speed, publishes motor commands. Every software gate open. +5. /motion_control/control_chassis capture: v_des=[0.05×3] on wheels but + **`mode: 0`** in every MotorControl msg — suspicious but unconfirmed + (never captured a working-drive reference for comparison). +6. **Chassis node LATCHES last target forever — NO staleness timeout** + (kept publishing v_des=0.05 minutes after stream stopped). RULE: always + end chassis sessions with a zero-stream. (Arm tracker does have + dead-man behavior; chassis does not!) +7. Later in the same boot, RC manual STOPPED driving too. Eliminated: RC + (all 4 axes sweep 240-1807, link healthy), e-stop (released, no error), + VCU faults (w1-w6 all zero), battery (54V, 99.9%!), stack state (fresh + restart), brake/acc streams during RC manual. NOTHING moves the wheels. +8. My missteps logged honestly: premature "DDS corruption" call (was my + own `ros2 daemon stop` + short timeouts emptying the CLI cache — + container discovery direct via rclpy was always fine); proposed + Mustafa-bypass (remap+fake mode) would NOT have helped (mode gate + provably open — ramp ran). +9. **Lead for next session:** Galaxea unboxing docs say R1 Lite ships with + THREE remotes: torso, chassis, and a separate WIRELESS E-STOP remote + ("software-based switch halting all operations"). We never checked it — + a pressed/dead e-stop fob (failsafe = STOP) would explain everything + incl. "worked earlier, died mid-session". CHECK THE E-STOP REMOTE FIRST. + Also in docs: "Wheel Motor Zero-Point Calibration Instruction" exists; + support: product@galaxea-dynamics.com. + +Session totals: gripper ✅ arm ✅ torso ⚠️(negative result, guarded) +chassis ⏸️(node-level fully validated; wheel-level refusal unresolved). diff --git a/scripts/r1lite_test/README.md b/scripts/r1lite_test/README.md new file mode 100644 index 0000000000..fd3a5efaff --- /dev/null +++ b/scripts/r1lite_test/README.md @@ -0,0 +1,62 @@ +# R1 Lite Integration — Bring-up Test Suite (scaffolding) + +Robot-side ROS 2 validation for the Galaxea R1 Lite, ported from +[`scripts/r1pro_test/`](../r1pro_test/) (Mustafa's R1 Pro work). These +scripts talk **plain ROS 2** to the robot — no dimos — so we can prove the +robot obeys commands before layering the dimos connection Module on top. + +> **Status: SCAFFOLDING.** Every topic name, joint count, domain ID, and +> pose in [`r1lite_config.py`](r1lite_config.py) is inherited from the R1 +> Pro and marked `TODO(recon)`. Nothing here has run against R1 Lite +> hardware yet. **Run `test_00_recon.py` first and reconcile the config.** + +## Known R1 Lite differences from R1 Pro (verify during recon) + +| Aspect | R1 Pro | R1 Lite (expected) | +|---|---|---| +| Arm DOF | 7 per arm | **6 per arm** (Galaxea spec) | +| Upper-body joints | 18 (4+7+7) | likely **16** (4+6+6) — confirm | +| ROS | ROS 2 Humble | docs ship ROS1 **and** ROS2 tracks — confirm which is installed | +| Chassis 3-gate problem | yes | unknown — test_03 assumes it may differ | +| ROS_DOMAIN_ID | 41 | unknown — run `domain_scan.py` | + +## Order of operations + +```bash +# 0. Find the robot's domain (robot stack must be running) +python3 scripts/r1lite_test/domain_scan.py + +# 1. Recon: dump topics/nodes/rates/joint-counts; then edit r1lite_config.py +export ROS_DOMAIN_ID= +python3 scripts/r1lite_test/test_00_recon.py + +# 2. Once config is reconciled, run the graded suite (single DDS session) +python3 scripts/r1lite_test/run_all_tests.py --skip-chassis --skip-arm # read-only first +python3 scripts/r1lite_test/run_all_tests.py # full, robot moves +``` + +All commands run **inside the dev container** (`ghcr.io/dimensionalos/ros-dev:dev`, +`--network host`) with `source /opt/ros/humble/setup.bash` and the py3.10 +venv active — same environment as the R1 Pro bring-up. + +## Files + +| File | Purpose | +|---|---| +| `r1lite_config.py` | Single source of truth for topics/DOF/domain — **the file to edit after recon** | +| `domain_scan.py` | Sweep ROS_DOMAIN_ID 0-101 to find the robot | +| `test_00_recon.py` | Read-only graph dump: topics, nodes, rates, joint counts | +| `test_01_topic_discovery.py` | Assert expected topics are visible | +| `test_02_read_arm_feedback.py` | Read left-arm JointState feedback | +| `test_03_chassis_command.py` | Drive chassis briefly (robot moves) | +| `test_04_arm_joint_command.py` | Move one arm joint and return (robot moves) | +| `test_06_torso_command.py` | Move torso to home pose and back (robot moves) | +| `run_all_tests.py` | Graded runner, one rclpy session, safety confirms between steps | + +## Open questions the recon must close + +1. ROS 1 or ROS 2 on this unit? (If ROS 1: apply Galaxea's ROS2 upgrade first.) +2. Real topic names + message types vs the R1 Pro `/hdas/*` + `/motion_target/*` set. +3. Arm/torso DOF and safe torso home pose (from the robot's startup scripts). +4. Does the chassis need a gatekeeper / have the 3-gate problem? +5. RMW implementation + FastDDS version. diff --git a/scripts/r1lite_test/RUNBOOK.md b/scripts/r1lite_test/RUNBOOK.md new file mode 100644 index 0000000000..e383be18cc --- /dev/null +++ b/scripts/r1lite_test/RUNBOOK.md @@ -0,0 +1,115 @@ +# R1 Lite Session Runbook + +Everything validated 2026-07-02/03, with exact recreate commands. +Consoles: **[laptop]** = plain laptop shell · **[robot]** = `ssh r1lite` · +**[container]** = shell inside `dimos-dev-r1lite`. + +--- + +## One-time setup (already done — recorded for rebuild-from-scratch) + +| What | Command | Status | +|---|---|---| +| ghcr access (needs `read:packages`) | `gh auth refresh -h github.com -s read:packages` then `gh auth token \| docker login ghcr.io -u KrishnaH96 --password-stdin` | ✅ | +| Pull ROS-track dev image (NOT `dev:latest` — that one has no ROS) | `docker pull ghcr.io/dimensionalos/ros-dev:dev` | ✅ | +| Create container | `docker run -d --name dimos-dev-r1lite --network host -v /home/krishnah/krishnah/dimos:/app -v /tmp/.X11-unix:/tmp/.X11-unix -v $HOME/.Xauthority:/root/.Xauthority:rw -e PYTHONUNBUFFERED=1 -e PYTHONPATH=/app -e DISPLAY=$DISPLAY -it ghcr.io/dimensionalos/ros-dev:dev /bin/bash` | ✅ | +| Rebuild venv as py3.10 (Humble/rclpy) — in container | `cd /app && ln -sf .envrc.r1pro .envrc && rm -rf .venv && UV_PYTHON=3.10 uv sync --all-extras --no-extra dds --no-extra unitree-dds` | ✅ | +| SSH alias + key | `~/.ssh/config` Host `r1lite` → `r1lite@192.168.1.85`; `ssh-copy-id r1lite` (password: `1`) | ✅ | +| Persistent laptop IP (2026-07-03) | `nmcli connection modify lidar +ipv4.addresses 10.42.0.100/24` — "lidar" profile now carries 192.168.1.5 AND 10.42.0.100 on every link-up | ✅ | + +## Every-session bring-up + +1. **[laptop] Verify cable link** (IP is persistent since 2026-07-03 — no nmcli needed): + ```bash + ping -c 2 10.42.0.2 # robot onboard PC over the cable + ``` +2. **[laptop] Container** (after reboot/suspend): + ```bash + docker start dimos-dev-r1lite + docker exec -it dimos-dev-r1lite bash # one per terminal needed + ``` +3. **[container] Per-shell env** (every new shell): + ```bash + source .venv/bin/activate && source /opt/ros/humble/setup.bash && export ROS_DOMAIN_ID=2 + ``` +4. **[robot] Stack** (if `tmux ls` shows nothing): + ```bash + cd ~/galaxea/install/startup_config/share/startup_config/script + ./robot_startup.sh boot ../sessions.d/ATCStandard/R1LITEBody.d + # wait ~30s (arms/grippers twitch = HDAS health sign), then: + tmux kill-session -t r1lite_teleop # keep GELLO teleop off the arms + ``` + Shutdown: `./robot_startup.sh kill` + +## Validated tests (all ✅, safe to recreate) + +**Discovery / read-only:** +```bash +# [container] full graph recon (topics, nodes, rates, DOF): +python scripts/r1lite_test/test_00_recon.py + +# [container] formal topic assertion (12 expected topics): +python scripts/r1lite_test/test_01_topic_discovery.py + +# [container] live position of any segment: +ros2 topic echo /hdas/feedback_arm_left --field position +# ("A message was lost!!!" spam on 200-488Hz topics = slow CLI subscriber, benign) +``` + +**Gripper (first actuation ✅ — 0–100 units, 0=closed, 100=open):** +```bash +# [container] shell B monitor: +ros2 topic echo /hdas/feedback_gripper_left --field position +# [container] shell A — MUST STREAM (-r 10); one-shot --once is ignored. +# Motion stops the moment you Ctrl-C (dead-man). +ros2 topic pub -r 10 /motion_target/target_position_gripper_left sensor_msgs/msg/JointState "{header: {stamp: now}, name: [''], position: [85.0], velocity: [0.0], effort: [0.0]}" +# reopen: same with position: [101.8] +``` + +**Arm wrist-roll (✅ — serial joints, independently commandable):** +```bash +# [container] shell B monitor: +ros2 topic echo /hdas/feedback_arm_left --field position +# [container] shell A — joints 1-5 held at rest values, joint 6 → 0.2 rad: +ros2 topic pub -r 10 /motion_target/target_joint_state_arm_left sensor_msgs/msg/JointState "{header: {stamp: now}, name: [''], position: [0.065, 0.0, 0.0, 0.008, -0.001, 0.2], velocity: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5], effort: [0.0]}" +# return home: same with last value 0.001 +``` + +## NEVER recreate + +- **Torso joint targets** (`target_joint_state_torso`) — parallelogram + linkage; single-joint deltas made the robot SHAKE (2026-07-03). + Galaxea docs: joint+velocity torso signals conflict, disable_torso=true + by default. test_06 is hard-guarded. Torso motion later = task-space + `target_speed_torso` only, as a designed experiment. +- **`findrobot_server.sh` on the robot** — rewrites DDS config to + discovery-server mode and reboots; would break our multicast setup. + +## Chassis: partially solved, one blocker (see BRINGUP_LOG for full saga) + +Validated: chassis node accepts streamed targets in RC **mode 5** (RC ON, +all 4 switches position 1), ramp reaches full speed with acc_limit + +brake=false streamed (test_03 does all gates itself). RC mode map: +all-pos1=5 (software), sw1@2+sw2@3=2 (brake), +sw3@mid=3 (RC manual drive). + +UNRESOLVED: wheels refuse to turn (both software AND RC manual) despite +healthy RC/VCU/battery/e-stop. **Next session, check FIRST: the separate +wireless E-STOP REMOTE** (R1 Lite ships 3 remotes: torso/chassis/e-stop — +a pressed or dead e-stop fob = failsafe stop, explains everything). +Fallbacks: Galaxea "Wheel Motor Zero-Point Calibration" doc, +product@galaxea-dynamics.com. + +⚠️ RULES learned the hard way: +- Chassis node LATCHES the last target forever (no dead-man!) — ALWAYS + end chassis work with a zero-velocity stream. +- Never power the robot on with e-stop pressed (whole-session inhibit). +- On-robot `ros2 topic list/echo` may show nothing after `ros2 daemon + stop` — daemon cache warming, not an outage; wait or use --no-daemon. + +## Not yet done + +- Chassis wheel-level unblock (e-stop remote lead above), then test_03 PASS. +- Torso task-space experiment (target_speed_torso, MPC path) — designed + session; joint path is guarded off. +- Mesh copy for LFS: `scp -r r1lite:~/galaxea/install/mobiman/share/mobiman/urdf/R1_Lite/meshes /tmp/` +- `R1LiteConnection` module (all facts in `r1lite_config.py`). diff --git a/scripts/r1lite_test/__init__.py b/scripts/r1lite_test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/scripts/r1lite_test/domain_scan.py b/scripts/r1lite_test/domain_scan.py new file mode 100644 index 0000000000..3a0ae250c4 --- /dev/null +++ b/scripts/r1lite_test/domain_scan.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +"""Sweep ROS_DOMAIN_ID 0-101 and report domains with foreign participants. + +DDS participant discovery is multicast and fast on a wired LAN, so a short +dwell per domain is enough to spot a robot whose domain we don't know. +""" + +import sys +import time + +import rclpy +from rclpy.context import Context +from rclpy.node import Node + + +def scan_domain(domain_id: int, dwell_s: float = 0.7) -> list[str]: + ctx = Context() + rclpy.init(context=ctx, domain_id=domain_id) + try: + node = Node(f"domain_scan_{domain_id}", context=ctx) + try: + time.sleep(dwell_s) + names = [ + f"{ns}/{name}" if ns != "/" else f"/{name}" + for name, ns in node.get_node_names_and_namespaces() + ] + return [n for n in names if f"domain_scan_{domain_id}" not in n] + finally: + node.destroy_node() + finally: + rclpy.shutdown(context=ctx) + + +def main() -> None: + hits = {} + for d in range(0, 102): + others = scan_domain(d) + if others: + hits[d] = others + print(f"DOMAIN {d}: {len(others)} foreign node(s): {others[:8]}", flush=True) + if d % 20 == 0: + print(f"...scanned up to domain {d}", file=sys.stderr, flush=True) + if not hits: + print("No foreign ROS 2 participants found on any domain 0-101.") + + +if __name__ == "__main__": + main() diff --git a/scripts/r1lite_test/r1lite_config.py b/scripts/r1lite_test/r1lite_config.py new file mode 100644 index 0000000000..b118a0f46c --- /dev/null +++ b/scripts/r1lite_test/r1lite_config.py @@ -0,0 +1,85 @@ +"""R1 Lite hardware constants — single source of truth for the test suite. + +Reconciled against live hardware 2026-07-02/03 (test_00_recon.py, boot +profile ATCStandard/R1LITEBody.d, unit hostname "r1lite"). Remaining +unknowns are marked TODO(recon). Full evidence trail: BRINGUP_LOG.md. + +Verified robot identity (/opt/galaxea/body/hardware.json): R1-LITE, +ARM=A1X, TORSO=T0, ECU=E03, CHASSIS=C1(+CS1), ARM_END=EE0, +HEAD_CAMERA=HC0 (stereo, no depth topic), WRIST_CAMERA=WC1 (RealSense). +Onboard computer: x86_64, Ubuntu 22.04.5, ROS 2 Humble, user "r1lite". +""" + +# --- Network (verified) ------------------------------------------------ +ROBOT_ETH_IP = "10.42.0.2" # enp2s0, factory-assigned; laptop uses 10.42.0.100 +ROBOT_WIFI_IP = "192.168.1.85" # office WiFi (wlo1) +ROS_DOMAIN_ID = 2 # robot .bashrc: ROS_DOMAIN_ID=02; plain multicast + +# --- Kinematics (verified via feedback rates/counts) -------------------- +ARM_DOF = 6 # /hdas/feedback_arm_*: 6 joints @ 200 Hz +TORSO_DOF = 4 # /hdas/feedback_torso: 4 joints @ 488 Hz +CHASSIS_FB_DOF = 3 # /hdas/feedback_chassis: 3 joints @ 200 Hz +GRIPPER_DOF = 1 # /hdas/feedback_gripper_*: 1 joint @ 200 Hz +# /joint_states aggregate: 25 joints @ 500 Hz (naming TODO below) +# /hdas/feedback_hand_*: exist but silent (no dexterous hands on this unit) + +# TODO(recon): safe torso "home" pose — read it from the robot's +# start_mobiman_torso_speed_control.sh (R1LITE arg) before running test_06. +TORSO_HOME_POSE: list[float] = [0.0] * TORSO_DOF + +# TODO(recon): joint NAMES for each feedback topic (echo --field name), +# and the 25-joint /joint_states naming; needed for dimos joint mapping. + +# --- Topics (verified live 2026-07-03) ---------------------------------- +FEEDBACK_ARM = "/hdas/feedback_arm_{side}" # JointState, 200 Hz +FEEDBACK_TORSO = "/hdas/feedback_torso" # JointState, 488 Hz +FEEDBACK_CHASSIS = "/hdas/feedback_chassis" # JointState, 200 Hz +FEEDBACK_GRIPPER = "/hdas/feedback_gripper_{side}" # JointState, 200 Hz +IMU_CHASSIS = "/hdas/imu_chassis" # sensor_msgs/Imu +IMU_TORSO = "/hdas/imu_torso" # sensor_msgs/Imu +CMD_ARM = "/motion_target/target_joint_state_arm_{side}" # JointState +CMD_TORSO = "/motion_target/target_joint_state_torso" # JointState +CMD_TORSO_SPEED = "/motion_target/target_speed_torso" # TwistStamped (new vs R1 Pro) +CMD_GRIPPER = "/motion_target/target_position_gripper_{side}" # JointState +CMD_CHASSIS_SPEED = "/motion_target/target_speed_chassis" # TwistStamped +CHASSIS_ACC_LIMIT = "/motion_target/chassis_acc_limit" # TwistStamped +BRAKE_MODE = "/motion_target/brake_mode" # std_msgs/Bool +CHASSIS_SPEED_FB = "/motion_control/chassis_speed" # TwistStamped (Gate-1 analog) +# TODO(recon): R1 Pro's gatekeeper /cmd_vel does NOT exist here — chassis +# command path is CMD_CHASSIS_SPEED directly; 3-gate behavior unverified +# (all three gate suspects exist: /controller topic, brake_mode, acc_limit). +CMD_VEL_GATEKEEPER = CMD_CHASSIS_SPEED + +# Perception (verified): wrist cams = full RealSense stacks; head = stereo +# RGB pair, NO depth topic; NO chassis cameras; NO lidar topic in this +# boot profile (livox driver installed but not launched). +HEAD_LEFT_COMPRESSED = "/hdas/camera_head/left_raw/image_raw_color/compressed" +HEAD_RIGHT_COMPRESSED = "/hdas/camera_head/right_raw/image_raw_color/compressed" +WRIST_COLOR_COMPRESSED = "/hdas/camera_wrist_{side}/color/image_raw/compressed" +WRIST_DEPTH_ALIGNED = "/hdas/camera_wrist_{side}/aligned_depth_to_color/image_raw" +ROBOT_DESCRIPTION = "/robot_description" # URDF published on-topic (pubs=2) + +# Topics that must exist for test_01 to pass. +EXPECTED_TOPICS = [ + FEEDBACK_ARM.format(side="left"), + FEEDBACK_ARM.format(side="right"), + FEEDBACK_CHASSIS, + FEEDBACK_TORSO, + FEEDBACK_GRIPPER.format(side="left"), + FEEDBACK_GRIPPER.format(side="right"), + IMU_CHASSIS, + IMU_TORSO, + CMD_CHASSIS_SPEED, + CMD_ARM.format(side="left"), + CMD_ARM.format(side="right"), + CMD_TORSO, +] + +# --- Remaining open questions ------------------------------------------- +# 1. Joint names per topic + /joint_states 25-joint composition. +# 2. Torso safe home pose. +# 3. Chassis gating: does r1_lite_chassis_control_node need subscriber/ +# brake/acc-limit unlocks like the R1 Pro's 3 gates? (test_03 will tell.) +# 4. Head depth: stereo-only? (calib/head_{left,right} suggest onboard +# stereo depth is possible but not published.) +# 5. Lidar: present on unit? (No topic; livox driver installed.) diff --git a/scripts/r1lite_test/r1lite_from_robot.urdf b/scripts/r1lite_test/r1lite_from_robot.urdf new file mode 100644 index 0000000000..11aae00bb9 --- /dev/null +++ b/scripts/r1lite_test/r1lite_from_robot.urdf @@ -0,0 +1,1842 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +--- diff --git a/scripts/r1lite_test/run_all_tests.py b/scripts/r1lite_test/run_all_tests.py new file mode 100644 index 0000000000..555d76a145 --- /dev/null +++ b/scripts/r1lite_test/run_all_tests.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +""" +R1 Lite Test Runner — Single DDS Session + +Runs all tests with ONE rclpy.init()/shutdown() cycle to avoid the FastDDS +2.x/3.x participant corruption that killed robot processes on the R1 Pro. + +Ported from scripts/r1pro_test/run_all_tests.py. + +Prereqs: + 1. Robot ROS 2 stack running (see README). + 2. Set ROS_DOMAIN_ID to match the robot (find it with domain_scan.py). + 3. Run test_00_recon.py first and reconcile r1lite_config.py. + +Usage: + export ROS_DOMAIN_ID= + python3 scripts/r1lite_test/run_all_tests.py + python3 scripts/r1lite_test/run_all_tests.py --skip-chassis + python3 scripts/r1lite_test/run_all_tests.py --skip-arm +""" +import argparse +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent.parent.parent)) + +import rclpy + +from scripts.r1lite_test import test_00_recon as t00 +from scripts.r1lite_test import test_01_topic_discovery as t01 +from scripts.r1lite_test import test_02_read_arm_feedback as t02 +from scripts.r1lite_test import test_03_chassis_command as t03 +from scripts.r1lite_test import test_04_arm_joint_command as t04 + + +def main(): + parser = argparse.ArgumentParser(description="R1 Lite integration tests") + parser.add_argument("--skip-chassis", action="store_true", help="Skip chassis test") + parser.add_argument("--skip-arm", action="store_true", help="Skip arm movement test") + parser.add_argument("--recon", action="store_true", help="Run recon dump then exit") + args = parser.parse_args() + + print("=" * 60) + print("R1 Lite Integration Tests (single DDS session)") + print("=" * 60) + + rclpy.init() + + if args.recon: + t00.main() + rclpy.shutdown() + return True + + results = {} + + def confirm(msg): + resp = input(f"\n{msg} Press Enter to continue, 'q' to quit: ").strip().lower() + return resp != "q" + + print("\n" + "=" * 60) + print("TEST 01: Topic Discovery") + print("=" * 60) + results["01_topic_discovery"] = t01.main() + + if not confirm("Ready for Test 02 (Arm Feedback — read only)?"): + print("Stopping early.") + rclpy.shutdown() + return True + print("=" * 60) + print("TEST 02: Arm Feedback") + print("=" * 60) + results["02_arm_feedback"] = t02.main() + + if args.skip_chassis: + print("\n[SKIPPED] Test 03: Chassis Command") + results["03_chassis_command"] = None + else: + if not confirm("Ready for Test 03 (Chassis — robot will move)?"): + print("Stopping early.") + rclpy.shutdown() + return True + print("=" * 60) + print("TEST 03: Chassis Command") + print("=" * 60) + results["03_chassis_command"] = t03.main() + + if args.skip_arm: + print("\n[SKIPPED] Test 04: Arm Movement") + results["04_arm_movement"] = None + else: + if not confirm("Ready for Test 04 (Arm Movement — arm will physically move)?"): + print("Stopping early.") + rclpy.shutdown() + return True + print("=" * 60) + print("TEST 04: Arm Movement (arm will move!)") + print("=" * 60) + results["04_arm_movement"] = t04.main(skip_prompt=True) + + rclpy.shutdown() + + print("\n" + "=" * 60) + print("RESULTS") + print("=" * 60) + for name, result in results.items(): + status = "SKIP" if result is None else ("PASS" if result else "FAIL") + print(f" [{status}] {name}") + + failed = sum(1 for r in results.values() if r is False) + print(f"\n{failed} test(s) FAILED" if failed else "\nAll tests passed!") + return failed == 0 + + +if __name__ == "__main__": + sys.exit(0 if main() else 1) diff --git a/scripts/r1lite_test/test_00_recon.py b/scripts/r1lite_test/test_00_recon.py new file mode 100644 index 0000000000..a242427908 --- /dev/null +++ b/scripts/r1lite_test/test_00_recon.py @@ -0,0 +1,72 @@ +""" +Test 0: Recon — dump everything the robot's ROS 2 graph exposes. + +Read-only. Run this FIRST (before any other test) and use its output to +correct r1lite_config.py. Prints: + - all topics with types and publisher/subscriber counts + - measured publish rate for each /hdas/* feedback topic + - message field sizes (joint counts) for JointState feedback topics + - node list + +Run: + export ROS_DOMAIN_ID= + python3 scripts/r1lite_test/test_00_recon.py +""" +import time + +import rclpy + +DISCOVERY_WAIT = 10.0 +RATE_SAMPLE_S = 3.0 + + +def main() -> bool: + node = rclpy.create_node("r1lite_recon") + + print(f"Waiting {DISCOVERY_WAIT}s for DDS discovery...") + deadline = time.time() + DISCOVERY_WAIT + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + topics = sorted(node.get_topic_names_and_types()) + print(f"\n=== {len(topics)} topics ===") + for name, types in topics: + pubs = node.count_publishers(name) + subs = node.count_subscribers(name) + print(f" {name} [{', '.join(types)}] pubs={pubs} subs={subs}") + + nodes = sorted(node.get_node_names_and_namespaces()) + print(f"\n=== {len(nodes)} nodes ===") + for n, ns in nodes: + print(f" {ns.rstrip('/')}/{n}") + + # Sample rates + joint counts on JointState feedback topics. + from sensor_msgs.msg import JointState + + js_topics = [n for n, t in topics if "sensor_msgs/msg/JointState" in t] + print(f"\n=== JointState topics: rate + joint count ({RATE_SAMPLE_S}s sample each) ===") + for topic in js_topics: + stats = {"count": 0, "dof": None} + + def cb(msg, s=stats): + s["count"] += 1 + s["dof"] = len(msg.position) + + sub = node.create_subscription(JointState, topic, cb, 10) + t0 = time.time() + while time.time() - t0 < RATE_SAMPLE_S: + rclpy.spin_once(node, timeout_sec=0.1) + node.destroy_subscription(sub) + rate = stats["count"] / RATE_SAMPLE_S + print(f" {topic}: {rate:.1f} Hz, {stats['dof']} joints") + + print("\nDone. Update scripts/r1lite_test/r1lite_config.py with the above.") + node.destroy_node() + return len(topics) > 2 # more than our own /rosout + /parameter_events + + +if __name__ == "__main__": + rclpy.init() + result = main() + rclpy.shutdown() + exit(0 if result else 1) diff --git a/scripts/r1lite_test/test_01_topic_discovery.py b/scripts/r1lite_test/test_01_topic_discovery.py new file mode 100644 index 0000000000..e2628d7632 --- /dev/null +++ b/scripts/r1lite_test/test_01_topic_discovery.py @@ -0,0 +1,63 @@ +""" +Test 1: Topic Discovery +Verify that the DiMOS laptop can see the R1 Lite's ROS2 topics over ethernet. + +Ported from scripts/r1pro_test/test_01_topic_discovery.py — expected topic +names come from r1lite_config.py (TODO(recon) until verified on hardware). + +Run standalone: + export ROS_DOMAIN_ID= + python3 scripts/r1lite_test/test_01_topic_discovery.py + +Or via run_all_tests.py (preferred — single DDS session). + +Pass condition: All expected R1 Lite topics found. +""" +import sys +import time +from pathlib import Path + +import rclpy + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +from r1lite_config import EXPECTED_TOPICS + +DISCOVERY_WAIT = 10.0 + + +def main() -> bool: + """Run topic discovery. Assumes rclpy.init() already called.""" + node = rclpy.create_node("dimos_probe") + + deadline = time.time() + DISCOVERY_WAIT + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + topics = node.get_topic_names_and_types() + print(f"\nFound {len(topics)} topics:\n") + for name, types in sorted(topics): + print(f" {name} [{', '.join(types)}]") + + topic_names = {name for name, _ in topics} + print("\n--- Expected topic check ---") + all_found = True + for t in EXPECTED_TOPICS: + found = t in topic_names + status = "OK" if found else "MISSING" + print(f" [{status}] {t}") + if not found: + all_found = False + + result = "PASS" if all_found else "FAIL" + detail = "All expected topics found" if all_found else "Some topics missing" + print(f"\n{result}: {detail}") + + node.destroy_node() + return all_found + + +if __name__ == "__main__": + rclpy.init() + result = main() + rclpy.shutdown() + exit(0 if result else 1) diff --git a/scripts/r1lite_test/test_02_read_arm_feedback.py b/scripts/r1lite_test/test_02_read_arm_feedback.py new file mode 100644 index 0000000000..f773681c44 --- /dev/null +++ b/scripts/r1lite_test/test_02_read_arm_feedback.py @@ -0,0 +1,74 @@ +""" +Test 2: Read Arm Feedback +Subscribe to left arm joint feedback and print 5 messages. + +Ported from scripts/r1pro_test/test_02_read_arm_feedback.py. + +Run standalone: + export ROS_DOMAIN_ID= + python3 scripts/r1lite_test/test_02_read_arm_feedback.py + +Or via run_all_tests.py (preferred — single DDS session). + +Pass condition: Prints 5 joint position arrays (ARM_DOF values each). +""" +import sys +import time +from pathlib import Path + +import rclpy +from sensor_msgs.msg import JointState + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +from r1lite_config import ARM_DOF, FEEDBACK_ARM + +MAX = 5 +DISCOVERY_WAIT = 5.0 +RECEIVE_TIMEOUT = 5.0 + +FEEDBACK_TOPIC = FEEDBACK_ARM.format(side="left") + + +def main() -> bool: + """Run arm feedback test. Assumes rclpy.init() already called.""" + node = rclpy.create_node("dimos_arm_reader") + count = [0] + + def cb(msg): + count[0] += 1 + if len(msg.position) != ARM_DOF: + print( + f"NOTE: feedback has {len(msg.position)} joints, " + f"r1lite_config.ARM_DOF says {ARM_DOF} — fix the config!" + ) + print(f"[{count[0]}/{MAX}] positions: {[round(p, 4) for p in msg.position]}") + print(f" velocities: {[round(v, 4) for v in msg.velocity]}") + print(f" efforts: {[round(e, 4) for e in msg.effort]}") + + node.create_subscription(JointState, FEEDBACK_TOPIC, cb, 10) + + print("Waiting for DDS peer discovery...") + deadline = time.time() + DISCOVERY_WAIT + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + print(f"Waiting for {FEEDBACK_TOPIC} messages...") + deadline = time.time() + RECEIVE_TIMEOUT + while count[0] < MAX and time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + passed = count[0] >= MAX + if passed: + print(f"\nPASS: Received {count[0]} messages") + else: + print(f"\nFAIL: Only received {count[0]}/{MAX} messages — is the robot stack running?") + + node.destroy_node() + return passed + + +if __name__ == "__main__": + rclpy.init() + result = main() + rclpy.shutdown() + exit(0 if result else 1) diff --git a/scripts/r1lite_test/test_03_chassis_command.py b/scripts/r1lite_test/test_03_chassis_command.py new file mode 100644 index 0000000000..2201ec8531 --- /dev/null +++ b/scripts/r1lite_test/test_03_chassis_command.py @@ -0,0 +1,168 @@ +""" +Test 3: Chassis Command — small forward move (~5 cm) with all gates held open. + +R1 Lite chassis findings (2026-07-03, see BRINGUP_LOG.md): + - Command topic is /motion_target/target_speed_chassis (TwistStamped) + directly — the R1 Pro's gatekeeper /cmd_vel does NOT exist here. + - The chassis node inherits the R1 Pro's gate design. All of these must + hold SIMULTANEOUSLY or commands produce ~0.3mm/s creep or nothing: + Gate 1: someone subscribed to /motion_control/chassis_speed + (this script's own monitor subscription holds it open) + Gate 2: brake_mode false (streamed every tick) + Gate 3: nonzero chassis_acc_limit (streamed every tick; boots at 0) + - Commands are dead-man streams: motion stops when streaming stops. + - Robot must have been POWERED ON with e-stop released — a latched + e-stop at boot inhibits all motors for the whole session. + +Move profile: vx = +0.05 m/s for 1.0 s ≈ 5 cm forward, then an active +zero-velocity stop stream. PASS requires measured chassis speed to rise +above threshold during the move AND settle back near zero after. + +Prerequisites: robot untethered (except ethernet — tend the slack), ~0.5 m +clear floor ahead, hand on e-stop, RC idle/off. + +Run: + export ROS_DOMAIN_ID=2 + python3 scripts/r1lite_test/test_03_chassis_command.py +""" +import sys +import time +from pathlib import Path + +import rclpy +from geometry_msgs.msg import TwistStamped +from std_msgs.msg import Bool + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +from r1lite_config import BRAKE_MODE, CHASSIS_ACC_LIMIT, CHASSIS_SPEED_FB, CMD_CHASSIS_SPEED + +VELOCITY = 0.05 # m/s forward — 5 cm over MOVE_DURATION +MOVE_DURATION = 1.0 # s +STOP_DURATION = 1.5 # s of active zero-velocity streaming after the move +ACC_LIMIT_XY = 0.5 # m/s^2 — gentle ramp (Gate 3 opener) +ACC_LIMIT_YAW = 0.5 # rad/s^2 +PUBLISH_HZ = 20 +DISCOVERY_WAIT = 3.0 +MOVE_THRESHOLD = 0.02 # m/s measured speed that counts as "it moved" +STOP_THRESHOLD = 0.01 # m/s measured speed that counts as "stopped" + + +def main(skip_prompt=False) -> bool: + node = rclpy.create_node("dimos_chassis_test") + + speed_pub = node.create_publisher(TwistStamped, CMD_CHASSIS_SPEED, 10) + acc_pub = node.create_publisher(TwistStamped, CHASSIS_ACC_LIMIT, 10) + brake_pub = node.create_publisher(Bool, BRAKE_MODE, 10) + + # Gate 1: our own subscription on the measured-speed topic. Also gives + # us the PASS/FAIL evidence. + peak_speed = [0.0] + last_speed = [0.0] + + def speed_cb(msg): + vx = abs(msg.twist.linear.x) + vy = abs(msg.twist.linear.y) + v = max(vx, vy) + last_speed[0] = v + if v > peak_speed[0]: + peak_speed[0] = v + + node.create_subscription(TwistStamped, CHASSIS_SPEED_FB, speed_cb, 10) + + if not skip_prompt: + print("!!! SAFETY CHECK !!!") + print("- ~0.5 m clear floor AHEAD of the robot?") + print("- Ethernet cable slack tended? Charger unplugged?") + print("- Hand on e-stop? RC idle?") + print("- Robot was powered on with e-stop RELEASED?") + if input("\nType 'yes' to proceed: ").strip().lower() != "yes": + print("Aborted.") + node.destroy_node() + return False + + print("Waiting for DDS discovery...") + deadline = time.time() + DISCOVERY_WAIT + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + subs = node.count_subscribers(CMD_CHASSIS_SPEED) + print(f"{CMD_CHASSIS_SPEED}: {subs} subscriber(s)") + if subs == 0: + print("FAIL: chassis controller not subscribed — is the robot stack running?") + node.destroy_node() + return False + + def stream(vx: float, label: str, duration: float) -> None: + """One tick = acc_limit + brake-off + speed target (gates 2+3 + command).""" + print(f"{label}: vx={vx} m/s for {duration}s") + period = 1.0 / PUBLISH_HZ + end = time.time() + duration + while time.time() < end: + stamp = node.get_clock().now().to_msg() + + acc = TwistStamped() + acc.header.stamp = stamp + acc.twist.linear.x = ACC_LIMIT_XY + acc.twist.linear.y = ACC_LIMIT_XY + acc.twist.angular.z = ACC_LIMIT_YAW + acc_pub.publish(acc) + + brake_pub.publish(Bool(data=False)) + + cmd = TwistStamped() + cmd.header.stamp = stamp + cmd.twist.linear.x = vx + speed_pub.publish(cmd) + + rclpy.spin_once(node, timeout_sec=0.0) + time.sleep(period) + + try: + stream(VELOCITY, "MOVE", MOVE_DURATION) + moved_peak = peak_speed[0] + stream(0.0, "STOP", STOP_DURATION) + except KeyboardInterrupt: + # Dead-man: with no stream the controller stops on its own, but + # send a short zero burst anyway as a courtesy stop. + print("\nInterrupted — sending stop burst...") + try: + stream(0.0, "STOP(interrupt)", 0.5) + except Exception: + pass + node.destroy_node() + return False + + # Let feedback settle, then read final speed. + settle_end = time.time() + 0.5 + while time.time() < settle_end: + rclpy.spin_once(node, timeout_sec=0.05) + + print(f"\nPeak measured speed during move: {moved_peak:.4f} m/s " + f"(threshold {MOVE_THRESHOLD})") + print(f"Final measured speed after stop: {last_speed[0]:.4f} m/s " + f"(threshold {STOP_THRESHOLD})") + + moved = moved_peak >= MOVE_THRESHOLD + stopped = last_speed[0] <= STOP_THRESHOLD + + if moved and stopped: + print("\nPASS: chassis moved (~5 cm) and stopped cleanly. " + "Confirm visually that it rolled forward.") + elif not moved: + print("\nFAIL: chassis did not reach measurable speed. Suspects, in order:\n" + " 1. e-stop was latched at power-on (whole-session inhibit)\n" + " 2. a gate not open (brake/acc/subscriber — this script streams all three)\n" + " 3. VCU fault — check /hdas/feedback_status_chassis on the robot") + else: + print("\nFAIL: chassis moved but measured speed did not settle to zero — " + "verify visually it is stationary; e-stop if not.") + + node.destroy_node() + return moved and stopped + + +if __name__ == "__main__": + rclpy.init() + result = main() + rclpy.shutdown() + exit(0 if result else 1) diff --git a/scripts/r1lite_test/test_04_arm_joint_command.py b/scripts/r1lite_test/test_04_arm_joint_command.py new file mode 100644 index 0000000000..72158ce3a4 --- /dev/null +++ b/scripts/r1lite_test/test_04_arm_joint_command.py @@ -0,0 +1,134 @@ +""" +Test 4: Arm Joint Movement (wrist roll) +Reads current arm position, moves the LAST joint (wrist roll) by DELTA +radians, holds, then returns home. + +HARDWARE-VALIDATED 2026-07-03: this exact motion (left wrist roll +0.2 rad +via streamed targets, all other joints held at their current positions) ran +cleanly on the R1 Lite. The wrist roll rotates the gripper in place — the +safest arm motion in the tabletop fold configuration. Do NOT change +JOINT_INDEX to 0 (R1 Pro style): the base joint sweeps the whole arm +horizontally toward the camera tower / other arm. + +Commands MUST be streamed continuously (jointTracker is a dead-man +follower — motion stops when the stream stops). Ctrl-C mid-run is always +safe: the arm simply stays where it is. + +WARNING: Arm will physically move (gripper rotates ~11 degrees in place). + +Run standalone: + export ROS_DOMAIN_ID= + python3 scripts/r1lite_test/test_04_arm_joint_command.py + +Or via run_all_tests.py (preferred — single DDS session). + +Pass condition: Arm moves noticeably then returns to start position. +""" +import sys +import time +from pathlib import Path + +import rclpy +from sensor_msgs.msg import JointState + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +from r1lite_config import ARM_DOF, CMD_ARM, FEEDBACK_ARM + +SIDE = "left" # change to "right" to test right arm +JOINT_INDEX = -1 # LAST joint = wrist roll — rotates in place (see header) +DELTA = 0.2 # radians (~11 degrees) — hardware-validated value +MOVE_DURATION = 4.0 # seconds of streaming per move (tracker needs the stream) +VELOCITY = 0.5 # rad/s tracking speed +DISCOVERY_WAIT = 5.0 + +FEEDBACK_TOPIC = FEEDBACK_ARM.format(side=SIDE) +CMD_TOPIC = CMD_ARM.format(side=SIDE) + + +def main(skip_prompt=False) -> bool: + """Run arm movement test. Assumes rclpy.init() already called.""" + node = rclpy.create_node("dimos_arm_cmd_test") + current_pos = [None] # first feedback (captured home) + latest_pos = [None] # continuously updated feedback + + def fb_cb(msg): + if len(msg.position) >= 1: + latest_pos[0] = list(msg.position) + if current_pos[0] is None: + current_pos[0] = list(msg.position) + + node.create_subscription(JointState, FEEDBACK_TOPIC, fb_cb, 10) + pub = node.create_publisher(JointState, CMD_TOPIC, 10) + + if not skip_prompt: + print("WARNING: Arm will move. Keep clear.") + if input("Type 'yes' to proceed: ").strip().lower() != "yes": + print("Aborted.") + node.destroy_node() + return False + + print("Waiting for DDS peer discovery...") + deadline = time.time() + DISCOVERY_WAIT + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + print(f"Waiting for {FEEDBACK_TOPIC}...") + deadline = time.time() + 5.0 + while current_pos[0] is None and time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + if current_pos[0] is None: + print(f"FAIL: No feedback from {FEEDBACK_TOPIC} within 5s") + node.destroy_node() + return False + + home = list(current_pos[0]) + dof = len(home) + if dof != ARM_DOF: + print(f"NOTE: arm has {dof} joints; r1lite_config.ARM_DOF says {ARM_DOF} — fix the config!") + print(f"Home position ({dof} joints): {[round(p, 3) for p in home]}") + + def send_positions(positions, label): + print(f"Moving to {label}: {[round(p, 3) for p in positions]}") + deadline = time.time() + MOVE_DURATION + while time.time() < deadline: + cmd = JointState() + cmd.header.stamp = node.get_clock().now().to_msg() + cmd.name = [""] + cmd.position = list(positions) + cmd.velocity = [VELOCITY] * dof + cmd.effort = [0.0] + pub.publish(cmd) + rclpy.spin_once(node, timeout_sec=0.02) + + target = list(home) + target[JOINT_INDEX] += DELTA + send_positions(target, f"wrist roll (joint {dof}) +{DELTA} rad") + + # Did the wrist actually track the target? + moved = latest_pos[0] is not None and abs(latest_pos[0][JOINT_INDEX] - target[JOINT_INDEX]) < 0.05 + if latest_pos[0] is not None: + print(f"After move: wrist at {latest_pos[0][JOINT_INDEX]:+.3f} (target {target[JOINT_INDEX]:+.3f})") + + send_positions(home, "home") + + returned = latest_pos[0] is not None and abs(latest_pos[0][JOINT_INDEX] - home[JOINT_INDEX]) < 0.05 + if latest_pos[0] is not None: + print(f"After return: wrist at {latest_pos[0][JOINT_INDEX]:+.3f} (home {home[JOINT_INDEX]:+.3f})") + + if moved and returned: + print("\nPASS: Wrist tracked target and returned home (verified via feedback).") + elif not moved: + print("\nFAIL: Wrist did not track the target — motors enabled? " + "(e-stop at boot inhibits everything — see BRINGUP_LOG.md)") + else: + print("\nFAIL: Wrist moved but did not return home — check feedback above.") + node.destroy_node() + return moved and returned + + +if __name__ == "__main__": + rclpy.init() + result = main() + rclpy.shutdown() + exit(0 if result else 1) diff --git a/scripts/r1lite_test/test_06_torso_command.py b/scripts/r1lite_test/test_06_torso_command.py new file mode 100644 index 0000000000..febac12c92 --- /dev/null +++ b/scripts/r1lite_test/test_06_torso_command.py @@ -0,0 +1,151 @@ +""" +Test 6: Torso Joint Movement — DO NOT RUN AS WRITTEN (see BRINGUP_LOG.md) + +!! HARDWARE-VERIFIED NEGATIVE RESULT (2026-07-03): the R1 Lite torso is a +!! PARALLELOGRAM lift — joints 1-3 are mechanically coupled. Streaming a +!! single-joint delta to /motion_target/target_joint_state_torso made the +!! robot SHAKE (motors fighting the linkage). Joint-space torso commands +!! are only valid as linkage-consistent tuples read back from feedback. +!! Torso motion should go through the task-space MPC path +!! (/motion_target/target_speed_torso) instead. This script is kept as an +!! R1-Pro-style template only and refuses to run without an explicit +!! override (R1LITE_TORSO_I_KNOW_WHAT_IM_DOING=1). + +Reads current torso position, moves to home pose, holds, then returns to zero. + +Ported from scripts/r1pro_test/test_06_torso_command.py. TORSO_DOF and +TORSO_HOME_POSE come from r1lite_config.py (TODO(recon): fill in the R1 +Lite's safe home pose from its Galaxea startup scripts before running). + +WARNING: Torso will physically move. Ensure clear workspace above and around robot. + +Run standalone: + export ROS_DOMAIN_ID= + python3 scripts/r1lite_test/test_06_torso_command.py + +Pass condition: Torso moves to home pose then returns to zero, positions +within 0.15 rad of commanded values. +""" +import sys +import time +from pathlib import Path + +import rclpy +from sensor_msgs.msg import JointState + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +from r1lite_config import CMD_TORSO, FEEDBACK_TORSO, TORSO_DOF, TORSO_HOME_POSE + +MOVE_DURATION = 3.0 +VELOCITY = 0.5 +DISCOVERY_WAIT = 5.0 +FEEDBACK_WAIT = 5.0 + +HOME_POSE = TORSO_HOME_POSE +ZERO_POSE = [0.0] * TORSO_DOF + + +def main(skip_prompt=False) -> bool: + """Run torso movement test. Assumes rclpy.init() already called.""" + import os + + if os.environ.get("R1LITE_TORSO_I_KNOW_WHAT_IM_DOING") != "1": + print( + "REFUSING TO RUN: joint-space torso commands shook the robot on " + "2026-07-03 (parallelogram linkage — joints are coupled). Read " + "the header + BRINGUP_LOG.md. Set R1LITE_TORSO_I_KNOW_WHAT_IM_DOING=1 " + "only with a linkage-consistent plan." + ) + return False + + if all(v == 0.0 for v in HOME_POSE): + print( + "WARNING: r1lite_config.TORSO_HOME_POSE is still all zeros " + "(placeholder). Fill in the real home pose before trusting this test." + ) + + node = rclpy.create_node("dimos_torso_cmd_test") + current_pos = [None] + + def fb_cb(msg): + if current_pos[0] is None and len(msg.position) >= TORSO_DOF: + current_pos[0] = list(msg.position[:TORSO_DOF]) + + node.create_subscription(JointState, FEEDBACK_TORSO, fb_cb, 10) + pub = node.create_publisher(JointState, CMD_TORSO, 10) + + if not skip_prompt: + print("WARNING: Torso will move. Ensure clear workspace around robot.") + if input("Type 'yes' to proceed: ").strip().lower() != "yes": + print("Aborted.") + node.destroy_node() + return False + + print("Waiting for DDS peer discovery...") + deadline = time.time() + DISCOVERY_WAIT + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + print(f"Waiting for {FEEDBACK_TORSO}...") + deadline = time.time() + FEEDBACK_WAIT + while current_pos[0] is None and time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + if current_pos[0] is None: + print(f"FAIL: No feedback from {FEEDBACK_TORSO} within {FEEDBACK_WAIT}s") + node.destroy_node() + return False + + initial = list(current_pos[0]) + print(f"Initial torso position: {[round(p, 3) for p in initial]}") + + def send_positions(positions, label): + print(f"Moving to {label}: {[round(p, 3) for p in positions]}") + deadline = time.time() + MOVE_DURATION + while time.time() < deadline: + cmd = JointState() + cmd.header.stamp = node.get_clock().now().to_msg() + cmd.name = [""] + cmd.position = list(positions) + cmd.velocity = [VELOCITY] * TORSO_DOF + cmd.effort = [0.0] + pub.publish(cmd) + rclpy.spin_once(node, timeout_sec=0.02) + + send_positions(HOME_POSE, "home pose") + + final_home = [None] + deadline = time.time() + 2.0 + while final_home[0] is None and time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if current_pos[0] is not None: + final_home[0] = list(current_pos[0]) + current_pos[0] = None + + if final_home[0] is not None: + print(f"Torso at home: {[round(p, 3) for p in final_home[0]]}") + errors = [abs(a - b) for a, b in zip(final_home[0], HOME_POSE)] + max_err = max(errors) if errors else 0.0 + if max_err > 0.15: + print(f"WARNING: max position error {max_err:.3f} rad > 0.15 rad") + + send_positions(ZERO_POSE, "zero pose") + + deadline = time.time() + 2.0 + final_zero = initial + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if current_pos[0] is not None: + final_zero = list(current_pos[0]) + print(f"Torso at zero: {[round(p, 3) for p in final_zero]}") + + print("\nPASS: Torso moved to home pose and returned to zero.") + node.destroy_node() + return True + + +if __name__ == "__main__": + rclpy.init() + result = main() + rclpy.shutdown() + exit(0 if result else 1)