Soccerbot orchestrator: in-process ACT pickup, clamping, and killswitch

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

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

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"

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 "$@"

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

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``."""