From f501f9dbda4028a53ad175801f9aaa4c3cfd6324 Mon Sep 17 00:00:00 2001 From: mustafab0 Date: Tue, 23 Jun 2026 00:03:20 -0700 Subject: [PATCH 1/2] feat(go2): command the robot with calibrated velocity (SPORT Move) over WebRTC MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit UnitreeWebRTCConnection.move() sent the body twist as raw normalized joystick stick deflections (lx/ly/rx) on WIRELESS_CONTROLLER, so the commanded wz/vx were never calibrated velocities — the firmware's nonlinear, speed-coupled gait mixer was the de-facto plant. move() now sends SPORT Move (api_id 1008) with real m/s & rad/s (x forward, y left, z yaw CCW), fire-and-forget at tick rate, via a shared _publish_move() helper that stop() also uses for a zero-velocity halt. Gait stays the caller's responsibility (GO2Connection already runs standup()/balance_stand() at start-up; Move works from BalanceStand). On hardware this removed a ~2.2x yaw over-turn: 0.8 rad/s commanded went from a 3.5s circle to ~10s (~224% -> ~78% of commanded yaw) — a clean linear gain characterization can now fit. Re-characterize K/tau/L on this command path. --- dimos/robot/unitree/connection.py | 53 +++++++++++++++++++------------ 1 file changed, 33 insertions(+), 20 deletions(-) diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index c5627ca8ad..62a1d7858a 100644 --- a/dimos/robot/unitree/connection.py +++ b/dimos/robot/unitree/connection.py @@ -15,6 +15,7 @@ import asyncio from dataclasses import dataclass import functools +import json import threading import time from typing import Any, TypeAlias, TypeVar @@ -25,6 +26,7 @@ from reactivex.observable import Observable from reactivex.subject import Subject from unitree_webrtc_connect.constants import ( + DATA_CHANNEL_TYPE, RTC_TOPIC, SPORT_CMD, VUI_COLOR, @@ -98,6 +100,7 @@ def __init__(self, ip: str, mode: str = "ai", aes_128_key: str | None = None) -> self.mode = mode self.stop_timer: threading.Timer | None = None self.cmd_vel_timeout = 0.2 + self._move_seq = 0 # monotonic request id for SPORT Move commands # Per-device AES-128 key for new Unitree firmware (data2=3 handshake); omitted when unset. self.conn = LegionConnection( WebRTCConnectionMethod.LocalSTA, ip=self.ip, aes_128_key=aes_128_key @@ -143,11 +146,9 @@ def stop(self) -> None: async def async_disconnect() -> None: try: - # Send stop command directly since we're already in the event loop. - self.conn.datachannel.pub_sub.publish_without_callback( - RTC_TOPIC["WIRELESS_CONTROLLER"], - data={"lx": 0, "ly": 0, "rx": 0, "ry": 0}, - ) + # Zero-velocity Move to halt before disconnecting (matches the + # command path; the firmware also stops on command loss). + self._publish_move(0.0, 0.0, 0.0) await self.conn.disconnect() except Exception: pass @@ -160,27 +161,39 @@ async def async_disconnect() -> None: if self.thread.is_alive(): self.thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) - def move(self, twist: Twist, duration: float = 0.0) -> bool: - """Send movement command to the robot using Twist commands. + def _publish_move(self, vx: float, vy: float, vyaw: float) -> None: + """Publish one SPORT ``Move`` (api_id 1008) velocity command. """ + self._move_seq += 1 + payload = { + "header": { + "identity": { + # Monotonic id; unique per command (nothing awaits a reply). + "id": self._move_seq, + "api_id": SPORT_CMD["Move"], # 1008 + } + }, + # parameter is a JSON STRING (firmware contract); publish_without_callback + # sends ``data`` verbatim and does not stringify it. + "parameter": json.dumps({"x": vx, "y": vy, "z": vyaw}), + } + self.conn.datachannel.pub_sub.publish_without_callback( + RTC_TOPIC["SPORT_MOD"], # "rt/api/sport/request" + data=payload, + msg_type=DATA_CHANNEL_TYPE["REQUEST"], # "req" + ) - Args: - twist: Twist message with linear and angular velocities - duration: How long to move (seconds). If 0, command is continuous + def move(self, twist: Twist, duration: float = 0.0) -> bool: + """Send a velocity command to the robot. - Returns: - bool: True if command was sent successfully + ``twist`` is a body-frame velocity (x forward, y left, z yaw CCW) in real + m/s & rad/s, sent via the calibrated SPORT ``Move`` API. + + Returns True if the command was sent successfully. """ x, y, yaw = twist.linear.x, twist.linear.y, twist.angular.z - # WebRTC coordinate mapping: - # x - Positive right, negative left - # y - positive forward, negative backwards - # yaw - Positive rotate right, negative rotate left async def async_move() -> None: - self.conn.datachannel.pub_sub.publish_without_callback( - RTC_TOPIC["WIRELESS_CONTROLLER"], - data={"lx": -y, "ly": x, "rx": -yaw, "ry": 0}, - ) + self._publish_move(x, y, yaw) async def async_move_duration() -> None: """Send movement commands continuously for the specified duration.""" From 7dd1357acd6b725342bd3ac5200c212869dbce0e Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Tue, 23 Jun 2026 07:26:32 +0000 Subject: [PATCH 2/2] [autofix.ci] apply automated fixes --- dimos/robot/unitree/connection.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index 62a1d7858a..f7040d9823 100644 --- a/dimos/robot/unitree/connection.py +++ b/dimos/robot/unitree/connection.py @@ -162,7 +162,7 @@ async def async_disconnect() -> None: self.thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) def _publish_move(self, vx: float, vy: float, vyaw: float) -> None: - """Publish one SPORT ``Move`` (api_id 1008) velocity command. """ + """Publish one SPORT ``Move`` (api_id 1008) velocity command.""" self._move_seq += 1 payload = { "header": { @@ -186,8 +186,8 @@ def move(self, twist: Twist, duration: float = 0.0) -> bool: """Send a velocity command to the robot. ``twist`` is a body-frame velocity (x forward, y left, z yaw CCW) in real - m/s & rad/s, sent via the calibrated SPORT ``Move`` API. - + m/s & rad/s, sent via the calibrated SPORT ``Move`` API. + Returns True if the command was sent successfully. """ x, y, yaw = twist.linear.x, twist.linear.y, twist.angular.z