SONIC whole-body controller for the G1

bin/hardware/g1/setup-sonic

@@ -0,0 +1,130 @@
+#!/usr/bin/env bash
+# Copyright 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.
+
+# Install the pinned SONIC GPU runtime for the detected JetPack release.
+set -euo pipefail
+
+usage() { echo "usage: bin/hardware/g1/setup-sonic [--check]"; }
+fail() { echo "$*" >&2; exit 1; }
+CHECK_ONLY=false
+case "${1:-}" in
+    --check) CHECK_ONLY=true ;;
+    -h|--help) usage; exit 0 ;;
+    "") ;;
+    *) usage >&2; exit 2 ;;
+esac
+[[ $# -le 1 ]] || { usage >&2; exit 2; }
+
+REPO_ROOT=$(cd "$(dirname "${BASH_SOURCE[0]}")/../../.." && pwd)
+cd "$REPO_ROOT"
+[[ $(uname -m) == aarch64 ]] || fail "expected an aarch64 Jetson"
+[[ -r /etc/nv_tegra_release ]] || fail "Jetson Linux release information is missing"
+read -r SONIC_RELEASE < /etc/nv_tegra_release
+SONIC_SYNC_ARGS=()
+SONIC_ORT_AUTO=""
+SONIC_ORT_DISTRIBUTION=""
+case "$SONIC_RELEASE" in
+    "# R35 "*)
+        SONIC_JETPACK=5
+        SONIC_CUDA=/usr/local/cuda-11.8
+        SONIC_CUDART="$SONIC_CUDA/lib64/libcudart.so.11.0"
+        SONIC_DRIVER="$SONIC_CUDA/compat/libcuda.so"
+        SONIC_CUDNN=/usr/lib/aarch64-linux-gnu/libcudnn.so.8
+        SONIC_ORT_AUTO=1.23.3
+        SONIC_ORT_DISTRIBUTION=1.18.1.11.8
+        SONIC_SYNC_ARGS+=(--no-install-package gtsam-extended)
+        ;;
+    "# R36 "*)
+        SONIC_JETPACK=6
+        SONIC_CUDA=/usr/local/cuda-12.6
+        SONIC_CUDART="$SONIC_CUDA/lib64/libcudart.so.12"
+        SONIC_DRIVER=/usr/lib/aarch64-linux-gnu/nvidia/libcuda.so.1
+        SONIC_CUDNN=/usr/lib/aarch64-linux-gnu/libcudnn.so.9
+        SONIC_ORT_WHEEL=https://pypi.jetson-ai-lab.io/jp6/cu126/+f/d98/0b934b9a29c1a/onnxruntime_gpu-1.24.0-cp310-cp310-linux_aarch64.whl
+        SONIC_ORT_SHA256=d980b934b9a29c1a9d6f39751edd7662b69fadd75556a10ff363773a58ce0950
+        ;;
+    *) fail "expected Jetson Linux R35/R36 (JetPack 5/6), found: $SONIC_RELEASE" ;;
+esac
+for library in "$SONIC_CUDART" "$SONIC_DRIVER" "$SONIC_CUDNN"; do
+    [[ -e "$library" ]] || fail "missing JetPack $SONIC_JETPACK prerequisite: $library"
+done
+command -v uv >/dev/null || fail "uv is required"
+echo "PASS  JetPack $SONIC_JETPACK SONIC prerequisites"
+[[ "$CHECK_ONLY" == false ]] || exit 0
+
+SONIC_VENV="$REPO_ROOT/.venv-sonic-jp$SONIC_JETPACK"
+export PATH="$SONIC_CUDA/bin:$PATH"
+export LD_LIBRARY_PATH="${SONIC_DRIVER%/*}:$SONIC_CUDA/lib64:/usr/lib/aarch64-linux-gnu${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
+if [[ ! -x "$SONIC_VENV/bin/python" ]]; then
+    uv venv --python 3.10 "$SONIC_VENV"
+fi
+[[ $("$SONIC_VENV/bin/python" -c 'import sys; print(sys.version_info[:2] == (3, 10))') == True ]] \
+    || fail "$SONIC_VENV must use Python 3.10; move it aside and retry"
+DIMOS_ALLOW_MISSING_COCKPIT=1 VIRTUAL_ENV="$SONIC_VENV" uv sync \
+    --active --python "$SONIC_VENV/bin/python" --locked --no-default-groups \
+    --extra unitree-dds --extra control --inexact \
+    --no-install-package onnxruntime --no-install-package onnxruntime-gpu \
+    "${SONIC_SYNC_ARGS[@]}"
+
+# These distributions share onnxruntime/. Remove old owners before installing
+# the Jetson wheel; use system CUDA libraries rather than PyPI's CUDA extras.
+uv pip uninstall --python "$SONIC_VENV/bin/python" \
+    onnxruntime onnxruntime-gpu onnxruntime-gpu-extended onnxruntime-gpu-extended-auto
+if [[ "$SONIC_JETPACK" == 5 ]]; then
+    uv pip install --python "$SONIC_VENV/bin/python" "numpy==1.26.4" pip
+    "$SONIC_VENV/bin/python" -m pip install --no-cache-dir \
+        "onnxruntime-gpu-extended-auto==$SONIC_ORT_AUTO"
+else
+    # NVIDIA supplies cp310, which the generic PyPI ORT constraint excludes.
+    uv --no-config pip install --python "$SONIC_VENV/bin/python" --no-deps --require-hashes \
+        "$SONIC_ORT_WHEEL#sha256=$SONIC_ORT_SHA256"
+fi
+
+"$SONIC_VENV/bin/python" - "$SONIC_JETPACK" "$SONIC_ORT_AUTO" "$SONIC_ORT_DISTRIBUTION" <<'PY'
+from importlib.metadata import version
+import sys
+
+import onnxruntime as ort
+
+from dimos.cli.dimos import cli_main
+from dimos.control.tasks.g1_sonic_wbc_task.sonic_onnx_runtime import prepare_sonic_onnx_runtime
+from dimos.core.o3dpickle import register_picklers
+from dimos.robot.unitree.g1.blueprints.basic.unitree_g1_sonic_wbc import unitree_g1_sonic_wbc
+
+if sys.argv[1] == "5":
+    expected = tuple(sys.argv[2:])
+    actual = (version("onnxruntime-gpu-extended-auto"), version("onnxruntime-gpu-extended"))
+    if actual != expected:
+        raise SystemExit(f"unexpected ONNX Runtime packages: expected {expected}, found {actual}")
+prepare_sonic_onnx_runtime()
+register_picklers()
+print(f"PASS  ONNX Runtime {ort.__version__}: {ort.get_available_providers()}")
+print("PASS  DimOS CLI and SONIC controller blueprint imports")
+PY
+
+"$SONIC_VENV/bin/python" -m dimos.control.tasks.g1_sonic_wbc_task.models
+
+cat <<EOF
+
+JetPack $SONIC_JETPACK SONIC environment is ready. Before each run:
+  source $SONIC_VENV/bin/activate
+  export DIMOS_TRANSPORT=zenoh
+  export PATH=$SONIC_CUDA/bin:\$PATH
+  export LD_LIBRARY_PATH=${SONIC_DRIVER%/*}:$SONIC_CUDA/lib64:/usr/lib/aarch64-linux-gnu\${LD_LIBRARY_PATH:+:\$LD_LIBRARY_PATH}
+  dimos-sonic-models --check
+
+Use this environment's dimos directly; uv run can replace the GPU wheel.
+This setup does not install system packages or change the Jetson power/clocks.
+EOF

dimos/control/coordinator.py

@@ -203,7 +203,13 @@ def __init__(self, *args: Any, **kwargs: Any) -> None:
         logger.info(f"ControlCoordinator initialized at {self.config.tick_rate}Hz")
 
     def _setup_from_config(self) -> None:
-        """Create hardware and tasks from config (called on start)."""
+        """Create hardware and tasks, then hand control to prepared adapters.
+
+        Connecting hardware is intentionally separate from activating it.  Task
+        construction can load large policy models; enabling actuators before
+        that work completes leaves a robot without a command producer during
+        the most vulnerable part of startup.
+        """
         hardware_added: list[str] = []
         tasks_added: list[TaskName] = []
 
@@ -219,6 +225,10 @@ def _setup_from_config(self) -> None:
                 if task_cfg.auto_start:
                     self.task_invoke(task.name, "start")
 
+            for component in self.config.hardware:
+                if component.auto_enable:
+                    self._activate_hardware(component.hardware_id)
+
         except Exception:
             # Roll back everything this call added, tasks first: an active task
             # blocks removal of the hardware whose joints it claims.
@@ -231,7 +241,7 @@ def _setup_from_config(self) -> None:
             raise
 
     def _setup_hardware(self, component: HardwareComponent) -> None:
-        """Connect and add a single hardware adapter."""
+        """Connect and register an adapter without enabling actuation."""
         adapter: ManipulatorAdapter | TwistBaseAdapter | WholeBodyAdapter
         if component.hardware_type == HardwareType.WHOLE_BODY:
             adapter = self._create_whole_body_adapter(component)
@@ -244,19 +254,22 @@ def _setup_hardware(self, component: HardwareComponent) -> None:
             raise RuntimeError(f"Failed to connect to {component.adapter_type} adapter")
 
         try:
-            if component.auto_enable:
-                activate = getattr(adapter, "activate", None)
-                if callable(activate):
-                    if activate() is False:
-                        raise RuntimeError(f"Failed to activate hardware {component.hardware_id}")
-                elif hasattr(adapter, "write_enable"):
-                    adapter.write_enable(True)
-
             self.add_hardware(adapter, component)
         except Exception:
             adapter.disconnect()
             raise
 
+    def _activate_hardware(self, hardware_id: HardwareId) -> None:
+        interface = self._hardware[hardware_id]
+        adapter = interface.adapter
+        activate = getattr(adapter, "activate", None)
+        if callable(activate):
+            if activate() is False:
+                raise RuntimeError(f"Failed to activate hardware {hardware_id}")
+            return
+        if hasattr(adapter, "write_enable"):
+            adapter.write_enable(True)
+
     def _create_adapter(self, component: HardwareComponent) -> ManipulatorAdapter:
         """Create a manipulator adapter from component config."""
         from dimos.hardware.manipulators.registry import adapter_registry
@@ -935,7 +948,7 @@ def stop(self) -> None:
         with self._hardware_lock:
             for hw_id, interface in self._hardware.items():
                 deactivate = getattr(interface.adapter, "deactivate", None)
-                if not callable(deactivate):
+                if not callable(deactivate) or not interface.adapter.is_connected():
                     continue
                 try:
                     if deactivate() is False:
@@ -946,6 +959,8 @@ def stop(self) -> None:
         # Disconnect all hardware adapters
         with self._hardware_lock:
             for hw_id, interface in self._hardware.items():
+                if not interface.adapter.is_connected():
+                    continue
                 try:
                     interface.disconnect()
                     logger.info(f"Disconnected hardware {hw_id}")

dimos/control/tasks/g1_sonic_wbc_task/README.md

@@ -0,0 +1,67 @@
+# G1 SONIC controller
+
+SONIC runs the planner, encoder and decoder at a 50 Hz policy rate. It accepts
+coordinator velocity commands, selectable gaits, motion clips and optional
+upper-body reference targets. No headset or teleop service is required.
+
+The default gait is `SLOW_WALK`. Velocity commands set direction and speed
+within that gait; they never select walking or running automatically. With a
+walking gait selected, zero input idles while preserving that selection. Use
+`set_locomotion_mode` through the coordinator to change modes; `None` restores
+`SLOW_WALK`.
+
+Install the workstation dependencies and NVIDIA assets:
+
+```bash
+uv sync --extra control --extra cuda --extra sim \
+  --no-install-package onnxruntime --reinstall-package onnxruntime-gpu
+source .venv/bin/activate
+dimos-sonic-models
+dimos-sonic-models --check
+dimos --transport zenoh --simulation mujoco --viewer none run unitree-g1-sonic-wbc
+```
+
+The CPU and GPU ONNX Runtime distributions share the same Python package;
+excluding the CPU distribution prevents it overwriting the CUDA provider.
+Keep these options when syncing this environment again.
+
+Assets live in the DimOS cache (`~/.cache/dimos/sonic` by default), outside
+the Git LFS data directory. `SONIC_MODEL_DIR` overrides this location for
+both the installer and blueprint. The installer pins the Hugging Face revision and verifies SHA-256 hashes for
+the policy, planner and observation configuration. The 13 example motion clips
+come from a pinned NVIDIA deployment revision. `--check` performs no downloads.
+SONIC v1.1 is the single supported policy bundle.
+
+MuJoCo starts in the SONIC pose and waits for a complete control command before
+advancing physics. Simulation arms automatically. Commands from a second shell:
+
+```bash
+dimos --transport zenoh shell
+```
+
+```python
+c = app.ControlCoordinator
+c.task_invoke("sonic_wbc", "state_snapshot")
+c.task_invoke("sonic_wbc", "list_locomotion_modes")
+c.task_invoke("sonic_wbc", "set_locomotion_mode", {"mode": "SLOW_WALK"})
+c.task_invoke("sonic_wbc", "list_motion_clips")
+c.task_invoke("sonic_wbc", "play_motion_clip", {"name": "macarena_001__A545"})
+c.task_invoke("sonic_wbc", "stop_motion_clip")
+c.set_estop(True)
+```
+
+E-stop latches damping. Releasing a control, resetting runtime state, or arming
+again cannot clear it; recovery requires restarting the stack. Hardware also
+checks feedback and command freshness in its independent motor publisher.
+
+For JetPack 5 or 6, use `bin/hardware/g1/setup-sonic`. It detects the release and
+installs the matching ONNX Runtime and model assets; `--check` checks system
+prerequisites only. JetPack 5 requires CUDA 11.8 with its compatibility driver
+and cuDNN 8; JetPack 6 requires CUDA 12.6 and cuDNN 9.
+Hardware starts unarmed with policy outputs in dry-run.
+The updated controller has not been validated by another physical activation.
+
+Squat and kneeling request zero translation. Centered sticks stop crawling
+while retaining its posture. Face-down mode 7 is unavailable, matching NVIDIA's
+selectable motion menu. Floor transitions remain timed; crawl stability and
+the earlier hardware instability remain open validation issues.

dimos/control/tasks/g1_sonic_wbc_task/_registry.py

@@ -0,0 +1,44 @@
+# 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.
+
+TASK_FACTORIES = {
+    "g1_sonic_wbc": "dimos.control.tasks.g1_sonic_wbc_task.g1_sonic_wbc_task:create_task",
+}
+
+TASK_CONSUMES: dict[str, dict[str, tuple[str, str]]] = {
+    "g1_sonic_wbc": {"twist_command": ("on_twist_command", "broadcast")},
+}
+
+TASK_EXPOSES: dict[str, list[str]] = {
+    "g1_sonic_wbc": [
+        "arm",
+        "disarm",
+        "set_dry_run",
+        "set_estop",
+        "reset_runtime_state",
+        "start",
+        "set_velocity_command",
+        "set_locomotion_mode",
+        "list_locomotion_modes",
+        "set_base_height",
+        "set_upper_body",
+        "clear_upper_body",
+        "state_snapshot",
+        "play_motion_clip",
+        "set_vr_3point",
+        "clear_vr_3point",
+        "stop_motion_clip",
+        "list_motion_clips",
+    ],
+}

dimos/control/tasks/g1_sonic_wbc_task/coordinator.py

@@ -0,0 +1,57 @@
+# Copyright 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.
+
+"""Connect SONIC task faults to the final G1 command publisher."""
+
+import asyncio
+
+from dimos.control.coordinator import ControlCoordinator
+from dimos.control.tasks.g1_sonic_wbc_task.g1_sonic_wbc_task import G1SonicWBCTask
+from dimos.core.core import rpc
+from dimos.core.global_config import global_config
+from dimos.core.stream import In, Out
+from dimos.msgs.sensor_msgs.JointState import JointState
+from dimos.msgs.std_msgs.String import String
+
+
+class SonicCoordinator(ControlCoordinator):
+    g1_joints: Out[JointState]
+    sonic_fault: Out[String]
+    g1_fault: In[String]
+
+    def _setup_from_config(self) -> None:
+        super()._setup_from_config()
+        for task in self._tasks.values():
+            if isinstance(task, G1SonicWBCTask) and not global_config.simulation:
+                task.set_fault_publisher(self._publish_sonic_fault)
+
+    def _publish_sonic_fault(self, reason: str) -> None:
+        self.sonic_fault.publish(String(reason))
+
+    @rpc
+    def stop(self) -> None:
+        super().stop()
+        with self._task_lock:
+            for task in self._tasks.values():
+                if isinstance(task, G1SonicWBCTask):
+                    task.stop()
+
+    async def handle_g1_fault(self, msg: String) -> None:
+        await asyncio.to_thread(self._apply_g1_fault, msg.data)
+
+    def _apply_g1_fault(self, reason: str) -> None:
+        with self._task_lock:
+            for task in self._tasks.values():
+                if isinstance(task, G1SonicWBCTask):
+                    task.on_hardware_fault(reason)