Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 19 additions & 0 deletions dimos/control/tasks/r1pro_sysid_task/_registry.py
Original file line number Diff line number Diff line change
@@ -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")}
2 changes: 2 additions & 0 deletions dimos/robot/all_blueprints.py
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand Down Expand Up @@ -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",
Expand Down
104 changes: 104 additions & 0 deletions dimos/robot/galaxea/r1pro/SYSID.md
Original file line number Diff line number Diff line change
@@ -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:** `<joint>:step:0.1`, `<joint>:step:0.25` (or negative),
or `<joint>: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.
103 changes: 103 additions & 0 deletions dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_sysid.py
Original file line number Diff line number Diff line change
@@ -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")
)
Loading