diff --git a/diagnose.sh b/diagnose.sh new file mode 100755 index 0000000..0096dad --- /dev/null +++ b/diagnose.sh @@ -0,0 +1,187 @@ +#!/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)" +cd "$REPO_ROOT" +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 (cwd=$(pwd))" + +# --- 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 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 (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) + 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/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/install.sh b/install.sh new file mode 100755 index 0000000..39a6c88 --- /dev/null +++ b/install.sh @@ -0,0 +1,45 @@ +#!/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 # 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 new file mode 100755 index 0000000..0ef38c8 --- /dev/null +++ b/killswitch.sh @@ -0,0 +1,27 @@ +#!/usr/bin/env bash +# 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)" +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}" + +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/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/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/g1_arms.py b/local-vla-inference/g1_arms.py index 63cda0a..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 @@ -252,8 +290,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 72e5451..15e0f0d 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", @@ -141,10 +140,55 @@ 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 = 45.0, + fps: float = 30.0, + device: str | None = None, + 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, + 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 +219,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 +252,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,15 +291,12 @@ 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 + 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) @@ -281,7 +323,35 @@ 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.""" + 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() + 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() + arms.detach() + 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 @@ -323,12 +393,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) @@ -363,14 +430,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 +479,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), @@ -436,7 +505,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 +518,32 @@ 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) + 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, 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) + 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/detach 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..e5fb708 100644 --- a/local-vla-inference/pyproject.toml +++ b/local-vla-inference/pyproject.toml @@ -11,6 +11,25 @@ dependencies = [ "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", + "dds_init", + "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/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/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/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/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/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..d889ef1 100644 --- a/scripted-behavior/pickup.py +++ b/scripted-behavior/pickup.py @@ -1,27 +1,43 @@ -"""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 and Ctrl+C graceful reset 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 importlib.util 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" +_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: @@ -35,43 +51,66 @@ 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 -# --------------------------------------------------------------------------- + local_vla = _load_local_vla_main() + + # 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, + ) + 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 +154,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..d707a08 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,50 @@ def _push_target(current_pose: dict[str, float]) -> dict[str, float]: } +# 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( arms, start: dict[str, float], target: dict[str, float], duration_s: float, control_dt: float = 0.02, -) -> None: - """Linearly interpolate the 14 arm joints from ``start`` to ``target``.""" + slew_clamp: float = THROW_SLEW_CLAMP, +) -> 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. 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) 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 = 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) -> 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 +232,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) @@ -216,15 +244,43 @@ def throw(arms) -> 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) + 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) + 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) + _interpolate_to(arms, cmd, 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..b0aff31 100644 --- a/soccerbot/README.md +++ b/soccerbot/README.md @@ -0,0 +1,59 @@ +# 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 | +| `scripted-behavior/` | Turn 180°, avoid/shuffle, throw, JSON trajectory replay | + +## Run + +```bash +# one-time robot setup (Python 3.12 + CycloneDDS) +./install.sh + +# 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 + +# 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 + +- **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 / 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 / + 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). + +## Diagnose + +```bash +./diagnose.sh --iface enp5s0 +``` diff --git a/soccerbot/pyproject.toml b/soccerbot/pyproject.toml index d8d9dfd..a893ba2 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", ] + +[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..43e3bdc --- /dev/null +++ b/soccerbot/src/soccerbot/config.py @@ -0,0 +1,46 @@ +"""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 = 45.0 + fps: float = 30.0 + device: str | None = None + 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: 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/deps.py b/soccerbot/src/soccerbot/deps.py new file mode 100644 index 0000000..4cc1c36 --- /dev/null +++ b/soccerbot/src/soccerbot/deps.py @@ -0,0 +1,84 @@ +"""Import helpers for workspace logic packages (not published to PyPI). + +``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.""" + 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() + +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: + """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/home.py b/soccerbot/src/soccerbot/home.py new file mode 100644 index 0000000..06ad293 --- /dev/null +++ b/soccerbot/src/soccerbot/home.py @@ -0,0 +1,192 @@ +"""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" +# 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]: + """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 + + 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 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. + # 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) + steps_needed = max(1, int(max_delta / max(slew_clamp, 1e-6)) + 1) + 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)", + 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 new file mode 100644 index 0000000..e0720a1 --- /dev/null +++ b/soccerbot/src/soccerbot/killswitch.py @@ -0,0 +1,383 @@ +"""G1 killswitch — CLI (default) and optional Tk GUI. + +Actions (same as physical pendant where applicable): + + 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) + +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 + +import argparse +import logging +import sys +import threading +import time + +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") + +FSM_NAMES = { + 0: "zero torque", + 1: "damp", + 2: "squat", + 3: "sit", + 4: "stand (locked)", + 200: "start / balance stand", + 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 + 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) + 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: + import tkinter as tk + from tkinter import messagebox, ttk + except ModuleNotFoundError as exc: + raise SystemExit( + "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 + + +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("520x640") + self.root.minsize(480, 600) + + tk.Label( + self.root, + text="G1 KILLSWITCH", + font=("Helvetica", 28, "bold"), + fg="#ff4444", + bg="#1a1a1a", + ).pack(pady=(16, 4)) + + tk.Label( + self.root, + text=f"iface: {iface or '(default DDS iface)'}", + 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, "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 + ) + 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.\n" + "CLI mode (no GUI): ./killswitch.sh --iface …\n" + "Physical 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: + loco = init_loco(self.iface) + with self._lock: + self._loco = loco + 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}") + + 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) + 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}") + 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_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 + 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 + self._set_status(_fsm_status(loco)) + + threading.Thread(target=worker, daemon=True).start() + self.root.after(1000, self._poll_fsm) + + 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="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) + + +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: + 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 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/soccerbot/src/soccerbot/main.py b/soccerbot/src/soccerbot/main.py index e69de29..7f3579b 100644 --- a/soccerbot/src/soccerbot/main.py +++ b/soccerbot/src/soccerbot/main.py @@ -0,0 +1,192 @@ +"""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`` (CLI) or ``./killswitch.sh --gui`` open for Stop / Home / +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=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") + 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( + "--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, + 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", + cfg.backend.value, + cfg.policy, + cfg.clamp, + cfg.camera, + cfg.iface, + ) + logger.info( + "Tip: keep ./killswitch.sh --iface %s (or --gui) 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..482cfd3 --- /dev/null +++ b/soccerbot/src/soccerbot/pickup.py @@ -0,0 +1,62 @@ +"""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, import_local_vla_main + +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}") + + # 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, + policy=cfg.policy, + iface=cfg.iface, + camera=cfg.camera, + clamp=cfg.clamp, + duration=cfg.pickup_duration_s, + fps=cfg.fps, + device=cfg.device, + 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..e392464 --- /dev/null +++ b/soccerbot/src/soccerbot/safety.py @@ -0,0 +1,162 @@ +"""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 + +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: + _ensure_dds(iface) + from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient + + 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, *, 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: + try: + _ensure_dds(iface) + _ensure_front(LOCAL_VLA_DIR) + from g1_arms import G1Arms + + # 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 + 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( + *, + 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). 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, iface=iface) + 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. + + 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) + 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 + # 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. + + Emergency ordering: ZeroTorque FIRST, arm cleanup after (best-effort). + """ + client = loco or init_loco(iface) + 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 + release_arms(iface=iface) + + +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..9f5701a 100644 --- a/uv.lock +++ b/uv.lock @@ -39,6 +39,7 @@ members = [ "remote-vla-inference", "scripted-behavior", "soccerbot", + "soccerbot-sim", "soccerbot-workspace", "training", ] @@ -3688,7 +3689,7 @@ wheels = [ [[package]] name = "local-vla-inference" version = "0.1.0" -source = { virtual = "local-vla-inference" } +source = { editable = "local-vla-inference" } dependencies = [ { name = "lerobot" }, { name = "pyzmq" }, @@ -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" }, + { name = "local-vla-inference" }, + { name = "scripted-behavior" }, +] + +[package.metadata] +requires-dist = [ + { name = "lerobot", 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"