diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index c5627ca8ad..f7040d9823 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 _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" + ) + def move(self, twist: Twist, duration: float = 0.0) -> bool: - """Send movement command to the robot using Twist commands. + """Send a velocity command to the robot. - Args: - twist: Twist message with linear and angular velocities - duration: How long to move (seconds). If 0, command is continuous + ``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: - bool: True if command was sent successfully + 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."""