From 57a9147e791798cb91777d2487b37d3d7b3f5439 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 07:34:43 +0000 Subject: [PATCH 1/9] Port soccerbot orchestrator with in-process ACT, clamping, and killswitch Make soccerbot the core workspace orchestrator that imports local-vla-inference (ACT pickup with ajkoder/g1-pickup-ball-act, clamp 0.002, working teleimager zmq://192.168.123.164:55555) and scripted-behavior stages in-process. Add Rerun telemetry, Ctrl+C graceful reset (StopMove + arm_sdk release), headed killswitch GUI, and root install/diagnose/run scripts. Leave front_camera single-port path untouched. Co-authored-by: arjuncoder1 --- diagnose.sh | 180 +++++++++++++++++ install.sh | 44 +++++ killswitch.sh | 23 +++ local-vla-inference/embodiment_g1_14d.py | 4 + local-vla-inference/main.py | 156 +++++++++++++-- local-vla-inference/pyproject.toml | 21 +- local-vla-inference/telemetry.py | 189 ++++++++++++++++++ run_soccerbot.sh | 36 ++++ scripted-behavior/main.py | 115 ++--------- scripted-behavior/pickup.py | 116 ++++++----- scripted-behavior/pyproject.toml | 28 ++- scripted-behavior/throw.py | 63 +++++- sim/pyproject.toml | 11 +- soccerbot/README.md | 46 +++++ soccerbot/pyproject.toml | 21 +- soccerbot/src/soccerbot/__init__.py | 3 + soccerbot/src/soccerbot/__main__.py | 3 + soccerbot/src/soccerbot/config.py | 44 +++++ soccerbot/src/soccerbot/deps.py | 46 +++++ soccerbot/src/soccerbot/killswitch.py | 240 +++++++++++++++++++++++ soccerbot/src/soccerbot/main.py | 191 ++++++++++++++++++ soccerbot/src/soccerbot/pickup.py | 63 ++++++ soccerbot/src/soccerbot/safety.py | 129 ++++++++++++ uv.lock | 28 ++- 24 files changed, 1612 insertions(+), 188 deletions(-) create mode 100755 diagnose.sh create mode 100755 install.sh create mode 100755 killswitch.sh create mode 100644 local-vla-inference/telemetry.py create mode 100755 run_soccerbot.sh create mode 100644 soccerbot/src/soccerbot/__init__.py create mode 100644 soccerbot/src/soccerbot/__main__.py create mode 100644 soccerbot/src/soccerbot/config.py create mode 100644 soccerbot/src/soccerbot/deps.py create mode 100644 soccerbot/src/soccerbot/killswitch.py create mode 100644 soccerbot/src/soccerbot/pickup.py create mode 100644 soccerbot/src/soccerbot/safety.py diff --git a/diagnose.sh b/diagnose.sh new file mode 100755 index 0000000..4e6a0ab --- /dev/null +++ b/diagnose.sh @@ -0,0 +1,180 @@ +#!/usr/bin/env bash +# Check that the robot workstation is ready to run soccerbot. +set -euo pipefail + +REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +VENV_DIR="${VENV_DIR:-$REPO_ROOT/.venv}" +CYCLONE_PREFIX="${CYCLONEDDS_HOME:-${CYCLONE_PREFIX:-$HOME/cyclonedds/install}}" +IFACE="" +STRICT=0 + +while [[ $# -gt 0 ]]; do + case "$1" in + --iface) IFACE="${2:-}"; shift 2 ;; + --iface=*) IFACE="${1#*=}"; shift ;; + --strict) STRICT=1; shift ;; + -h|--help) + echo "Usage: ./diagnose.sh [--iface enp5s0] [--strict]" + exit 0 + ;; + *) echo "unknown arg: $1" >&2; exit 2 ;; + esac +done + +pass=0 +fail=0 +warn=0 + +ok() { echo " [OK] $*"; pass=$((pass + 1)); } +bad() { echo " [FAIL] $*"; fail=$((fail + 1)); } +soft() { echo " [WARN] $*"; warn=$((warn + 1)); } + +echo "==> Soccerbot diagnose" +echo " repo: $REPO_ROOT" + +# --- toolchain --- +if command -v uv >/dev/null 2>&1; then ok "uv: $(command -v uv)"; else bad "uv not on PATH"; fi +if [[ -x "$VENV_DIR/bin/python" ]]; then + PY_VER="$("$VENV_DIR/bin/python" -c 'import sys; print(f"{sys.version_info.major}.{sys.version_info.minor}")')" + ok "venv python $PY_VER at $VENV_DIR" + if [[ "$PY_VER" == "3.13" || "$PY_VER" == "3.14" ]]; then + bad "venv is Python $PY_VER; robot needs 3.12 for cyclonedds 0.10.2 (run ./install.sh)" + fi +else + bad "missing $VENV_DIR — run ./install.sh" +fi + +if [[ -f "$CYCLONE_PREFIX/lib/libddsc.so" || -f "$CYCLONE_PREFIX/lib/libddsc.dylib" ]]; then + ok "CycloneDDS at $CYCLONE_PREFIX" +else + bad "CycloneDDS missing at $CYCLONE_PREFIX — run ./install.sh" +fi + +export CYCLONEDDS_HOME="$CYCLONE_PREFIX" +export LD_LIBRARY_PATH="${CYCLONE_PREFIX}/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" +export PATH="$VENV_DIR/bin:$PATH" + +# --- python imports --- +if [[ -x "$VENV_DIR/bin/python" ]]; then + if "$VENV_DIR/bin/python" - <<'PY' +import cyclonedds +import unitree_sdk2py +from unitree_sdk2py.utils.crc import CRC +CRC() +print("cyclonedds+unitree ok") +PY + then ok "cyclonedds + unitree_sdk2py import" + else bad "cyclonedds / unitree_sdk2py import failed" + fi + + if "$VENV_DIR/bin/python" - <<'PY' +import torch, zmq, cv2 +print("torch", torch.__version__) +PY + then ok "torch + zmq + cv2" + else bad "torch/zmq/cv2 import failed" + fi + + if "$VENV_DIR/bin/python" - <<'PY' +import sys +from pathlib import Path +sys.path.insert(0, str(Path("soccerbot/src").resolve())) +import soccerbot +from soccerbot.config import DEFAULT_POLICY, DEFAULT_CLAMP_RAD, DEFAULT_CAMERA +assert DEFAULT_POLICY == "ajkoder/g1-pickup-ball-act" +assert DEFAULT_CLAMP_RAD == 0.002 +assert "55555" in DEFAULT_CAMERA +print("soccerbot ok", soccerbot.__version__) +PY + then ok "soccerbot package (policy/clamp/camera defaults)" + else soft "soccerbot import via src path failed (uv sync may still be needed)" + fi + + if "$VENV_DIR/bin/python" - <<'PY' +import rerun # noqa: F401 +print("rerun ok") +PY + then ok "rerun-sdk (viz)" + else soft "rerun-sdk missing — ACT will run without live viz (lerobot[viz])" + fi + + if "$VENV_DIR/bin/python" - <<'PY' +import tkinter # noqa: F401 +print("tkinter ok") +PY + then ok "tkinter (headed killswitch)" + else soft "tkinter missing — install python3-tk for ./killswitch.sh" + fi + + # local-vla defaults (avoid importing cv2/torch-heavy main unless deps present) + if "$VENV_DIR/bin/python" - <<'PY' +import sys +from pathlib import Path +sys.path.insert(0, str(Path("local-vla-inference").resolve())) +import embodiment_g1_14d as layout +assert layout.DEFAULT_POLICY_ID == "ajkoder/g1-pickup-ball-act" +# Prefer full API check when vision/torch stack is installed. +try: + import main as local_vla + args = local_vla.build_args() + assert args.policy == "ajkoder/g1-pickup-ball-act" + assert args.clamp == 0.002 + assert args.camera.endswith(":55555") + print("local-vla build_args ok") +except ModuleNotFoundError as exc: + print("local-vla layout defaults ok; full main import skipped:", exc) +PY + then ok "local-vla-inference defaults / API" + else bad "local-vla-inference defaults failed" + fi + + if "$VENV_DIR/bin/python" - <<'PY' +import sys +from pathlib import Path +sys.path.insert(0, str(Path("scripted-behavior").resolve())) +import arm_replay, throw, turn_180, avoid +from throw import throw_ball +print("scripted-behavior stages ok") +PY + then ok "scripted-behavior stages importable" + else bad "scripted-behavior import failed" + fi +fi + +# --- network (optional) --- +if [[ -n "$IFACE" ]]; then + if ip link show "$IFACE" >/dev/null 2>&1; then + ok "iface $IFACE exists" + else + bad "iface $IFACE not found" + fi + if ping -c 1 -W 1 192.168.123.161 >/dev/null 2>&1; then + ok "ping G1 192.168.123.161" + else + soft "cannot ping 192.168.123.161 (robot off / wrong network?)" + fi + if ping -c 1 -W 1 192.168.123.164 >/dev/null 2>&1; then + ok "ping teleimager host 192.168.123.164" + else + soft "cannot ping 192.168.123.164 (teleimager host)" + fi + if [[ -x "$VENV_DIR/bin/python" ]]; then + if "$REPO_ROOT/local-vla-inference/run.sh" diag_state.py --iface "$IFACE" --once >/tmp/soccerbot_diag_state.txt 2>&1; then + ok "diag_state.py --once (see /tmp/soccerbot_diag_state.txt)" + else + soft "diag_state.py failed (robot not ready?) — /tmp/soccerbot_diag_state.txt" + fi + fi +else + soft "pass --iface enp5s0 to also check NIC / robot / teleimager reachability" +fi + +echo +echo "Summary: $pass ok, $warn warn, $fail fail" +if [[ "$fail" -gt 0 ]]; then + exit 1 +fi +if [[ "$STRICT" -eq 1 && "$warn" -gt 0 ]]; then + exit 1 +fi +exit 0 diff --git a/install.sh b/install.sh new file mode 100755 index 0000000..536efdb --- /dev/null +++ b/install.sh @@ -0,0 +1,44 @@ +#!/usr/bin/env bash +# Root install for the soccerbot workspace on the robot machine. +# Wraps local-vla-inference/install.sh (Python 3.12 + CycloneDDS + uv sync). +set -euo pipefail + +REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$REPO_ROOT" + +log() { echo "==> $*"; } + +log "Soccerbot workspace install (delegating CycloneDDS + 3.12 venv to local-vla-inference)" +"$REPO_ROOT/local-vla-inference/install.sh" + +# Headed killswitch needs Tk +if command -v apt-get >/dev/null 2>&1; then + log "Ensuring python3-tk for headed killswitch" + sudo DEBIAN_FRONTEND=noninteractive apt-get install -y python3-tk >/dev/null || \ + echo "warning: could not install python3-tk (killswitch GUI needs it)" >&2 +fi + +# Optional: RealSense python bindings (avoid stage / local depth detector). +if [[ -x "$REPO_ROOT/realsense-human-detection/install.sh" ]]; then + log "Installing realsense-human-detection extras" + "$REPO_ROOT/realsense-human-detection/install.sh" || \ + echo "warning: realsense install failed (teleimager-based avoid still works)" >&2 +fi + +# Ensure soccerbot is an editable install (hatchling) after sync. +VENV_DIR="${VENV_DIR:-$REPO_ROOT/.venv}" +if [[ -x "$VENV_DIR/bin/python" ]]; then + log "Re-syncing workspace so soccerbot entry points are available" + export CYCLONEDDS_HOME="${CYCLONEDDS_HOME:-$HOME/cyclonedds/install}" + export LD_LIBRARY_PATH="${CYCLONEDDS_HOME}/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" + uv sync -p "$VENV_DIR/bin/python" --all-packages +fi + +echo +echo "Done. Next:" +echo " source $REPO_ROOT/.venv/bin/activate" +echo " export CYCLONEDDS_HOME=\${CYCLONEDDS_HOME:-\$HOME/cyclonedds/install}" +echo " export LD_LIBRARY_PATH=\$CYCLONEDDS_HOME/lib:\${LD_LIBRARY_PATH:-}" +echo " ./diagnose.sh --iface enp5s0" +echo " ./killswitch.sh --iface enp5s0 # headed safety panel" +echo " ./run_soccerbot.sh --iface enp5s0" diff --git a/killswitch.sh b/killswitch.sh new file mode 100755 index 0000000..1f6c90f --- /dev/null +++ b/killswitch.sh @@ -0,0 +1,23 @@ +#!/usr/bin/env bash +# Headed G1 killswitch panel (Stop / Damp / ZeroTorque / Start). +set -euo pipefail + +REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +VENV_DIR="${VENV_DIR:-$REPO_ROOT/.venv}" +CYCLONE_PREFIX="${CYCLONEDDS_HOME:-${CYCLONE_PREFIX:-$HOME/cyclonedds/install}}" + +if [[ ! -x "$VENV_DIR/bin/python" ]]; then + echo "error: missing $VENV_DIR — run ./install.sh first" >&2 + exit 1 +fi + +export CYCLONEDDS_HOME="$CYCLONE_PREFIX" +export LD_LIBRARY_PATH="${CYCLONE_PREFIX}/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" +export PATH="$VENV_DIR/bin:$PATH" +export PYTHONPATH="$REPO_ROOT/soccerbot/src${PYTHONPATH:+:$PYTHONPATH}" + +# Prefer console script if installed; else module path. +if command -v soccerbot-killswitch >/dev/null 2>&1; then + exec soccerbot-killswitch "$@" +fi +exec python -m soccerbot.killswitch "$@" diff --git a/local-vla-inference/embodiment_g1_14d.py b/local-vla-inference/embodiment_g1_14d.py index 721d469..2f182c0 100644 --- a/local-vla-inference/embodiment_g1_14d.py +++ b/local-vla-inference/embodiment_g1_14d.py @@ -49,6 +49,10 @@ CAMERA_KEY = "color_0" IMAGE_SHAPE = (720, 1280, 3) # H, W, C — cleaned dataset records 720p +# Default Hub id for 14-D G1 ball-pickup ACT (may be private; pass a local +# --policy path if Hub auth is unavailable). +DEFAULT_POLICY_ID = "ajkoder/g1-pickup-ball-act" + def dataset_features() -> dict: """LeRobot feature dict for ``build_inference_frame`` / ``make_robot_action``.""" diff --git a/local-vla-inference/main.py b/local-vla-inference/main.py index 72e5451..f153849 100644 --- a/local-vla-inference/main.py +++ b/local-vla-inference/main.py @@ -28,7 +28,6 @@ from typing import Any import numpy as np -import torch from front_camera import make_front_camera from g1_arms import G1Arms @@ -121,7 +120,7 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace: metavar="RAD", help="Slew limit: max radians any arm joint may move per control step toward the " "policy target. Default 0.01 (~0.3 rad/s at 30 fps = super slow). " - "Use --clamp 0 to disable.", + "Use --clamp 0 to disable. Soccerbot pickup uses 0.002.", ) p.add_argument( "--log", @@ -130,6 +129,16 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace: help="CSV log of every step (measured, policy target, emitted command per joint). " "Default: act_log_.csv in the current directory.", ) + p.add_argument( + "--rerun", + action="store_true", + help="Spawn a Rerun viewer and stream teleimager RGB + arm target/cmd/measured.", + ) + p.add_argument( + "--no-rerun", + action="store_true", + help="Disable Rerun even if the caller defaulted it on.", + ) p.add_argument( "--dry-run", action="store_true", @@ -141,10 +150,58 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace: help="Read one real camera frame + arm angles (rt/lowstate), run policy, print the " "predicted action chunk; never publish rt/arm_sdk / never command motors.", ) + p.add_argument( + "--leave-arms-engaged", + action="store_true", + help="On clean exit, leave arm_sdk engaged at the last pose so a following " + "scripted stage can take over without a re-engage jerk. Ctrl+C still does a " + "graceful reset (StopMove + release).", + ) return p.parse_args(argv) -def resolve_device(name: str | None) -> torch.device: +def build_args( + *, + layout: str = "14d", + policy: str = "ajkoder/g1-pickup-ball-act", + iface: str | None = None, + camera: str = "zmq://192.168.123.164:55555", + clamp: float = 0.002, + duration: float = 30.0, + fps: float = 30.0, + device: str | None = None, + rerun: bool = True, + leave_arms_engaged: bool = True, + dry_run: bool = False, + image_no_motors: bool = False, + log: str | None = None, + kp: float = 60.0, + kd: float = 1.5, +) -> argparse.Namespace: + """Programmatic Namespace for in-process callers (soccerbot orchestrator).""" + return argparse.Namespace( + layout=layout, + policy=policy, + iface=iface, + camera=camera, + clamp=clamp, + duration=duration, + fps=fps, + device=device, + rerun=rerun, + no_rerun=not rerun, + dry_run=dry_run, + image_no_motors=image_no_motors, + leave_arms_engaged=leave_arms_engaged, + log=log, + kp=kp, + kd=kd, + ) + + +def resolve_device(name: str | None): + import torch + if name: return torch.device(name) if torch.cuda.is_available(): @@ -175,7 +232,7 @@ def action_to_dds(layout: ModuleType, robot_action: dict[str, float]) -> dict[st return {f"{j}.q": float(robot_action[f"{j}.q"]) for j in layout.ARM_JOINTS} -def dry_run(layout: ModuleType, policy, preprocess, postprocess, device: torch.device) -> None: +def dry_run(layout: ModuleType, policy, preprocess, postprocess, device) -> None: from lerobot.policies.utils import build_inference_frame, make_robot_action features = layout.dataset_features() @@ -208,10 +265,11 @@ def print_action_trajectory( postprocess, features: dict, observation: dict[str, Any], - device: torch.device, + device, measured: dict[str, float] | None = None, ) -> None: """Run one ACT chunk prediction and print joint angles in policy units (radians).""" + import torch from lerobot.policies.utils import build_inference_frame, make_robot_action arm_keys = [f"{j}.q" for j in layout.ARM_JOINTS] @@ -246,7 +304,7 @@ def image_no_motors( postprocess, features: dict, pack_obs, - device: torch.device, + device, ) -> None: """Real camera + read-only lowstate → print predicted trajectory. Never write motors.""" from unitree_sdk2py.core.channel import ChannelFactoryInitialize @@ -281,11 +339,37 @@ def image_no_motors( logger.info("image-no-motors done (no motors commanded)") +def _graceful_interrupt(arms: G1Arms, iface: str | None, front) -> None: + """Ctrl+C: stop loco velocity and hand arms back to the balancer.""" + logger.warning("Ctrl+C — graceful reset (StopMove + release arm_sdk)") + try: + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + + loco = LocoClient() + loco.SetTimeout(3.0) + loco.Init() + loco.StopMove() + logger.info("LocoClient.StopMove() sent") + except Exception as exc: # noqa: BLE001 + logger.warning("StopMove during interrupt failed: %s", exc) + try: + arms.release() + except Exception as exc: # noqa: BLE001 + logger.warning("arm release during interrupt failed: %s", exc) + try: + front.disconnect() + except Exception as exc: # noqa: BLE001 + logger.warning("camera disconnect during interrupt failed: %s", exc) + + def run(args: argparse.Namespace) -> None: + import torch from lerobot.policies import make_pre_post_processors from lerobot.policies.act import ACTPolicy from lerobot.policies.utils import build_inference_frame, make_robot_action + from telemetry import Telemetry + if args.dry_run and args.image_no_motors: raise SystemExit("Use either --dry-run or --image-no-motors, not both.") @@ -339,6 +423,10 @@ def run(args: argparse.Namespace) -> None: # Engage arm_sdk smoothly at the current pose before the policy takes over. arms.hold_current_pose(ramp_s=2.0) + rerun_on = bool(getattr(args, "rerun", False)) and not bool(getattr(args, "no_rerun", False)) + telemetry = Telemetry(enabled=rerun_on, session_name="soccerbot-act") + telemetry.start() + h, w, _ = layout.IMAGE_SHAPE dt = 1.0 / args.fps t0 = time.time() @@ -363,14 +451,15 @@ def run(args: argparse.Namespace) -> None: ) logger.info( "ACT loop @ %.1f Hz via rt/arm_sdk, layout=%s, clamp=%.3f rad/step, log=%s " - "(Ctrl+C to FREEZE and stop)", + "(Ctrl+C = graceful reset: StopMove + release arm_sdk)", args.fps, args.layout, args.clamp, log_path, ) - frozen = False + interrupted = False + leave_engaged = bool(getattr(args, "leave_arms_engaged", False)) try: while True: loop_start = time.perf_counter() @@ -411,9 +500,10 @@ def run(args: argparse.Namespace) -> None: gaps = [abs(float(dds_action[k]) - measured[k]) for k in arm_keys] max_gap = max(gaps) loop_ms = (time.perf_counter() - loop_start) * 1000 + elapsed = time.time() - t0 log_writer.writerow( [ - round(time.time() - t0, 4), + round(elapsed, 4), step, round(cam_ms, 1), round(policy_ms, 1), @@ -426,6 +516,24 @@ def run(args: argparse.Namespace) -> None: + [round(snapshot[k], 5) for k in snapshot_keys] ) + telemetry.log_step( + step=step, + elapsed_s=elapsed, + rgb=front_rgb, + measured=measured, + target=dds_action, + commanded=cmd_q, + extras={ + "clamp_hits": float(clamp_hits), + "max_target_gap": float(max_gap), + "cam_ms": float(cam_ms), + "policy_ms": float(policy_ms), + "imu_pitch": float(snapshot.get("imu.pitch", 0.0)), + "imu_roll": float(snapshot.get("imu.roll", 0.0)), + }, + stage="pickup", + ) + step += 1 if step % int(args.fps) == 0: worst = arm_keys[int(np.argmax(gaps))] @@ -436,7 +544,7 @@ def run(args: argparse.Namespace) -> None: "step=%d elapsed=%.1fs | target gap max=%.3f rad (%s) clamped=%d/14 | " "leg max|dq|=%.2f rad/s | cam=%.0fms policy=%.0fms", step, - time.time() - t0, + elapsed, max_gap, worst.removeprefix("k").removesuffix(".q"), clamp_hits, @@ -449,17 +557,29 @@ def run(args: argparse.Namespace) -> None: if sleep > 0: time.sleep(sleep) except KeyboardInterrupt: - # Cut actions and freeze: hold the last commanded pose, keep arm_sdk engaged. - logger.info("Ctrl+C — freezing arms at last commanded pose") - arms.freeze(cmd_q) - frozen = True + interrupted = True + _graceful_interrupt(arms, args.iface, front) finally: log_file.close() logger.info("Step log written to %s (%d steps)", log_path, step) - front.disconnect() - if not frozen: - arms.disconnect() # normal exit: ramp arm_sdk weight back to 0 - logger.info("Done (frozen=%s)", frozen) + telemetry.stop() + if not interrupted: + try: + front.disconnect() + except Exception as exc: # noqa: BLE001 + logger.warning("camera disconnect failed: %s", exc) + if leave_engaged: + # Hold last pose with arm_sdk still on for the next scripted stage. + try: + arms.freeze(cmd_q) + logger.info("Clean exit: arm_sdk left engaged for next stage") + except Exception as exc: # noqa: BLE001 + logger.warning("leave-engaged freeze failed: %s", exc) + else: + arms.disconnect() + logger.info("Done (interrupted=%s leave_engaged=%s)", interrupted, leave_engaged) + if interrupted: + raise def main(argv: list[str] | None = None) -> None: diff --git a/local-vla-inference/pyproject.toml b/local-vla-inference/pyproject.toml index 468d4d4..4ac9901 100644 --- a/local-vla-inference/pyproject.toml +++ b/local-vla-inference/pyproject.toml @@ -6,11 +6,30 @@ readme = "README.md" # cyclonedds 0.10.2 (unitree_sdk2py) needs Python <3.13. Sync the repo root with 3.12. requires-python = ">=3.12" dependencies = [ - "lerobot", + "lerobot[viz]", "pyzmq>=26.0.0", "unitree_sdk2py @ git+https://github.com/unitreerobotics/unitree_sdk2_python.git", ] +[build-system] +requires = ["setuptools>=61"] +build-backend = "setuptools.build_meta" + +[tool.setuptools] +py-modules = [ + "main", + "g1_arms", + "front_camera", + "telemetry", + "embodiment_g1_14d", + "embodiment_g1d_16d", + "diag_health", + "diag_joints", + "diag_state", + "record_arms", + "replay_arms", +] + [tool.uv.sources] lerobot = { workspace = true } diff --git a/local-vla-inference/telemetry.py b/local-vla-inference/telemetry.py new file mode 100644 index 0000000..3514840 --- /dev/null +++ b/local-vla-inference/telemetry.py @@ -0,0 +1,189 @@ +"""Rerun telemetry helpers for live ACT / scripted G1 control. + +Logs teleimager RGB (the existing single-port JPEG stream), optional depth +when a caller already has it (e.g. local RealSense HumanDetector — teleimager +itself publishes color JPEGs only), measured/commanded arm joints, policy +targets, clamp hits, and timing. Safe no-op when Rerun is unavailable or +disabled. +""" + +from __future__ import annotations + +import logging +from typing import Any + +import numpy as np + +logger = logging.getLogger(__name__) + + +class Telemetry: + """Thin wrapper around the Rerun SDK (optional).""" + + def __init__(self, enabled: bool = True, session_name: str = "soccerbot") -> None: + self.enabled = enabled + self.session_name = session_name + self._rr = None + self._step = 0 + + def start(self) -> None: + if not self.enabled: + return + try: + from lerobot.utils.visualization_utils import init_rerun + + init_rerun(session_name=self.session_name) + import rerun as rr + + self._rr = rr + logger.info("Rerun telemetry started (session=%s)", self.session_name) + except Exception as exc: # noqa: BLE001 -- viz is optional on the robot + logger.warning("Rerun unavailable (%s); continuing without visualization", exc) + self.enabled = False + self._rr = None + + def stop(self) -> None: + if not self.enabled or self._rr is None: + return + try: + from lerobot.utils.visualization_utils import shutdown_rerun + + shutdown_rerun() + except Exception as exc: # noqa: BLE001 + logger.debug("Rerun shutdown failed: %s", exc) + self._rr = None + + def set_time(self, step: int | None = None, seconds: float | None = None) -> None: + if self._rr is None: + return + if step is not None: + self._step = int(step) + self._rr.set_time("step", sequence=self._step) + if seconds is not None: + self._rr.set_time("time", timestamp=float(seconds)) + + def log_image(self, path: str, image: np.ndarray, *, compress: bool = True) -> None: + if self._rr is None or image is None: + return + arr = np.asarray(image) + if arr.ndim == 2: + # Depth / mono: log as depth image when uint16, else grayscale. + if arr.dtype == np.uint16: + self._rr.log(path, self._rr.DepthImage(arr)) + else: + self._rr.log(path, self._rr.Image(arr)) + return + if arr.ndim == 3 and arr.shape[-1] == 3: + if compress: + try: + self._rr.log(path, self._rr.Image(arr).compress(jpeg_quality=75)) + return + except Exception: # noqa: BLE001 + pass + self._rr.log(path, self._rr.Image(arr)) + return + logger.debug("Skipping unsupported image shape %s at %s", arr.shape, path) + + def log_scalars(self, path: str, values: dict[str, float]) -> None: + if self._rr is None: + return + for key, value in values.items(): + try: + self._rr.log(f"{path}/{key}", self._rr.Scalars(float(value))) + except Exception: # noqa: BLE001 + continue + + def log_step( + self, + *, + step: int, + elapsed_s: float, + rgb: np.ndarray | None = None, + depth: np.ndarray | None = None, + measured: dict[str, float] | None = None, + target: dict[str, float] | None = None, + commanded: dict[str, float] | None = None, + extras: dict[str, float] | None = None, + stage: str = "pickup", + ) -> None: + if self._rr is None: + return + self.set_time(step=step, seconds=elapsed_s) + if rgb is not None: + self.log_image(f"{stage}/camera/rgb", rgb) + if depth is not None: + self.log_image(f"{stage}/camera/depth", depth, compress=False) + if measured: + self.log_scalars(f"{stage}/arm/measured", _strip_q_suffix(measured)) + if target: + self.log_scalars(f"{stage}/arm/target", _strip_q_suffix(target)) + if commanded: + self.log_scalars(f"{stage}/arm/commanded", _strip_q_suffix(commanded)) + if extras: + self.log_scalars(f"{stage}/stats", extras) + + def log_detection( + self, + *, + step: int, + elapsed_s: float, + rgb: np.ndarray | None, + nearest_m: float | None, + n_people: int, + stage: str = "avoid", + ) -> None: + if self._rr is None: + return + self.set_time(step=step, seconds=elapsed_s) + if rgb is not None: + self.log_image(f"{stage}/camera/rgb", rgb) + extras: dict[str, float] = {"n_people": float(n_people)} + if nearest_m is not None: + extras["nearest_m"] = float(nearest_m) + self.log_scalars(f"{stage}/detect", extras) + + +def _strip_q_suffix(joints: dict[str, float]) -> dict[str, float]: + out: dict[str, float] = {} + for key, value in joints.items(): + name = key.removesuffix(".q") + out[name] = float(value) + return out + + +def apply_slew_clamp( + cmd_q: dict[str, float], + target_q: dict[str, float], + keys: list[str], + clamp_rad: float, +) -> tuple[dict[str, float], int]: + """Move ``cmd_q`` toward ``target_q`` by at most ``clamp_rad`` per joint. + + Returns the updated command dict and the number of joints that hit the clamp. + """ + if clamp_rad <= 0: + updated = {k: float(target_q[k]) for k in keys} + return updated, 0 + + hits = 0 + updated = dict(cmd_q) + for key in keys: + delta = float(target_q[key]) - float(updated[key]) + if abs(delta) > clamp_rad: + hits += 1 + updated[key] = float(updated[key] + float(np.clip(delta, -clamp_rad, clamp_rad))) + return updated, hits + + +def namespace_to_observation( + measured: dict[str, float], + rgb: np.ndarray | None, + depth: np.ndarray | None = None, +) -> dict[str, Any]: + """Build a LeRobot-style observation dict for ``log_rerun_data`` fallbacks.""" + obs: dict[str, Any] = dict(measured) + if rgb is not None: + obs["images.front"] = rgb + if depth is not None: + obs["images.depth"] = depth + return obs diff --git a/run_soccerbot.sh b/run_soccerbot.sh new file mode 100755 index 0000000..d531413 --- /dev/null +++ b/run_soccerbot.sh @@ -0,0 +1,36 @@ +#!/usr/bin/env bash +# Run the soccerbot orchestrator with the robot venv + CycloneDDS env. +set -euo pipefail + +REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +VENV_DIR="${VENV_DIR:-$REPO_ROOT/.venv}" +CYCLONE_PREFIX="${CYCLONEDDS_HOME:-${CYCLONE_PREFIX:-$HOME/cyclonedds/install}}" + +if [[ ! -x "$VENV_DIR/bin/python" ]]; then + echo "error: missing $VENV_DIR — run ./install.sh first" >&2 + exit 1 +fi + +PY_VER="$("$VENV_DIR/bin/python" -c 'import sys; print(f"{sys.version_info.major}.{sys.version_info.minor}")')" +if [[ "$PY_VER" == "3.13" || "$PY_VER" == "3.14" ]]; then + echo "error: root .venv is Python $PY_VER; cyclonedds 0.10.2 needs 3.12" >&2 + echo "fix: ./install.sh" >&2 + exit 1 +fi + +if [[ ! -d "$CYCLONE_PREFIX" ]]; then + echo "error: CycloneDDS not found at $CYCLONE_PREFIX — run ./install.sh" >&2 + exit 1 +fi + +export CYCLONEDDS_HOME="$CYCLONE_PREFIX" +export LD_LIBRARY_PATH="${CYCLONE_PREFIX}/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}" +export PATH="$VENV_DIR/bin:$PATH" +export PYTHONPATH="$REPO_ROOT/soccerbot/src${PYTHONPATH:+:$PYTHONPATH}" + +echo "tip: keep ./killswitch.sh running in another terminal" >&2 + +if command -v soccerbot >/dev/null 2>&1; then + exec soccerbot "$@" +fi +exec python -m soccerbot "$@" diff --git a/scripted-behavior/main.py b/scripted-behavior/main.py index f3820cd..7c0112e 100644 --- a/scripted-behavior/main.py +++ b/scripted-behavior/main.py @@ -1,114 +1,25 @@ -"""High-level orchestrator for the G1 soccer ball pickup demo. +"""Thin wrapper — the core orchestrator now lives in ``soccerbot``. -Pipeline (each stage lives in its own module; any raise aborts the demo): +Prefer: - 1. PICKUP -- ``pickup.run_pickup_policy`` (subprocess to VLA, or replay) - 2. TURN_180 -- ``turn_180.turn_180_degrees`` (LocoClient yaw + arm hold) - 3. AVOID -- ``avoid.avoid_humans`` (shuffle + realsense) - 4. THROW -- ``throw.throw_ball`` (hardcoded replay; TODO) + python -m soccerbot --iface enp5s0 + ./run_soccerbot.sh --iface enp5s0 + +This module keeps the old ``scripted-behavior/main.py`` entry point working +by forwarding to ``soccerbot.main``. """ from __future__ import annotations -import argparse -import logging import sys -import time - -from avoid import avoid_humans -from config import OrchestratorConfig, PickupBackend -from pickup import run_pickup_policy -from throw import throw_ball -from turn_180 import turn_180_degrees - -logger = logging.getLogger("scripted_behavior.orchestrator") - - -def run_demo(cfg: OrchestratorConfig) -> None: - logger.info("=== Stage 1/4: PICKUP (%s) ===", cfg.backend.value) - run_pickup_policy(cfg) - - logger.info("=== Stage 2/4: TURN 180 ===") - turn_180_degrees(cfg) - - logger.info("=== Stage 3/4: AVOID (shuffle until clear) ===") - avoid_humans(cfg) - - logger.info("=== Stage 4/4: THROW (hardcoded) ===") - throw_ball(cfg) - - logger.info("Demo complete") - - -def parse_args(argv: list[str] | None = None) -> OrchestratorConfig: - p = argparse.ArgumentParser( - description="G1 soccer-ball pickup demo orchestrator " - "(pickup -> turn 180 -> shuffle-avoid -> throw)." - ) - p.add_argument( - "--backend", - type=PickupBackend, - choices=list(PickupBackend), - default=PickupBackend.LOCAL, - help="Which pickup to run: local (ACT), remote (pi0.5), or replay.", - ) - p.add_argument( - "--iface", - default=None, - help="Network interface to the robot (passed to local-vla-inference).", - ) - p.add_argument( - "--pickup-duration", - type=float, - default=30.0, - help="Seconds to run the pickup policy before advancing to stage 2.", - ) - p.add_argument( - "--remote-server", - default=None, - help="HOST:PORT of the remote pi0.5 policy server (backend=remote).", - ) - p.add_argument( - "pickup_extra", - nargs=argparse.REMAINDER, - help="Extra args forwarded to the pickup launcher after '--'.", - ) - args = p.parse_args(argv) - - extra = args.pickup_extra or [] - if extra and extra[0] == "--": - extra = extra[1:] - - return OrchestratorConfig( - backend=args.backend, - iface=args.iface, - pickup_duration_s=args.pickup_duration, - pickup_extra_args=extra, - remote_server=args.remote_server, - ) - +from pathlib import Path -def main(argv: list[str] | None = None) -> int: - logging.basicConfig( - level=logging.INFO, - format="%(asctime)s %(levelname)s %(name)s: %(message)s", - ) - cfg = parse_args(argv) - t0 = time.time() - try: - run_demo(cfg) - except KeyboardInterrupt: - logger.warning("Interrupted by user after %.1fs", time.time() - t0) - return 130 - except NotImplementedError as exc: - logger.error("Blocked on unimplemented stage: %s", exc) - return 2 - except Exception: # noqa: BLE001 -- top-level guard for a live demo - logger.exception("Demo failed after %.1fs", time.time() - t0) - return 1 - logger.info("Demo finished in %.1fs", time.time() - t0) - return 0 +_REPO_ROOT = Path(__file__).resolve().parent.parent +_SOCCERBOT_SRC = _REPO_ROOT / "soccerbot" / "src" +if str(_SOCCERBOT_SRC) not in sys.path: + sys.path.insert(0, str(_SOCCERBOT_SRC)) +from soccerbot.main import main # noqa: E402 if __name__ == "__main__": sys.exit(main()) diff --git a/scripted-behavior/pickup.py b/scripted-behavior/pickup.py index 383be93..13dbd1d 100644 --- a/scripted-behavior/pickup.py +++ b/scripted-behavior/pickup.py @@ -1,27 +1,26 @@ -"""Stage 1: run the learned VLA pickup policy in a subprocess. +"""Stage 1: run the learned ACT pickup policy in-process (or replay a trajectory). -Delegates to ``local-vla-inference/run.sh`` or -``remote-vla-inference/run_client.sh`` so we don't drag torch / lerobot -into the orchestrator process. Ctrl+C in the orchestrator propagates -to the child. +Delegates to ``local-vla-inference`` via import (no subprocess) so slew +clamping, Ctrl+C graceful reset, and Rerun telemetry stay in one process. -A third backend, ``replay``, streams the pre-recorded arm-qpos -trajectory at ``trajectories/pickup_ep148_prod2.json`` directly over -``rt/arm_sdk`` -- no learned policy, no camera, no torch. +A third backend, ``replay``, streams a pre-recorded arm-qpos trajectory from +``trajectories/`` over ``rt/arm_sdk``. """ from __future__ import annotations import logging -import subprocess +import sys from config import REPO_ROOT, OrchestratorConfig, PickupBackend logger = logging.getLogger("scripted_behavior.pickup") -LOCAL_VLA_RUN = REPO_ROOT / "local-vla-inference" / "run.sh" -REMOTE_VLA_RUN = REPO_ROOT / "remote-vla-inference" / "run_client.sh" +LOCAL_VLA_DIR = REPO_ROOT / "local-vla-inference" REPLAY_TRAJECTORY = REPO_ROOT / "scripted-behavior" / "trajectories" / "pickup_ep148_prod2.json" +DEFAULT_POLICY = "ajkoder/g1-pickup-ball-act" +DEFAULT_CLAMP = 0.002 +DEFAULT_CAMERA = "zmq://192.168.123.164:55555" def run_pickup_policy(cfg: OrchestratorConfig) -> None: @@ -35,43 +34,69 @@ def run_pickup_policy(cfg: OrchestratorConfig) -> None: logger.info("Pickup replay finished") return - if cfg.backend is PickupBackend.LOCAL: - script = LOCAL_VLA_RUN - cmd = [str(script), f"--duration={cfg.pickup_duration_s}"] - if cfg.iface: - cmd.append(f"--iface={cfg.iface}") - elif cfg.backend is PickupBackend.REMOTE: - script = REMOTE_VLA_RUN - if not cfg.remote_server: - raise ValueError( - "remote backend requires --remote-server HOST:PORT " - "(policy server started via remote-vla-inference/run_server.sh)" - ) - cmd = [str(script), f"--server_address={cfg.remote_server}"] - # remote client currently reads iface via env var, not CLI. - else: # pragma: no cover -- exhaustive - raise AssertionError(f"unknown backend: {cfg.backend}") - - cmd.extend(cfg.pickup_extra_args) - - if not script.exists(): - raise FileNotFoundError(f"pickup launcher missing: {script}") - - logger.info("Starting pickup policy: %s", " ".join(cmd)) - result = subprocess.run(cmd, check=False) - if result.returncode != 0: - raise RuntimeError( - f"pickup policy exited non-zero ({result.returncode}); aborting demo" + if cfg.backend is PickupBackend.REMOTE: + raise NotImplementedError( + "remote backend is not imported in-process yet; use soccerbot --backend local|replay " + "or the remote-vla-inference client directly" ) - logger.info("Pickup policy finished cleanly") + if cfg.backend is not PickupBackend.LOCAL: + raise AssertionError(f"unknown backend: {cfg.backend}") -# --------------------------------------------------------------------------- -# Standalone entry point: run just Stage 1. -# python3 pickup.py --backend local --iface eth0 --pickup-duration 15 -# python3 pickup.py --backend remote --remote-server modal.host:50051 -# python3 pickup.py --backend replay --iface eth0 -# --------------------------------------------------------------------------- + if str(LOCAL_VLA_DIR) not in sys.path: + sys.path.insert(0, str(LOCAL_VLA_DIR)) + import main as local_vla # type: ignore[import-not-found] + + # Optional extras: --policy / --clamp / --camera forwarded after '--'. + policy = DEFAULT_POLICY + clamp = DEFAULT_CLAMP + camera = DEFAULT_CAMERA + layout = "14d" + extra = list(cfg.pickup_extra_args) + # Tiny argv parse for common flags without another ArgumentParser. + i = 0 + while i < len(extra): + tok = extra[i] + if tok.startswith("--policy="): + policy = tok.split("=", 1)[1] + elif tok == "--policy" and i + 1 < len(extra): + i += 1 + policy = extra[i] + elif tok.startswith("--clamp="): + clamp = float(tok.split("=", 1)[1]) + elif tok == "--clamp" and i + 1 < len(extra): + i += 1 + clamp = float(extra[i]) + elif tok.startswith("--camera="): + camera = tok.split("=", 1)[1] + elif tok == "--camera" and i + 1 < len(extra): + i += 1 + camera = extra[i] + elif tok.startswith("--layout="): + layout = tok.split("=", 1)[1] + elif tok == "--layout" and i + 1 < len(extra): + i += 1 + layout = extra[i] + i += 1 + + args = local_vla.build_args( + layout=layout, + policy=policy, + iface=cfg.iface, + camera=camera, + clamp=clamp, + duration=cfg.pickup_duration_s, + leave_arms_engaged=True, + rerun=True, + ) + logger.info( + "Starting in-process ACT pickup: policy=%s clamp=%.3f camera=%s", + policy, + clamp, + camera, + ) + local_vla.run(args) + logger.info("Pickup policy finished cleanly") def _cli() -> int: @@ -115,5 +140,4 @@ def _cli() -> int: if __name__ == "__main__": - import sys sys.exit(_cli()) diff --git a/scripted-behavior/pyproject.toml b/scripted-behavior/pyproject.toml index 7866c24..5de49af 100644 --- a/scripted-behavior/pyproject.toml +++ b/scripted-behavior/pyproject.toml @@ -5,11 +5,33 @@ description = "Hardcoded (non-learned) post-pickup state machine: turn, human-de readme = "README.md" requires-python = ">=3.12" # unitree_sdk2py / cyclonedds 0.10.2 need Python 3.12 — install on the robot via -# local-vla-inference/install.sh (or a dedicated 3.12 venv), not the root 3.13 sync. -# throw.py / g1_arm_fk.py have zero third-party dependencies (stdlib only) so -# --dry-run and the test suite run anywhere; only --execute needs unitree_sdk2py. +# ./install.sh (or local-vla-inference/install.sh), not a 3.13-only sync. +# Logic imports sibling local-vla-inference via sys.path (see arm_replay / throw); +# keep this package free of a hard install dep so throw dry-run stays lightweight. dependencies = [] +[build-system] +requires = ["setuptools>=61"] +build-backend = "setuptools.build_meta" + +[tool.setuptools] +py-modules = [ + "main", + "config", + "dds", + "pickup", + "turn_180", + "sidestep", + "avoid", + "throw", + "arm_replay", + "g1_arm_fk", + "human_detector_teleimager", + "diag_loco", + "diag_log", + "test_throw", +] + [tool.pytest.ini_options] testpaths = ["."] python_files = ["test_*.py"] diff --git a/scripted-behavior/throw.py b/scripted-behavior/throw.py index e6f7a43..4680080 100644 --- a/scripted-behavior/throw.py +++ b/scripted-behavior/throw.py @@ -69,6 +69,7 @@ import time from pathlib import Path +from dds import ensure_dds from g1_arm_fk import LEFT_ARM, RIGHT_ARM, check_limits, left_elbow_position, left_hand_position logger = logging.getLogger(__name__) @@ -179,26 +180,44 @@ def _push_target(current_pose: dict[str, float]) -> dict[str, float]: } +# Default per-tick slew limit while interpolating the throw (rad/step @ 50 Hz). +THROW_SLEW_CLAMP = 0.02 + + def _interpolate_to( arms, start: dict[str, float], target: dict[str, float], duration_s: float, control_dt: float = 0.02, + slew_clamp: float = THROW_SLEW_CLAMP, ) -> None: - """Linearly interpolate the 14 arm joints from ``start`` to ``target``.""" + """Linearly interpolate the 14 arm joints from ``start`` to ``target``. + + Each tick is also slew-rate limited so a short ``duration_s`` cannot command + an unsafe jump even if the start/target gap is large. + """ num_steps = max(1, int(duration_s / control_dt)) + cmd = dict(start) for step in range(1, num_steps + 1): step_start = time.time() alpha = step / num_steps - action = {key: start[key] * (1 - alpha) + target[key] * alpha for key in ARM_JOINT_KEYS} - arms.send_arm_positions(action) + desired = {key: start[key] * (1 - alpha) + target[key] * alpha for key in ARM_JOINT_KEYS} + if slew_clamp and slew_clamp > 0: + for key in ARM_JOINT_KEYS: + delta = desired[key] - cmd[key] + if abs(delta) > slew_clamp: + delta = slew_clamp if delta > 0 else -slew_clamp + cmd[key] = cmd[key] + delta + else: + cmd = desired + arms.send_arm_positions(cmd) elapsed = time.time() - step_start sleep_s = max(0.0, control_dt - elapsed) time.sleep(sleep_s) -def throw(arms) -> None: +def throw(arms, *, slew_clamp: float = THROW_SLEW_CLAMP) -> None: """Run the gentle push against a connected, already-engaged ``G1Arms``. Reads the CURRENT arm position and treats it as the ball-holding pose -- @@ -207,7 +226,10 @@ def throw(arms) -> None: is already engaged (e.g. via ``arms.hold_current_pose()``); does not release it afterward -- the caller decides what happens next. """ - logger.info("Starting gentle goalkeeper push from current arm position") + logger.info( + "Starting gentle goalkeeper push from current arm position (slew=%.3f rad/step)", + slew_clamp, + ) start_pose = arms.get_arm_positions() release_pose = _push_target(start_pose) follow_through_pose = dict(release_pose) @@ -217,14 +239,39 @@ def throw(arms) -> None: follow_through_pose[key] = max(lo, min(hi, release_pose[key] + _FOLLOW_THROUGH_EXTRA_WRIST_PITCH)) logger.info("-> release (%.2fs)", RELEASE_DURATION_S) - _interpolate_to(arms, start_pose, release_pose, RELEASE_DURATION_S) + _interpolate_to(arms, start_pose, release_pose, RELEASE_DURATION_S, slew_clamp=slew_clamp) logger.info("-> follow_through (%.2fs)", FOLLOW_THROUGH_DURATION_S) - _interpolate_to(arms, release_pose, follow_through_pose, FOLLOW_THROUGH_DURATION_S) + _interpolate_to( + arms, release_pose, follow_through_pose, FOLLOW_THROUGH_DURATION_S, slew_clamp=slew_clamp + ) logger.info("-> recover (%.2fs)", RECOVER_DURATION_S) - _interpolate_to(arms, follow_through_pose, start_pose, RECOVER_DURATION_S) + _interpolate_to(arms, follow_through_pose, start_pose, RECOVER_DURATION_S, slew_clamp=slew_clamp) logger.info("Push complete.") +def throw_ball(cfg, *, slew_clamp: float = THROW_SLEW_CLAMP) -> None: + """Orchestrator entry point: connect arms (or reuse engaged overlay) and push. + + Expects DDS already initialized by a prior stage (pickup / turn / avoid). + Uses a no-ramp ``weight=1`` publish so we don't jerk if arm_sdk is already on. + """ + ensure_dds(getattr(cfg, "iface", None)) + # g1_arms lives in the sibling local-vla-inference package (flat layout). + if str(_LOCAL_VLA_INFERENCE_DIR) not in sys.path: + sys.path.insert(0, str(_LOCAL_VLA_INFERENCE_DIR)) + from g1_arms import G1Arms # noqa: E402 -- path bootstrap for sibling package + + arms = G1Arms(kp=60.0, kd=1.5) + arms.connect() + hold = dict(arms.get_arm_positions()) + arms.send_arm_positions(hold, weight=1.0) + try: + throw(arms, slew_clamp=slew_clamp) + finally: + # Leave engaged — caller / orchestrator owns teardown + Ctrl+C reset. + pass + + def _mirror_for_check(active: dict[str, float]) -> dict[str, float]: """Expand a single-arm {fk_joint_name: value} dict into a full 14-joint ".q"-keyed pose, mirroring roll/yaw for the right arm.""" diff --git a/sim/pyproject.toml b/sim/pyproject.toml index 95ee399..b794cbd 100644 --- a/sim/pyproject.toml +++ b/sim/pyproject.toml @@ -6,5 +6,12 @@ requires-python = ">=3.12" dependencies = [] [build-system] -requires = ["hatchling"] -build-backend = "hatchling.build" +requires = ["setuptools>=61"] +build-backend = "setuptools.build_meta" + +# Project lives in sim/ with modules at the package root; expose as import `sim`. +[tool.setuptools] +packages = ["sim"] + +[tool.setuptools.package-dir] +sim = "." diff --git a/soccerbot/README.md b/soccerbot/README.md index e69de29..e4e21ec 100644 --- a/soccerbot/README.md +++ b/soccerbot/README.md @@ -0,0 +1,46 @@ +# soccerbot + +Core orchestrator for the Unitree G1 soccer-ball pickup demo. + +This package is **workspace-local** (not published to PyPI). The logic pieces live +in sibling packages that soccerbot imports in-process: + +| Dependency | Role | +|---|---| +| `local-vla-inference/` | ACT pickup (`ajkoder/g1-pickup-ball-act`), slew clamp, teleimager RGB, Rerun | +| `scripted-behavior/` | Turn 180°, avoid/shuffle, throw, JSON trajectory replay | + +## Run + +```bash +# one-time robot setup (Python 3.12 + CycloneDDS) +./install.sh + +# headed killswitch in a second terminal (keep open during demos) +./killswitch.sh --iface enp5s0 + +# full demo: ACT pickup → turn → avoid → throw +./run_soccerbot.sh --iface enp5s0 + +# safer smoke: replay recorded pickup trajectory instead of ACT +./run_soccerbot.sh --iface enp5s0 --backend replay +``` + +Defaults match the validated local ACT command: + +- `--layout 14d` +- `--policy ajkoder/g1-pickup-ball-act` +- `--clamp 0.002` +- `--camera zmq://192.168.123.164:55555` (working teleimager head JPEG port) + +## Safety + +- Every arm command path is slew-clamped (ACT + replay + throw). +- **Ctrl+C** → graceful reset: `LocoClient.StopMove()` + release `arm_sdk`. +- **`./killswitch.sh`** → headed GUI: Stop Move / Damp / Zero Torque / Start. + +## Diagnose + +```bash +./diagnose.sh --iface enp5s0 +``` diff --git a/soccerbot/pyproject.toml b/soccerbot/pyproject.toml index d8d9dfd..b100e16 100644 --- a/soccerbot/pyproject.toml +++ b/soccerbot/pyproject.toml @@ -1,12 +1,27 @@ [project] name = "soccerbot" version = "0.1.0" -description = "Add your description here" +description = "Core G1 soccer-ball pickup orchestrator (workspace-local; not published to PyPI)" readme = "README.md" requires-python = ">=3.12" dependencies = [ - "realsense-human-detection", "local-vla-inference", - "remote-vla-inference", "scripted-behavior", + "lerobot[viz]", ] + +[project.scripts] +soccerbot = "soccerbot.main:main" +soccerbot-killswitch = "soccerbot.killswitch:main" + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[tool.hatch.build.targets.wheel] +packages = ["src/soccerbot"] + +[tool.uv.sources] +local-vla-inference = { workspace = true } +scripted-behavior = { workspace = true } +lerobot = { workspace = true } diff --git a/soccerbot/src/soccerbot/__init__.py b/soccerbot/src/soccerbot/__init__.py new file mode 100644 index 0000000..a1da3d0 --- /dev/null +++ b/soccerbot/src/soccerbot/__init__.py @@ -0,0 +1,3 @@ +"""Soccerbot — core G1 soccer-ball pickup orchestrator.""" + +__version__ = "0.1.0" diff --git a/soccerbot/src/soccerbot/__main__.py b/soccerbot/src/soccerbot/__main__.py new file mode 100644 index 0000000..083fac0 --- /dev/null +++ b/soccerbot/src/soccerbot/__main__.py @@ -0,0 +1,3 @@ +from soccerbot.main import main + +raise SystemExit(main()) diff --git a/soccerbot/src/soccerbot/config.py b/soccerbot/src/soccerbot/config.py new file mode 100644 index 0000000..6805e69 --- /dev/null +++ b/soccerbot/src/soccerbot/config.py @@ -0,0 +1,44 @@ +"""Orchestrator config for the soccerbot demo.""" + +from __future__ import annotations + +import enum +from dataclasses import dataclass, field +from pathlib import Path + +from soccerbot.deps import REPO_ROOT + +# Working teleimager head stream (single ZMQ JPEG port — see local-vla-inference). +DEFAULT_CAMERA = "zmq://192.168.123.164:55555" +DEFAULT_POLICY = "ajkoder/g1-pickup-ball-act" +DEFAULT_CLAMP_RAD = 0.002 +DEFAULT_REPLAY_TRAJECTORY = ( + REPO_ROOT / "scripted-behavior" / "trajectories" / "pickup_ep148_prod2.json" +) + + +class PickupBackend(str, enum.Enum): + LOCAL = "local" # in-process ACT via local-vla-inference + REPLAY = "replay" # JSON arm trajectory in scripted-behavior/trajectories/ + REMOTE = "remote" # optional remote pi0.5 server + + +@dataclass +class OrchestratorConfig: + backend: PickupBackend = PickupBackend.LOCAL + iface: str | None = None + camera: str = DEFAULT_CAMERA + policy: str = DEFAULT_POLICY + layout: str = "14d" + clamp: float = DEFAULT_CLAMP_RAD + pickup_duration_s: float = 30.0 + fps: float = 30.0 + device: str | None = None + rerun: bool = True + teleimager_host: str = "192.168.123.164" + remote_server: str | None = None + replay_trajectory: Path = DEFAULT_REPLAY_TRAJECTORY + # Scripted-stage slew clamps (rad/frame). + replay_slew_clamp: float = 0.05 + throw_slew_clamp: float = 0.02 + pickup_extra_args: list[str] = field(default_factory=list) diff --git a/soccerbot/src/soccerbot/deps.py b/soccerbot/src/soccerbot/deps.py new file mode 100644 index 0000000..87d5ca1 --- /dev/null +++ b/soccerbot/src/soccerbot/deps.py @@ -0,0 +1,46 @@ +"""Import helpers for workspace logic packages (not published to PyPI). + +``local-vla-inference`` and ``scripted-behavior`` are flat virtual workspace +members. Soccerbot is the core orchestrator and loads them by putting their +directories on ``sys.path`` so we can ``import main`` / ``import arm_replay`` +in-process — no subprocess. +""" + +from __future__ import annotations + +import sys +from pathlib import Path + +def _discover_repo_root() -> Path: + """Find the workspace root that contains the logic packages.""" + here = Path(__file__).resolve() + candidates = [ + here.parents[3], # soccerbot/src/soccerbot/deps.py → repo + here.parents[2], + Path.cwd(), + *here.parents, + ] + for candidate in candidates: + if (candidate / "local-vla-inference").is_dir() and ( + candidate / "scripted-behavior" + ).is_dir(): + return candidate + return here.parents[3] + + +REPO_ROOT = _discover_repo_root() + +_LOGIC_DIRS = ( + REPO_ROOT / "local-vla-inference", + REPO_ROOT / "scripted-behavior", + REPO_ROOT / "realsense-human-detection", +) + + +def ensure_logic_imports() -> Path: + """Prepend logic-package dirs to ``sys.path``. Idempotent.""" + for path in _LOGIC_DIRS: + text = str(path) + if path.is_dir() and text not in sys.path: + sys.path.insert(0, text) + return REPO_ROOT diff --git a/soccerbot/src/soccerbot/killswitch.py b/soccerbot/src/soccerbot/killswitch.py new file mode 100644 index 0000000..1add474 --- /dev/null +++ b/soccerbot/src/soccerbot/killswitch.py @@ -0,0 +1,240 @@ +"""Headed killswitch panel for the Unitree G1. + +Big on-screen buttons (tkinter) that map to the same safety actions as the +physical pendant: + + STOP / STAND — StopMove + (optional) leave balancer standing + DAMP — LocoClient.Damp() (pendant L2+B) + ZERO TORQUE — LocoClient.ZeroTorque() (pendant L2+A) + START — LocoClient.Start() (re-engage balancer) + +Run separately from the demo so it stays usable even if the policy process +hangs: + + ./killswitch.sh --iface enp5s0 + python -m soccerbot.killswitch --iface enp5s0 +""" + +from __future__ import annotations + +import argparse +import logging +import sys +import threading +import time + +from soccerbot.safety import balance_stand, enter_damp, enter_zero_torque, stop_loco + +logger = logging.getLogger("soccerbot.killswitch") + +FSM_NAMES = { + 0: "zero torque", + 1: "damp", + 2: "squat", + 3: "sit", + 4: "stand (locked)", + 200: "start / balance stand", + 500: "advanced (main operation)", +} + + +def _require_tkinter(): + try: + import tkinter as tk + from tkinter import messagebox, ttk + except ModuleNotFoundError as exc: + raise SystemExit( + "tkinter is required for the headed killswitch.\n" + "Install: sudo apt-get install -y python3-tk\n" + f"Original error: {exc}" + ) from exc + return tk, messagebox, ttk + + +class KillswitchApp: + def __init__(self, iface: str | None) -> None: + tk, messagebox, ttk = _require_tkinter() + self._tk = tk + self._messagebox = messagebox + self.iface = iface + self._loco = None + self._lock = threading.Lock() + + self.root = tk.Tk() + self.root.title("G1 KILLSWITCH") + self.root.configure(bg="#1a1a1a") + self.root.geometry("520x560") + self.root.minsize(480, 520) + + title = tk.Label( + self.root, + text="G1 KILLSWITCH", + font=("Helvetica", 28, "bold"), + fg="#ff4444", + bg="#1a1a1a", + ) + title.pack(pady=(16, 4)) + + iface_text = iface or "(default DDS iface)" + tk.Label( + self.root, + text=f"iface: {iface_text}", + font=("Helvetica", 11), + fg="#aaaaaa", + bg="#1a1a1a", + ).pack() + + self.status = tk.Label( + self.root, + text="Connecting…", + font=("Helvetica", 13), + fg="#eeeeee", + bg="#1a1a1a", + wraplength=480, + justify="center", + ) + self.status.pack(pady=12) + + btn_frame = tk.Frame(self.root, bg="#1a1a1a") + btn_frame.pack(fill="both", expand=True, padx=24, pady=8) + + self._mk_button(btn_frame, "STOP MOVE\n(stay standing)", "#cc8800", self._on_stop).pack( + fill="x", pady=6 + ) + self._mk_button(btn_frame, "DAMP\n(L2+B)", "#dd6622", self._on_damp).pack(fill="x", pady=6) + self._mk_button(btn_frame, "ZERO TORQUE\n(L2+A — limp)", "#cc2222", self._on_zero).pack( + fill="x", pady=6 + ) + self._mk_button(btn_frame, "START / STAND\n(re-engage balancer)", "#227744", self._on_start).pack( + fill="x", pady=6 + ) + + ttk.Separator(self.root).pack(fill="x", padx=24, pady=8) + tk.Label( + self.root, + text="Keep this window open during demos.\nPhysical pendant still works in parallel.", + font=("Helvetica", 10), + fg="#888888", + bg="#1a1a1a", + justify="center", + ).pack(pady=(0, 12)) + + self.root.after(100, self._connect_async) + self.root.after(1000, self._poll_fsm) + + def _mk_button(self, parent, text: str, color: str, command): + return self._tk.Button( + parent, + text=text, + font=("Helvetica", 16, "bold"), + fg="#ffffff", + bg=color, + activebackground=color, + activeforeground="#ffffff", + relief="raised", + bd=4, + height=2, + command=command, + ) + + def _connect_async(self) -> None: + def worker() -> None: + try: + from soccerbot.safety import init_loco + + loco = init_loco(self.iface) + with self._lock: + self._loco = loco + self._set_status("DDS connected — killswitch armed") + except Exception as exc: # noqa: BLE001 + logger.exception("killswitch connect failed") + self._set_status(f"CONNECT FAILED: {exc}") + + threading.Thread(target=worker, daemon=True).start() + + def _set_status(self, text: str) -> None: + def apply() -> None: + self.status.config(text=text) + + self.root.after(0, apply) + + def _with_loco(self, fn, label: str) -> None: + def worker() -> None: + with self._lock: + loco = self._loco + try: + fn(loco) + self._set_status(f"{label} OK @ {time.strftime('%H:%M:%S')}") + except Exception as exc: # noqa: BLE001 + logger.exception("%s failed", label) + self._set_status(f"{label} FAILED: {exc}") + self.root.after( + 0, lambda: self._messagebox.showerror("Killswitch", f"{label} failed:\n{exc}") + ) + + threading.Thread(target=worker, daemon=True).start() + + def _on_stop(self) -> None: + self._with_loco(lambda loco: stop_loco(loco, iface=self.iface), "STOP MOVE") + + def _on_damp(self) -> None: + if not self._messagebox.askokcancel("DAMP", "Enter Damp mode? Robot will go passive."): + return + self._with_loco(lambda loco: enter_damp(iface=self.iface, loco=loco), "DAMP") + + def _on_zero(self) -> None: + if not self._messagebox.askokcancel( + "ZERO TORQUE", + "Enter ZeroTorque? Motors go limp — spotter must be ready.", + ): + return + self._with_loco(lambda loco: enter_zero_torque(iface=self.iface, loco=loco), "ZERO TORQUE") + + def _on_start(self) -> None: + self._with_loco(lambda loco: balance_stand(iface=self.iface, loco=loco), "START") + + def _poll_fsm(self) -> None: + def worker() -> None: + with self._lock: + loco = self._loco + if loco is None: + return + try: + code, fsm_id = loco.GetFsmId() + name = FSM_NAMES.get(int(fsm_id), f"fsm {fsm_id}") + self._set_status(f"FSM={fsm_id} ({name}) rpc={code}") + except Exception: # noqa: BLE001 + pass + + threading.Thread(target=worker, daemon=True).start() + self.root.after(1000, self._poll_fsm) + + def run(self) -> None: + self.root.mainloop() + + +def parse_args(argv: list[str] | None = None) -> argparse.Namespace: + p = argparse.ArgumentParser(description="Headed G1 killswitch panel.") + p.add_argument( + "--iface", + default=None, + help="DDS network interface (e.g. enp5s0). Omit for SDK default.", + ) + return p.parse_args(argv) + + +def main(argv: list[str] | None = None) -> int: + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s %(levelname)s %(name)s: %(message)s", + ) + args = parse_args(argv) + try: + KillswitchApp(args.iface).run() + except KeyboardInterrupt: + return 130 + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/soccerbot/src/soccerbot/main.py b/soccerbot/src/soccerbot/main.py index e69de29..346b67e 100644 --- a/soccerbot/src/soccerbot/main.py +++ b/soccerbot/src/soccerbot/main.py @@ -0,0 +1,191 @@ +"""Soccerbot core orchestrator. + +Depends on workspace logic packages (not published to PyPI): + + - ``local-vla-inference`` — ACT pickup (imported in-process, never subprocess) + - ``scripted-behavior`` — turn / avoid / throw + trajectory replay + +Pipeline: + + 1. PICKUP — ACT (``ajkoder/g1-pickup-ball-act``, clamp 0.002) or JSON replay + 2. TURN_180 — LocoClient yaw while holding arms + 3. AVOID — teleimager YOLO + sidestep shuffle + 4. THROW — relative push (slew-clamped) + +Ctrl+C runs a graceful reset (StopMove + release arm_sdk). Keep +``./killswitch.sh`` open in another terminal for headed Damp / ZeroTorque. +""" + +from __future__ import annotations + +import argparse +import logging +import sys +import time + +from soccerbot.config import ( + DEFAULT_CAMERA, + DEFAULT_CLAMP_RAD, + DEFAULT_POLICY, + OrchestratorConfig, + PickupBackend, +) +from soccerbot.deps import ensure_logic_imports +from soccerbot.pickup import run_pickup +from soccerbot.safety import graceful_reset + +logger = logging.getLogger("soccerbot.orchestrator") + + +def run_demo(cfg: OrchestratorConfig) -> None: + ensure_logic_imports() + + # Scripted stage modules (flat imports from scripted-behavior/). + from avoid import avoid_humans + from config import OrchestratorConfig as ScriptedConfig + from config import PickupBackend as ScriptedBackend + from throw import throw_ball + from turn_180 import turn_180_degrees + + # Bridge soccerbot config → scripted-behavior config (shared field names). + scripted = ScriptedConfig( + backend=ScriptedBackend(cfg.backend.value), + iface=cfg.iface, + pickup_duration_s=cfg.pickup_duration_s, + teleimager_host=cfg.teleimager_host, + remote_server=cfg.remote_server, + ) + + logger.info("=== Stage 1/4: PICKUP (%s) ===", cfg.backend.value) + run_pickup(cfg) + + logger.info("=== Stage 2/4: TURN 180 ===") + turn_180_degrees(scripted) + + logger.info("=== Stage 3/4: AVOID (shuffle until clear) ===") + avoid_humans(scripted) + + logger.info("=== Stage 4/4: THROW ===") + throw_ball(scripted, slew_clamp=cfg.throw_slew_clamp) + + logger.info("Demo complete") + + +def parse_args(argv: list[str] | None = None) -> OrchestratorConfig: + p = argparse.ArgumentParser( + description="Soccerbot orchestrator: ACT pickup → turn → avoid → throw.", + ) + p.add_argument( + "--backend", + type=PickupBackend, + choices=list(PickupBackend), + default=PickupBackend.LOCAL, + help="Pickup source: local ACT (default), replay trajectory, or remote.", + ) + p.add_argument("--iface", default=None, help="DDS NIC (e.g. enp5s0).") + p.add_argument( + "--camera", + default=DEFAULT_CAMERA, + help="Teleimager / camera spec (default: working zmq://192.168.123.164:55555).", + ) + p.add_argument( + "--policy", + default=DEFAULT_POLICY, + help="ACT Hub id or local pretrained_model dir.", + ) + p.add_argument("--layout", choices=("14d", "16d"), default="14d") + p.add_argument( + "--clamp", + type=float, + default=DEFAULT_CLAMP_RAD, + help="ACT slew clamp rad/step (default 0.002).", + ) + p.add_argument("--pickup-duration", type=float, default=30.0) + p.add_argument("--fps", type=float, default=30.0) + p.add_argument("--device", default=None) + p.add_argument("--teleimager-host", default="192.168.123.164") + p.add_argument("--remote-server", default=None) + p.add_argument( + "--replay-trajectory", + default=None, + help="JSON path for --backend replay (default: pickup_ep148_prod2.json).", + ) + p.add_argument("--no-rerun", action="store_true", help="Disable Rerun visualization.") + p.add_argument( + "--dry-run-config", + action="store_true", + help="Print resolved config and exit (no robot).", + ) + args = p.parse_args(argv) + + from pathlib import Path + + from soccerbot.config import DEFAULT_REPLAY_TRAJECTORY + + cfg = OrchestratorConfig( + backend=args.backend, + iface=args.iface, + camera=args.camera, + policy=args.policy, + layout=args.layout, + clamp=args.clamp, + pickup_duration_s=args.pickup_duration, + fps=args.fps, + device=args.device, + rerun=not args.no_rerun, + teleimager_host=args.teleimager_host, + remote_server=args.remote_server, + replay_trajectory=( + Path(args.replay_trajectory) if args.replay_trajectory else DEFAULT_REPLAY_TRAJECTORY + ), + ) + if args.dry_run_config: + print(cfg) + raise SystemExit(0) + return cfg + + +def main(argv: list[str] | None = None) -> int: + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s %(levelname)s %(name)s: %(message)s", + ) + cfg = parse_args(argv) + logger.info( + "Soccerbot starting: backend=%s policy=%s clamp=%.3f camera=%s iface=%s rerun=%s", + cfg.backend.value, + cfg.policy, + cfg.clamp, + cfg.camera, + cfg.iface, + cfg.rerun, + ) + logger.info("Tip: keep ./killswitch.sh --iface %s open in another terminal", cfg.iface or "") + + t0 = time.time() + try: + run_demo(cfg) + except KeyboardInterrupt: + logger.warning("Interrupted after %.1fs — graceful reset", time.time() - t0) + try: + graceful_reset(iface=cfg.iface) + except Exception: # noqa: BLE001 + logger.exception("graceful_reset failed") + return 130 + except NotImplementedError as exc: + logger.error("Blocked on unimplemented stage: %s", exc) + return 2 + except Exception: # noqa: BLE001 + logger.exception("Demo failed after %.1fs — attempting graceful reset", time.time() - t0) + try: + graceful_reset(iface=cfg.iface) + except Exception: # noqa: BLE001 + logger.exception("graceful_reset failed") + return 1 + + logger.info("Demo finished in %.1fs", time.time() - t0) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/soccerbot/src/soccerbot/pickup.py b/soccerbot/src/soccerbot/pickup.py new file mode 100644 index 0000000..95746d6 --- /dev/null +++ b/soccerbot/src/soccerbot/pickup.py @@ -0,0 +1,63 @@ +"""Stage 1: ACT pickup via in-process ``local-vla-inference`` (or trajectory replay).""" + +from __future__ import annotations + +import logging + +from soccerbot.config import OrchestratorConfig, PickupBackend +from soccerbot.deps import ensure_logic_imports + +logger = logging.getLogger("soccerbot.pickup") + + +def run_pickup(cfg: OrchestratorConfig) -> None: + ensure_logic_imports() + + if cfg.backend is PickupBackend.REPLAY: + from arm_replay import replay_arm_trajectory + + path = cfg.replay_trajectory + if not path.exists(): + raise FileNotFoundError(f"pickup replay trajectory missing: {path}") + logger.info("Replaying pickup trajectory: %s (slew=%.3f)", path, cfg.replay_slew_clamp) + replay_arm_trajectory( + path, + iface=cfg.iface, + slew_clamp=cfg.replay_slew_clamp, + ) + logger.info("Pickup replay finished") + return + + if cfg.backend is PickupBackend.REMOTE: + raise NotImplementedError( + "remote pickup is not wired through soccerbot yet; use --backend local|replay" + ) + + if cfg.backend is not PickupBackend.LOCAL: + raise AssertionError(f"unknown backend: {cfg.backend}") + + # Import the ACT runner from the sibling package (same process, no subprocess). + import main as local_vla # type: ignore[import-not-found] + + args = local_vla.build_args( + layout=cfg.layout, + policy=cfg.policy, + iface=cfg.iface, + camera=cfg.camera, + clamp=cfg.clamp, + duration=cfg.pickup_duration_s, + fps=cfg.fps, + device=cfg.device, + rerun=cfg.rerun, + leave_arms_engaged=True, + ) + logger.info( + "Starting in-process ACT pickup: policy=%s layout=%s clamp=%.3f camera=%s duration=%.1fs", + cfg.policy, + cfg.layout, + cfg.clamp, + cfg.camera, + cfg.pickup_duration_s, + ) + local_vla.run(args) + logger.info("ACT pickup finished cleanly") diff --git a/soccerbot/src/soccerbot/safety.py b/soccerbot/src/soccerbot/safety.py new file mode 100644 index 0000000..93596e2 --- /dev/null +++ b/soccerbot/src/soccerbot/safety.py @@ -0,0 +1,129 @@ +"""Robot safety helpers: graceful stop / damp / zero-torque. + +Ctrl+C during a live demo should **not** leave ``arm_sdk`` engaged forever. +Default interrupt behaviour matches ``remote-vla-inference``: + + 1. ``LocoClient.StopMove()`` — zero loco velocity, stay standing + 2. release ``arm_sdk`` (ramp weight → 0) so the balancer takes the arms + +The headed killswitch can additionally enter ``Damp`` / ``ZeroTorque`` FSM +states (same actions as the physical pendant ``L2+B`` / ``L2+A``). +""" + +from __future__ import annotations + +import logging +from typing import Any + +logger = logging.getLogger(__name__) + + +def init_loco(iface: str | None = None) -> Any: + from unitree_sdk2py.core.channel import ChannelFactoryInitialize + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + + # ChannelFactoryInitialize is process-global; calling twice raises on some + # SDK builds, so tolerate an already-initialized factory. + try: + if iface: + ChannelFactoryInitialize(0, iface) + else: + ChannelFactoryInitialize(0) + except Exception as exc: # noqa: BLE001 + logger.debug("DDS already initialized or init soft-failed: %s", exc) + + loco = LocoClient() + loco.SetTimeout(3.0) + loco.Init() + return loco + + +def stop_loco(loco: Any | None = None, *, iface: str | None = None) -> None: + """Zero locomotion velocity; robot stays up on the balancer.""" + client = loco + if client is None: + try: + client = init_loco(iface) + except Exception as exc: # noqa: BLE001 + logger.warning("Could not init LocoClient for StopMove: %s", exc) + return + try: + rc = client.StopMove() + logger.info("LocoClient.StopMove() -> %s", rc) + except Exception as exc: # noqa: BLE001 + logger.warning("StopMove failed: %s", exc) + + +def release_arms(arms: Any | None) -> None: + """Hand arms back to the stock balancer (ramp arm_sdk weight → 0).""" + if arms is None: + return + try: + if hasattr(arms, "release"): + arms.release() + elif hasattr(arms, "disconnect"): + arms.disconnect() + logger.info("arm_sdk released (arms back to balancer)") + except Exception as exc: # noqa: BLE001 + logger.warning("arm release failed: %s", exc) + + +def graceful_reset( + *, + arms: Any | None = None, + loco: Any | None = None, + iface: str | None = None, + camera: Any | None = None, +) -> None: + """Ctrl+C / abort path: stop motion, release arms, disconnect camera. + + Leaves the robot standing (does **not** enter damp/zero-torque unless the + operator uses the killswitch). + """ + logger.warning("Graceful reset: StopMove + release arm_sdk") + stop_loco(loco, iface=iface) + release_arms(arms) + if camera is not None: + try: + camera.disconnect() + except Exception as exc: # noqa: BLE001 + logger.warning("camera disconnect failed: %s", exc) + + +def enter_damp(*, iface: str | None = None, loco: Any | None = None) -> None: + """Passive damp mode (pendant L2+B). Soft-falls the robot if unsupported.""" + client = loco or init_loco(iface) + stop_loco(client) + try: + rc = client.Damp() + logger.warning("LocoClient.Damp() -> %s (passive damping)", rc) + except Exception as exc: # noqa: BLE001 + logger.error("Damp failed: %s", exc) + raise + + +def enter_zero_torque(*, iface: str | None = None, loco: Any | None = None) -> None: + """Zero-torque mode (pendant L2+A). Robot goes fully limp — spotter ready.""" + client = loco or init_loco(iface) + stop_loco(client) + try: + rc = client.ZeroTorque() + logger.warning("LocoClient.ZeroTorque() -> %s (motors limp)", rc) + except Exception as exc: # noqa: BLE001 + logger.error("ZeroTorque failed: %s", exc) + raise + + +def balance_stand(*, iface: str | None = None, loco: Any | None = None) -> None: + """Re-engage balancer stand / start FSM after a killswitch event.""" + client = loco or init_loco(iface) + try: + if hasattr(client, "Start"): + rc = client.Start() + logger.info("LocoClient.Start() -> %s", rc) + else: + rc = client.SetFsmId(500) + logger.info("LocoClient.SetFsmId(500) -> %s", rc) + except Exception as exc: # noqa: BLE001 + logger.error("balance stand / Start failed: %s", exc) + raise diff --git a/uv.lock b/uv.lock index 67076f6..2cb2f74 100644 --- a/uv.lock +++ b/uv.lock @@ -39,6 +39,7 @@ members = [ "remote-vla-inference", "scripted-behavior", "soccerbot", + "soccerbot-sim", "soccerbot-workspace", "training", ] @@ -3688,16 +3689,16 @@ wheels = [ [[package]] name = "local-vla-inference" version = "0.1.0" -source = { virtual = "local-vla-inference" } +source = { editable = "local-vla-inference" } dependencies = [ - { name = "lerobot" }, + { name = "lerobot", extra = ["viz"] }, { name = "pyzmq" }, { name = "unitree-sdk2py" }, ] [package.metadata] requires-dist = [ - { name = "lerobot", editable = "thirdparty/lerobot" }, + { name = "lerobot", extras = ["viz"], editable = "thirdparty/lerobot" }, { name = "pyzmq", specifier = ">=26.0.0" }, { name = "unitree-sdk2py", git = "https://github.com/unitreerobotics/unitree_sdk2_python.git" }, ] @@ -6400,7 +6401,7 @@ wheels = [ [[package]] name = "scripted-behavior" version = "0.1.0" -source = { virtual = "scripted-behavior" } +source = { editable = "scripted-behavior" } [[package]] name = "send2trash" @@ -6523,7 +6524,24 @@ wheels = [ [[package]] name = "soccerbot" version = "0.1.0" -source = { virtual = "soccerbot" } +source = { editable = "soccerbot" } +dependencies = [ + { name = "lerobot", extra = ["viz"] }, + { name = "local-vla-inference" }, + { name = "scripted-behavior" }, +] + +[package.metadata] +requires-dist = [ + { name = "lerobot", extras = ["viz"], editable = "thirdparty/lerobot" }, + { name = "local-vla-inference", editable = "local-vla-inference" }, + { name = "scripted-behavior", editable = "scripted-behavior" }, +] + +[[package]] +name = "soccerbot-sim" +version = "0.1.0" +source = { editable = "sim" } [[package]] name = "soccerbot-workspace" From 23ff9b4eea45d42ba146b21d246a44a736da9064 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 08:01:55 +0000 Subject: [PATCH 2/9] Fix Bugbot: ACT import shadowing, DDS guard, Ctrl+C arm release - Load local-vla-inference/main as local_vla_inference_main so scripted main.py cannot shadow build_args/run - Share process-wide ensure_dds (dds_init) across ACT and scripted stages - graceful_reset always opens a temp G1Arms to release arm_sdk when no handle - ACT leave-engaged path freezes then detach() so only one rt/arm_sdk publisher - diagnose.sh cds to REPO_ROOT before relative import checks Co-authored-by: arjuncoder1 --- diagnose.sh | 3 +- local-vla-inference/dds_init.py | 34 +++++++++++++++ local-vla-inference/g1_arms.py | 15 ++++++- local-vla-inference/main.py | 28 +++++++------ local-vla-inference/pyproject.toml | 1 + scripted-behavior/dds.py | 27 ++++-------- scripted-behavior/pickup.py | 21 ++++++++-- soccerbot/src/soccerbot/deps.py | 66 +++++++++++++++++++++++------- soccerbot/src/soccerbot/pickup.py | 6 +-- soccerbot/src/soccerbot/safety.py | 59 ++++++++++++++++++-------- 10 files changed, 189 insertions(+), 71 deletions(-) create mode 100644 local-vla-inference/dds_init.py diff --git a/diagnose.sh b/diagnose.sh index 4e6a0ab..e6bcd6a 100755 --- a/diagnose.sh +++ b/diagnose.sh @@ -3,6 +3,7 @@ set -euo pipefail REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$REPO_ROOT" VENV_DIR="${VENV_DIR:-$REPO_ROOT/.venv}" CYCLONE_PREFIX="${CYCLONEDDS_HOME:-${CYCLONE_PREFIX:-$HOME/cyclonedds/install}}" IFACE="" @@ -30,7 +31,7 @@ bad() { echo " [FAIL] $*"; fail=$((fail + 1)); } soft() { echo " [WARN] $*"; warn=$((warn + 1)); } echo "==> Soccerbot diagnose" -echo " repo: $REPO_ROOT" +echo " repo: $REPO_ROOT (cwd=$(pwd))" # --- toolchain --- if command -v uv >/dev/null 2>&1; then ok "uv: $(command -v uv)"; else bad "uv not on PATH"; fi diff --git a/local-vla-inference/dds_init.py b/local-vla-inference/dds_init.py new file mode 100644 index 0000000..3b89b5d --- /dev/null +++ b/local-vla-inference/dds_init.py @@ -0,0 +1,34 @@ +"""Process-wide DDS ChannelFactoryInitialize guard. + +``ChannelFactoryInitialize`` may only be called once per process. Every +entry point that talks to the robot (ACT loop, scripted stages, killswitch) +must go through ``ensure_dds`` instead of calling the SDK directly. +""" + +from __future__ import annotations + +import logging + +logger = logging.getLogger(__name__) + +_initialized = False + + +def ensure_dds(iface: str | None = None) -> None: + """Idempotent ``ChannelFactoryInitialize``. Safe to call from any stage.""" + global _initialized + if _initialized: + return + from unitree_sdk2py.core.channel import ChannelFactoryInitialize + + if iface: + logger.info("DDS ChannelFactoryInitialize(0, %r)", iface) + ChannelFactoryInitialize(0, iface) + else: + logger.info("DDS ChannelFactoryInitialize(0) [default interface]") + ChannelFactoryInitialize(0) + _initialized = True + + +def dds_initialized() -> bool: + return _initialized diff --git a/local-vla-inference/g1_arms.py b/local-vla-inference/g1_arms.py index 63cda0a..d3e0c6d 100644 --- a/local-vla-inference/g1_arms.py +++ b/local-vla-inference/g1_arms.py @@ -252,8 +252,19 @@ def release(self, ramp_s: float = 1.0, control_dt: float = 0.02) -> None: except Exception as e: logger.warning("Failed to release arm_sdk cleanly: %s", e) + def detach(self) -> None: + """Drop local DDS handles without changing arm_sdk weight. + + Use after ``freeze()`` when handing control to the next in-process stage + so only one ``rt/arm_sdk`` publisher is alive at a time. The robot keeps + the last commanded pose (weight stays 1) until another publisher takes over + or ``release()`` ramps weight to 0. + """ + self._publisher = None + self._subscriber = None + logger.info("G1Arms detached (local DDS handles dropped; arm_sdk weight unchanged)") + def disconnect(self) -> None: if not self._state_only: self.release() - self._publisher = None - self._subscriber = None + self.detach() diff --git a/local-vla-inference/main.py b/local-vla-inference/main.py index f153849..fc50b0c 100644 --- a/local-vla-inference/main.py +++ b/local-vla-inference/main.py @@ -307,12 +307,9 @@ def image_no_motors( device, ) -> None: """Real camera + read-only lowstate → print predicted trajectory. Never write motors.""" - from unitree_sdk2py.core.channel import ChannelFactoryInitialize + from dds_init import ensure_dds - if args.iface: - ChannelFactoryInitialize(0, args.iface) - else: - ChannelFactoryInitialize(0) + ensure_dds(args.iface) arms = G1Arms(kp=args.kp, kd=args.kd) front = make_front_camera(args.camera) @@ -341,8 +338,11 @@ def image_no_motors( def _graceful_interrupt(arms: G1Arms, iface: str | None, front) -> None: """Ctrl+C: stop loco velocity and hand arms back to the balancer.""" + from dds_init import ensure_dds + logger.warning("Ctrl+C — graceful reset (StopMove + release arm_sdk)") try: + ensure_dds(iface) from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient loco = LocoClient() @@ -354,6 +354,7 @@ def _graceful_interrupt(arms: G1Arms, iface: str | None, front) -> None: logger.warning("StopMove during interrupt failed: %s", exc) try: arms.release() + arms.detach() except Exception as exc: # noqa: BLE001 logger.warning("arm release during interrupt failed: %s", exc) try: @@ -407,12 +408,9 @@ def run(args: argparse.Namespace) -> None: return # One DDS init per process; shared by arms + camera clients. - from unitree_sdk2py.core.channel import ChannelFactoryInitialize + from dds_init import ensure_dds - if args.iface: - ChannelFactoryInitialize(0, args.iface) - else: - ChannelFactoryInitialize(0) + ensure_dds(args.iface) arms = G1Arms(kp=args.kp, kd=args.kd) front = make_front_camera(args.camera) @@ -569,12 +567,16 @@ def run(args: argparse.Namespace) -> None: except Exception as exc: # noqa: BLE001 logger.warning("camera disconnect failed: %s", exc) if leave_engaged: - # Hold last pose with arm_sdk still on for the next scripted stage. + # Hold last pose, then drop local DDS handles so the next + # in-process stage can own rt/arm_sdk without a second publisher. try: arms.freeze(cmd_q) - logger.info("Clean exit: arm_sdk left engaged for next stage") + arms.detach() + logger.info( + "Clean exit: arm_sdk left engaged; local publisher detached for next stage" + ) except Exception as exc: # noqa: BLE001 - logger.warning("leave-engaged freeze failed: %s", exc) + logger.warning("leave-engaged freeze/detach failed: %s", exc) else: arms.disconnect() logger.info("Done (interrupted=%s leave_engaged=%s)", interrupted, leave_engaged) diff --git a/local-vla-inference/pyproject.toml b/local-vla-inference/pyproject.toml index 4ac9901..f5a12e2 100644 --- a/local-vla-inference/pyproject.toml +++ b/local-vla-inference/pyproject.toml @@ -21,6 +21,7 @@ py-modules = [ "g1_arms", "front_camera", "telemetry", + "dds_init", "embodiment_g1_14d", "embodiment_g1d_16d", "diag_health", diff --git a/scripted-behavior/dds.py b/scripted-behavior/dds.py index ca258b2..c3ab4ec 100644 --- a/scripted-behavior/dds.py +++ b/scripted-behavior/dds.py @@ -1,4 +1,4 @@ -"""Process-wide DDS init guard. +"""Process-wide DDS init guard (re-exports local-vla-inference's singleton). Every live stage that talks to the robot must call ``ensure_dds`` before opening any ``unitree_sdk2py`` channels; the SDK's @@ -7,24 +7,13 @@ from __future__ import annotations -import logging +import sys +from pathlib import Path -logger = logging.getLogger("scripted_behavior.dds") +_LOCAL_VLA = Path(__file__).resolve().parent.parent / "local-vla-inference" +if str(_LOCAL_VLA) not in sys.path: + sys.path.insert(0, str(_LOCAL_VLA)) -_initialized = False +from dds_init import dds_initialized, ensure_dds # noqa: E402 - -def ensure_dds(iface: str | None) -> None: - """Idempotent ``ChannelFactoryInitialize``. Safe to call from any stage.""" - global _initialized - if _initialized: - return - from unitree_sdk2py.core.channel import ChannelFactoryInitialize - - if iface: - logger.info("DDS ChannelFactoryInitialize(0, %r)", iface) - ChannelFactoryInitialize(0, iface) - else: - logger.info("DDS ChannelFactoryInitialize(0) [default interface]") - ChannelFactoryInitialize(0) - _initialized = True +__all__ = ["ensure_dds", "dds_initialized"] diff --git a/scripted-behavior/pickup.py b/scripted-behavior/pickup.py index 13dbd1d..184df65 100644 --- a/scripted-behavior/pickup.py +++ b/scripted-behavior/pickup.py @@ -9,6 +9,7 @@ from __future__ import annotations +import importlib.util import logging import sys @@ -21,6 +22,22 @@ DEFAULT_POLICY = "ajkoder/g1-pickup-ball-act" DEFAULT_CLAMP = 0.002 DEFAULT_CAMERA = "zmq://192.168.123.164:55555" +_LOCAL_VLA_MODULE = "local_vla_inference_main" + + +def _load_local_vla_main(): + """Load ACT runner under a unique module name (avoids shadowing this package's main).""" + if str(LOCAL_VLA_DIR) not in sys.path: + sys.path.insert(0, str(LOCAL_VLA_DIR)) + if _LOCAL_VLA_MODULE in sys.modules: + return sys.modules[_LOCAL_VLA_MODULE] + spec = importlib.util.spec_from_file_location(_LOCAL_VLA_MODULE, LOCAL_VLA_DIR / "main.py") + if spec is None or spec.loader is None: + raise ImportError(f"Cannot load ACT runner from {LOCAL_VLA_DIR / 'main.py'}") + mod = importlib.util.module_from_spec(spec) + sys.modules[_LOCAL_VLA_MODULE] = mod + spec.loader.exec_module(mod) + return mod def run_pickup_policy(cfg: OrchestratorConfig) -> None: @@ -43,9 +60,7 @@ def run_pickup_policy(cfg: OrchestratorConfig) -> None: if cfg.backend is not PickupBackend.LOCAL: raise AssertionError(f"unknown backend: {cfg.backend}") - if str(LOCAL_VLA_DIR) not in sys.path: - sys.path.insert(0, str(LOCAL_VLA_DIR)) - import main as local_vla # type: ignore[import-not-found] + local_vla = _load_local_vla_main() # Optional extras: --policy / --clamp / --camera forwarded after '--'. policy = DEFAULT_POLICY diff --git a/soccerbot/src/soccerbot/deps.py b/soccerbot/src/soccerbot/deps.py index 87d5ca1..4cc1c36 100644 --- a/soccerbot/src/soccerbot/deps.py +++ b/soccerbot/src/soccerbot/deps.py @@ -1,15 +1,18 @@ """Import helpers for workspace logic packages (not published to PyPI). -``local-vla-inference`` and ``scripted-behavior`` are flat virtual workspace -members. Soccerbot is the core orchestrator and loads them by putting their -directories on ``sys.path`` so we can ``import main`` / ``import arm_replay`` -in-process — no subprocess. +``local-vla-inference`` and ``scripted-behavior`` both expose a top-level +``main`` module. Never ``import main`` after putting both on ``sys.path``. +Load ACT via :func:`import_local_vla_main` (unique module name). """ from __future__ import annotations +import importlib +import importlib.util import sys from pathlib import Path +from types import ModuleType + def _discover_repo_root() -> Path: """Find the workspace root that contains the logic packages.""" @@ -30,17 +33,52 @@ def _discover_repo_root() -> Path: REPO_ROOT = _discover_repo_root() -_LOGIC_DIRS = ( - REPO_ROOT / "local-vla-inference", - REPO_ROOT / "scripted-behavior", - REPO_ROOT / "realsense-human-detection", -) +LOCAL_VLA_DIR = REPO_ROOT / "local-vla-inference" +SCRIPTED_DIR = REPO_ROOT / "scripted-behavior" +REALSENSE_DIR = REPO_ROOT / "realsense-human-detection" + + +def _ensure_front(directory: Path) -> None: + """Put ``directory`` at the front of ``sys.path`` (idempotent move-to-front).""" + text = str(directory) + if not directory.is_dir(): + return + if text in sys.path: + sys.path.remove(text) + sys.path.insert(0, text) def ensure_logic_imports() -> Path: - """Prepend logic-package dirs to ``sys.path``. Idempotent.""" - for path in _LOGIC_DIRS: - text = str(path) - if path.is_dir() and text not in sys.path: - sys.path.insert(0, text) + """Put scripted-behavior (and optional realsense) on ``sys.path``. + + Does **not** load ``local-vla-inference/main.py`` as ``main``. Use + :func:`import_local_vla_main` for the ACT runner. + """ + _ensure_front(REALSENSE_DIR) + _ensure_front(SCRIPTED_DIR) return REPO_ROOT + + +def import_local_vla_main() -> ModuleType: + """Load ``local-vla-inference/main.py`` as ``local_vla_inference_main``.""" + module_name = "local_vla_inference_main" + if module_name in sys.modules: + return sys.modules[module_name] + + # Sibling imports (g1_arms, front_camera, dds_init, …) need this dir on path. + _ensure_front(LOCAL_VLA_DIR) + + path = LOCAL_VLA_DIR / "main.py" + spec = importlib.util.spec_from_file_location(module_name, path) + if spec is None or spec.loader is None: + raise ImportError(f"Cannot load local-vla-inference main from {path}") + mod = importlib.util.module_from_spec(spec) + sys.modules[module_name] = mod + spec.loader.exec_module(mod) + return mod + + +def import_scripted(name: str) -> ModuleType: + """Import a scripted-behavior module by name (``avoid``, ``throw``, …).""" + ensure_logic_imports() + return importlib.import_module(name) diff --git a/soccerbot/src/soccerbot/pickup.py b/soccerbot/src/soccerbot/pickup.py index 95746d6..ca74fdb 100644 --- a/soccerbot/src/soccerbot/pickup.py +++ b/soccerbot/src/soccerbot/pickup.py @@ -5,7 +5,7 @@ import logging from soccerbot.config import OrchestratorConfig, PickupBackend -from soccerbot.deps import ensure_logic_imports +from soccerbot.deps import ensure_logic_imports, import_local_vla_main logger = logging.getLogger("soccerbot.pickup") @@ -36,8 +36,8 @@ def run_pickup(cfg: OrchestratorConfig) -> None: if cfg.backend is not PickupBackend.LOCAL: raise AssertionError(f"unknown backend: {cfg.backend}") - # Import the ACT runner from the sibling package (same process, no subprocess). - import main as local_vla # type: ignore[import-not-found] + # Load by unique module name so scripted-behavior/main.py cannot shadow it. + local_vla = import_local_vla_main() args = local_vla.build_args( layout=cfg.layout, diff --git a/soccerbot/src/soccerbot/safety.py b/soccerbot/src/soccerbot/safety.py index 93596e2..947d725 100644 --- a/soccerbot/src/soccerbot/safety.py +++ b/soccerbot/src/soccerbot/safety.py @@ -15,23 +15,22 @@ import logging from typing import Any +from soccerbot.deps import LOCAL_VLA_DIR, _ensure_front + logger = logging.getLogger(__name__) +def _ensure_dds(iface: str | None = None) -> None: + _ensure_front(LOCAL_VLA_DIR) + from dds_init import ensure_dds + + ensure_dds(iface) + + def init_loco(iface: str | None = None) -> Any: - from unitree_sdk2py.core.channel import ChannelFactoryInitialize + _ensure_dds(iface) from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient - # ChannelFactoryInitialize is process-global; calling twice raises on some - # SDK builds, so tolerate an already-initialized factory. - try: - if iface: - ChannelFactoryInitialize(0, iface) - else: - ChannelFactoryInitialize(0) - except Exception as exc: # noqa: BLE001 - logger.debug("DDS already initialized or init soft-failed: %s", exc) - loco = LocoClient() loco.SetTimeout(3.0) loco.Init() @@ -54,18 +53,42 @@ def stop_loco(loco: Any | None = None, *, iface: str | None = None) -> None: logger.warning("StopMove failed: %s", exc) -def release_arms(arms: Any | None) -> None: - """Hand arms back to the stock balancer (ramp arm_sdk weight → 0).""" +def release_arms(arms: Any | None = None, *, iface: str | None = None) -> None: + """Hand arms back to the stock balancer (ramp arm_sdk weight → 0). + + If ``arms`` is None (e.g. interrupt during a scripted stage that owns its + own short-lived ``G1Arms``), open a temporary publisher solely to release. + """ + owned = False if arms is None: - return + try: + _ensure_dds(iface) + _ensure_front(LOCAL_VLA_DIR) + from g1_arms import G1Arms + + arms = G1Arms(kp=60.0, kd=1.5) + arms.connect() + owned = True + except Exception as exc: # noqa: BLE001 + logger.warning("Could not open G1Arms to release arm_sdk: %s", exc) + return try: if hasattr(arms, "release"): arms.release() elif hasattr(arms, "disconnect"): arms.disconnect() + return + if hasattr(arms, "detach"): + arms.detach() logger.info("arm_sdk released (arms back to balancer)") except Exception as exc: # noqa: BLE001 logger.warning("arm release failed: %s", exc) + finally: + if owned and arms is not None and hasattr(arms, "detach"): + try: + arms.detach() + except Exception: # noqa: BLE001 + pass def graceful_reset( @@ -78,11 +101,12 @@ def graceful_reset( """Ctrl+C / abort path: stop motion, release arms, disconnect camera. Leaves the robot standing (does **not** enter damp/zero-torque unless the - operator uses the killswitch). + operator uses the killswitch). Always attempts arm_sdk release even when + the caller has no ``G1Arms`` handle. """ logger.warning("Graceful reset: StopMove + release arm_sdk") stop_loco(loco, iface=iface) - release_arms(arms) + release_arms(arms, iface=iface) if camera is not None: try: camera.disconnect() @@ -95,6 +119,8 @@ def enter_damp(*, iface: str | None = None, loco: Any | None = None) -> None: client = loco or init_loco(iface) stop_loco(client) try: + # Best-effort: release arm overlay before going passive. + release_arms(iface=iface) rc = client.Damp() logger.warning("LocoClient.Damp() -> %s (passive damping)", rc) except Exception as exc: # noqa: BLE001 @@ -107,6 +133,7 @@ def enter_zero_torque(*, iface: str | None = None, loco: Any | None = None) -> N client = loco or init_loco(iface) stop_loco(client) try: + release_arms(iface=iface) rc = client.ZeroTorque() logger.warning("LocoClient.ZeroTorque() -> %s (motors limp)", rc) except Exception as exc: # noqa: BLE001 From 0f1d81f9981eba8288e9a75dc3c5c64dee508b36 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 15:02:14 +0000 Subject: [PATCH 3/9] Add CLI + GUI killswitch with go-home arm pose Default killswitch is a no-GUI CLI (interactive or one-shot stop/damp/zero/ start/home/status). --gui keeps the Tk panel and adds a GO HOME button. Home is Unitree zeros by default, overridable via scripted-behavior/home_pose.json, and motion is slew-clamped. Co-authored-by: arjuncoder1 --- diagnose.sh | 18 ++- install.sh | 3 +- killswitch.sh | 8 +- scripted-behavior/home_pose.json | 22 +++ soccerbot/README.md | 9 +- soccerbot/src/soccerbot/home.py | 169 +++++++++++++++++++++ soccerbot/src/soccerbot/killswitch.py | 203 ++++++++++++++++++++++---- soccerbot/src/soccerbot/main.py | 8 +- 8 files changed, 398 insertions(+), 42 deletions(-) create mode 100644 scripted-behavior/home_pose.json create mode 100644 soccerbot/src/soccerbot/home.py diff --git a/diagnose.sh b/diagnose.sh index e6bcd6a..0517b62 100755 --- a/diagnose.sh +++ b/diagnose.sh @@ -99,12 +99,26 @@ PY else soft "rerun-sdk missing — ACT will run without live viz (lerobot[viz])" fi + if "$VENV_DIR/bin/python" - <<'PY' +import sys +from pathlib import Path +sys.path.insert(0, str(Path("soccerbot/src").resolve())) +from soccerbot.home import DEFAULT_HOME_Q, load_home_pose +pose = load_home_pose() +assert len(pose) == 14 +assert all(abs(v) < 1e-9 for v in pose.values()) or Path("scripted-behavior/home_pose.json").is_file() +print("home pose ok", len(pose)) +PY + then ok "home pose loader" + else soft "home pose loader failed" + fi + if "$VENV_DIR/bin/python" - <<'PY' import tkinter # noqa: F401 print("tkinter ok") PY - then ok "tkinter (headed killswitch)" - else soft "tkinter missing — install python3-tk for ./killswitch.sh" + then ok "tkinter (optional --gui killswitch)" + else soft "tkinter missing — CLI killswitch still works; install python3-tk for --gui" fi # local-vla defaults (avoid importing cv2/torch-heavy main unless deps present) diff --git a/install.sh b/install.sh index 536efdb..39a6c88 100755 --- a/install.sh +++ b/install.sh @@ -40,5 +40,6 @@ echo " source $REPO_ROOT/.venv/bin/activate" echo " export CYCLONEDDS_HOME=\${CYCLONEDDS_HOME:-\$HOME/cyclonedds/install}" echo " export LD_LIBRARY_PATH=\$CYCLONEDDS_HOME/lib:\${LD_LIBRARY_PATH:-}" echo " ./diagnose.sh --iface enp5s0" -echo " ./killswitch.sh --iface enp5s0 # headed safety panel" +echo " ./killswitch.sh --iface enp5s0 # CLI killswitch (stop/damp/zero/home)" +echo " ./killswitch.sh --gui --iface enp5s0 # Tk GUI killswitch" echo " ./run_soccerbot.sh --iface enp5s0" diff --git a/killswitch.sh b/killswitch.sh index 1f6c90f..0ef38c8 100755 --- a/killswitch.sh +++ b/killswitch.sh @@ -1,5 +1,10 @@ #!/usr/bin/env bash -# Headed G1 killswitch panel (Stop / Damp / ZeroTorque / Start). +# G1 killswitch — CLI by default; pass --gui for the Tk panel. +# +# ./killswitch.sh --iface enp5s0 # interactive CLI +# ./killswitch.sh --iface enp5s0 stop # one-shot StopMove +# ./killswitch.sh --iface enp5s0 home # arms → home pose +# ./killswitch.sh --gui --iface enp5s0 # Tk GUI (+ GO HOME button) set -euo pipefail REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" @@ -16,7 +21,6 @@ export LD_LIBRARY_PATH="${CYCLONE_PREFIX}/lib${LD_LIBRARY_PATH:+:$LD_LIBRARY_PAT export PATH="$VENV_DIR/bin:$PATH" export PYTHONPATH="$REPO_ROOT/soccerbot/src${PYTHONPATH:+:$PYTHONPATH}" -# Prefer console script if installed; else module path. if command -v soccerbot-killswitch >/dev/null 2>&1; then exec soccerbot-killswitch "$@" fi diff --git a/scripted-behavior/home_pose.json b/scripted-behavior/home_pose.json new file mode 100644 index 0000000..64e8fa9 --- /dev/null +++ b/scripted-behavior/home_pose.json @@ -0,0 +1,22 @@ +{ + "name": "g1_arm_home_zeros", + "kind": "arm_qpos_14d", + "joints_order": [ + "kLeftShoulderPitch", + "kLeftShoulderRoll", + "kLeftShoulderYaw", + "kLeftElbow", + "kLeftWristRoll", + "kLeftWristPitch", + "kLeftWristYaw", + "kRightShoulderPitch", + "kRightShoulderRoll", + "kRightShoulderYaw", + "kRightElbow", + "kRightWristRoll", + "kRightWristPitch", + "kRightWristYaw" + ], + "q": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], + "notes": "Unitree arm SDK home (ctrl_dual_arm_go_home): all arm joints at 0 rad. Edit this file to set a custom ready pose." +} diff --git a/soccerbot/README.md b/soccerbot/README.md index e4e21ec..4df1a2c 100644 --- a/soccerbot/README.md +++ b/soccerbot/README.md @@ -16,8 +16,10 @@ in sibling packages that soccerbot imports in-process: # one-time robot setup (Python 3.12 + CycloneDDS) ./install.sh -# headed killswitch in a second terminal (keep open during demos) +# killswitch in a second terminal (CLI by default — no tkinter needed) ./killswitch.sh --iface enp5s0 +./killswitch.sh --iface enp5s0 home # one-shot arms → home +./killswitch.sh --gui --iface enp5s0 # Tk GUI (+ GO HOME button) # full demo: ACT pickup → turn → avoid → throw ./run_soccerbot.sh --iface enp5s0 @@ -35,9 +37,10 @@ Defaults match the validated local ACT command: ## Safety -- Every arm command path is slew-clamped (ACT + replay + throw). +- Every arm command path is slew-clamped (ACT + replay + throw + go-home). - **Ctrl+C** → graceful reset: `LocoClient.StopMove()` + release `arm_sdk`. -- **`./killswitch.sh`** → headed GUI: Stop Move / Damp / Zero Torque / Start. +- **`./killswitch.sh`** → CLI (default) or `--gui`: Stop / Go Home / Damp / Zero Torque / Start. +- Home pose defaults to Unitree zeros; override via `scripted-behavior/home_pose.json`. ## Diagnose diff --git a/soccerbot/src/soccerbot/home.py b/soccerbot/src/soccerbot/home.py new file mode 100644 index 0000000..808e4f2 --- /dev/null +++ b/soccerbot/src/soccerbot/home.py @@ -0,0 +1,169 @@ +"""Move G1 arms to a home pose (slew-clamped). + +Unitree's arm SDK treats home as all arm joints at 0 rad +(``ctrl_dual_arm_go_home``). Override with a JSON file if needed: + + { + "kind": "arm_qpos_14d", + "joints_order": ["kLeftShoulderPitch", ...], + "q": [0, 0, ...] + } + +or a flat ``{"kLeftShoulderPitch.q": 0.0, ...}`` map. +""" + +from __future__ import annotations + +import json +import logging +import time +from pathlib import Path +from typing import Any + +from soccerbot.deps import LOCAL_VLA_DIR, REPO_ROOT, _ensure_front + +logger = logging.getLogger(__name__) + +# Same joint order as local-vla-inference G1Arms / 14-D layout. +ARM_JOINTS: tuple[str, ...] = ( + "kLeftShoulderPitch", + "kLeftShoulderRoll", + "kLeftShoulderYaw", + "kLeftElbow", + "kLeftWristRoll", + "kLeftWristPitch", + "kLeftWristYaw", + "kRightShoulderPitch", + "kRightShoulderRoll", + "kRightShoulderYaw", + "kRightElbow", + "kRightWristRoll", + "kRightWristPitch", + "kRightWristYaw", +) + +# Unitree arm SDK home = zeros. +DEFAULT_HOME_Q: dict[str, float] = {f"{name}.q": 0.0 for name in ARM_JOINTS} + +DEFAULT_HOME_JSON = REPO_ROOT / "scripted-behavior" / "home_pose.json" +HOME_SLEW_CLAMP = 0.02 # rad/step +HOME_CONTROL_DT = 0.02 +HOME_ENGAGE_RAMP_S = 1.0 + + +def load_home_pose(path: Path | None = None) -> dict[str, float]: + """Load home pose from JSON, or return the Unitree zero-pose default.""" + candidate = path or DEFAULT_HOME_JSON + if candidate is None or not Path(candidate).is_file(): + return dict(DEFAULT_HOME_Q) + + with open(candidate) as f: + data = json.load(f) + + if isinstance(data, dict) and "q" in data and "joints_order" in data: + joints = data["joints_order"] + qs = data["q"] + if len(joints) != 14 or len(qs) != 14: + raise ValueError(f"home pose must be 14-D, got {len(joints)}/{len(qs)}") + return {f"{name}.q": float(q) for name, q in zip(joints, qs)} + + if isinstance(data, dict) and all(isinstance(v, (int, float)) for v in data.values()): + out = dict(DEFAULT_HOME_Q) + for key, value in data.items(): + k = key if key.endswith(".q") else f"{key}.q" + if k in out: + out[k] = float(value) + return out + + raise ValueError(f"Unrecognized home pose schema in {candidate}") + + +def go_home( + *, + iface: str | None = None, + pose: dict[str, float] | None = None, + pose_path: Path | None = None, + slew_clamp: float = HOME_SLEW_CLAMP, + duration_s: float | None = None, + release_after: bool = False, +) -> None: + """Slew-limit interpolate arms from current pose to home. + + Engages ``arm_sdk`` if needed. By default leaves it engaged at home so a + following stage can take over; pass ``release_after=True`` to hand back. + """ + from soccerbot.safety import _ensure_dds + + _ensure_dds(iface) + _ensure_front(LOCAL_VLA_DIR) + from g1_arms import G1Arms + + target = pose if pose is not None else load_home_pose(pose_path) + missing = [k for k in DEFAULT_HOME_Q if k not in target] + if missing: + raise ValueError(f"home pose missing joints: {missing}") + + arms = G1Arms(kp=60.0, kd=1.5) + arms.connect() + try: + start = arms.get_arm_positions() + # Engage without yanking: ramp weight while holding current pose. + arms.hold_current_pose(ramp_s=HOME_ENGAGE_RAMP_S) + + # Estimate duration from max joint delta if not provided. + if duration_s is None: + max_delta = max(abs(target[k] - start.get(k, 0.0)) for k in DEFAULT_HOME_Q) + # At slew_clamp rad/step and HOME_CONTROL_DT, need enough steps. + steps_needed = max(1, int(max_delta / max(slew_clamp, 1e-6)) + 1) + duration_s = steps_needed * HOME_CONTROL_DT + duration_s = max(2.0, min(duration_s, 20.0)) + + logger.info( + "Going home over %.1fs (slew=%.3f rad/step, release_after=%s)", + duration_s, + slew_clamp, + release_after, + ) + _interpolate_clamped(arms, start, target, duration_s, slew_clamp) + + # Hold home briefly so it settles. + settle_end = time.monotonic() + 0.4 + while time.monotonic() < settle_end: + arms.send_arm_positions(target, weight=1.0) + time.sleep(HOME_CONTROL_DT) + logger.info("Home pose reached") + finally: + if release_after: + arms.disconnect() # release + detach + else: + arms.detach() # keep arm_sdk engaged at home for next owner + + +def _interpolate_clamped( + arms: Any, + start: dict[str, float], + target: dict[str, float], + duration_s: float, + slew_clamp: float, +) -> None: + keys = list(DEFAULT_HOME_Q) + cmd = {k: float(start.get(k, 0.0)) for k in keys} + num_steps = max(1, int(duration_s / HOME_CONTROL_DT)) + for step in range(1, num_steps + 1): + t0 = time.perf_counter() + alpha = step / num_steps + desired = { + k: float(start.get(k, 0.0)) * (1.0 - alpha) + float(target[k]) * alpha for k in keys + } + if slew_clamp > 0: + for k in keys: + delta = desired[k] - cmd[k] + if abs(delta) > slew_clamp: + delta = slew_clamp if delta > 0 else -slew_clamp + cmd[k] = cmd[k] + delta + else: + cmd = desired + arms.send_arm_positions(cmd, weight=1.0) + sleep = HOME_CONTROL_DT - (time.perf_counter() - t0) + if sleep > 0: + time.sleep(sleep) diff --git a/soccerbot/src/soccerbot/killswitch.py b/soccerbot/src/soccerbot/killswitch.py index 1add474..5b279ec 100644 --- a/soccerbot/src/soccerbot/killswitch.py +++ b/soccerbot/src/soccerbot/killswitch.py @@ -1,18 +1,25 @@ -"""Headed killswitch panel for the Unitree G1. +"""G1 killswitch — CLI (default) and optional Tk GUI. -Big on-screen buttons (tkinter) that map to the same safety actions as the -physical pendant: +Actions (same as physical pendant where applicable): - STOP / STAND — StopMove + (optional) leave balancer standing - DAMP — LocoClient.Damp() (pendant L2+B) - ZERO TORQUE — LocoClient.ZeroTorque() (pendant L2+A) - START — LocoClient.Start() (re-engage balancer) + stop — LocoClient.StopMove() (stay standing) + damp — LocoClient.Damp() (pendant L2+B) + zero — LocoClient.ZeroTorque() (pendant L2+A) + start — LocoClient.Start() (re-engage balancer) + home — slew-clamped go-to-home arm pose (zeros / home_pose.json) -Run separately from the demo so it stays usable even if the policy process -hangs: +Usage: + # CLI interactive (no GUI / no tkinter required) ./killswitch.sh --iface enp5s0 python -m soccerbot.killswitch --iface enp5s0 + + # CLI one-shot + ./killswitch.sh --iface enp5s0 stop + ./killswitch.sh --iface enp5s0 home + + # Tk GUI + ./killswitch.sh --gui --iface enp5s0 """ from __future__ import annotations @@ -23,7 +30,8 @@ import threading import time -from soccerbot.safety import balance_stand, enter_damp, enter_zero_torque, stop_loco +from soccerbot.home import go_home +from soccerbot.safety import balance_stand, enter_damp, enter_zero_torque, init_loco, stop_loco logger = logging.getLogger("soccerbot.killswitch") @@ -37,6 +45,102 @@ 500: "advanced (main operation)", } +ACTIONS = ("stop", "damp", "zero", "start", "home", "status") + + +def _fsm_status(loco) -> str: + try: + code, fsm_id = loco.GetFsmId() + name = FSM_NAMES.get(int(fsm_id), f"fsm {fsm_id}") + return f"FSM={fsm_id} ({name}) rpc={code}" + except Exception as exc: # noqa: BLE001 + return f"FSM read failed: {exc}" + + +def run_action(action: str, *, iface: str | None, loco=None) -> None: + """Execute one killswitch / home action.""" + action = action.lower().strip() + if action == "stop": + stop_loco(loco, iface=iface) + elif action == "damp": + enter_damp(iface=iface, loco=loco) + elif action == "zero": + enter_zero_torque(iface=iface, loco=loco) + elif action == "start": + balance_stand(iface=iface, loco=loco) + elif action == "home": + go_home(iface=iface, release_after=False) + elif action == "status": + client = loco or init_loco(iface) + print(_fsm_status(client), flush=True) + else: + raise ValueError(f"unknown action {action!r}; choose from {ACTIONS}") + + +# --------------------------------------------------------------------------- +# CLI (no GUI) +# --------------------------------------------------------------------------- + + +def run_cli(iface: str | None, action: str | None = None) -> int: + """One-shot action, or interactive prompt loop.""" + loco = None + try: + loco = init_loco(iface) + print(f"killswitch CLI armed iface={iface or '(default)'} {_fsm_status(loco)}", flush=True) + except Exception as exc: # noqa: BLE001 + logger.warning("DDS connect deferred (%s); will retry on first command", exc) + + if action: + run_action(action, iface=iface, loco=loco) + return 0 + + print( + "Commands: stop | damp | zero | start | home | status | quit\n" + " stop = StopMove (stay standing)\n" + " damp = Damp (L2+B)\n" + " zero = ZeroTorque (L2+A — limp)\n" + " start = balancer Start\n" + " home = arms go home (slew-clamped)\n", + flush=True, + ) + while True: + try: + line = input("killswitch> ").strip() + except (EOFError, KeyboardInterrupt): + print(flush=True) + return 130 + if not line: + continue + cmd = line.split()[0].lower() + if cmd in ("q", "quit", "exit"): + return 0 + if cmd in ("help", "?"): + print("Commands:", ", ".join(ACTIONS), "| quit", flush=True) + continue + if cmd == "damp": + confirm = input("Enter Damp? [y/N] ").strip().lower() + if confirm not in ("y", "yes"): + continue + if cmd == "zero": + confirm = input("Enter ZeroTorque (limp)? Spotter ready? [y/N] ").strip().lower() + if confirm not in ("y", "yes"): + continue + try: + if loco is None: + loco = init_loco(iface) + run_action(cmd, iface=iface, loco=loco) + if loco is not None and cmd != "status": + print(_fsm_status(loco), flush=True) + except Exception as exc: # noqa: BLE001 + logger.exception("%s failed", cmd) + print(f"ERROR: {exc}", flush=True) + + +# --------------------------------------------------------------------------- +# GUI (tkinter) +# --------------------------------------------------------------------------- + def _require_tkinter(): try: @@ -44,8 +148,9 @@ def _require_tkinter(): from tkinter import messagebox, ttk except ModuleNotFoundError as exc: raise SystemExit( - "tkinter is required for the headed killswitch.\n" + "tkinter is required for --gui.\n" "Install: sudo apt-get install -y python3-tk\n" + "Or use the CLI (default): ./killswitch.sh --iface enp5s0\n" f"Original error: {exc}" ) from exc return tk, messagebox, ttk @@ -63,22 +168,20 @@ def __init__(self, iface: str | None) -> None: self.root = tk.Tk() self.root.title("G1 KILLSWITCH") self.root.configure(bg="#1a1a1a") - self.root.geometry("520x560") - self.root.minsize(480, 520) + self.root.geometry("520x640") + self.root.minsize(480, 600) - title = tk.Label( + tk.Label( self.root, text="G1 KILLSWITCH", font=("Helvetica", 28, "bold"), fg="#ff4444", bg="#1a1a1a", - ) - title.pack(pady=(16, 4)) + ).pack(pady=(16, 4)) - iface_text = iface or "(default DDS iface)" tk.Label( self.root, - text=f"iface: {iface_text}", + text=f"iface: {iface or '(default DDS iface)'}", font=("Helvetica", 11), fg="#aaaaaa", bg="#1a1a1a", @@ -101,6 +204,9 @@ def __init__(self, iface: str | None) -> None: self._mk_button(btn_frame, "STOP MOVE\n(stay standing)", "#cc8800", self._on_stop).pack( fill="x", pady=6 ) + self._mk_button(btn_frame, "GO HOME\n(arms → home pose)", "#2266aa", self._on_home).pack( + fill="x", pady=6 + ) self._mk_button(btn_frame, "DAMP\n(L2+B)", "#dd6622", self._on_damp).pack(fill="x", pady=6) self._mk_button(btn_frame, "ZERO TORQUE\n(L2+A — limp)", "#cc2222", self._on_zero).pack( fill="x", pady=6 @@ -112,7 +218,9 @@ def __init__(self, iface: str | None) -> None: ttk.Separator(self.root).pack(fill="x", padx=24, pady=8) tk.Label( self.root, - text="Keep this window open during demos.\nPhysical pendant still works in parallel.", + text="Keep this window open during demos.\n" + "CLI mode (no GUI): ./killswitch.sh --iface …\n" + "Physical pendant still works in parallel.", font=("Helvetica", 10), fg="#888888", bg="#1a1a1a", @@ -140,12 +248,10 @@ def _mk_button(self, parent, text: str, color: str, command): def _connect_async(self) -> None: def worker() -> None: try: - from soccerbot.safety import init_loco - loco = init_loco(self.iface) with self._lock: self._loco = loco - self._set_status("DDS connected — killswitch armed") + self._set_status(f"DDS connected — {_fsm_status(loco)}") except Exception as exc: # noqa: BLE001 logger.exception("killswitch connect failed") self._set_status(f"CONNECT FAILED: {exc}") @@ -164,7 +270,10 @@ def worker() -> None: loco = self._loco try: fn(loco) - self._set_status(f"{label} OK @ {time.strftime('%H:%M:%S')}") + with self._lock: + loco = self._loco + extra = f" {_fsm_status(loco)}" if loco is not None else "" + self._set_status(f"{label} OK @ {time.strftime('%H:%M:%S')}{extra}") except Exception as exc: # noqa: BLE001 logger.exception("%s failed", label) self._set_status(f"{label} FAILED: {exc}") @@ -177,6 +286,14 @@ def worker() -> None: def _on_stop(self) -> None: self._with_loco(lambda loco: stop_loco(loco, iface=self.iface), "STOP MOVE") + def _on_home(self) -> None: + if not self._messagebox.askokcancel( + "GO HOME", + "Move arms to home pose (slew-clamped)?\nSpotter should be ready.", + ): + return + self._with_loco(lambda _loco: go_home(iface=self.iface, release_after=False), "GO HOME") + def _on_damp(self) -> None: if not self._messagebox.askokcancel("DAMP", "Enter Damp mode? Robot will go passive."): return @@ -199,12 +316,7 @@ def worker() -> None: loco = self._loco if loco is None: return - try: - code, fsm_id = loco.GetFsmId() - name = FSM_NAMES.get(int(fsm_id), f"fsm {fsm_id}") - self._set_status(f"FSM={fsm_id} ({name}) rpc={code}") - except Exception: # noqa: BLE001 - pass + self._set_status(_fsm_status(loco)) threading.Thread(target=worker, daemon=True).start() self.root.after(1000, self._poll_fsm) @@ -213,13 +325,37 @@ def run(self) -> None: self.root.mainloop() +def run_gui(iface: str | None) -> int: + KillswitchApp(iface).run() + return 0 + + +# --------------------------------------------------------------------------- +# Entry +# --------------------------------------------------------------------------- + + def parse_args(argv: list[str] | None = None) -> argparse.Namespace: - p = argparse.ArgumentParser(description="Headed G1 killswitch panel.") + p = argparse.ArgumentParser( + description="G1 killswitch (CLI by default; --gui for Tk panel).", + ) p.add_argument( "--iface", default=None, help="DDS network interface (e.g. enp5s0). Omit for SDK default.", ) + p.add_argument( + "--gui", + action="store_true", + help="Open the Tk killswitch window (needs python3-tk).", + ) + p.add_argument( + "action", + nargs="?", + default=None, + choices=list(ACTIONS), + help="Optional one-shot CLI action (stop/damp/zero/start/home/status).", + ) return p.parse_args(argv) @@ -230,10 +366,13 @@ def main(argv: list[str] | None = None) -> int: ) args = parse_args(argv) try: - KillswitchApp(args.iface).run() + if args.gui: + if args.action: + logger.warning("Ignoring one-shot action %r in --gui mode", args.action) + return run_gui(args.iface) + return run_cli(args.iface, action=args.action) except KeyboardInterrupt: return 130 - return 0 if __name__ == "__main__": diff --git a/soccerbot/src/soccerbot/main.py b/soccerbot/src/soccerbot/main.py index 346b67e..10fd20f 100644 --- a/soccerbot/src/soccerbot/main.py +++ b/soccerbot/src/soccerbot/main.py @@ -13,7 +13,8 @@ 4. THROW — relative push (slew-clamped) Ctrl+C runs a graceful reset (StopMove + release arm_sdk). Keep -``./killswitch.sh`` open in another terminal for headed Damp / ZeroTorque. +``./killswitch.sh`` (CLI) or ``./killswitch.sh --gui`` open for Stop / Home / +Damp / ZeroTorque. """ from __future__ import annotations @@ -160,7 +161,10 @@ def main(argv: list[str] | None = None) -> int: cfg.iface, cfg.rerun, ) - logger.info("Tip: keep ./killswitch.sh --iface %s open in another terminal", cfg.iface or "") + logger.info( + "Tip: keep ./killswitch.sh --iface %s (or --gui) open in another terminal", + cfg.iface or "", + ) t0 = time.time() try: From eb8757cbf8ad5d4078137ac23d8fb60e87226614 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 15:07:02 +0000 Subject: [PATCH 4/9] Harden safety: URDF limit clamp at choke point, emergency-first damp/zero - G1Arms.send_arm_positions hard-clamps every commanded q to URDF limits (single choke point for ACT / replay / holds / throw / home) - Damp & ZeroTorque switch FSM first, arm cleanup after; temp release publisher uses 3s lowstate timeout so emergencies never stall - throw phases chain from the actual last command (no step-jump when the slew clamp lags the blend); verified with fake-arms delta test - go-home: no-ramp weight=1 engage (avoids re-engage jerk), home_pose.json validated against URDF limits at load - Rerun telemetry disables itself on any error instead of breaking control Co-authored-by: arjuncoder1 --- local-vla-inference/g1_arms.py | 42 ++++++++++++++++++++-- local-vla-inference/telemetry.py | 52 ++++++++++++++++----------- scripted-behavior/throw.py | 20 +++++++---- soccerbot/README.md | 19 +++++++--- soccerbot/src/soccerbot/home.py | 21 ++++++++--- soccerbot/src/soccerbot/killswitch.py | 4 +++ soccerbot/src/soccerbot/safety.py | 22 +++++++----- 7 files changed, 135 insertions(+), 45 deletions(-) diff --git a/local-vla-inference/g1_arms.py b/local-vla-inference/g1_arms.py index d3e0c6d..2c26c91 100644 --- a/local-vla-inference/g1_arms.py +++ b/local-vla-inference/g1_arms.py @@ -37,6 +37,26 @@ WEIGHT_JOINT = 29 # kNotUsedJoint: q = 1 enables arm_sdk, 0 releases it +# Arm joint limits (rad) from the Unitree g1_description 29-DoF URDF. +# Enforced as an absolute position clamp in send_arm_positions() — the single +# choke point every arm command path goes through (ACT, replay, throw, home). +ARM_JOINT_LIMITS: dict[str, tuple[float, float]] = { + "kLeftShoulderPitch": (-3.0892, 2.6704), + "kLeftShoulderRoll": (-1.5882, 2.2515), + "kLeftShoulderYaw": (-2.618, 2.618), + "kLeftElbow": (-1.0472, 2.0944), + "kLeftWristRoll": (-1.9722, 1.9722), + "kLeftWristPitch": (-1.6144, 1.6144), + "kLeftWristYaw": (-1.6144, 1.6144), + "kRightShoulderPitch": (-3.0892, 2.6704), + "kRightShoulderRoll": (-2.2515, 1.5882), + "kRightShoulderYaw": (-2.618, 2.618), + "kRightElbow": (-1.0472, 2.0944), + "kRightWristRoll": (-1.9722, 1.9722), + "kRightWristPitch": (-1.6144, 1.6144), + "kRightWristYaw": (-1.6144, 1.6144), +} + # Legs + waist yaw (read-only here; never commanded). Used for diagnosis # logging to see the balance controller react (e.g. sidestepping). LEG_JOINT_INDEX: dict[str, int] = { @@ -160,20 +180,38 @@ def get_full_snapshot(self) -> dict[str, float]: return snap def send_arm_positions(self, action: dict[str, float], weight: float = 1.0) -> None: - """Publish arm joint targets. ``action`` keys are '.q'.""" + """Publish arm joint targets. ``action`` keys are '.q'. + + Every commanded position is hard-clamped to the real URDF joint limits + (``ARM_JOINT_LIMITS``) — last line of defense regardless of which caller + (policy, replay, throw, home) produced the target. + """ if self._publisher is None: raise RuntimeError("arm_sdk publisher not available (connected state_only)") cmd = self._cmd cmd.motor_cmd[WEIGHT_JOINT].q = float(np.clip(weight, 0.0, 1.0)) + limit_hits = 0 for name, idx in ARM_JOINT_INDEX.items(): key = f"{name}.q" if key not in action: continue - cmd.motor_cmd[idx].q = float(action[key]) + q = float(action[key]) + lo, hi = ARM_JOINT_LIMITS[name] + q_clamped = min(hi, max(lo, q)) + if q_clamped != q: + limit_hits += 1 + cmd.motor_cmd[idx].q = q_clamped cmd.motor_cmd[idx].dq = 0.0 cmd.motor_cmd[idx].tau = 0.0 cmd.motor_cmd[idx].kp = self.kp cmd.motor_cmd[idx].kd = self.kd + if limit_hits: + now = time.monotonic() + if now - getattr(self, "_last_limit_warn", 0.0) > 1.0: + self._last_limit_warn = now + logger.warning( + "send_arm_positions: %d joint target(s) clamped to URDF limits", limit_hits + ) # arm_sdk controls waist joints (12-14) in addition to arm joints (15-28) # per the official g1_arm7_sdk_dds_example. Always hold them at the current # measured position with full stiffness so they don't go limp when arm_sdk diff --git a/local-vla-inference/telemetry.py b/local-vla-inference/telemetry.py index 3514840..ef8d70f 100644 --- a/local-vla-inference/telemetry.py +++ b/local-vla-inference/telemetry.py @@ -108,19 +108,26 @@ def log_step( ) -> None: if self._rr is None: return - self.set_time(step=step, seconds=elapsed_s) - if rgb is not None: - self.log_image(f"{stage}/camera/rgb", rgb) - if depth is not None: - self.log_image(f"{stage}/camera/depth", depth, compress=False) - if measured: - self.log_scalars(f"{stage}/arm/measured", _strip_q_suffix(measured)) - if target: - self.log_scalars(f"{stage}/arm/target", _strip_q_suffix(target)) - if commanded: - self.log_scalars(f"{stage}/arm/commanded", _strip_q_suffix(commanded)) - if extras: - self.log_scalars(f"{stage}/stats", extras) + # Telemetry must NEVER break the control loop: any viz failure just + # disables further logging for this session. + try: + self.set_time(step=step, seconds=elapsed_s) + if rgb is not None: + self.log_image(f"{stage}/camera/rgb", rgb) + if depth is not None: + self.log_image(f"{stage}/camera/depth", depth, compress=False) + if measured: + self.log_scalars(f"{stage}/arm/measured", _strip_q_suffix(measured)) + if target: + self.log_scalars(f"{stage}/arm/target", _strip_q_suffix(target)) + if commanded: + self.log_scalars(f"{stage}/arm/commanded", _strip_q_suffix(commanded)) + if extras: + self.log_scalars(f"{stage}/stats", extras) + except Exception as exc: # noqa: BLE001 -- viz must not affect control + logger.warning("Rerun log_step failed (%s); disabling telemetry", exc) + self._rr = None + self.enabled = False def log_detection( self, @@ -134,13 +141,18 @@ def log_detection( ) -> None: if self._rr is None: return - self.set_time(step=step, seconds=elapsed_s) - if rgb is not None: - self.log_image(f"{stage}/camera/rgb", rgb) - extras: dict[str, float] = {"n_people": float(n_people)} - if nearest_m is not None: - extras["nearest_m"] = float(nearest_m) - self.log_scalars(f"{stage}/detect", extras) + try: + self.set_time(step=step, seconds=elapsed_s) + if rgb is not None: + self.log_image(f"{stage}/camera/rgb", rgb) + extras: dict[str, float] = {"n_people": float(n_people)} + if nearest_m is not None: + extras["nearest_m"] = float(nearest_m) + self.log_scalars(f"{stage}/detect", extras) + except Exception as exc: # noqa: BLE001 -- viz must not affect control + logger.warning("Rerun log_detection failed (%s); disabling telemetry", exc) + self._rr = None + self.enabled = False def _strip_q_suffix(joints: dict[str, float]) -> dict[str, float]: diff --git a/scripted-behavior/throw.py b/scripted-behavior/throw.py index 4680080..6f5590c 100644 --- a/scripted-behavior/throw.py +++ b/scripted-behavior/throw.py @@ -191,11 +191,13 @@ def _interpolate_to( duration_s: float, control_dt: float = 0.02, slew_clamp: float = THROW_SLEW_CLAMP, -) -> None: +) -> dict[str, float]: """Linearly interpolate the 14 arm joints from ``start`` to ``target``. Each tick is also slew-rate limited so a short ``duration_s`` cannot command - an unsafe jump even if the start/target gap is large. + an unsafe jump even if the start/target gap is large. Returns the FINAL + commanded pose — callers must chain it as the next phase's start so the + command trajectory has no discontinuity when the clamp lagged the blend. """ num_steps = max(1, int(duration_s / control_dt)) cmd = dict(start) @@ -210,11 +212,12 @@ def _interpolate_to( delta = slew_clamp if delta > 0 else -slew_clamp cmd[key] = cmd[key] + delta else: - cmd = desired + cmd = dict(desired) arms.send_arm_positions(cmd) elapsed = time.time() - step_start sleep_s = max(0.0, control_dt - elapsed) time.sleep(sleep_s) + return cmd def throw(arms, *, slew_clamp: float = THROW_SLEW_CLAMP) -> None: @@ -238,14 +241,17 @@ def throw(arms, *, slew_clamp: float = THROW_SLEW_CLAMP) -> None: lo, hi = (-1.6144, 1.6144) follow_through_pose[key] = max(lo, min(hi, release_pose[key] + _FOLLOW_THROUGH_EXTRA_WRIST_PITCH)) + # Chain each phase from the ACTUAL last commanded pose (the slew clamp can + # lag the blend); starting the next phase from the nominal waypoint would + # step-jump the position target by the accumulated lag. logger.info("-> release (%.2fs)", RELEASE_DURATION_S) - _interpolate_to(arms, start_pose, release_pose, RELEASE_DURATION_S, slew_clamp=slew_clamp) + cmd = _interpolate_to(arms, start_pose, release_pose, RELEASE_DURATION_S, slew_clamp=slew_clamp) logger.info("-> follow_through (%.2fs)", FOLLOW_THROUGH_DURATION_S) - _interpolate_to( - arms, release_pose, follow_through_pose, FOLLOW_THROUGH_DURATION_S, slew_clamp=slew_clamp + cmd = _interpolate_to( + arms, cmd, follow_through_pose, FOLLOW_THROUGH_DURATION_S, slew_clamp=slew_clamp ) logger.info("-> recover (%.2fs)", RECOVER_DURATION_S) - _interpolate_to(arms, follow_through_pose, start_pose, RECOVER_DURATION_S, slew_clamp=slew_clamp) + _interpolate_to(arms, cmd, start_pose, RECOVER_DURATION_S, slew_clamp=slew_clamp) logger.info("Push complete.") diff --git a/soccerbot/README.md b/soccerbot/README.md index 4df1a2c..e0d1263 100644 --- a/soccerbot/README.md +++ b/soccerbot/README.md @@ -37,10 +37,21 @@ Defaults match the validated local ACT command: ## Safety -- Every arm command path is slew-clamped (ACT + replay + throw + go-home). -- **Ctrl+C** → graceful reset: `LocoClient.StopMove()` + release `arm_sdk`. -- **`./killswitch.sh`** → CLI (default) or `--gui`: Stop / Go Home / Damp / Zero Torque / Start. -- Home pose defaults to Unitree zeros; override via `scripted-behavior/home_pose.json`. +- **URDF hard limit clamp at the choke point**: `G1Arms.send_arm_positions` clamps + every commanded joint to the real G1 URDF limits — applies to ALL callers + (ACT, replay, arm holds, throw, go-home) regardless of what they compute. +- Every arm command path is additionally slew-clamped (ACT 0.002 rad/step + + replay + throw + go-home), and throw phases chain from the actual last + command so the clamp can never cause a position step-jump. +- **Ctrl+C** → graceful reset: `LocoClient.StopMove()` + release `arm_sdk` + (opens a short-timeout temp publisher if the interrupted stage held none). +- **`./killswitch.sh`** → CLI (default) or `--gui`: Stop / Go Home / Damp / + Zero Torque / Start. Damp / ZeroTorque switch the FSM FIRST, then clean up + the arm overlay — an emergency never waits on an arm connect. +- Home pose defaults to Unitree zeros; override via + `scripted-behavior/home_pose.json` (validated against URDF limits at load). +- Rerun telemetry is fail-safe: any viz error disables logging, never the + control loop. ## Diagnose diff --git a/soccerbot/src/soccerbot/home.py b/soccerbot/src/soccerbot/home.py index 808e4f2..0f42842 100644 --- a/soccerbot/src/soccerbot/home.py +++ b/soccerbot/src/soccerbot/home.py @@ -46,9 +46,8 @@ DEFAULT_HOME_Q: dict[str, float] = {f"{name}.q": 0.0 for name in ARM_JOINTS} DEFAULT_HOME_JSON = REPO_ROOT / "scripted-behavior" / "home_pose.json" -HOME_SLEW_CLAMP = 0.02 # rad/step +HOME_SLEW_CLAMP = 0.02 # rad/step (@50 Hz → max 1 rad/s per joint) HOME_CONTROL_DT = 0.02 -HOME_ENGAGE_RAMP_S = 1.0 def load_home_pose(path: Path | None = None) -> dict[str, float]: @@ -98,17 +97,31 @@ def go_home( _ensure_front(LOCAL_VLA_DIR) from g1_arms import G1Arms + from g1_arms import ARM_JOINT_LIMITS + target = pose if pose is not None else load_home_pose(pose_path) missing = [k for k in DEFAULT_HOME_Q if k not in target] if missing: raise ValueError(f"home pose missing joints: {missing}") + # Refuse out-of-limit custom poses up front (send_arm_positions would clamp + # them anyway, but a bad home_pose.json should fail loudly, not silently). + for name, (lo, hi) in ARM_JOINT_LIMITS.items(): + q = float(target[f"{name}.q"]) + if not (lo <= q <= hi): + raise ValueError( + f"home pose joint {name}={q:.3f} outside URDF limits [{lo:.3f}, {hi:.3f}]" + ) arms = G1Arms(kp=60.0, kd=1.5) arms.connect() try: start = arms.get_arm_positions() - # Engage without yanking: ramp weight while holding current pose. - arms.hold_current_pose(ramp_s=HOME_ENGAGE_RAMP_S) + # Engage without the 0->1 weight ramp: if arm_sdk is already engaged + # (e.g. mid-demo), ramping from 0 hands the arms back to the balancer + # for ~0.5s and causes a visible jerk (see HANDOVER §5). A single + # weight=1 publish is a no-op when engaged and an instant + # engage-at-current-pose otherwise. + arms.send_arm_positions(start, weight=1.0) # Estimate duration from max joint delta if not provided. if duration_s is None: diff --git a/soccerbot/src/soccerbot/killswitch.py b/soccerbot/src/soccerbot/killswitch.py index 5b279ec..e0720a1 100644 --- a/soccerbot/src/soccerbot/killswitch.py +++ b/soccerbot/src/soccerbot/killswitch.py @@ -126,6 +126,10 @@ def run_cli(iface: str | None, action: str | None = None) -> int: confirm = input("Enter ZeroTorque (limp)? Spotter ready? [y/N] ").strip().lower() if confirm not in ("y", "yes"): continue + if cmd == "home": + confirm = input("Move arms to home pose (slew-clamped)? [y/N] ").strip().lower() + if confirm not in ("y", "yes"): + continue try: if loco is None: loco = init_loco(iface) diff --git a/soccerbot/src/soccerbot/safety.py b/soccerbot/src/soccerbot/safety.py index 947d725..e392464 100644 --- a/soccerbot/src/soccerbot/safety.py +++ b/soccerbot/src/soccerbot/safety.py @@ -66,7 +66,8 @@ def release_arms(arms: Any | None = None, *, iface: str | None = None) -> None: _ensure_front(LOCAL_VLA_DIR) from g1_arms import G1Arms - arms = G1Arms(kp=60.0, kd=1.5) + # Short state timeout: never stall a safety path waiting on lowstate. + arms = G1Arms(kp=60.0, kd=1.5, state_timeout_s=3.0) arms.connect() owned = True except Exception as exc: # noqa: BLE001 @@ -115,30 +116,35 @@ def graceful_reset( def enter_damp(*, iface: str | None = None, loco: Any | None = None) -> None: - """Passive damp mode (pendant L2+B). Soft-falls the robot if unsupported.""" + """Passive damp mode (pendant L2+B). Soft-falls the robot if unsupported. + + Emergency ordering: the FSM change happens FIRST (immediate), then arm_sdk + cleanup best-effort — never block the emergency on an arm connect/ramp. + """ client = loco or init_loco(iface) - stop_loco(client) try: - # Best-effort: release arm overlay before going passive. - release_arms(iface=iface) rc = client.Damp() logger.warning("LocoClient.Damp() -> %s (passive damping)", rc) except Exception as exc: # noqa: BLE001 logger.error("Damp failed: %s", exc) raise + # In Damp the firmware ignores arm_sdk anyway; this just tidies our overlay. + release_arms(iface=iface) def enter_zero_torque(*, iface: str | None = None, loco: Any | None = None) -> None: - """Zero-torque mode (pendant L2+A). Robot goes fully limp — spotter ready.""" + """Zero-torque mode (pendant L2+A). Robot goes fully limp — spotter ready. + + Emergency ordering: ZeroTorque FIRST, arm cleanup after (best-effort). + """ client = loco or init_loco(iface) - stop_loco(client) try: - release_arms(iface=iface) rc = client.ZeroTorque() logger.warning("LocoClient.ZeroTorque() -> %s (motors limp)", rc) except Exception as exc: # noqa: BLE001 logger.error("ZeroTorque failed: %s", exc) raise + release_arms(iface=iface) def balance_stand(*, iface: str | None = None, loco: Any | None = None) -> None: From 35dbcc571dd48b9a1bf5934b6734be7f9f1c680e Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 15:08:10 +0000 Subject: [PATCH 5/9] Loosen throw slew clamp to 0.06 rad/step (3 rad/s) so the push keeps its dynamics; clamp stays as a garbage-target net Co-authored-by: arjuncoder1 --- scripted-behavior/throw.py | 7 +++++-- soccerbot/src/soccerbot/config.py | 4 +++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/scripted-behavior/throw.py b/scripted-behavior/throw.py index 6f5590c..d707a08 100644 --- a/scripted-behavior/throw.py +++ b/scripted-behavior/throw.py @@ -180,8 +180,11 @@ def _push_target(current_pose: dict[str, float]) -> dict[str, float]: } -# Default per-tick slew limit while interpolating the throw (rad/step @ 50 Hz). -THROW_SLEW_CLAMP = 0.02 +# Default per-tick slew limit while interpolating the throw (rad/step @ 50 Hz +# = 3 rad/s per joint). Deliberately loose: the release phase needs ~1.7 rad/s +# to actually push the ball, so this only catches garbage targets — it is a +# safety net, not a damper. The URDF limit clamp in G1Arms still applies. +THROW_SLEW_CLAMP = 0.06 def _interpolate_to( diff --git a/soccerbot/src/soccerbot/config.py b/soccerbot/src/soccerbot/config.py index 6805e69..f47be07 100644 --- a/soccerbot/src/soccerbot/config.py +++ b/soccerbot/src/soccerbot/config.py @@ -40,5 +40,7 @@ class OrchestratorConfig: replay_trajectory: Path = DEFAULT_REPLAY_TRAJECTORY # Scripted-stage slew clamps (rad/frame). replay_slew_clamp: float = 0.05 - throw_slew_clamp: float = 0.02 + # Loose on purpose: the throw needs ~1.7 rad/s to work; 0.06 @50Hz = 3 rad/s + # only catches garbage targets (see throw.THROW_SLEW_CLAMP). + throw_slew_clamp: float = 0.06 pickup_extra_args: list[str] = field(default_factory=list) From 45389dcc1414a8915d499ed16f864a6af93d7ab9 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 15:17:58 +0000 Subject: [PATCH 6/9] Remove Rerun telemetry integration (scrapped for now): drop telemetry.py, --rerun flags, lerobot[viz] deps, and diagnose check Co-authored-by: arjuncoder1 --- diagnose.sh | 8 -- local-vla-inference/main.py | 38 ------ local-vla-inference/pyproject.toml | 3 +- local-vla-inference/telemetry.py | 201 ----------------------------- scripted-behavior/pickup.py | 3 +- soccerbot/README.md | 4 +- soccerbot/pyproject.toml | 2 +- soccerbot/src/soccerbot/config.py | 1 - soccerbot/src/soccerbot/main.py | 5 +- soccerbot/src/soccerbot/pickup.py | 1 - uv.lock | 8 +- 11 files changed, 9 insertions(+), 265 deletions(-) delete mode 100644 local-vla-inference/telemetry.py diff --git a/diagnose.sh b/diagnose.sh index 0517b62..0096dad 100755 --- a/diagnose.sh +++ b/diagnose.sh @@ -91,14 +91,6 @@ PY else soft "soccerbot import via src path failed (uv sync may still be needed)" fi - if "$VENV_DIR/bin/python" - <<'PY' -import rerun # noqa: F401 -print("rerun ok") -PY - then ok "rerun-sdk (viz)" - else soft "rerun-sdk missing — ACT will run without live viz (lerobot[viz])" - fi - if "$VENV_DIR/bin/python" - <<'PY' import sys from pathlib import Path diff --git a/local-vla-inference/main.py b/local-vla-inference/main.py index fc50b0c..fbdb98f 100644 --- a/local-vla-inference/main.py +++ b/local-vla-inference/main.py @@ -129,16 +129,6 @@ def parse_args(argv: list[str] | None = None) -> argparse.Namespace: help="CSV log of every step (measured, policy target, emitted command per joint). " "Default: act_log_.csv in the current directory.", ) - p.add_argument( - "--rerun", - action="store_true", - help="Spawn a Rerun viewer and stream teleimager RGB + arm target/cmd/measured.", - ) - p.add_argument( - "--no-rerun", - action="store_true", - help="Disable Rerun even if the caller defaulted it on.", - ) p.add_argument( "--dry-run", action="store_true", @@ -170,7 +160,6 @@ def build_args( duration: float = 30.0, fps: float = 30.0, device: str | None = None, - rerun: bool = True, leave_arms_engaged: bool = True, dry_run: bool = False, image_no_motors: bool = False, @@ -188,8 +177,6 @@ def build_args( duration=duration, fps=fps, device=device, - rerun=rerun, - no_rerun=not rerun, dry_run=dry_run, image_no_motors=image_no_motors, leave_arms_engaged=leave_arms_engaged, @@ -369,8 +356,6 @@ def run(args: argparse.Namespace) -> None: from lerobot.policies.act import ACTPolicy from lerobot.policies.utils import build_inference_frame, make_robot_action - from telemetry import Telemetry - if args.dry_run and args.image_no_motors: raise SystemExit("Use either --dry-run or --image-no-motors, not both.") @@ -421,10 +406,6 @@ def run(args: argparse.Namespace) -> None: # Engage arm_sdk smoothly at the current pose before the policy takes over. arms.hold_current_pose(ramp_s=2.0) - rerun_on = bool(getattr(args, "rerun", False)) and not bool(getattr(args, "no_rerun", False)) - telemetry = Telemetry(enabled=rerun_on, session_name="soccerbot-act") - telemetry.start() - h, w, _ = layout.IMAGE_SHAPE dt = 1.0 / args.fps t0 = time.time() @@ -514,24 +495,6 @@ def run(args: argparse.Namespace) -> None: + [round(snapshot[k], 5) for k in snapshot_keys] ) - telemetry.log_step( - step=step, - elapsed_s=elapsed, - rgb=front_rgb, - measured=measured, - target=dds_action, - commanded=cmd_q, - extras={ - "clamp_hits": float(clamp_hits), - "max_target_gap": float(max_gap), - "cam_ms": float(cam_ms), - "policy_ms": float(policy_ms), - "imu_pitch": float(snapshot.get("imu.pitch", 0.0)), - "imu_roll": float(snapshot.get("imu.roll", 0.0)), - }, - stage="pickup", - ) - step += 1 if step % int(args.fps) == 0: worst = arm_keys[int(np.argmax(gaps))] @@ -560,7 +523,6 @@ def run(args: argparse.Namespace) -> None: finally: log_file.close() logger.info("Step log written to %s (%d steps)", log_path, step) - telemetry.stop() if not interrupted: try: front.disconnect() diff --git a/local-vla-inference/pyproject.toml b/local-vla-inference/pyproject.toml index f5a12e2..e5fb708 100644 --- a/local-vla-inference/pyproject.toml +++ b/local-vla-inference/pyproject.toml @@ -6,7 +6,7 @@ readme = "README.md" # cyclonedds 0.10.2 (unitree_sdk2py) needs Python <3.13. Sync the repo root with 3.12. requires-python = ">=3.12" dependencies = [ - "lerobot[viz]", + "lerobot", "pyzmq>=26.0.0", "unitree_sdk2py @ git+https://github.com/unitreerobotics/unitree_sdk2_python.git", ] @@ -20,7 +20,6 @@ py-modules = [ "main", "g1_arms", "front_camera", - "telemetry", "dds_init", "embodiment_g1_14d", "embodiment_g1d_16d", diff --git a/local-vla-inference/telemetry.py b/local-vla-inference/telemetry.py deleted file mode 100644 index ef8d70f..0000000 --- a/local-vla-inference/telemetry.py +++ /dev/null @@ -1,201 +0,0 @@ -"""Rerun telemetry helpers for live ACT / scripted G1 control. - -Logs teleimager RGB (the existing single-port JPEG stream), optional depth -when a caller already has it (e.g. local RealSense HumanDetector — teleimager -itself publishes color JPEGs only), measured/commanded arm joints, policy -targets, clamp hits, and timing. Safe no-op when Rerun is unavailable or -disabled. -""" - -from __future__ import annotations - -import logging -from typing import Any - -import numpy as np - -logger = logging.getLogger(__name__) - - -class Telemetry: - """Thin wrapper around the Rerun SDK (optional).""" - - def __init__(self, enabled: bool = True, session_name: str = "soccerbot") -> None: - self.enabled = enabled - self.session_name = session_name - self._rr = None - self._step = 0 - - def start(self) -> None: - if not self.enabled: - return - try: - from lerobot.utils.visualization_utils import init_rerun - - init_rerun(session_name=self.session_name) - import rerun as rr - - self._rr = rr - logger.info("Rerun telemetry started (session=%s)", self.session_name) - except Exception as exc: # noqa: BLE001 -- viz is optional on the robot - logger.warning("Rerun unavailable (%s); continuing without visualization", exc) - self.enabled = False - self._rr = None - - def stop(self) -> None: - if not self.enabled or self._rr is None: - return - try: - from lerobot.utils.visualization_utils import shutdown_rerun - - shutdown_rerun() - except Exception as exc: # noqa: BLE001 - logger.debug("Rerun shutdown failed: %s", exc) - self._rr = None - - def set_time(self, step: int | None = None, seconds: float | None = None) -> None: - if self._rr is None: - return - if step is not None: - self._step = int(step) - self._rr.set_time("step", sequence=self._step) - if seconds is not None: - self._rr.set_time("time", timestamp=float(seconds)) - - def log_image(self, path: str, image: np.ndarray, *, compress: bool = True) -> None: - if self._rr is None or image is None: - return - arr = np.asarray(image) - if arr.ndim == 2: - # Depth / mono: log as depth image when uint16, else grayscale. - if arr.dtype == np.uint16: - self._rr.log(path, self._rr.DepthImage(arr)) - else: - self._rr.log(path, self._rr.Image(arr)) - return - if arr.ndim == 3 and arr.shape[-1] == 3: - if compress: - try: - self._rr.log(path, self._rr.Image(arr).compress(jpeg_quality=75)) - return - except Exception: # noqa: BLE001 - pass - self._rr.log(path, self._rr.Image(arr)) - return - logger.debug("Skipping unsupported image shape %s at %s", arr.shape, path) - - def log_scalars(self, path: str, values: dict[str, float]) -> None: - if self._rr is None: - return - for key, value in values.items(): - try: - self._rr.log(f"{path}/{key}", self._rr.Scalars(float(value))) - except Exception: # noqa: BLE001 - continue - - def log_step( - self, - *, - step: int, - elapsed_s: float, - rgb: np.ndarray | None = None, - depth: np.ndarray | None = None, - measured: dict[str, float] | None = None, - target: dict[str, float] | None = None, - commanded: dict[str, float] | None = None, - extras: dict[str, float] | None = None, - stage: str = "pickup", - ) -> None: - if self._rr is None: - return - # Telemetry must NEVER break the control loop: any viz failure just - # disables further logging for this session. - try: - self.set_time(step=step, seconds=elapsed_s) - if rgb is not None: - self.log_image(f"{stage}/camera/rgb", rgb) - if depth is not None: - self.log_image(f"{stage}/camera/depth", depth, compress=False) - if measured: - self.log_scalars(f"{stage}/arm/measured", _strip_q_suffix(measured)) - if target: - self.log_scalars(f"{stage}/arm/target", _strip_q_suffix(target)) - if commanded: - self.log_scalars(f"{stage}/arm/commanded", _strip_q_suffix(commanded)) - if extras: - self.log_scalars(f"{stage}/stats", extras) - except Exception as exc: # noqa: BLE001 -- viz must not affect control - logger.warning("Rerun log_step failed (%s); disabling telemetry", exc) - self._rr = None - self.enabled = False - - def log_detection( - self, - *, - step: int, - elapsed_s: float, - rgb: np.ndarray | None, - nearest_m: float | None, - n_people: int, - stage: str = "avoid", - ) -> None: - if self._rr is None: - return - try: - self.set_time(step=step, seconds=elapsed_s) - if rgb is not None: - self.log_image(f"{stage}/camera/rgb", rgb) - extras: dict[str, float] = {"n_people": float(n_people)} - if nearest_m is not None: - extras["nearest_m"] = float(nearest_m) - self.log_scalars(f"{stage}/detect", extras) - except Exception as exc: # noqa: BLE001 -- viz must not affect control - logger.warning("Rerun log_detection failed (%s); disabling telemetry", exc) - self._rr = None - self.enabled = False - - -def _strip_q_suffix(joints: dict[str, float]) -> dict[str, float]: - out: dict[str, float] = {} - for key, value in joints.items(): - name = key.removesuffix(".q") - out[name] = float(value) - return out - - -def apply_slew_clamp( - cmd_q: dict[str, float], - target_q: dict[str, float], - keys: list[str], - clamp_rad: float, -) -> tuple[dict[str, float], int]: - """Move ``cmd_q`` toward ``target_q`` by at most ``clamp_rad`` per joint. - - Returns the updated command dict and the number of joints that hit the clamp. - """ - if clamp_rad <= 0: - updated = {k: float(target_q[k]) for k in keys} - return updated, 0 - - hits = 0 - updated = dict(cmd_q) - for key in keys: - delta = float(target_q[key]) - float(updated[key]) - if abs(delta) > clamp_rad: - hits += 1 - updated[key] = float(updated[key] + float(np.clip(delta, -clamp_rad, clamp_rad))) - return updated, hits - - -def namespace_to_observation( - measured: dict[str, float], - rgb: np.ndarray | None, - depth: np.ndarray | None = None, -) -> dict[str, Any]: - """Build a LeRobot-style observation dict for ``log_rerun_data`` fallbacks.""" - obs: dict[str, Any] = dict(measured) - if rgb is not None: - obs["images.front"] = rgb - if depth is not None: - obs["images.depth"] = depth - return obs diff --git a/scripted-behavior/pickup.py b/scripted-behavior/pickup.py index 184df65..d889ef1 100644 --- a/scripted-behavior/pickup.py +++ b/scripted-behavior/pickup.py @@ -1,7 +1,7 @@ """Stage 1: run the learned ACT pickup policy in-process (or replay a trajectory). Delegates to ``local-vla-inference`` via import (no subprocess) so slew -clamping, Ctrl+C graceful reset, and Rerun telemetry stay in one process. +clamping and Ctrl+C graceful reset stay in one process. A third backend, ``replay``, streams a pre-recorded arm-qpos trajectory from ``trajectories/`` over ``rt/arm_sdk``. @@ -102,7 +102,6 @@ def run_pickup_policy(cfg: OrchestratorConfig) -> None: clamp=clamp, duration=cfg.pickup_duration_s, leave_arms_engaged=True, - rerun=True, ) logger.info( "Starting in-process ACT pickup: policy=%s clamp=%.3f camera=%s", diff --git a/soccerbot/README.md b/soccerbot/README.md index e0d1263..a4418b5 100644 --- a/soccerbot/README.md +++ b/soccerbot/README.md @@ -7,7 +7,7 @@ in sibling packages that soccerbot imports in-process: | Dependency | Role | |---|---| -| `local-vla-inference/` | ACT pickup (`ajkoder/g1-pickup-ball-act`), slew clamp, teleimager RGB, Rerun | +| `local-vla-inference/` | ACT pickup (`ajkoder/g1-pickup-ball-act`), slew clamp, teleimager RGB | | `scripted-behavior/` | Turn 180°, avoid/shuffle, throw, JSON trajectory replay | ## Run @@ -50,8 +50,6 @@ Defaults match the validated local ACT command: the arm overlay — an emergency never waits on an arm connect. - Home pose defaults to Unitree zeros; override via `scripted-behavior/home_pose.json` (validated against URDF limits at load). -- Rerun telemetry is fail-safe: any viz error disables logging, never the - control loop. ## Diagnose diff --git a/soccerbot/pyproject.toml b/soccerbot/pyproject.toml index b100e16..a893ba2 100644 --- a/soccerbot/pyproject.toml +++ b/soccerbot/pyproject.toml @@ -7,7 +7,7 @@ requires-python = ">=3.12" dependencies = [ "local-vla-inference", "scripted-behavior", - "lerobot[viz]", + "lerobot", ] [project.scripts] diff --git a/soccerbot/src/soccerbot/config.py b/soccerbot/src/soccerbot/config.py index f47be07..f339f0e 100644 --- a/soccerbot/src/soccerbot/config.py +++ b/soccerbot/src/soccerbot/config.py @@ -34,7 +34,6 @@ class OrchestratorConfig: pickup_duration_s: float = 30.0 fps: float = 30.0 device: str | None = None - rerun: bool = True teleimager_host: str = "192.168.123.164" remote_server: str | None = None replay_trajectory: Path = DEFAULT_REPLAY_TRAJECTORY diff --git a/soccerbot/src/soccerbot/main.py b/soccerbot/src/soccerbot/main.py index 10fd20f..0336f7a 100644 --- a/soccerbot/src/soccerbot/main.py +++ b/soccerbot/src/soccerbot/main.py @@ -111,7 +111,6 @@ def parse_args(argv: list[str] | None = None) -> OrchestratorConfig: default=None, help="JSON path for --backend replay (default: pickup_ep148_prod2.json).", ) - p.add_argument("--no-rerun", action="store_true", help="Disable Rerun visualization.") p.add_argument( "--dry-run-config", action="store_true", @@ -133,7 +132,6 @@ def parse_args(argv: list[str] | None = None) -> OrchestratorConfig: pickup_duration_s=args.pickup_duration, fps=args.fps, device=args.device, - rerun=not args.no_rerun, teleimager_host=args.teleimager_host, remote_server=args.remote_server, replay_trajectory=( @@ -153,13 +151,12 @@ def main(argv: list[str] | None = None) -> int: ) cfg = parse_args(argv) logger.info( - "Soccerbot starting: backend=%s policy=%s clamp=%.3f camera=%s iface=%s rerun=%s", + "Soccerbot starting: backend=%s policy=%s clamp=%.3f camera=%s iface=%s", cfg.backend.value, cfg.policy, cfg.clamp, cfg.camera, cfg.iface, - cfg.rerun, ) logger.info( "Tip: keep ./killswitch.sh --iface %s (or --gui) open in another terminal", diff --git a/soccerbot/src/soccerbot/pickup.py b/soccerbot/src/soccerbot/pickup.py index ca74fdb..482cfd3 100644 --- a/soccerbot/src/soccerbot/pickup.py +++ b/soccerbot/src/soccerbot/pickup.py @@ -48,7 +48,6 @@ def run_pickup(cfg: OrchestratorConfig) -> None: duration=cfg.pickup_duration_s, fps=cfg.fps, device=cfg.device, - rerun=cfg.rerun, leave_arms_engaged=True, ) logger.info( diff --git a/uv.lock b/uv.lock index 2cb2f74..9f5701a 100644 --- a/uv.lock +++ b/uv.lock @@ -3691,14 +3691,14 @@ name = "local-vla-inference" version = "0.1.0" source = { editable = "local-vla-inference" } dependencies = [ - { name = "lerobot", extra = ["viz"] }, + { name = "lerobot" }, { name = "pyzmq" }, { name = "unitree-sdk2py" }, ] [package.metadata] requires-dist = [ - { name = "lerobot", extras = ["viz"], editable = "thirdparty/lerobot" }, + { name = "lerobot", editable = "thirdparty/lerobot" }, { name = "pyzmq", specifier = ">=26.0.0" }, { name = "unitree-sdk2py", git = "https://github.com/unitreerobotics/unitree_sdk2_python.git" }, ] @@ -6526,14 +6526,14 @@ name = "soccerbot" version = "0.1.0" source = { editable = "soccerbot" } dependencies = [ - { name = "lerobot", extra = ["viz"] }, + { name = "lerobot" }, { name = "local-vla-inference" }, { name = "scripted-behavior" }, ] [package.metadata] requires-dist = [ - { name = "lerobot", extras = ["viz"], editable = "thirdparty/lerobot" }, + { name = "lerobot", editable = "thirdparty/lerobot" }, { name = "local-vla-inference", editable = "local-vla-inference" }, { name = "scripted-behavior", editable = "scripted-behavior" }, ] From 666f7c43a68334044224003cf877f147e4a629e2 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 15:24:50 +0000 Subject: [PATCH 7/9] Tighten home/replay slew clamps for smoother recovery Go-home now matches ACT at 0.002 rad/step (was 0.02 / 1 rad/s), with a longer duration budget so the tight slew can still reach the pose. Replay safety net drops to 0.01; throw stays loose at 0.06 for push dynamics. Co-authored-by: arjuncoder1 --- scripted-behavior/arm_replay.py | 4 ++-- soccerbot/README.md | 7 ++++--- soccerbot/src/soccerbot/config.py | 5 +++-- soccerbot/src/soccerbot/home.py | 18 ++++++++++++++---- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/scripted-behavior/arm_replay.py b/scripted-behavior/arm_replay.py index c9f2801..7049155 100644 --- a/scripted-behavior/arm_replay.py +++ b/scripted-behavior/arm_replay.py @@ -43,8 +43,8 @@ # Smooth blend from current measured pose to frames[0] over this many seconds. REPLAY_RAMP_S = 2.0 # Max radians any single joint may move per replay frame. At 30 fps this is -# 0.05 rad * 30 = 1.5 rad/s which is already quite fast; safety net only. -REPLAY_SLEW_CLAMP = 0.05 +# 0.01 rad * 30 = 0.3 rad/s — calm safety net (spikes / bad frames only). +REPLAY_SLEW_CLAMP = 0.01 def _import_g1_arms(): diff --git a/soccerbot/README.md b/soccerbot/README.md index a4418b5..b0aff31 100644 --- a/soccerbot/README.md +++ b/soccerbot/README.md @@ -40,9 +40,10 @@ Defaults match the validated local ACT command: - **URDF hard limit clamp at the choke point**: `G1Arms.send_arm_positions` clamps every commanded joint to the real G1 URDF limits — applies to ALL callers (ACT, replay, arm holds, throw, go-home) regardless of what they compute. -- Every arm command path is additionally slew-clamped (ACT 0.002 rad/step + - replay + throw + go-home), and throw phases chain from the actual last - command so the clamp can never cause a position step-jump. +- Every arm command path is additionally slew-clamped: ACT / go-home **0.002** + rad/step (recovery stays as calm as inference), replay **0.01**, throw + **0.06** (loose so the push keeps dynamics). Throw phases chain from the + actual last command so the clamp can never cause a position step-jump. - **Ctrl+C** → graceful reset: `LocoClient.StopMove()` + release `arm_sdk` (opens a short-timeout temp publisher if the interrupted stage held none). - **`./killswitch.sh`** → CLI (default) or `--gui`: Stop / Go Home / Damp / diff --git a/soccerbot/src/soccerbot/config.py b/soccerbot/src/soccerbot/config.py index f339f0e..29dac8f 100644 --- a/soccerbot/src/soccerbot/config.py +++ b/soccerbot/src/soccerbot/config.py @@ -38,8 +38,9 @@ class OrchestratorConfig: remote_server: str | None = None replay_trajectory: Path = DEFAULT_REPLAY_TRAJECTORY # Scripted-stage slew clamps (rad/frame). - replay_slew_clamp: float = 0.05 - # Loose on purpose: the throw needs ~1.7 rad/s to work; 0.06 @50Hz = 3 rad/s + # Replay: tighter than a raw demo spike, still enough for normal recordings. + replay_slew_clamp: float = 0.01 + # Throw stays loose on purpose: needs ~1.7 rad/s; 0.06 @50Hz = 3 rad/s # only catches garbage targets (see throw.THROW_SLEW_CLAMP). throw_slew_clamp: float = 0.06 pickup_extra_args: list[str] = field(default_factory=list) diff --git a/soccerbot/src/soccerbot/home.py b/soccerbot/src/soccerbot/home.py index 0f42842..06ad293 100644 --- a/soccerbot/src/soccerbot/home.py +++ b/soccerbot/src/soccerbot/home.py @@ -46,8 +46,11 @@ DEFAULT_HOME_Q: dict[str, float] = {f"{name}.q": 0.0 for name in ARM_JOINTS} DEFAULT_HOME_JSON = REPO_ROOT / "scripted-behavior" / "home_pose.json" -HOME_SLEW_CLAMP = 0.02 # rad/step (@50 Hz → max 1 rad/s per joint) +# Match ACT's cautious rate: home/reset is recovery, not a performance move. +# At 50 Hz this is ~0.1 rad/s per joint — slow on purpose after jerky resets. +HOME_SLEW_CLAMP = 0.002 # rad/step HOME_CONTROL_DT = 0.02 +HOME_MAX_DURATION_S = 90.0 # allow full travel under the tight slew cap def load_home_pose(path: Path | None = None) -> dict[str, float]: @@ -124,12 +127,19 @@ def go_home( arms.send_arm_positions(start, weight=1.0) # Estimate duration from max joint delta if not provided. + # Must not truncate below steps_needed or the slew clamp never reaches home. if duration_s is None: max_delta = max(abs(target[k] - start.get(k, 0.0)) for k in DEFAULT_HOME_Q) - # At slew_clamp rad/step and HOME_CONTROL_DT, need enough steps. steps_needed = max(1, int(max_delta / max(slew_clamp, 1e-6)) + 1) - duration_s = steps_needed * HOME_CONTROL_DT - duration_s = max(2.0, min(duration_s, 20.0)) + duration_s = max(2.0, steps_needed * HOME_CONTROL_DT) + if duration_s > HOME_MAX_DURATION_S: + logger.warning( + "Home travel needs %.1fs at slew=%.3f; capping at %.1fs", + duration_s, + slew_clamp, + HOME_MAX_DURATION_S, + ) + duration_s = HOME_MAX_DURATION_S logger.info( "Going home over %.1fs (slew=%.3f rad/step, release_after=%s)", From 8ae13c1e154e681491797458da89963854987853 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 15:44:00 +0000 Subject: [PATCH 8/9] Default ACT pickup duration to 45s Soccerbot --pickup-duration and in-process build_args now default to 45 seconds instead of 30. Co-authored-by: arjuncoder1 --- soccerbot/src/soccerbot/config.py | 2 +- soccerbot/src/soccerbot/main.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/soccerbot/src/soccerbot/config.py b/soccerbot/src/soccerbot/config.py index 29dac8f..43e3bdc 100644 --- a/soccerbot/src/soccerbot/config.py +++ b/soccerbot/src/soccerbot/config.py @@ -31,7 +31,7 @@ class OrchestratorConfig: policy: str = DEFAULT_POLICY layout: str = "14d" clamp: float = DEFAULT_CLAMP_RAD - pickup_duration_s: float = 30.0 + pickup_duration_s: float = 45.0 fps: float = 30.0 device: str | None = None teleimager_host: str = "192.168.123.164" diff --git a/soccerbot/src/soccerbot/main.py b/soccerbot/src/soccerbot/main.py index 0336f7a..7f3579b 100644 --- a/soccerbot/src/soccerbot/main.py +++ b/soccerbot/src/soccerbot/main.py @@ -101,7 +101,7 @@ def parse_args(argv: list[str] | None = None) -> OrchestratorConfig: default=DEFAULT_CLAMP_RAD, help="ACT slew clamp rad/step (default 0.002).", ) - p.add_argument("--pickup-duration", type=float, default=30.0) + p.add_argument("--pickup-duration", type=float, default=45.0) p.add_argument("--fps", type=float, default=30.0) p.add_argument("--device", default=None) p.add_argument("--teleimager-host", default="192.168.123.164") From 02f9e97cb8ec9c7e0681f8497dce2a48238043c0 Mon Sep 17 00:00:00 2001 From: Cursor Agent Date: Sun, 19 Jul 2026 15:44:04 +0000 Subject: [PATCH 9/9] Align in-process ACT and how_to_run with 45s pickup default Co-authored-by: arjuncoder1 --- how_to_run.md | 2 +- local-vla-inference/main.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/how_to_run.md b/how_to_run.md index 141d3aa..a7bceb4 100644 --- a/how_to_run.md +++ b/how_to_run.md @@ -27,7 +27,7 @@ The live commands need: cd scripted-behavior # Local ACT policy for pickup (default backend). -python3 main.py --iface eth0 --pickup-duration 30 +python3 main.py --iface eth0 --pickup-duration 45 # Remote pi0.5 policy server for pickup. python3 main.py --backend remote --iface eth0 --remote-server 192.168.1.42:8000 diff --git a/local-vla-inference/main.py b/local-vla-inference/main.py index fbdb98f..15e0f0d 100644 --- a/local-vla-inference/main.py +++ b/local-vla-inference/main.py @@ -157,7 +157,7 @@ def build_args( iface: str | None = None, camera: str = "zmq://192.168.123.164:55555", clamp: float = 0.002, - duration: float = 30.0, + duration: float = 45.0, fps: float = 30.0, device: str | None = None, leave_arms_engaged: bool = True,