From 5fe97f70d2c7d54182ce50882f61130bab433525 Mon Sep 17 00:00:00 2001 From: cc Date: Sat, 21 Nov 2026 13:28:09 -0800 Subject: [PATCH] feat(r1pro): add guarded SysID diagnostics and recording --- .../tasks/r1pro_sysid_task/_registry.py | 19 + dimos/robot/all_blueprints.py | 2 + dimos/robot/galaxea/r1pro/SYSID.md | 104 +++++ .../r1pro/blueprints/basic/r1pro_sysid.py | 103 +++++ dimos/robot/galaxea/r1pro/connection.py | 46 +- dimos/robot/galaxea/r1pro/diagnostic_tap.py | 119 +++++ dimos/robot/galaxea/r1pro/sysid.py | 410 ++++++++++++++++++ dimos/robot/galaxea/r1pro/test_connection.py | 25 ++ dimos/robot/galaxea/r1pro/test_sysid.py | 253 +++++++++++ 9 files changed, 1072 insertions(+), 9 deletions(-) create mode 100644 dimos/control/tasks/r1pro_sysid_task/_registry.py create mode 100644 dimos/robot/galaxea/r1pro/SYSID.md create mode 100644 dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_sysid.py create mode 100644 dimos/robot/galaxea/r1pro/diagnostic_tap.py create mode 100644 dimos/robot/galaxea/r1pro/sysid.py create mode 100644 dimos/robot/galaxea/r1pro/test_sysid.py diff --git a/dimos/control/tasks/r1pro_sysid_task/_registry.py b/dimos/control/tasks/r1pro_sysid_task/_registry.py new file mode 100644 index 00000000000..026f2d3ba2b --- /dev/null +++ b/dimos/control/tasks/r1pro_sysid_task/_registry.py @@ -0,0 +1,19 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""R1 Pro diagnostic excitation task card.""" + +TASK_FACTORIES = {"r1pro_sysid": "dimos.robot.galaxea.r1pro.sysid:create_task"} +TASK_CONSUMES = {"r1pro_sysid": {"r1pro_diagnostics": ("on_diagnostics", "broadcast")}} +TASK_EXPOSES = {"r1pro_sysid": ("run_trial", "abort", "status")} diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 169d0fb9fd3..f7a45ab500f 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -131,6 +131,7 @@ "r1pro-manipulation": "dimos.robot.galaxea.r1pro.blueprints.manipulation.r1pro_manipulation:r1pro_manipulation", "r1pro-nav": "dimos.robot.galaxea.r1pro.blueprints.navigation.r1pro_nav:r1pro_nav", "r1pro-planar-preview": "dimos.robot.galaxea.r1pro.blueprints.manipulation.r1pro_planar_preview:r1pro_planar_preview", + "r1pro-sysid": "dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_sysid:r1pro_sysid", "r1pro-teleop": "dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_teleop:r1pro_teleop", "real-sense-camera-vis": "dimos.hardware.sensors.camera.realsense.blueprints:real_sense_camera_vis", "relocalize-mid360": "dimos.mapping.relocalization.blueprints:relocalize_mid360", @@ -384,6 +385,7 @@ "r1-pro-connection": "dimos.robot.galaxea.r1pro.connection.R1ProConnection", "r1-pro-coordinator": "dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_coordinator.R1ProCoordinator", "r1-pro-lio": "dimos.robot.galaxea.r1pro.lio.R1ProLio", + "r1-pro-sys-id-coordinator": "dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_sysid.R1ProSysIdCoordinator", "raw-robot-bridge": "dimos.robot.raw_robot_bridge.RawRobotBridge", "ray-tracing-voxel-map": "dimos.mapping.ray_tracing.module.RayTracingVoxelMap", "real-sense-camera": "dimos.hardware.sensors.camera.realsense.camera.RealSenseCamera", diff --git a/dimos/robot/galaxea/r1pro/SYSID.md b/dimos/robot/galaxea/r1pro/SYSID.md new file mode 100644 index 00000000000..c349af378f3 --- /dev/null +++ b/dimos/robot/galaxea/r1pro/SYSID.md @@ -0,0 +1,104 @@ +# R1 Pro SysID collection + +**No robot commissioning has been performed.** Do not run the live graph until the +robot operator approves the site and confirms the physical E-stop. Use the +[R1 Pro safety checklist](../../../../docs/platforms/safety/r1pro.md), no payload, +a clear/level area with stopping room in both directions, and a spotter at the +E-stop. Only this DimOS graph may publish motor commands: stop teleop, navigation +and other producers, including ROS publishers in the vendor domain. Run the graph +and RPC client on the robot host. Source the ROS/vendor environment (`rclpy`, +`hdas_msg`); this blueprint does not boot HDAS or change the vendor control mode. +While idle it sends no commands; connection shutdown still invokes its standard +zero/hold routine. The software checks do **not** perform collision, stability, +workspace or firmware-stop verification. + +## 1. Preview, without ROS or a robot + +```sh +CUDA_VISIBLE_DEVICES='' python -m dimos.robot.galaxea.r1pro.sysid --dry-run --case=torso-j2:chirp > preview.jsonl +``` + +The preview is on an exact 50 Hz grid with a synthetic zero posture. It does not +certify clearance, the live entry posture, or actual ROS publication cadence. +Use `--help` for case names. + +## 2. Record and run one short trial at a time + +Use a unique `DIMOS_RUN_ID` and an unused Zenoh discovery address/domain. Keep +both settings identical for the recorder and operator client. The recorder uses +all seven stream names in the whitelist; all are typed Zenoh streams. + +```sh +CUDA_VISIBLE_DEVICES='' dimos --record mcap --record-engine rust \ + --zenoh-domain r1pro_sysid --zenoh-scout-addr 224.0.0.224:19546 \ + --record-topics r1pro_diagnostics,r1pro_position_command,r1pro_velocity_command,joint_state,odometry,imu_chassis,imu_torso \ + run r1pro-sysid +``` + +Wait for recorder readiness and fresh feedback on the three upper-body segment +topics plus `/motion_control/chassis_speed`. In a second terminal on the robot, +create the operator client; keep it open for abort/status: + +```sh +CUDA_VISIBLE_DEVICES='' DIMOS_TRANSPORT=zenoh ZENOH_DOMAIN=r1pro_sysid \ + ZENOH_SCOUT_ADDR=224.0.0.224:19546 python -i +``` + +```python +from dimos.core.rpc_client import RPCClient +from dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_sysid import R1ProSysIdCoordinator + +rpc = RPCClient.remote(R1ProSysIdCoordinator) +rpc.task_invoke("sysid", "status") +rpc.task_invoke("sysid", "run_trial", {"case": "vx:0.2"}) # preview only +# After clearance and operator authorization, explicitly request one trial: +rpc.task_invoke("sysid", "run_trial", {"case": "vx:0.2", "live": True}) +rpc.task_invoke("sysid", "status") +# If anything is unexpected, use the physical E-stop; software abort is secondary. +rpc.task_invoke("sysid", "abort") +``` + +- **A — chassis:** `vx:0.2`, `vx:0.5`, `vx:0.8`; `vy:0.2`, `vy:0.5`, + `vy:0.8`; `wz:0.3`, `wz:0.8`, `wz:1.5` (insert `-` after `:` for the + negative trial). Each step holds 1.5 s before zero. `deadband` ramps forward + from 0.01 to 0.10 m/s over 2 s; `coupled` is 0.5 m/s and 0.8 rad/s for + 1.5 s. `vx:1.2` and `vx:-1.2` are **preview only**, because the deployed + connection and Coordinator model limit is ±1.0 m/s. Do not bypass this cap. +- **B — upper body:** `:step:0.1`, `:step:0.25` (or negative), + or `:chirp`. Choose `torso-j2`, `shoulder` (left J1), `elbow` (left + J4), or `wrist` (left J6). The six-second chirp sweeps through 0.5/1/2/3 Hz, + with a 0.15→0.10 rad amplitude schedule and tapered endpoints. The task + always uses complete torso/left/right position frames. +- **C — gravity:** `zero`, `arms-forward`, `torso-bent`. Use a separately + approved method to stage the robot within 0.35 rad per joint of the desired + pose, then remove that command producer. The trial approaches over 1.5 s and + holds for 3 s; it is not a posture planner. Arms-forward uses −0.95 rad on + both shoulder J1s; torso-bent uses +0.20 rad on torso J2. + +Each trial requires fresh stamped feedback, a stationary chassis at entry, and +model-limit margin. Software abort fences new moving publications at the +Coordinator, then requests zero body speed and a full measured (or last valid) +position hold on the next eligible tick. A command already in transport/ROS may +still reach the vendor. If the control path is lost, the configured connection +deadman is 0.1 s; do not treat that as physical stop latency. E-stop clear does +not resume a trial. Confirm zero motion and read `status` before selecting a new +case. Stop the operator client with `rpc.stop_rpc_client()` and end the recording +with Ctrl+C after post-roll, allowing the recorder to flush. + +## 3. Verify the resulting `memory.mcap` + +- `r1pro_diagnostics`: `std_msgs.String` containing v1 JSON. `rx` events carry + raw control feedback, status and IMUs as seen by ROS callbacks. `tx` events + contain the actual vendor ROS fields (including tracking-speed arrays) when + `RawROS.publish` returns, not motor-application acknowledgement. `header_ns` + is the original ROS stamp (null for headerless messages); `monotonic_ns` + marks receive entry or pre-publish send, and `unix_ns` pairs it to wall time. +- `r1pro_position_command`, `r1pro_velocity_command`: Coordinator output; + `joint_state`, `odometry`, `imu_chassis`, `imu_torso`: decoded feedback. Do + not substitute recorder log time or fused joint-state time for the raw RX + clock. Effort values are vendor feedback, not calibrated torque. +- Check the recorder-reported path, channel counts, raw header ages, `seq` + gaps, actual TX cadence, start/stop zeros, and matched RX/TX before fitting. + The tap drops on overflow and the bus/recorder may also lose samples; sort + cross-thread events by `monotonic_ns`. Record run ID, git revision, case, + posture/payload, abort reason and robot firmware alongside the artifact. diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_sysid.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_sysid.py new file mode 100644 index 00000000000..ae6dc6d848b --- /dev/null +++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_sysid.py @@ -0,0 +1,103 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Dedicated R1 Pro SysID graph: no teleop, camera modules, or planner.""" + +from __future__ import annotations + +from dimos.control.connection.connection_module import ConnectionConfig +from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.stream import In, Out +from dimos.core.transport import ZenohTransport +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.String import String +from dimos.robot.galaxea.r1pro.connection import ( + R1PRO_GRIPPER_JOINTS, + R1ProConnection, + R1ProConnectionConfig, +) +from dimos.robot.galaxea.r1pro.sysid import BASE, HZ, UPPER, R1ProSysIdTask + + +class R1ProSysIdCoordinator(ControlCoordinator): + r1pro_position_command: Out[JointState] + r1pro_velocity_command: Out[JointState] + r1pro_joint_state: In[JointState] + r1pro_diagnostics: In[String] + + def _publish_command(self, robot: str, values: dict[str, float]) -> None: + task = self._tasks.get("sysid") + if isinstance(task, R1ProSysIdTask): + # Serialize publication admission with abort. The inherited writer + # still owns ROS-independent command routing and the E-stop gate. + publish = super()._publish_command + task.admit_write(lambda: publish(robot, values)) + + +RECORD_STREAMS = { + "r1pro_diagnostics": String, + "r1pro_position_command": JointState, + "r1pro_velocity_command": JointState, + "joint_state": JointState, + "odometry": Odometry, + "imu_chassis": Imu, + "imu_torso": Imu, +} + +_declaration = R1ProConnectionConfig( + boot_vendor_stack=False, + diagnostic_tap=True, + enable_head_depth=False, + publish_odom_tf=False, +) +_declaration.joint_commands = { + joint: interfaces + for joint, interfaces in _declaration.joint_commands.items() + if joint not in R1PRO_GRIPPER_JOINTS +} + +r1pro_sysid = ( + autoconnect( + R1ProConnection.blueprint(**_declaration.model_dump()), + R1ProSysIdCoordinator.blueprint( + instance_name="R1ProSysIdCoordinator", + tick_rate=HZ, + reading_timeout_s=0.15, + publish_joint_state=False, + connections=[ + ConnectionConfig.model_validate( + _declaration.model_dump(include=set(ConnectionConfig.model_fields)) + ) + ], + tasks=[TaskConfig(name="sysid", type="r1pro_sysid", joint_names=[*UPPER, *BASE])], + ), + ) + .remappings( + [ + (R1ProConnection, "position_command", "r1pro_position_command"), + (R1ProConnection, "velocity_command", "r1pro_velocity_command"), + (R1ProSysIdCoordinator, "r1pro_joint_state", "joint_state"), + ] + ) + .transports( + { + (name, msg_type): ZenohTransport.spec(f"/{name}", msg_type) + for name, msg_type in RECORD_STREAMS.items() + } + ) + .global_config(transport="zenoh", n_workers=2, viewer="none") +) diff --git a/dimos/robot/galaxea/r1pro/connection.py b/dimos/robot/galaxea/r1pro/connection.py index 491ff0d5d58..46e1894075b 100644 --- a/dimos/robot/galaxea/r1pro/connection.py +++ b/dimos/robot/galaxea/r1pro/connection.py @@ -80,9 +80,11 @@ from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.std_msgs.String import String from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.robot.assets.model import RobotModel from dimos.robot.galaxea.r1pro.config import R1PRO_BODY_AXES, R1PRO_CONTROL_MODEL +from dimos.robot.galaxea.r1pro.diagnostic_tap import DiagnosticTap from dimos.robot.galaxea.r1pro.joints import GRIPPER_JOINTS, UPPER_BODY_JOINTS, coordinator_name from dimos.utils.logging_config import setup_logger @@ -189,6 +191,8 @@ class R1ProConnectionConfig(ConnectionConfig): publish_rate_hz: How often the aggregated joint_state goes out. sensor_stats_interval_s: Seconds between per-stream stats log lines (0 disables). + enable_head_depth: Subscribe to and publish vendor head depth. + diagnostic_tap: Capture ROS control feedback, IMUs, and commands. gripper_feedback_timeout_s: How old a jaw's feedback may be before its commands are dropped and its readback withheld. boot_vendor_stack: Boot the vendor stack (HDAS, mobiman), which @@ -210,7 +214,9 @@ class R1ProConnectionConfig(ConnectionConfig): frame_id: str = Field(default="base_link") odom_frame_id: str = Field(default="odom") sensor_stats_interval_s: float = Field(default=10.0) + enable_head_depth: bool = True gripper_feedback_timeout_s: float = Field(default=1.0) + diagnostic_tap: bool = False boot_vendor_stack: bool = Field(default=True) vendor_profile: str = Field(default="") @@ -244,6 +250,8 @@ class R1ProConnection(ConnectionModule): odometry: Out[Odometry] tf: Out[TFMessage] + r1pro_diagnostics: Out[String] + # Perception. head_depth: Out[Image] @@ -254,6 +262,7 @@ def __init__(self, **kwargs: Any) -> None: self._ros: RawROS | None = None self._sensors: RawROS | None = None self._cmd_torso_topic: RawROSTopic | None = None + self._tap: DiagnosticTap | None = None self._cmd_left_topic: RawROSTopic | None = None self._cmd_right_topic: RawROSTopic | None = None self._speed_topic: RawROSTopic | None = None @@ -324,6 +333,8 @@ def connect(self) -> None: self._sensors = RawROS(node_name="r1pro_sensors") self._sensors.start() + if self.config.diagnostic_tap: + self._tap = DiagnosticTap(self.r1pro_diagnostics.publish) self._setup_control_topics() self._setup_sensor_streams() @@ -406,6 +417,9 @@ def stop(self) -> None: logger.warning(f"RawROS stop raised: {e}") self._sensors = None self._ros = None + if self._tap is not None: + self._tap.stop() + self._tap = None with self._lock: self._torso_seen = self._left_seen = self._right_seen = False @@ -518,7 +532,7 @@ def _send_joint_targets(self, q: list[float], stamp: Any) -> None: cmd.position = segment cmd.velocity = [speed] * len(segment) cmd.effort = [0.0] - ros.publish(topic, cmd) + self._publish_ros(ros, topic, cmd) def _send_chassis_speed(self, vx: float, vy: float, wz: float, stamp: Any) -> None: from geometry_msgs.msg import TwistStamped @@ -532,7 +546,7 @@ def _send_chassis_speed(self, vx: float, vy: float, wz: float, stamp: Any) -> No cmd.twist.linear.x = vx cmd.twist.linear.y = vy cmd.twist.angular.z = wz - ros.publish(self._speed_topic, cmd) + self._publish_ros(ros, self._speed_topic, cmd) def _send_gripper_target(self, side: str, opening_m: float, stamp: Any) -> None: """One jaw's opening target on its vendor topic, in millimetres.""" @@ -546,7 +560,16 @@ def _send_gripper_target(self, side: str, opening_m: float, stamp: Any) -> None: cmd.header.stamp = stamp cmd.name = [f"{side}_gripper"] cmd.position = [opening_m * _GRIPPER_MM_PER_M] - ros.publish(topic, cmd) + self._publish_ros(ros, topic, cmd) + + def _publish_ros(self, ros: RawROS, topic: RawROSTopic, msg: Any) -> None: + tap = self._tap + if tap is None: + ros.publish(topic, msg) + return + mono, unix = time.monotonic_ns(), time.time_ns() + ros.publish(topic, msg) + tap.offer("tx", topic.topic, msg, mono, unix) # Control topics @@ -576,6 +599,7 @@ def _setup_control_topics(self) -> None: for side in self._grippers } + wrap = self._tap.wrap_rx if self._tap is not None else lambda cb: cb for topic, cb in ( (RawROSTopic("/hdas/feedback_torso", RosJointState, qos=qos), self._on_feedback_torso), ( @@ -587,13 +611,13 @@ def _setup_control_topics(self) -> None: self._on_feedback_right, ), ): - self._control_unsubs.append(self._ros.subscribe(topic, cb)) + self._control_unsubs.append(self._ros.subscribe(topic, wrap(cb))) for side in self._grippers: self._control_unsubs.append( self._ros.subscribe( RawROSTopic(_GRIPPER_FEEDBACK_TOPIC.format(side), RosJointState, qos=qos), - self._on_gripper_feedback, + wrap(self._on_gripper_feedback), ) ) from hdas_msg.msg import FeedbackStatus # type: ignore[import-not-found, import-untyped] @@ -602,7 +626,7 @@ def _setup_control_topics(self) -> None: self._control_unsubs.append( self._ros.subscribe( RawROSTopic(_GRIPPER_STATUS_TOPIC.format(segment), FeedbackStatus, qos=qos), - self._on_gripper_status, + wrap(self._on_gripper_status), ) ) @@ -610,7 +634,7 @@ def _setup_control_topics(self) -> None: self._control_unsubs.append( self._ros.subscribe( RawROSTopic("/motion_control/chassis_speed", TwistStamped, qos=qos), - self._on_chassis_speed, + wrap(self._on_chassis_speed), ) ) @@ -642,17 +666,21 @@ def add_stream( q: queue.Queue[Any] = queue.Queue(maxsize=maxsize) self._queues[stream] = q assert self._sensors is not None + callback = self._make_rx_cb(stream, q) + if self._tap is not None and stream.startswith("imu_"): + callback = self._tap.wrap_rx(callback) self._sensor_unsubs.append( self._sensors.subscribe( RawROSTopic(ros_topic, ros_type, qos=qos), - self._make_rx_cb(stream, q), + callback, ) ) self._sensor_workers.append( Thread(target=worker, args=(stream, q, *args), daemon=True, name=f"r1pro-{stream}") ) - add_stream("head_depth", _HEAD_DEPTH_TOPIC, RosImage, self._convert_loop, Image) + if self.config.enable_head_depth: + add_stream("head_depth", _HEAD_DEPTH_TOPIC, RosImage, self._convert_loop, Image) add_stream("imu_chassis", "/hdas/imu_chassis", RosImu, self._imu_loop, maxsize=4) add_stream("imu_torso", "/hdas/imu_torso", RosImu, self._imu_loop, maxsize=4) diff --git a/dimos/robot/galaxea/r1pro/diagnostic_tap.py b/dimos/robot/galaxea/r1pro/diagnostic_tap.py new file mode 100644 index 00000000000..9eace6240cd --- /dev/null +++ b/dimos/robot/galaxea/r1pro/diagnostic_tap.py @@ -0,0 +1,119 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Bounded, asynchronous capture of the R1 Pro ROS control boundary.""" + +from __future__ import annotations + +from collections.abc import Callable +from itertools import count +import json +import queue +import threading +import time +from typing import Any + +from dimos.msgs.std_msgs.String import String +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +def _xyz(vector: Any) -> list[float]: + return [vector.x, vector.y, vector.z] + + +def _record(direction: str, topic: str, msg: Any, mono: int, unix: int, seq: int) -> String: + header = getattr(msg, "header", None) + stamp = header.stamp if header is not None else None + if hasattr(msg, "twist"): + fields = {"linear": _xyz(msg.twist.linear), "angular": _xyz(msg.twist.angular)} + elif hasattr(msg, "position"): + fields = { + key: list(getattr(msg, key)) for key in ("name", "position", "velocity", "effort") + } + elif hasattr(msg, "errors"): + fields = {"errors": [[e.name, e.error_code, e.error_description] for e in msg.errors]} + else: # sensor_msgs/Imu + fields = { + "orientation": [*_xyz(msg.orientation), msg.orientation.w], + "angular_velocity": _xyz(msg.angular_velocity), + "linear_acceleration": _xyz(msg.linear_acceleration), + **{ + key: list(getattr(msg, key)) + for key in ( + "orientation_covariance", + "angular_velocity_covariance", + "linear_acceleration_covariance", + ) + }, + } + return String( + json.dumps( + { + "schema": 1, + "direction": direction, + "topic": topic, + "seq": seq, + "monotonic_ns": mono, + "unix_ns": unix, + "header_ns": None if stamp is None else stamp.sec * 10**9 + stamp.nanosec, + "frame_id": None if header is None else header.frame_id, + "values": fields, + }, + separators=(",", ":"), + ) + ) + + +class DiagnosticTap: + def __init__(self, publish: Callable[[String], None]) -> None: + self._publish = publish + self._pending: queue.Queue[tuple[str, str, Any, int, int, int]] = queue.Queue(1024) + self._sequence = count() + self._stop = threading.Event() + self._thread = threading.Thread(target=self._run, name="r1pro-diagnostic-tap", daemon=True) + self._thread.start() + + def offer(self, direction: str, topic: str, msg: Any, mono: int, unix: int) -> None: + # ROS gives these callbacks their own messages, and neither the bridge + # nor its control publishers mutate the objects after this boundary. + try: + self._pending.put_nowait((direction, topic, msg, mono, unix, next(self._sequence))) + except queue.Full: + pass # A sequence gap, never backpressure on robot I/O. + + def wrap_rx(self, callback: Callable[[Any, Any], None]) -> Callable[[Any, Any], None]: + def traced(msg: Any, topic: Any) -> None: + self.offer("rx", topic.topic, msg, time.monotonic_ns(), time.time_ns()) + callback(msg, topic) + + return traced + + def stop(self) -> None: + self._stop.set() + self._thread.join(timeout=1.0) + + def _run(self) -> None: + while True: + try: + event = self._pending.get(timeout=0.05) + except queue.Empty: + if self._stop.is_set(): + return + continue + try: + self._publish(_record(*event)) + except Exception: + logger.exception("R1 Pro diagnostic event dropped") diff --git a/dimos/robot/galaxea/r1pro/sysid.py b/dimos/robot/galaxea/r1pro/sysid.py new file mode 100644 index 00000000000..e60fa4be3e8 --- /dev/null +++ b/dimos/robot/galaxea/r1pro/sysid.py @@ -0,0 +1,410 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Bounded single-trial excitation plans and a passive Coordinator task.""" + +from __future__ import annotations + +import argparse +from collections.abc import Callable, Mapping +from dataclasses import dataclass +import json +import math +from threading import Lock +import time +from typing import TYPE_CHECKING, Any + +from dimos.control.task import BaseControlTask, CoordinatorState, JointCommandOutput, ResourceClaim +from dimos.hardware.manipulators.spec import ControlMode +from dimos.msgs.std_msgs.String import String +from dimos.robot.galaxea.r1pro.config import R1PRO_BODY_AXES, R1PRO_PLANAR_BASE +from dimos.robot.galaxea.r1pro.joints import UPPER_BODY_JOINTS, coordinator_name + +if TYPE_CHECKING: + from dimos.control.connection_source import ConnectionSource + from dimos.control.contract.limits import Limits + from dimos.control.coordinator import TaskConfig + +HZ = 50 +DT = 1 / HZ +UPPER = tuple(map(coordinator_name, UPPER_BODY_JOINTS)) +BASE = R1PRO_BODY_AXES +SEGMENTS = {"/hdas/feedback_torso": 4, "/hdas/feedback_arm_left": 7, "/hdas/feedback_arm_right": 7} +CHASSIS = "/motion_control/chassis_speed" +JOINTS = {"torso-j2": 1, "shoulder": 4, "elbow": 7, "wrist": 9} +MAX_FEEDBACK_AGE = 0.15 +MAX_TICK_GAP = 0.065 +SETTLE_S = 0.5 + + +@dataclass(frozen=True) +class Trial: + kind: str + duration: float + axis: int = 0 + amplitude: float = 0.0 + pose: tuple[float, ...] = () + + def velocity(self, t: float) -> tuple[float, float, float]: + if self.kind == "ramp": + return (0.01 + 0.09 * t / 2 if 0 <= t <= 2 else 0.0, 0.0, 0.0) + if self.kind == "coupled": + return (0.5, 0.0, 0.8) if 0 <= t < 1.5 else (0.0, 0.0, 0.0) + speed = self.amplitude if self.kind == "base" and 0 <= t < 1.5 else 0.0 + return ( + speed if self.axis == 0 else 0.0, + speed if self.axis == 1 else 0.0, + speed if self.axis == 2 else 0.0, + ) + + def position(self, t: float, anchor: tuple[float, ...]) -> tuple[float, ...]: + if len(anchor) != len(UPPER): + raise ValueError("expected an 18-axis anchor") + if self.kind == "pose": + # Slow linear approach (1.5 s) followed by 3 s at the exact pose. + u = min(1.0, max(0.0, t / 1.5)) + return tuple(a + u * (b - a) for a, b in zip(anchor, self.pose, strict=True)) + offset = 0.0 + if self.kind == "step" and 0 <= t < 1.5: + offset = self.amplitude + elif self.kind == "chirp" and 0 <= t < 6.0: + frequencies = (0.5, 1.0, 2.0, 3.0) + section = min(int(t / 2), 2) + u = t - 2 * section + cycles = sum(frequencies[i] + frequencies[i + 1] for i in range(section)) + start, end = frequencies[section : section + 2] + cycles += start * u + (end - start) * u**2 / 4 + envelope = min(1.0, t / 0.35, (6.0 - t) / 0.35) + amplitude = 0.15 - 0.05 * min(1.0, t / 4) + offset = amplitude * envelope * math.sin(math.tau * cycles) + return tuple(q + offset if i == self.axis else q for i, q in enumerate(anchor)) + + +def _pose(joints: dict[int, float]) -> tuple[float, ...]: + return tuple(joints.get(i, 0.0) for i in range(len(UPPER))) + + +TRIALS = { + **{ + f"{axis}:{sign * magnitude:g}": Trial("base", 2.0, i, sign * magnitude) + for i, (axis, magnitudes) in enumerate( + (("vx", (0.2, 0.5, 0.8, 1.2)), ("vy", (0.2, 0.5, 0.8)), ("wz", (0.3, 0.8, 1.5))) + ) + for magnitude in magnitudes + for sign in (1, -1) + }, + "deadband": Trial("ramp", 2.5), + "coupled": Trial("coupled", 2.0), + **{ + f"{joint}:step:{amplitude:g}": Trial("step", 2.0, index, amplitude) + for joint, index in JOINTS.items() + for amplitude in (0.10, -0.10, 0.25, -0.25) + }, + **{f"{joint}:chirp": Trial("chirp", 6.0, index) for joint, index in JOINTS.items()}, + "zero": Trial("pose", 4.5, pose=_pose({})), + "arms-forward": Trial("pose", 4.5, pose=_pose({4: -0.95, 11: -0.95})), + "torso-bent": Trial("pose", 4.5, pose=_pose({1: 0.20})), +} + + +def chassis_allowed(target: tuple[float, float, float]) -> bool: + vx, vy, wz = target + return ( + all( + math.isfinite(v) and abs(v) <= bound + for v, bound in zip(target, R1PRO_PLANAR_BASE.velocity_limits, strict=True) + ) + and math.hypot(vx, vy) <= 1.0 + and abs(wz) <= 1.5 + ) + + +class R1ProSysIdTask(BaseControlTask): + """One explicitly requested trial. Idle/preview never admit a write.""" + + def __init__(self, name: str, priority: int, limits: Mapping[str, Limits]) -> None: + self._name = name + self._priority = priority + self._limits = limits + self._feedback: dict[str, tuple[float, tuple[float, ...]]] = {} + self._faults: set[str] = set() + self._gate = Lock() + self._write_enabled = False + self._estopped = False + self._trial: Trial | None = None + self._case = "" + self._phase = "idle" + self._anchor: tuple[float, ...] | None = None + self._last_good: tuple[float, ...] | None = None + self._hold: tuple[float, ...] | None = None + self._started = self._last_tick = self._settle_until = self._requested_at = 0.0 + self._reason = "" + + def on_diagnostics(self, msg: String) -> bool: + # Called under the Coordinator task lock, like compute and task RPCs. + topic: str | None = None + try: + event = json.loads(msg.data) + if event["schema"] != 1 or event["direction"] != "rx": + return False + topic = event["topic"] + if topic in ( + "/hdas/feedback_status_torso", + "/hdas/feedback_status_arm_left", + "/hdas/feedback_status_arm_right", + ): + if any(code or text for _, code, text in event["values"]["errors"]): + self._faults.add(topic) + self.abort("vendor upper-body fault") + else: + self._faults.discard(topic) + return True + if topic not in SEGMENTS and topic != CHASSIS: + return False + rx = event["monotonic_ns"] / 1e9 + age = (event["unix_ns"] - event["header_ns"]) / 1e9 + source_at = rx - max(0.0, age) + if not -0.01 <= age <= MAX_FEEDBACK_AGE: + raise ValueError("stale or future header") + fields = event["values"] + values = ( + (fields["linear"][0], fields["linear"][1], fields["angular"][2]) + if topic == CHASSIS + else tuple(fields["position"]) + ) + if len(values) != SEGMENTS.get(topic, 3) or not all(map(math.isfinite, values)): + raise ValueError("incomplete or non-finite feedback") + previous = self._feedback.get(topic) + if previous is not None and source_at <= previous[0]: + return False # repeated/reordered source timestamp never refreshes it + self._feedback[topic] = (source_at, values) + return True + except (KeyError, IndexError, TypeError, ValueError, AttributeError): + if isinstance(topic, str): + self._feedback.pop(topic, None) + if self._phase in ("running", "pending"): + self.abort("malformed or stale ROS feedback") + return False + + def _fresh(self, topic: str, now: float) -> tuple[float, ...] | None: + data = self._feedback.get(topic) + return data[1] if data is not None and -0.01 <= now - data[0] <= MAX_FEEDBACK_AGE else None + + def _bounded_position(self, q: tuple[float, ...], margin: float = 0.0) -> bool: + if len(q) != len(UPPER): + return False + for joint, value in zip(UPPER, q, strict=True): + limit = self._limits[joint] + if ( + limit.lo is None + or limit.hi is None + or not limit.lo + margin <= value <= limit.hi - margin + ): + return False + return True + + def _measured(self, now: float) -> tuple[float, ...] | None: + segments = [self._fresh(topic, now) for topic in SEGMENTS] + if any(q is None for q in segments): + return None + q = tuple(x for part in segments if part is not None for x in part) + return q if self._bounded_position(q) else None + + def run_trial(self, case: str, live: bool = False) -> dict[str, Any]: + trial = TRIALS[case] + caps = all(chassis_allowed(trial.velocity(i / HZ)) for i in range(int(trial.duration * HZ))) + if live is not True: + return { + "case": case, + "duration_s": trial.duration, + "rate_hz": HZ, + "chassis_within_limits": caps, + "dry_run": True, + } + if self._estopped or self.is_active() or self._faults: + raise ValueError("E-stop, active trial or vendor fault") + if not caps: + raise ValueError("trial exceeds the deployed chassis envelope; preview only") + self._trial, self._case = trial, case + self._anchor = self._last_good = self._hold = None + self._phase, self._reason = "pending", "" + self._requested_at = time.perf_counter() + return self.status() + + def status(self) -> dict[str, Any]: + return {"case": self._case, "phase": self._phase, "reason": self._reason} + + def abort(self, reason: str = "operator abort") -> bool: + if self._phase == "settling" and self._reason: + return False + with self._gate: + self._write_enabled = False # fence any moving frame not yet submitted + if self._phase in ("pending", "running", "settling"): + self._phase, self._reason = "stopping", reason + return True + return False + + def set_estop(self, estopped: bool) -> None: + self._estopped = estopped + if estopped: + self._enable_write(False) + self._phase, self._reason = "aborted", "coordinator E-stop" + + def reset_runtime_state(self, reactivate: bool | None = None) -> bool: + return self.abort("runtime reset") + + def admit_write(self, publish: Callable[[], None]) -> None: + with self._gate: + if self._write_enabled: + publish() + + def _enable_write(self, enabled: bool) -> None: + with self._gate: + self._write_enabled = enabled + + def is_active(self) -> bool: + return self._phase in ("pending", "running", "stopping", "settling") + + def claim(self) -> ResourceClaim: + # The Coordinator's arbiter uses each output's mode and joint names. + moving_base = ( + self._phase in ("running", "pending") + and self._trial is not None + and self._trial.kind in ("base", "coupled", "ramp") + ) + return ResourceClaim( + frozenset(BASE if moving_base else UPPER), + self._priority, + ControlMode.VELOCITY if moving_base else ControlMode.POSITION, + ) + + def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: + self.abort(f"preempted by {by_task}") + + def _position(self, values: tuple[float, ...]) -> JointCommandOutput: + return JointCommandOutput(list(UPPER), positions=list(values)) + + def _settle(self, now: float, measured: tuple[float, ...] | None) -> JointCommandOutput | None: + self._hold = measured if measured is not None else self._last_good + if self._hold is None: + self._phase = "aborted" + self._enable_write(False) + return None # Never synthesize a hold or re-enable motion without feedback. + self._settle_until = now + SETTLE_S + self._phase = "settling" + self._enable_write(True) + # No chassis winner: the dedicated Coordinator fills native axes with zero. + return self._position(self._hold) + + def compute(self, state: CoordinatorState) -> JointCommandOutput | None: + if not self.is_active(): + return None + now = state.t_now + measured = self._measured(now) + base = self._fresh(CHASSIS, now) + assert self._trial is not None + trial = self._trial + chassis = trial.kind in ("base", "coupled", "ramp") + if self._phase == "stopping": + return self._settle(now, measured if self._anchor is not None else None) + if self._phase == "settling": + if now >= self._settle_until or self._hold is None: + self._enable_write(False) + self._phase = "aborted" if self._reason else "complete" + return None + return self._position(self._hold) + if measured is None or base is None or self._faults: + self.abort("missing/stale ROS feedback or vendor fault") + return self._settle(now, measured if self._anchor is not None else None) + stationary = math.hypot(*base[:2]) <= 0.05 and abs(base[2]) <= 0.10 + priming = self._phase == "pending" + if priming: + if now - self._requested_at > MAX_FEEDBACK_AGE or not stationary: + self.abort("expired start request or chassis not stationary") + return self._settle(now, None) + self._anchor = measured + self._started = now + DT if chassis else now + self._last_tick = now + self._phase = "running" + for i in range(int(trial.duration * HZ) + 1): + target = trial.position(i / HZ, measured) + delta = max(abs(a - b) for a, b in zip(target, measured, strict=True)) + envelope = 0.35 if trial.kind == "pose" else 0.25 + if delta > envelope + 1e-9 or not self._bounded_position(target, 0.03): + self._anchor = None + self.abort("trial outside position/entry envelope") + return self._settle(now, None) + assert self._anchor is not None + elapsed = now - self._started + last = self._last_tick - self._started + self._last_tick = now + self._last_good = measured + previous_target = trial.position(last, self._anchor) + if ( + elapsed - last > MAX_TICK_GAP + or not chassis_allowed(base) + or (not chassis and not stationary) + or max(abs(a - b) for a, b in zip(previous_target, measured, strict=True)) > 0.40 + ): + self.abort("tick overrun, unexpected chassis motion or tracking error") + return self._settle(now, measured) + if elapsed >= trial.duration: + return self._settle(now, measured) + self._enable_write(True) + if chassis and priming: + return self._position(self._anchor) # seed a fresh arm hold before moving the chassis + if chassis: + return JointCommandOutput( + list(BASE), velocities=list(trial.velocity(elapsed)), mode=ControlMode.VELOCITY + ) + return self._position(trial.position(elapsed, self._anchor)) + + +def create_task( + cfg: TaskConfig, hardware: Any, connections: Mapping[str, ConnectionSource] +) -> R1ProSysIdTask: + source = connections["r1pro"] + return R1ProSysIdTask( + cfg.name, cfg.priority, {joint: source.position_limit(joint) for joint in UPPER} + ) + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Offline, synthetic zero-anchor R1 Pro SysID waveform preview" + ) + parser.add_argument("--dry-run", action="store_true", required=True) + parser.add_argument("--case", choices=TRIALS, required=True) + args = parser.parse_args() + trial = TRIALS[args.case] + for i in range(int(trial.duration * HZ) + 1): + t = i / HZ + velocity = trial.velocity(t) + print( + json.dumps( + { + "case": args.case, + "t_s": t, + "body_velocity": dict(zip(BASE, velocity, strict=True)), + "upper_position": dict( + zip(UPPER, trial.position(t, (0.0,) * len(UPPER)), strict=True) + ), + "chassis_within_limits": chassis_allowed(velocity), + } + ) + ) + + +if __name__ == "__main__": + main() diff --git a/dimos/robot/galaxea/r1pro/test_connection.py b/dimos/robot/galaxea/r1pro/test_connection.py index 2f4a492abeb..2de9d803099 100644 --- a/dimos/robot/galaxea/r1pro/test_connection.py +++ b/dimos/robot/galaxea/r1pro/test_connection.py @@ -17,6 +17,7 @@ from __future__ import annotations from collections.abc import Callable, Iterator +import json import math import time from types import SimpleNamespace @@ -338,3 +339,27 @@ def no_hardware(*args, **kwargs): assert not {"r1pro/base_x", "r1pro/base_y", "r1pro/base_yaw"} & set(source.joint_names) for task in config.tasks: assert "yaw_joint" not in task.params + + +@pytest.mark.parametrize("enabled", [True, False]) +def test_sysid_diagnostics_record_raw_rx_and_actual_tx(rig, enabled): + r = rig(start=False, diagnostic_tap=enabled, tracking_speed=0.42) + r.module.connect() + r.feed(100.125) + r.module._send_joint_targets(TORSO_Q + LEFT_Q + RIGHT_Q, connection_module._ros_stamp_now()) + r.module._send_chassis_speed(0.5, 0.0, 0.8, connection_module._ros_stamp_now()) + stream = r.ports["r1pro_diagnostics"].sent + assert r.speeds()[-1].twist.angular.z == 0.8 + if not enabled: + assert r.module._tap is None and not stream + return + wait_until(lambda: len(stream) == 7, timeout=2.0) + events = [json.loads(msg.data) for msg in stream] + rx = next(e for e in events if e["topic"] == "/hdas/feedback_torso") + assert rx["direction"] == "rx" and rx["header_ns"] == 100125000000 + assert rx["values"]["position"] == list(TORSO_Q) + torso = next(e for e in events if e["topic"] == TORSO_TOPIC) + assert torso["direction"] == "tx" and torso["values"]["velocity"] == [0.42] * 4 + tx = next(e for e in events if e["topic"] == SPEED_TOPIC) + assert tx["values"]["linear"] == [0.5, 0.0, 0.0] and tx["values"]["angular"] == [0.0, 0.0, 0.8] + assert 0 < rx["monotonic_ns"] <= tx["monotonic_ns"] diff --git a/dimos/robot/galaxea/r1pro/test_sysid.py b/dimos/robot/galaxea/r1pro/test_sysid.py new file mode 100644 index 00000000000..5367bfa27c6 --- /dev/null +++ b/dimos/robot/galaxea/r1pro/test_sysid.py @@ -0,0 +1,253 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""CPU-only SysID plan, guard, and real blueprint parsing tests.""" + +from __future__ import annotations + +from itertools import pairwise +import json +import time +from types import SimpleNamespace + +import pytest + +from dimos.control.contract.limits import Limits +from dimos.control.coordinator import ControlCoordinatorConfig +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser +from dimos.core.coordination.blueprints import MetaBlueprint +from dimos.core.coordination.module_coordinator import _materialize_transports, stream_name_types +from dimos.core.global_config import global_config +from dimos.experimental.memory.rust_cli_recorder import make_plan +from dimos.msgs.std_msgs.String import String +from dimos.robot.galaxea.r1pro._connection_test_fakes import LocalTransport +from dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_sysid import ( + RECORD_STREAMS, + R1ProSysIdCoordinator, +) +from dimos.robot.galaxea.r1pro.connection import R1ProConnection +from dimos.robot.galaxea.r1pro.sysid import BASE, CHASSIS, SEGMENTS, TRIALS, UPPER, R1ProSysIdTask +from dimos.robot.get_all_blueprints import load_blueprint + + +def test_waveforms_and_order() -> None: + assert TRIALS["vx:-1.2"].velocity(1.49) == (-1.2, 0.0, 0.0) + assert TRIALS["vy:0.8"].velocity(1.5) == (0.0, 0.0, 0.0) + assert TRIALS["wz:-1.5"].velocity(0) == (0.0, 0.0, -1.5) + assert TRIALS["deadband"].velocity(0)[0] == 0.01 + assert TRIALS["deadband"].velocity(1.98)[0] == pytest.approx(0.0991) + assert TRIALS["coupled"].velocity(0) == (0.5, 0.0, 0.8) + q = (0.0,) * 18 + step = TRIALS["elbow:step:-0.25"] + assert step.position(0, q) == tuple(-0.25 if i == 7 else 0.0 for i in range(18)) + assert step.position(1.5, q) == q + chirp = TRIALS["torso-j2:chirp"] + samples = [chirp.position(i / 50, q) for i in range(301)] + assert samples[0] == samples[-1] == q + assert 0.10 < max(abs(x[1]) for x in samples) <= 0.15 + assert sum(x[1] * y[1] < 0 for x, y in pairwise(samples)) >= 15 + for case in ("zero", "arms-forward", "torso-bent"): + trial = TRIALS[case] + assert trial.position(0, q) == q + assert trial.position(1.5, q) == trial.position(4.5, q) == trial.pose + + +@pytest.fixture +def task() -> R1ProSysIdTask: + return R1ProSysIdTask("sysid", 10, dict.fromkeys(UPPER, Limits(-2, 2))) + + +def feedback( + task: R1ProSysIdTask, now: float, *, omit: str = "", angle: float = 0.0, age: float = 0.0 +) -> None: + for topic, size in {**SEGMENTS, CHASSIS: 3}.items(): + if topic == omit: + continue + task.on_diagnostics( + String( + json.dumps( + { + "schema": 1, + "direction": "rx", + "topic": topic, + "monotonic_ns": round(now * 1e9), + "unix_ns": round(1000e9), + "header_ns": round((1000 - age) * 1e9), + "values": { + "position": [angle] * size, + "linear": [0.0] * 3, + "angular": [0.0] * 3, + }, + } + ) + ) + ) + + +def tick(task: R1ProSysIdTask, now: float): + return task.compute(CoordinatorState(JointStateSnapshot(), t_now=now)) + + +def test_wire_gate_preview_abort_and_no_replay(task) -> None: + wrote = [] + + def publish() -> None: + wrote.append(True) + + assert task.run_trial("vx:1.2")["chassis_within_limits"] is False + with pytest.raises(ValueError, match="preview only"): + task.run_trial("vx:1.2", live=True) + task.admit_write(publish) + assert wrote == [] + t = time.perf_counter() + feedback(task, t) + task.run_trial("vx:0.5", live=True) + assert tick(task, t + 0.01).joint_names == list(UPPER) # prime the measured hold + motion = tick(task, t + 0.03) + assert motion.joint_names == list(BASE) + assert motion.velocities == [0.5, 0.0, 0.0] + task.admit_write(publish) + assert task.abort() + task.admit_write(publish) + assert wrote == [True] + stop = tick(task, t + 0.05) + assert stop.joint_names == list(UPPER) and stop.positions == [0.0] * 18 + task.admit_write(publish) + assert wrote == [True, True] + assert tick(task, t + 0.60) is None + assert task.status()["phase"] == "aborted" + task.set_estop(True) + with pytest.raises(ValueError, match="E-stop"): + task.run_trial("vx:0.5", live=True) + task.set_estop(False) + assert not task.is_active() + + +def test_estop_while_abort_hold_closes_write_gate(task) -> None: + t = time.perf_counter() + feedback(task, t) + task.run_trial("elbow:step:0.1", live=True) + assert tick(task, t + 0.01).positions[7] == 0.1 + assert task.abort() + assert tick(task, t + 0.03).positions == [0.0] * 18 + assert not task.abort() # duplicate abort does not extend the hold window + task.set_estop(True) + task.set_estop(False) + task.admit_write(lambda: pytest.fail("E-stop clear replayed an aborted trial")) + assert tick(task, t + 0.05) is None + + +@pytest.mark.parametrize( + "invalid", ["stale", "old header", "position limit", "unsafe pose", "tick gap"] +) +def test_fails_closed(task, invalid) -> None: + t = time.perf_counter() + feedback( + task, + t, + omit=CHASSIS if invalid == "stale" else "", + angle=1.9 if invalid == "position limit" else 0.0, + age=0.16 if invalid == "old header" else 0.0, + ) + task.run_trial("arms-forward" if invalid == "unsafe pose" else "elbow:step:0.25", live=True) + first = tick(task, t + 0.01) + if invalid == "tick gap": + assert first.positions[7] == 0.25 + feedback(task, t + 0.1) + assert tick(task, t + 0.1).positions == [0.0] * 18 + assert task.status()["phase"] == "settling" + else: + assert first is None and task.status()["phase"] == "aborted" + + +def test_registered_blueprint_config_and_rust_plan(monkeypatch) -> None: + blueprint = load_blueprint("r1pro-sysid") + if isinstance(blueprint, MetaBlueprint): + blueprint = blueprint.solidify() + parsed = BlueprintConfigParser(blueprint).parse( + [], + environ={}, + global_overrides={ + "record": "mcap", + "record_engine": "rust", + "record_topics": ",".join(RECORD_STREAMS), + "zenoh_domain": "sysid_test", + "zenoh_scout_addr": "224.0.0.224:19546", + }, + ) + config = ControlCoordinatorConfig(**parsed.module_kwargs("R1ProSysIdCoordinator")) + assert config.tick_rate == 50 and [t.type for t in config.tasks] == ["r1pro_sysid"] + assert set(config.connections[0].joint_commands) == {*BASE, *UPPER} + driver = next(atom for atom in blueprint.blueprints if atom.module is R1ProConnection) + assert parsed.module_kwargs(driver.name)["diagnostic_tap"] is True + assert parsed.module_kwargs(driver.name)["boot_vendor_stack"] is False + assert set(RECORD_STREAMS.items()) <= stream_name_types(blueprint) + for key, value in parsed.global_config_values().items(): + if key in {"transport", "record_topics", "zenoh_domain", "zenoh_scout_addr"}: + monkeypatch.setattr(global_config, key, value) + plan = make_plan(_materialize_transports(blueprint, {})) + assert set(plan.payload_types) == set(RECORD_STREAMS) + coordinator = R1ProSysIdCoordinator(**parsed.module_kwargs("R1ProSysIdCoordinator")) + transports = {name: LocalTransport() for name in {**coordinator.inputs, **coordinator.outputs}} + for name, transport in transports.items(): + coordinator.set_transport(name, transport) + coordinator._connect_robots() + coordinator._setup_from_config() + # The real subscription lifecycle requires a running tick loop; keep + # execution deterministic here and step the task explicitly. + coordinator._tick_loop = SimpleNamespace(is_running=True, stop=lambda: None) + try: + coordinator._sync_stream_subscriptions() + task = coordinator.get_task("sysid") + assert isinstance(task, R1ProSysIdTask) + assert len(transports["r1pro_diagnostics"].subscribers) == 1 + source = coordinator._connections["r1pro"] + zeros = dict.fromkeys( + [*(f"{j}/position" for j in UPPER), *(f"{j}/velocity" for j in BASE)], 0.0 + ) + coordinator._publish_command("r1pro", zeros) + assert not transports["r1pro_velocity_command"].sent + t = time.perf_counter() + feedback(task, t) + coordinator.task_invoke("sysid", "run_trial", {"case": "vx:0.2", "live": True}) + prime = tick(task, t + 0.01) + assert prime and prime.positions + coordinator._publish_command("r1pro", zeros) + motion = tick(task, t + 0.035) + assert motion and motion.velocities + values = source.command( + { + joint: (value, motion.mode) + for joint, value in zip(motion.joint_names, motion.velocities, strict=True) + } + ) + coordinator._publish_command("r1pro", values) + assert list(transports["r1pro_velocity_command"].sent[-1].velocity) == [0.2, 0.0, 0.0] + coordinator.task_invoke("sysid", "abort") + coordinator._publish_command("r1pro", values) # already computed before abort + assert len(transports["r1pro_velocity_command"].sent) == 2 + hold = tick(task, t + 0.05) + assert hold and hold.positions + values = source.command( + { + joint: (value, hold.mode) + for joint, value in zip(hold.joint_names, hold.positions, strict=True) + } + ) + coordinator._publish_command("r1pro", values) + assert list(transports["r1pro_velocity_command"].sent[-1].velocity) == [0.0] * 3 + assert list(transports["r1pro_position_command"].sent[-1].position) == [0.0] * 18 + finally: + coordinator.stop()