diff --git a/src/lerobot/cameras/opencv/camera_opencv.py b/src/lerobot/cameras/opencv/camera_opencv.py index e50d24c01..1dac0d976 100644 --- a/src/lerobot/cameras/opencv/camera_opencv.py +++ b/src/lerobot/cameras/opencv/camera_opencv.py @@ -241,7 +241,12 @@ class OpenCVCamera(Camera): actual_fps = self.videocapture.get(cv2.CAP_PROP_FPS) # Use math.isclose for robust float comparison if not success or not math.isclose(self.fps, actual_fps, rel_tol=1e-3): - raise RuntimeError(f"{self} failed to set fps={self.fps} ({actual_fps=}).") + # Some cameras only run at a fixed rate (e.g. 90 fps). Rather than abort, adopt the + # camera's actual fps; downstream consumers can sample frames at whatever rate they need. + logger.warning( + f"{self} failed to set fps={self.fps} ({actual_fps=}); using the camera's actual fps." + ) + self.fps = actual_fps def _validate_fourcc(self) -> None: """Validates and sets the camera's FOURCC code.""" @@ -276,17 +281,23 @@ class OpenCVCamera(Camera): width_success = self.videocapture.set(cv2.CAP_PROP_FRAME_WIDTH, float(self.capture_width)) height_success = self.videocapture.set(cv2.CAP_PROP_FRAME_HEIGHT, float(self.capture_height)) + # Trust the measured resolution: some fixed-format V4L2 cameras return False from + # set() even when the value is already correct. Only fail if the actual value is wrong. actual_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH))) - if not width_success or self.capture_width != actual_width: + if self.capture_width != actual_width: raise RuntimeError( f"{self} failed to set capture_width={self.capture_width} ({actual_width=}, {width_success=})." ) + if not width_success: + logger.warning(f"{self} set(CAP_PROP_FRAME_WIDTH) returned False but {actual_width=} is correct.") actual_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT))) - if not height_success or self.capture_height != actual_height: + if self.capture_height != actual_height: raise RuntimeError( f"{self} failed to set capture_height={self.capture_height} ({actual_height=}, {height_success=})." ) + if not height_success: + logger.warning(f"{self} set(CAP_PROP_FRAME_HEIGHT) returned False but {actual_height=} is correct.") @staticmethod def find_cameras() -> list[dict[str, Any]]: diff --git a/src/lerobot/cameras/zmq/image_server.py b/src/lerobot/cameras/zmq/image_server.py index b8b6f8e74..b21d5e35f 100644 --- a/src/lerobot/cameras/zmq/image_server.py +++ b/src/lerobot/cameras/zmq/image_server.py @@ -31,7 +31,7 @@ import cv2 import numpy as np import zmq -from ..configs import ColorMode +from ..configs import ColorMode, Cv2Backends from ..opencv import OpenCVCamera, OpenCVCameraConfig logger = logging.getLogger(__name__) @@ -97,13 +97,21 @@ class ImageServer: for name, cfg in config.get("cameras", {}).items(): shape = cfg.get("shape", [480, 640]) - cam_config = OpenCVCameraConfig( - index_or_path=cfg.get("device_id", 0), - fps=self.fps, - width=shape[1], - height=shape[0], - color_mode=ColorMode.RGB, - ) + cam_kwargs = { + "index_or_path": cfg.get("device_id", 0), + "fps": self.fps, + "width": shape[1], + "height": shape[0], + "color_mode": ColorMode.RGB, + # Force V4L2 (Linux): the default FFMPEG backend is read-only for capture + # props, so it can't set FOURCC/resolution (e.g. RealSense color nodes). + "backend": Cv2Backends.V4L2, + } + # Some cameras (e.g. RealSense color nodes) won't apply a resolution unless the + # pixel format is forced first, so pass a FOURCC through when provided. + if cfg.get("fourcc"): + cam_kwargs["fourcc"] = cfg["fourcc"] + cam_config = OpenCVCameraConfig(**cam_kwargs) camera = OpenCVCamera(cam_config) camera.connect() self.cameras[name] = camera @@ -125,7 +133,6 @@ class ImageServer: def run(self): frame_count = 0 frame_times = deque(maxlen=60) - last_published_ts: dict[str, float] = {} # Start all capture threads for capture_thread in self.capture_threads.values(): @@ -142,18 +149,20 @@ class ImageServer: while True: t0 = time.time() - # Build message + # Build message. Always include EVERY camera's latest frame so each message + # is complete: clients pick their own stream by name, and a partial message + # makes them fall back to another camera's image (cross-feed flicker). message = {"timestamps": {}, "images": {}} for name, capture_thread in self.capture_threads.items(): encoded, timestamp = capture_thread.get_latest() - if encoded is not None and timestamp > last_published_ts.get(name, 0.0): + if encoded is not None: message["timestamps"][name] = timestamp message["images"][name] = encoded - last_published_ts[name] = timestamp # Send as JSON string (suppress if buffer full) - with contextlib.suppress(zmq.Again): - self.socket.send_string(json.dumps(message), zmq.NOBLOCK) + if message["images"]: + with contextlib.suppress(zmq.Again): + self.socket.send_string(json.dumps(message), zmq.NOBLOCK) frame_count += 1 frame_times.append(time.time() - t0) diff --git a/src/lerobot/robots/unitree_g1/run_g1_server.py b/src/lerobot/robots/unitree_g1/run_g1_server.py index b5bd0baf8..dd409867c 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_server.py +++ b/src/lerobot/robots/unitree_g1/run_g1_server.py @@ -30,7 +30,8 @@ import contextlib import json import threading import time -from typing import Any +from dataclasses import dataclass +from typing import TYPE_CHECKING, Any import zmq from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient @@ -41,6 +42,9 @@ from unitree_sdk2py.utils.crc import CRC from lerobot.cameras.zmq.image_server import ImageServer +if TYPE_CHECKING: + from lerobot.motors.damiao.damiao import DamiaoMotorsBus + # DDS topic names follow Unitree SDK naming conventions # ruff: noqa: N816 kTopicLowCommand_Debug = "rt/lowcmd" # action to robot @@ -48,9 +52,106 @@ kTopicLowState = "rt/lowstate" # observation from robot LOWCMD_PORT = 6000 LOWSTATE_PORT = 6001 +# Side-channel for gripper commands sent by the teleop laptop (exo R3/L3 clicks). +# The exo joystick buttons are only known laptop-side, so the robot object forwards +# them here as JSON {"L": 0/1, "R": 0/1}; see UnitreeG1._send_gripper_cmd. +GRIPPER_PORT = 6002 NUM_MOTORS = 35 +@dataclass +class Gripper: + """A single Damiao gripper that only writes to CAN when the open/close state changes.""" + + name: str + bus: "DamiaoMotorsBus" + open_deg: float + close_deg: float + _last_cmd: str | None = None # "open" | "close" + + def apply(self, want_close: bool) -> None: + want = "close" if want_close else "open" + if want == self._last_cmd: + return + target = self.close_deg if want_close else self.open_deg + self.bus.write("Goal_Position", "gripper", target) + self._last_cmd = want + print(f"[gripper] {self.name} -> {want.upper()} ({target:.1f} deg)") + + +def build_gripper( + name: str, + port: str, + send_id: int, + recv_id: int, + motor_type: str, + use_can_fd: bool, + open_deg: float, + close_deg: float, + kp: float, + kd: float, +) -> Gripper: + from lerobot.motors.damiao.damiao import DamiaoMotorsBus + from lerobot.motors.motors_bus import Motor, MotorNormMode + + motors = { + "gripper": Motor( + id=send_id, + model=motor_type, + norm_mode=MotorNormMode.DEGREES, + motor_type_str=motor_type, + recv_id=recv_id, + ) + } + bus = DamiaoMotorsBus(port=port, motors=motors, use_can_fd=use_can_fd) + print(f"Connecting {name} gripper on {port} (fd={use_can_fd})...") + bus.connect(handshake=True) + bus.write("Kp", "gripper", kp) + bus.write("Kd", "gripper", kd) + bus.write("Goal_Position", "gripper", open_deg) # start open + print(f" {name}: connected, torque enabled, opened.") + return Gripper(name, bus, open_deg, close_deg, _last_cmd="open") + + +def parse_camera_specs(spec: str, default_width: int, default_height: int) -> dict[str, dict]: + """Parse a multi-camera spec string into an ImageServer `cameras` dict. + + Format: comma-separated ``name:device_id[:WxH[:FOURCC]]`` entries, e.g. + ``head_camera:4,left_wrist:0,right_wrist:1,ego:2``. ``device_id`` may be an + integer index or an explicit device path (e.g. ``/dev/video4``); the path form + is more reliable when the bare integer index fails to open. The optional ``WxH`` + overrides the default resolution (e.g. ``left_wrist:0:640x480``). The optional + ``FOURCC`` forces a pixel format (e.g. ``head_camera:/dev/video8:1280x720:YUYV``), + which some cameras (e.g. RealSense color nodes) require before the resolution + can be applied. + """ + cameras: dict[str, dict] = {} + for entry in spec.split(","): + entry = entry.strip() + if not entry: + continue + parts = entry.split(":") + if len(parts) < 2: + raise ValueError(f"Invalid camera spec '{entry}', expected 'name:device_id[:WxH[:FOURCC]]'") + name = parts[0].strip() + raw_id = parts[1].strip() + # Accept either an integer index or an explicit device path (e.g. /dev/video4). + device_id: int | str = int(raw_id) if raw_id.lstrip("-").isdigit() else raw_id + width, height = default_width, default_height + if len(parts) >= 3 and parts[2].strip(): + wh = parts[2].lower().split("x") + if len(wh) != 2: + raise ValueError(f"Invalid resolution '{parts[2]}' in '{entry}', expected 'WxH'") + width, height = int(wh[0]), int(wh[1]) + fourcc = parts[3].strip().upper() if len(parts) >= 4 and parts[3].strip() else None + if name in cameras: + raise ValueError(f"Duplicate camera name '{name}' in --cameras") + cameras[name] = {"device_id": device_id, "shape": [height, width], "fourcc": fourcc} + if not cameras: + raise ValueError("No cameras parsed from --cameras spec") + return cameras + + def lowstate_to_dict(msg: hg_LowState) -> dict[str, Any]: """Convert LowState SDK message to a JSON-serializable dictionary.""" motor_states = [] @@ -98,6 +199,34 @@ def dict_to_lowcmd(data: dict[str, Any]) -> hg_LowCmd: return cmd +def gripper_cmd_loop( + gripper_sock: zmq.Socket, + grippers: dict[str, Gripper], + shutdown_event: threading.Event, +) -> None: + """Receive gripper commands from the teleop laptop and apply them. + + Payload is JSON ``{"L": 0/1, "R": 0/1}`` where 1 = close, 0 = open. Only writes + CAN when a gripper's state actually changes (handled by Gripper.apply). + """ + while not shutdown_event.is_set(): + try: + payload = gripper_sock.recv() + except zmq.ContextTerminated: + break + except zmq.Again: + continue + try: + cmd = json.loads(payload.decode("utf-8")) + except (json.JSONDecodeError, UnicodeDecodeError): + continue + print(f"[gripper] recv {cmd}") + if "L" in grippers and "L" in cmd: + grippers["L"].apply(bool(cmd["L"])) + if "R" in grippers and "R" in cmd: + grippers["R"].apply(bool(cmd["R"])) + + def state_forward_loop( lowstate_sub: ChannelSubscriber, lowstate_sock: zmq.Socket, @@ -156,28 +285,58 @@ def main() -> None: parser = argparse.ArgumentParser(description="DDS-to-ZMQ bridge server for Unitree G1") parser.add_argument("--camera", action="store_true", help="Also launch camera server") parser.add_argument("--camera-device", type=int, default=4, help="Camera device ID (default: 4)") + parser.add_argument( + "--cameras", + type=str, + default=None, + help=( + "Multi-camera spec 'name:device_id[:WxH]' comma-separated, e.g. " + "'head_camera:4,left_wrist:0,right_wrist:1,ego:2'. Overrides --camera-device " + "and implies --camera. Per-camera resolution optional (defaults to " + "--camera-width/--camera-height)." + ), + ) parser.add_argument("--camera-fps", type=int, default=30, help="Camera FPS (default: 30)") parser.add_argument("--camera-width", type=int, default=640, help="Camera width (default: 640)") parser.add_argument("--camera-height", type=int, default=480, help="Camera height (default: 480)") parser.add_argument("--camera-port", type=int, default=5555, help="Camera ZMQ port (default: 5555)") + # Gripper control from wireless-remote R3/L3 + parser.add_argument( + "--grippers", action="store_true", help="Enable Damiao gripper control from wireless remote R3/L3" + ) + parser.add_argument("--gripper-port-left", default="can1", help="CAN interface for LEFT gripper") + parser.add_argument("--gripper-port-right", default="can0", help="CAN interface for RIGHT gripper") + parser.add_argument("--gripper-send-id", type=lambda x: int(x, 0), default=0x08, help="Motor send CAN id") + parser.add_argument("--gripper-recv-id", type=lambda x: int(x, 0), default=0x18, help="Motor recv CAN id") + parser.add_argument("--gripper-motor-type", default="dm4310", help="Damiao motor type") + parser.add_argument("--gripper-open-deg", type=float, default=-65.0, help="Gripper OPEN position (deg)") + parser.add_argument("--gripper-close-deg", type=float, default=0.0, help="Gripper CLOSE position (deg)") + parser.add_argument("--gripper-kp", type=float, default=15.0, help="MIT position gain (stiffness)") + parser.add_argument("--gripper-kd", type=float, default=0.5, help="MIT damping gain") + parser.add_argument( + "--gripper-no-fd", dest="gripper_fd", action="store_false", help="Classic CAN (non-FD adapter)" + ) + parser.set_defaults(gripper_fd=True) args = parser.parse_args() # Optionally start camera server in background thread camera_thread = None - if args.camera: - camera_config = { - "fps": args.camera_fps, - "cameras": { + if args.camera or args.cameras: + if args.cameras: + cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height) + else: + cameras = { "head_camera": { "device_id": args.camera_device, "shape": [args.camera_height, args.camera_width], } - }, - } + } + camera_config = {"fps": args.camera_fps, "cameras": cameras} camera_server = ImageServer(camera_config, port=args.camera_port) camera_thread = threading.Thread(target=camera_server.run, daemon=True) camera_thread.start() - print(f"Camera server started on port {args.camera_port} (device {args.camera_device})") + cam_summary = ", ".join(f"{name}(dev {c['device_id']})" for name, c in cameras.items()) + print(f"Camera server started on port {args.camera_port}: {cam_summary}") # initialize DDS ChannelFactoryInitialize(0) @@ -214,6 +373,25 @@ def main() -> None: lowstate_sock = ctx.socket(zmq.PUB) lowstate_sock.bind(f"tcp://0.0.0.0:{LOWSTATE_PORT}") + # Optionally connect Damiao grippers driven by exo R3/L3 (forwarded from the laptop) + grippers: dict[str, Gripper] = {} + gripper_sock = None + if args.grippers: + try: + grippers["L"] = build_gripper( + "L", args.gripper_port_left, args.gripper_send_id, args.gripper_recv_id, + args.gripper_motor_type, args.gripper_fd, args.gripper_open_deg, + args.gripper_close_deg, args.gripper_kp, args.gripper_kd, + ) + grippers["R"] = build_gripper( + "R", args.gripper_port_right, args.gripper_send_id, args.gripper_recv_id, + args.gripper_motor_type, args.gripper_fd, args.gripper_open_deg, + args.gripper_close_deg, args.gripper_kp, args.gripper_kd, + ) + except Exception as e: # noqa: BLE001 + print(f"WARNING: gripper setup failed ({e}); continuing without grippers.") + grippers = {} + state_period = 0.002 # ~500 hz shutdown_event = threading.Event() @@ -224,6 +402,18 @@ def main() -> None: ) t_state.start() + # start gripper command listener (commands come from the teleop laptop) + t_gripper = None + if grippers: + gripper_sock = ctx.socket(zmq.PULL) + gripper_sock.bind(f"tcp://0.0.0.0:{GRIPPER_PORT}") + t_gripper = threading.Thread( + target=gripper_cmd_loop, + args=(gripper_sock, grippers, shutdown_event), + ) + t_gripper.start() + print(f"Grippers enabled: listening for R3/L3 commands on port {GRIPPER_PORT}") + print("bridge running (lowstate -> zmq, lowcmd -> dds)") # run command forwarding in main thread @@ -235,8 +425,15 @@ def main() -> None: shutdown_event.set() ctx.term() # terminates blocking zmq.recv() calls t_state.join(timeout=2.0) + if t_gripper is not None: + t_gripper.join(timeout=2.0) if camera_thread is not None: camera_thread.join(timeout=2.0) + for g in grippers.values(): + try: + g.bus.disconnect(disable_torque=True) + except Exception as exc: # noqa: BLE001 + print(f" {g.name} gripper disconnect error: {exc}") if __name__ == "__main__": diff --git a/src/lerobot/robots/unitree_g1/unitree_g1.py b/src/lerobot/robots/unitree_g1/unitree_g1.py index 5b8be0941..a065d8d2f 100644 --- a/src/lerobot/robots/unitree_g1/unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/unitree_g1.py @@ -79,6 +79,10 @@ class LocomotionController(Protocol): kTopicLowCommand_Debug = "rt/lowcmd" kTopicLowState = "rt/lowstate" +# Side-channel port on the robot for forwarding exo R3/L3 gripper commands +# (see run_g1_server.gripper_cmd_loop). Real-robot only. +GRIPPER_CMD_PORT = 6002 + @dataclass class MotorState: @@ -305,6 +309,23 @@ class UnitreeG1(Robot): else: self._ChannelFactoryInitialize(0, config=self.config) + # Gripper command side-channel (real robot only): forwards exo R3/L3 clicks to + # run_g1_server, which drives the Damiao grippers over CAN. + self._gripper_sock = None + self._last_gripper_cmd = None + if not self.config.is_simulation: + try: + import zmq + + sock = zmq.Context.instance().socket(zmq.PUSH) + sock.setsockopt(zmq.SNDHWM, 2) + sock.setsockopt(zmq.LINGER, 0) + sock.connect(f"tcp://{self.config.robot_ip}:{GRIPPER_CMD_PORT}") + self._gripper_sock = sock + except Exception as e: # noqa: BLE001 + logger.warning(f"Gripper command channel setup failed ({e}); grippers disabled.") + self._gripper_sock = None + # Initialize direct motor control interface self.lowcmd_publisher = self._ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) self.lowcmd_publisher.Init() @@ -352,6 +373,13 @@ class UnitreeG1(Robot): # Start controller thread if enabled if self.controller is not None: + # Soft-start: ramp the arms from their current pose to the default position + # before the locomotion policy takes over, avoiding a jump at startup. The + # controller owns the legs, so send_action only moves the arms here. Runs in + # both sim and on the real robot so the ramp is visible in MuJoCo too. + logger.info("Soft-start: ramping arms to default position...") + self._interpolate_to_default(duration=3.0) + self._controller_thread = threading.Thread(target=self._controller_loop, daemon=True) self._controller_thread.start() fps = int(1.0 / self.controller.control_dt) @@ -412,6 +440,12 @@ class UnitreeG1(Robot): self.sim_env = None self._env_wrapper = None + # Close gripper command channel + sock = getattr(self, "_gripper_sock", None) + if sock is not None: + sock.close(linger=0) + self._gripper_sock = None + # Disconnect cameras for cam in self._cameras.values(): cam.disconnect() @@ -500,8 +534,36 @@ class UnitreeG1(Robot): tau[joint.value] = arm_tau[local_idx] self.publish_lowcmd(action_to_publish, tau=tau) + self._send_gripper_cmd(action) return action + def _send_gripper_cmd(self, action: RobotAction) -> None: + """Forward exo R3/L3 button flags to run_g1_server to open/close the grippers. + + L3 (left stick, button.4) -> left gripper, R3 (right stick, button.0) -> right. + Only sends when the state changes to avoid flooding the channel. + """ + sock = getattr(self, "_gripper_sock", None) + if sock is None: + return + l3 = action.get("remote.button.4") + r3 = action.get("remote.button.0") + if l3 is None and r3 is None: + logger.warning("[gripper] no remote.button.0/4 in action — teleop not emitting exo buttons") + return + cmd = {"L": int(bool(l3)), "R": int(bool(r3))} + if cmd == self._last_gripper_cmd: + return + self._last_gripper_cmd = cmd + + import zmq + + try: + sock.send_json(cmd, zmq.NOBLOCK) + logger.info(f"[gripper] sent {cmd} to {self.config.robot_ip}:{GRIPPER_CMD_PORT}") + except zmq.ZMQError as e: + logger.warning(f"[gripper] send failed ({e})") + def _update_controller_action(self, action: RobotAction) -> None: """Update controller input state from incoming teleop action.""" with self._controller_action_lock: @@ -527,6 +589,48 @@ class UnitreeG1(Robot): def cameras(self) -> dict: return self._cameras + def _interpolate_to_default( + self, + duration: float = 3.0, + control_dt: float | None = None, + default_positions: np.ndarray | list[float] | None = None, + ) -> None: + """Smoothly ramp joints from their current pose to the default pose (real robot). + + When a locomotion controller owns the legs, ``send_action`` filters to the arm + joints, so this effectively ramps only the arms — enough to avoid a startup snap. + """ + if control_dt is None: + control_dt = self.config.control_dt + if default_positions is None: + default_positions = np.array(self.config.default_positions, dtype=np.float32) + + num_steps = max(1, int(duration / control_dt)) + + # record current positions + obs = self.get_observation() + init_dof_pos = np.zeros(29, dtype=np.float32) + for motor in G1_29_JointIndex: + init_dof_pos[motor.value] = obs[f"{motor.name}.q"] + + # Interpolate to default position + for step in range(num_steps): + start_time = time.time() + + alpha = step / num_steps + action_dict = {} + for motor in G1_29_JointIndex: + target_pos = default_positions[motor.value] + interp_pos = init_dof_pos[motor.value] * (1 - alpha) + target_pos * alpha + action_dict[f"{motor.name}.q"] = float(interp_pos) + + self.send_action(action_dict) + + # Maintain constant control rate + elapsed = time.time() - start_time + sleep_time = max(0, control_dt - elapsed) + time.sleep(sleep_time) + def reset( self, control_dt: float | None = None, @@ -543,34 +647,9 @@ class UnitreeG1(Robot): {f"{motor.name}.q": float(default_positions[motor.value]) for motor in G1_29_JointIndex} ) else: - total_time = 3.0 - num_steps = int(total_time / control_dt) - - # get current state - obs = self.get_observation() - - # record current positions - init_dof_pos = np.zeros(29, dtype=np.float32) - for motor in G1_29_JointIndex: - init_dof_pos[motor.value] = obs[f"{motor.name}.q"] - - # Interpolate to default position - for step in range(num_steps): - start_time = time.time() - - alpha = step / num_steps - action_dict = {} - for motor in G1_29_JointIndex: - target_pos = default_positions[motor.value] - interp_pos = init_dof_pos[motor.value] * (1 - alpha) + target_pos * alpha - action_dict[f"{motor.name}.q"] = float(interp_pos) - - self.send_action(action_dict) - - # Maintain constant control rate - elapsed = time.time() - start_time - sleep_time = max(0, control_dt - elapsed) - time.sleep(sleep_time) + self._interpolate_to_default( + duration=3.0, control_dt=control_dt, default_positions=default_positions + ) # Reset controller internal state (gait phase, obs history, etc.) if self.controller is not None and hasattr(self.controller, "reset"): diff --git a/src/lerobot/teleoperators/unitree_g1/exo_serial.py b/src/lerobot/teleoperators/unitree_g1/exo_serial.py index ce5492537..c15f0dc4d 100644 --- a/src/lerobot/teleoperators/unitree_g1/exo_serial.py +++ b/src/lerobot/teleoperators/unitree_g1/exo_serial.py @@ -71,12 +71,16 @@ class ExoskeletonArm: calibration_fpath: Path side: str baud_rate: int = 115200 + ema_window: int = 10 _ser: serial.Serial | None = None calibration: ExoskeletonCalibration | None = None def __post_init__(self): require_package("pyserial", extra="unitree_g1", import_name="serial") + # EMA smoothing on joint angles to reduce jitter (mirrors homunculus glove). + self._ema_alpha = 2 / (self.ema_window + 1) + self._ema: dict[str, float] = {} if self.calibration_fpath.is_file(): self._load_calibration() @@ -124,8 +128,28 @@ class ExoskeletonArm: def get_angles(self) -> dict[str, float]: if not self.calibration: raise RuntimeError("exoskeleton not calibrated") - raw = self.read_raw() - return {} if raw is None else exo_raw_to_angles(raw, self.calibration) + return self.angles_from_raw(self.read_raw()) + + def angles_from_raw(self, raw: list[int] | None) -> dict[str, float]: + """Convert an already-read raw frame to EMA-smoothed joint angles. + + Lets callers read the raw frame once (e.g. to also inspect the joystick + button channel) without consuming a second serial sample. + """ + if not self.calibration: + raise RuntimeError("exoskeleton not calibrated") + if raw is None: + return {} + return self._apply_ema(exo_raw_to_angles(raw, self.calibration)) + + def _apply_ema(self, angles: dict[str, float]) -> dict[str, float]: + """Exponential moving average per joint angle; lazily initialised on first read.""" + smoothed: dict[str, float] = {} + for joint, value in angles.items(): + prev = self._ema.get(joint) + self._ema[joint] = value if prev is None else self._ema_alpha * value + (1 - self._ema_alpha) * prev + smoothed[joint] = self._ema[joint] + return smoothed def calibrate(self) -> None: if not self.is_connected: diff --git a/src/lerobot/teleoperators/unitree_g1/unitree_g1.py b/src/lerobot/teleoperators/unitree_g1/unitree_g1.py index 242613e7e..66a60b39c 100644 --- a/src/lerobot/teleoperators/unitree_g1/unitree_g1.py +++ b/src/lerobot/teleoperators/unitree_g1/unitree_g1.py @@ -209,10 +209,12 @@ class UnitreeG1Teleoperator(Teleoperator): @cached_property def action_features(self) -> dict[str, type]: remote_features = dict.fromkeys(self.remote_controller.remote_action, float) + # Exo joystick clicks (R3/L3) surfaced as button flags for gripper control. + gripper_features = {"remote.button.0": float, "remote.button.4": float} if not self._arm_control_enabled: - return remote_features + return {**remote_features, **gripper_features} joint_features = {f"{name}.q": float for name in self._g1_arm_joint_names} - return {**joint_features, **remote_features} + return {**joint_features, **remote_features, **gripper_features} @cached_property def feedback_features(self) -> dict[str, type]: @@ -276,8 +278,8 @@ class UnitreeG1Teleoperator(Teleoperator): left_raw = self.left_arm.read_raw() right_raw = self.right_arm.read_raw() - left_angles = self.left_arm.get_angles() - right_angles = self.right_arm.get_angles() + left_angles = self.left_arm.angles_from_raw(left_raw) + right_angles = self.right_arm.angles_from_raw(right_raw) joint_action = self.ik_helper.compute_g1_joints_from_exo(left_angles, right_angles) # Wireless remote has priority when non-zero; otherwise, use exo joystick. @@ -290,7 +292,23 @@ class UnitreeG1Teleoperator(Teleoperator): rc.set_from_exo(right_raw, "right") rc._sync_remote_action() - return {**joint_action, **rc.remote_action} + gripper_buttons = self._exo_gripper_buttons(left_raw, right_raw) + return {**joint_action, **rc.remote_action, **gripper_buttons} + + def _exo_gripper_buttons(self, left_raw: list[int] | None, right_raw: list[int] | None) -> dict[str, float]: + """Exo joystick clicks as button flags: L3 (left stick) -> button.4, R3 (right) -> button.0. + + Reads the raw joystick-button ADC channel directly (pressed pulls it below mid-scale), + so it is independent of the wireless-remote priority logic above. UnitreeG1.send_action + forwards these to the robot server to open/close the grippers. + """ + rc = self.remote_controller + idx = rc.JOYSTICK_BTN_IDX + + def pressed(raw: list[int] | None) -> float: + return 1.0 if (raw is not None and len(raw) > idx and raw[idx] < rc.ADC_HALF) else 0.0 + + return {"remote.button.4": pressed(left_raw), "remote.button.0": pressed(right_raw)} def send_feedback(self, feedback: dict[str, Any]) -> None: wireless_remote = feedback.get("wireless_remote")