Add Unitree G1 gripper control and multi-camera streaming support

- Add dedicated ZMQ channel (port 6002) for exo R3/L3 gripper commands
- Support explicit device paths and FOURCC in G1 camera server specs
- Force V4L2 backend so FOURCC/resolution can be set on RealSense/UVC
- Tolerate V4L2 set() quirks for width/height/fps in OpenCVCamera
- Always publish every camera's latest frame to fix ZMQ cross-feed flicker
- Add soft-start arm interpolation to default pose before Groot controller
- Add EMA smoothing to exoskeleton joint angles

Co-authored-by: Cursor <cursoragent@cursor.com>

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]]:

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(
+            cam_kwargs = {
-                index_or_path=cfg.get("device_id", 0),
+                "index_or_path": cfg.get("device_id", 0),
-                fps=self.fps,
+                "fps": self.fps,
-                width=shape[1],
+                "width": shape[1],
-                height=shape[0],
+                "height": shape[0],
-                color_mode=ColorMode.RGB,
+                "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):
+                if message["images"]:
-                    self.socket.send_string(json.dumps(message), zmq.NOBLOCK)
+                    with contextlib.suppress(zmq.Again):
+                        self.socket.send_string(json.dumps(message), zmq.NOBLOCK)
 
                 frame_count += 1
                 frame_times.append(time.time() - t0)

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:
+    if args.camera or args.cameras:
-        camera_config = {
+        if args.cameras:
-            "fps": args.camera_fps,
+            cameras = parse_camera_specs(args.cameras, args.camera_width, args.camera_height)
-            "cameras": {
+        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__":

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
+            self._interpolate_to_default(
-            num_steps = int(total_time / control_dt)
+                duration=3.0, control_dt=control_dt, default_positions=default_positions
-
+            )
-            # 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)
 
         # Reset controller internal state (gait phase, obs history, etc.)
         if self.controller is not None and hasattr(self.controller, "reset"):

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 self.angles_from_raw(self.read_raw())
-        return {} if raw is None else exo_raw_to_angles(raw, self.calibration)
+
+    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:

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()
+            left_angles = self.left_arm.angles_from_raw(left_raw)
-            right_angles = self.right_arm.get_angles()
+            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")