diff --git a/src/lerobot/robots/unitree_g1/run_g1_onboard.py b/src/lerobot/robots/unitree_g1/run_g1_onboard.py index 6f1f921cd..731fd34eb 100644 --- a/src/lerobot/robots/unitree_g1/run_g1_onboard.py +++ b/src/lerobot/robots/unitree_g1/run_g1_onboard.py @@ -22,12 +22,16 @@ action (arm joint targets + joystick axes + gripper flags) as JSON over ZMQ. Thi applies each action via ``UnitreeG1.send_action`` while the onboard controller thread keeps the legs balanced. -Pair with ``run_g1_teleop_client.py`` on the laptop. Grippers/cameras are handled -separately by ``run_g1_server.py`` and are intentionally out of scope here. +Pair with ``run_g1_teleop_client.py`` on the laptop. Grippers (exo L3/R3) are driven +directly over CAN here when ``--grippers`` is passed; cameras stay in ``run_g1_server.py``. -Example (on the robot): +Examples (on the robot): python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController + + # with grippers (bring CAN up first, e.g. lerobot-setup-can --mode=setup --interfaces=can0,can1) + python -m lerobot.robots.unitree_g1.run_g1_onboard --controller GrootLocomotionController \ + --grippers --gripper-port-left can1 --gripper-port-right can0 """ import argparse @@ -39,6 +43,7 @@ import threading import zmq from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config +from lerobot.robots.unitree_g1.run_g1_server import Gripper, build_gripper from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1 logging.basicConfig(level=logging.INFO, format="%(asctime)s [%(levelname)s] %(message)s", force=True) @@ -57,6 +62,21 @@ def main() -> None: action="store_true", help="Enable arm gravity compensation (needs pinocchio/casadi on the robot)", ) + # Gripper control from exo L3/R3 (same wiring as run_g1_server.py). + p.add_argument("--grippers", action="store_true", help="Drive Damiao grippers from exo L3/R3") + p.add_argument("--gripper-port-left", default="can1", help="CAN interface for LEFT gripper") + p.add_argument("--gripper-port-right", default="can0", help="CAN interface for RIGHT gripper") + p.add_argument("--gripper-send-id", type=lambda x: int(x, 0), default=0x08, help="Motor send CAN id") + p.add_argument("--gripper-recv-id", type=lambda x: int(x, 0), default=0x18, help="Motor recv CAN id") + p.add_argument("--gripper-motor-type", default="dm4310", help="Damiao motor type") + p.add_argument("--gripper-open-deg", type=float, default=-65.0, help="Gripper OPEN position (deg)") + p.add_argument("--gripper-close-deg", type=float, default=0.0, help="Gripper CLOSE position (deg)") + p.add_argument("--gripper-kp", type=float, default=15.0, help="MIT position gain (stiffness)") + p.add_argument("--gripper-kd", type=float, default=0.5, help="MIT damping gain") + p.add_argument( + "--gripper-no-fd", dest="gripper_fd", action="store_false", help="Classic CAN (non-FD adapter)" + ) + p.set_defaults(gripper_fd=True) args = p.parse_args() cfg = UnitreeG1Config( @@ -71,6 +91,25 @@ def main() -> None: logger.info("Connecting onboard robot (controller=%s)...", args.controller) robot.connect() + # Grippers: driven directly over CAN from the exo L3/R3 flags in each action + # (L3 = remote.button.4 -> left, R3 = remote.button.0 -> right; pressed = close). + grippers: dict[str, Gripper] = {} + if args.grippers: + for side, port in (("L", args.gripper_port_left), ("R", args.gripper_port_right)): + grippers[side] = build_gripper( + side, + port, + 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, + ) + logger.info("Grippers enabled: L3 -> left, R3 -> right") + ctx = zmq.Context.instance() sock = ctx.socket(zmq.PULL) sock.setsockopt(zmq.CONFLATE, 1) # only ever act on the freshest command @@ -99,6 +138,14 @@ def main() -> None: continue robot.send_action(action) + + if grippers: + # L3 = remote.button.4 -> left, R3 = remote.button.0 -> right. + if "L" in grippers and "remote.button.4" in action: + grippers["L"].apply(bool(action["remote.button.4"])) + if "R" in grippers and "remote.button.0" in action: + grippers["R"].apply(bool(action["remote.button.0"])) + n += 1 if n % 60 == 0: axes = { @@ -109,6 +156,11 @@ def main() -> None: logger.info("Applied %d actions | axes=%s buttons=%s", n, axes, btn) finally: logger.info("Shutting down onboard controller...") + for g in grippers.values(): + try: + g.bus.disconnect() + except Exception as e: # noqa: BLE001 + logger.warning("Gripper %s disconnect failed: %s", g.name, e) robot.disconnect()