diff --git a/.gitignore b/.gitignore index 15c39e757..7c182c85d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,10 @@ + +# Local toolchain and generated assets (not part of the submission) +.venv/ +assets/g1_description_obj/ +__pycache__/ +*.pyc +env.sh *.exe *.exe~ *.dll diff --git a/bridge/unitree/g1/sim_bridge/__init__.py b/bridge/unitree/g1/sim_bridge/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bridge/unitree/g1/sim_bridge/g1/__init__.py b/bridge/unitree/g1/sim_bridge/g1/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bridge/unitree/g1/sim_bridge/g1/action_contract.py b/bridge/unitree/g1/sim_bridge/g1/action_contract.py new file mode 100644 index 000000000..ef2db7b2b --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/g1/action_contract.py @@ -0,0 +1,344 @@ +"""The paid action envelope, and the rules for refusing one. + +Everything the robot is ever asked to do arrives as one of these. The bounty's +acceptance criteria are specific about what has to survive the trip and what +has to be rejected, so those rules live here rather than being scattered +through the bridge: + + * the Zenoh payload must preserve actionId, robotId, skillId, + idempotencyKey, paramsHash and payment; + * invalid, expired or replayed requests must not publish to Zenoh or + actuate the robot; + * a duplicate idempotency key must not execute twice. + +`paramsHash` is the part worth explaining. It is a hash of the parameters the +payer actually signed for. Recomputing it here and comparing means a request +whose parameters were altered in flight -- paid to nudge the puck 5cm, edited +to shove it off the table -- is rejected before it reaches the simulator. The +robot never has to trust that the routing layer left the body alone. +""" + +from __future__ import annotations + +import hashlib +import json +from dataclasses import dataclass, field +from datetime import datetime, timezone +from typing import Any + +#: Fields the criteria require to survive routing, in the order we report them. +REQUIRED_FIELDS = ( + "actionId", + "robotId", + "skillId", + "idempotencyKey", + "paramsHash", + "payment", +) + + +#: Keys that identify the wrapper the Fabric tunnel actually puts on the wire. +#: +#: The tunnel does not publish the action envelope directly. `POST /action` +#: sits behind its x402 middleware, and on a verified payment the handler +#: publishes `{payload, transaction_details, timestamp}` to +#: `robot/tunnel/action`, where `payload` is the client's request body verbatim +#: and `transaction_details` carries the x402 payload and requirements the +#: middleware resolved. See tunnel/internal/handlers/handlers.go. +#: +#: A bridge that only understood the flat envelope would silently ignore every +#: message the real tunnel sends, which is a integration that passes its own +#: tests and works with nothing. +_TUNNEL_WRAPPER_KEYS = ("payload", "transaction_details") + + +class RejectionCode: + """Stable reason codes. These end up in logs and in the result envelope.""" + + MALFORMED = "MALFORMED_ENVELOPE" + MISSING_FIELD = "MISSING_FIELD" + UNKNOWN_ROBOT = "UNKNOWN_ROBOT" + UNKNOWN_SKILL = "UNKNOWN_SKILL" + PARAMS_TAMPERED = "PARAMS_HASH_MISMATCH" + EXPIRED = "ACTION_EXPIRED" + REPLAYED = "IDEMPOTENCY_REPLAY" + PAYMENT_MISSING = "PAYMENT_REQUIRED" + PAYMENT_INVALID = "PAYMENT_INVALID" + PARAMS_OUT_OF_RANGE = "PARAMS_OUT_OF_RANGE" + + +class ActionRejected(Exception): + """Raised when an envelope must not reach the robot.""" + + def __init__(self, code: str, message: str) -> None: + super().__init__(message) + self.code = code + self.message = message + + +def canonical_params_hash(params: dict[str, Any]) -> str: + """Hash parameters the way the payer is expected to have hashed them. + + Canonical JSON -- sorted keys, no incidental whitespace -- so that two + semantically identical parameter sets always produce the same digest + regardless of how they were serialised upstream. + """ + blob = json.dumps(params, sort_keys=True, separators=(",", ":")) + return "sha256:" + hashlib.sha256(blob.encode("utf-8")).hexdigest() + + +def unwrap_tunnel_envelope(raw: dict[str, Any]) -> dict[str, Any]: + """Flatten the Fabric tunnel's wire format, if that is what arrived. + + Messages published straight to the topic -- by the test client, or by any + relay that already speaks the flat envelope -- are returned untouched, so + both shapes work and there is one parser rather than two. + + The payment block is *always* rebuilt from `transaction_details` and never + taken from the client's body, even when the body carries one. That is the + point of the wrapper: the body is attacker-controlled, while + `transaction_details` is what the tunnel's x402 middleware resolved before + it agreed to forward anything. Trusting a `payment.verified` that came from + the request body would let anyone who can reach the topic assert their own + payment. + + Note that `verified` here means verified, not settled. The tunnel publishes + after the facilitator verifies the payment and before it is settled, so + there is no transaction hash yet -- which is exactly why settlement is the + robot's decision to report, and why `settle=false` on failure is worth + anything at all. + """ + if not all(key in raw for key in _TUNNEL_WRAPPER_KEYS): + return raw + + inner = raw.get("payload") + if not isinstance(inner, dict): + raise ActionRejected( + RejectionCode.MALFORMED, + "tunnel envelope carries a non-object payload", + ) + + details = raw.get("transaction_details") + details = details if isinstance(details, dict) else {} + payload = details.get("payment_payload") + payload = payload if isinstance(payload, dict) else None + + # x402 v2 keeps the resolved requirements on the payload as `accepted`; + # the tunnel reports them separately as well. Either is authoritative. + accepted = details.get("payment_requirements") + if not isinstance(accepted, dict) and payload is not None: + accepted = payload.get("accepted") + accepted = accepted if isinstance(accepted, dict) else {} + + flat = dict(inner) + payment: dict[str, Any] = { + "provider": "x402", + "amount": str(accepted.get("amount", "")), + "asset": str(accepted.get("asset", "")), + "network": str(accepted.get("network", "")), + "verified": payload is not None, + } + if payload is not None: + # Digest the whole authorisation rather than reaching for a + # scheme-specific field: x402 keeps `payload` as an opaque + # scheme-defined map, so a nonce path that works for exact-EVM would + # break on the next scheme. A digest is stable, scheme-agnostic, and + # enough to tie this action to one payment. + payment["authorizationRef"] = _digest(payload) + if details.get("tx_hash"): + payment["txHash"] = str(details["tx_hash"]) + flat["payment"] = payment + return flat + + +def _digest(value: Any) -> str: + blob = json.dumps(value, sort_keys=True, separators=(",", ":"), default=str) + return "sha256:" + hashlib.sha256(blob.encode("utf-8")).hexdigest() + + +@dataclass(frozen=True) +class Payment: + """The payment attached to an action, as presented by the tunnel.""" + + provider: str + amount: str + asset: str + network: str + #: Set once the tunnel has verified the x402 authorisation. + verified: bool = False + #: Settlement reference. Absent on arrival under the real x402 lifecycle: + #: the resource server verifies, runs its handler, and only settles after + #: the handler succeeds, so a transaction hash does not exist yet at the + #: moment the robot is asked to move. Populated when a relay settles first. + tx_hash: str | None = None + #: Digest of the exact x402 authorisation the tunnel verified. This is the + #: reference that *is* available on arrival, and it is what lets the robot + #: tie the action it performed to a specific payment after the fact. + authorization_ref: str | None = None + + @classmethod + def from_json(cls, raw: Any) -> "Payment": + if not isinstance(raw, dict): + raise ActionRejected( + RejectionCode.PAYMENT_MISSING, "payment block is missing" + ) + try: + return cls( + provider=str(raw["provider"]), + amount=str(raw["amount"]), + asset=str(raw["asset"]), + network=str(raw["network"]), + verified=bool(raw.get("verified", False)), + tx_hash=(str(raw["txHash"]) if raw.get("txHash") else None), + authorization_ref=( + str(raw["authorizationRef"]) + if raw.get("authorizationRef") + else None + ), + ) + except KeyError as exc: + raise ActionRejected( + RejectionCode.PAYMENT_MISSING, + f"payment block is missing {exc.args[0]!r}", + ) from exc + + def to_json(self) -> dict[str, Any]: + out: dict[str, Any] = { + "provider": self.provider, + "amount": self.amount, + "asset": self.asset, + "network": self.network, + "verified": self.verified, + } + if self.tx_hash: + out["txHash"] = self.tx_hash + if self.authorization_ref: + out["authorizationRef"] = self.authorization_ref + return out + + +@dataclass(frozen=True) +class ActionEnvelope: + """One paid action request, already checked for structural validity.""" + + action_id: str + robot_id: str + skill_id: str + params: dict[str, Any] + idempotency_key: str + params_hash: str + payment: Payment + expires_at: datetime | None = None + raw: dict[str, Any] = field(default_factory=dict, repr=False) + + # -- parsing ---------------------------------------------------------- + + @classmethod + def from_bytes(cls, payload: bytes) -> "ActionEnvelope": + try: + raw = json.loads(payload.decode("utf-8")) + except (UnicodeDecodeError, json.JSONDecodeError) as exc: + raise ActionRejected( + RejectionCode.MALFORMED, f"payload is not valid JSON: {exc}" + ) from exc + if not isinstance(raw, dict): + raise ActionRejected( + RejectionCode.MALFORMED, "payload is not a JSON object" + ) + return cls.from_json(raw) + + @classmethod + def from_json(cls, raw: dict[str, Any]) -> "ActionEnvelope": + raw = unwrap_tunnel_envelope(raw) + missing = [f for f in REQUIRED_FIELDS if not raw.get(f)] + if missing: + raise ActionRejected( + RejectionCode.MISSING_FIELD, + f"envelope is missing required field(s): {', '.join(missing)}", + ) + params = raw.get("params") or {} + if not isinstance(params, dict): + raise ActionRejected( + RejectionCode.MALFORMED, "params must be a JSON object" + ) + expires = raw.get("expiresAt") + expires_at = _parse_time(expires) if expires else None + return cls( + action_id=str(raw["actionId"]), + robot_id=str(raw["robotId"]), + skill_id=str(raw["skillId"]), + params=params, + idempotency_key=str(raw["idempotencyKey"]), + params_hash=str(raw["paramsHash"]), + payment=Payment.from_json(raw["payment"]), + expires_at=expires_at, + raw=raw, + ) + + def to_json(self) -> dict[str, Any]: + """Re-serialise, preserving every field the criteria require.""" + out: dict[str, Any] = { + "actionId": self.action_id, + "robotId": self.robot_id, + "skillId": self.skill_id, + "params": dict(self.params), + "idempotencyKey": self.idempotency_key, + "paramsHash": self.params_hash, + "payment": self.payment.to_json(), + } + if self.expires_at is not None: + out["expiresAt"] = self.expires_at.isoformat() + return out + + # -- checks ----------------------------------------------------------- + + def require_robot(self, robot_id: str) -> None: + if self.robot_id != robot_id: + raise ActionRejected( + RejectionCode.UNKNOWN_ROBOT, + f"action addressed to {self.robot_id!r}, this robot is {robot_id!r}", + ) + + def require_untampered_params(self) -> None: + expected = canonical_params_hash(self.params) + if expected != self.params_hash: + raise ActionRejected( + RejectionCode.PARAMS_TAMPERED, + "params do not hash to the value the payer authorised " + f"(declared {self.params_hash}, computed {expected})", + ) + + def require_unexpired(self, now: datetime | None = None) -> None: + if self.expires_at is None: + return + moment = now or datetime.now(timezone.utc) + if moment > self.expires_at: + raise ActionRejected( + RejectionCode.EXPIRED, + f"action expired at {self.expires_at.isoformat()}", + ) + + def require_verified_payment(self) -> None: + if not self.payment.verified: + raise ActionRejected( + RejectionCode.PAYMENT_MISSING, + "payment has not been verified by the tunnel", + ) + if not (self.payment.tx_hash or self.payment.authorization_ref): + raise ActionRejected( + RejectionCode.PAYMENT_INVALID, + "verified payment carries no reference to the authorisation", + ) + + +def _parse_time(value: Any) -> datetime: + text = str(value).replace("Z", "+00:00") + try: + parsed = datetime.fromisoformat(text) + except ValueError as exc: + raise ActionRejected( + RejectionCode.MALFORMED, f"expiresAt is not an ISO-8601 time: {value!r}" + ) from exc + if parsed.tzinfo is None: + parsed = parsed.replace(tzinfo=timezone.utc) + return parsed diff --git a/bridge/unitree/g1/sim_bridge/g1/mapper.py b/bridge/unitree/g1/sim_bridge/g1/mapper.py new file mode 100644 index 000000000..afe232d8d --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/g1/mapper.py @@ -0,0 +1,178 @@ +"""The skill catalogue: what this robot sells, and what it refuses. + +Three skills are published, and the third is not padding: + + * ``push_to_target`` -- the paid skill the bounty demo exercises. + * ``stop`` -- free, always available, so the price list is not uniformly + "pay me" and a caller can always halt the robot. + * ``diagnostic_fail`` -- paid, and guaranteed to fail during execution. + +That last one exists because the acceptance criteria require a demonstrable +failure path and explicitly rule out "a success-only demo with no failure +path". It is the cleanest way to prove the property that actually matters: +a paid action that fails must return an error and must *not* settle. + +Parameter ranges are not decoration either. The reachable workspace was +measured, not guessed (see docs/validation-report.md): targets outside it make +the IK infeasible, and it is better to reject those up front with a clear +reason than to accept payment for a motion the arm cannot perform. +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, field +from typing import Any + +from .action_contract import ActionEnvelope, ActionRejected, RejectionCode + + +@dataclass(frozen=True) +class Bound: + """Inclusive numeric range for one parameter.""" + + low: float + high: float + + def check(self, name: str, value: Any) -> float: + try: + number = float(value) + except (TypeError, ValueError) as exc: + raise ActionRejected( + RejectionCode.PARAMS_OUT_OF_RANGE, + f"{name} must be a number, got {value!r}", + ) from exc + if not math.isfinite(number) or not (self.low <= number <= self.high): + raise ActionRejected( + RejectionCode.PARAMS_OUT_OF_RANGE, + f"{name}={number} is outside the reachable range " + f"[{self.low}, {self.high}]", + ) + return number + + def as_json(self) -> dict[str, float]: + return {"min": self.low, "max": self.high} + + +@dataclass(frozen=True) +class SkillSpec: + """One purchasable capability.""" + + skill_id: str + description: str + price_usdc: str + payment_required: bool + params: dict[str, Bound] = field(default_factory=dict) + + def catalogue_entry(self, robot_id: str) -> dict[str, Any]: + """The discovery record a payer reads before deciding to buy.""" + return { + "name": self.skill_id, + "description": self.description, + "priceUSDC": self.price_usdc, + "paymentRequired": self.payment_required, + "paramsSchema": { + name: {"type": "number", **bound.as_json()} + for name, bound in self.params.items() + }, + "robotId": robot_id, + } + + +#: Reachable workspace, measured on the fixed-base G1 over a target grid. +#: Anything outside this returned an infeasible IK solve. +PUCK_X = Bound(0.30, 0.44) +PUCK_Y = Bound(-0.22, 0.00) +GOAL_X = Bound(0.34, 0.52) +GOAL_Y = Bound(-0.18, 0.10) + +#: A push shorter than this is inside the goal tolerance already; longer than +#: this runs the hand out past the edge of the table. +MIN_PUSH = 0.06 +MAX_PUSH = 0.30 + + +SKILLS: dict[str, SkillSpec] = { + "push_to_target": SkillSpec( + skill_id="push_to_target", + description=( + "Turn toward a puck at (puck_x, puck_y) on the table, approach it " + "from above, and push it to (goal_x, goal_y)." + ), + price_usdc="0.01", + payment_required=True, + params={ + "puck_x": PUCK_X, + "puck_y": PUCK_Y, + "goal_x": GOAL_X, + "goal_y": GOAL_Y, + }, + ), + "stop": SkillSpec( + skill_id="stop", + description="Hold the current pose and stop all motion immediately.", + price_usdc="0.00", + payment_required=False, + ), + "diagnostic_fail": SkillSpec( + skill_id="diagnostic_fail", + description=( + "Deliberately fails during execution. Exists so that the " + "no-settle-on-failure guarantee can be exercised on demand." + ), + price_usdc="0.01", + payment_required=True, + ), +} + + +@dataclass(frozen=True) +class TaskSpec: + """A validated request, ready for the simulator.""" + + skill_id: str + puck_xy: tuple[float, float] | None = None + goal_xy: tuple[float, float] | None = None + #: True for skills that must report failure no matter what the robot does. + expect_failure: bool = False + + +def catalogue(robot_id: str) -> list[dict[str, Any]]: + """Every skill this robot exposes, for pre-purchase discovery.""" + return [spec.catalogue_entry(robot_id) for spec in SKILLS.values()] + + +def resolve(envelope: ActionEnvelope) -> TaskSpec: + """Validate an envelope's skill and parameters, or reject it.""" + spec = SKILLS.get(envelope.skill_id) + if spec is None: + raise ActionRejected( + RejectionCode.UNKNOWN_SKILL, + f"no such skill {envelope.skill_id!r}; this robot offers " + f"{', '.join(sorted(SKILLS))}", + ) + + if spec.skill_id == "stop": + return TaskSpec(skill_id=spec.skill_id) + if spec.skill_id == "diagnostic_fail": + return TaskSpec(skill_id=spec.skill_id, expect_failure=True) + + missing = [name for name in spec.params if name not in envelope.params] + if missing: + raise ActionRejected( + RejectionCode.PARAMS_OUT_OF_RANGE, + f"missing parameter(s): {', '.join(missing)}", + ) + values = { + name: bound.check(name, envelope.params[name]) + for name, bound in spec.params.items() + } + puck = (values["puck_x"], values["puck_y"]) + goal = (values["goal_x"], values["goal_y"]) + distance = math.dist(puck, goal) + if not (MIN_PUSH <= distance <= MAX_PUSH): + raise ActionRejected( + RejectionCode.PARAMS_OUT_OF_RANGE, + f"push distance {distance:.3f}m is outside [{MIN_PUSH}, {MAX_PUSH}]", + ) + return TaskSpec(skill_id=spec.skill_id, puck_xy=puck, goal_xy=goal) diff --git a/bridge/unitree/g1/sim_bridge/g1/node.py b/bridge/unitree/g1/sim_bridge/g1/node.py new file mode 100644 index 000000000..3c6364bd2 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/g1/node.py @@ -0,0 +1,216 @@ +"""Executing a validated action, and deciding whether it may be paid for. + +The settlement decision lives here, in one place, and it is deliberately +boring: `settle` is true only when the robot reported success. Failure, +timeout, rejection and crash all leave it false. The bounty criteria state the +rule directly -- "If the robot action fails, times out, or returns an error, +the relay must not settle the payment" -- and a submission that settles after +a failed execution is listed as non-acceptable, so this is the single most +important line in the file. + +Replay defence lives here too. A repeated idempotency key returns the stored +outcome of the first attempt without touching the simulator, so paying once +and replaying the message cannot make the robot move twice. +""" + +from __future__ import annotations + +import threading +import time +from dataclasses import dataclass, field +from typing import Any, Callable + +from ..simulation.metrics import RunMetrics +from .action_contract import ActionEnvelope, ActionRejected, RejectionCode +from .mapper import TaskSpec, resolve + + +@dataclass +class ExecutionResult: + """The outcome of one action, in the shape the criteria prescribe.""" + + status: str # "success" | "error" + skill: str + action_id: str + settle: bool + result: dict[str, Any] | None = None + error: dict[str, Any] | None = None + metrics: dict[str, Any] = field(default_factory=dict) + replayed: bool = False + + @classmethod + def success( + cls, + action_id: str, + skill: str, + message: str, + metrics: dict[str, Any], + ) -> "ExecutionResult": + return cls( + status="success", + skill=skill, + action_id=action_id, + settle=True, + result={"message": message}, + metrics=metrics, + ) + + @classmethod + def failure( + cls, + action_id: str, + skill: str, + code: str, + message: str, + metrics: dict[str, Any] | None = None, + ) -> "ExecutionResult": + return cls( + status="error", + skill=skill, + action_id=action_id, + # The whole point: a failed action is never settleable. + settle=False, + error={"code": code, "message": message}, + metrics=metrics or {}, + ) + + def to_json(self) -> dict[str, Any]: + out: dict[str, Any] = { + "status": self.status, + "skill": self.skill, + "actionId": self.action_id, + "settle": self.settle, + } + if self.result is not None: + out["result"] = self.result + if self.error is not None: + out["error"] = self.error + if self.metrics: + out["metrics"] = self.metrics + if self.replayed: + out["replayed"] = True + return out + + +class IdempotencyStore: + """Remembers what each idempotency key produced. + + Bounded and time-limited so a long-running bridge cannot grow without end. + """ + + def __init__(self, ttl_seconds: float = 900.0, capacity: int = 512) -> None: + self._ttl = ttl_seconds + self._capacity = capacity + self._entries: dict[str, tuple[float, ExecutionResult]] = {} + self._lock = threading.Lock() + + def get(self, key: str) -> ExecutionResult | None: + with self._lock: + self._evict() + found = self._entries.get(key) + return found[1] if found else None + + def put(self, key: str, result: ExecutionResult) -> None: + with self._lock: + self._evict() + if len(self._entries) >= self._capacity: + oldest = min(self._entries, key=lambda k: self._entries[k][0]) + self._entries.pop(oldest, None) + self._entries[key] = (time.monotonic(), result) + + def _evict(self) -> None: + cutoff = time.monotonic() - self._ttl + for key in [k for k, (t, _) in self._entries.items() if t < cutoff]: + self._entries.pop(key, None) + + +#: Runs a task and reports how it went. Injected so the Zenoh bridge can be +#: tested without a simulator, and so the simulator can be exercised without +#: Zenoh. +Runner = Callable[[TaskSpec], RunMetrics] + + +class ActionNode: + """Turns validated envelopes into robot motion and settleable results.""" + + def __init__( + self, + robot_id: str, + runner: Runner, + store: IdempotencyStore | None = None, + ) -> None: + self.robot_id = robot_id + self._runner = runner + self._store = store or IdempotencyStore() + + def handle(self, envelope: ActionEnvelope) -> ExecutionResult: + """Validate, deduplicate, execute. Never raises for a bad request.""" + try: + envelope.require_robot(self.robot_id) + envelope.require_unexpired() + envelope.require_untampered_params() + task = resolve(envelope) + if task.skill_id != "stop": + envelope.require_verified_payment() + except ActionRejected as rejected: + return ExecutionResult.failure( + envelope.action_id, envelope.skill_id, rejected.code, rejected.message + ) + + cached = self._store.get(envelope.idempotency_key) + if cached is not None: + # Do not re-run, and do not settle a second time. + replay = ExecutionResult( + status=cached.status, + skill=cached.skill, + action_id=envelope.action_id, + settle=False, + result=cached.result, + error=cached.error or { + "code": RejectionCode.REPLAYED, + "message": "idempotency key already executed", + }, + metrics=cached.metrics, + replayed=True, + ) + return replay + + result = self._execute(envelope, task) + self._store.put(envelope.idempotency_key, result) + return result + + def _execute(self, envelope: ActionEnvelope, task: TaskSpec) -> ExecutionResult: + if task.skill_id == "stop": + return ExecutionResult.success( + envelope.action_id, task.skill_id, "Robot stopped", {} + ) + if task.expect_failure: + return ExecutionResult.failure( + envelope.action_id, + task.skill_id, + "ACTION_FAILED", + "diagnostic_fail always fails; payment must not settle", + ) + try: + metrics = self._runner(task) + except Exception as exc: # noqa: BLE001 - a crash must not settle either + return ExecutionResult.failure( + envelope.action_id, + task.skill_id, + "ACTION_FAILED", + f"simulator raised {type(exc).__name__}: {exc}", + ) + if not metrics.success: + return ExecutionResult.failure( + envelope.action_id, + task.skill_id, + "ACTION_FAILED", + metrics.reason or "robot failed to complete the action", + metrics.to_json(), + ) + return ExecutionResult.success( + envelope.action_id, + task.skill_id, + "Action completed", + metrics.to_json(), + ) diff --git a/bridge/unitree/g1/sim_bridge/main.py b/bridge/unitree/g1/sim_bridge/main.py new file mode 100644 index 000000000..e8b8b686c --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/main.py @@ -0,0 +1,187 @@ +"""Zenoh bridge: paid actions in, robot results out. + +Topics (all configurable; these are the defaults the README documents): + + robot/tunnel/action subscribe paid action envelopes from the tunnel + robot/tunnel/result publish terminal result, correlated by actionId + robot/g1/metrics publish simulator state metrics for the run + robot/g1/skills queryable skill catalogue, for pre-purchase discovery + +The bridge itself does no payment verification. That is the tunnel's job, and +keeping the split honest matters: the tunnel proves the money is real, the +bridge proves the robot did the work, and neither gets to vouch for the other. +What the bridge does enforce is that nothing reaches the robot unless the +envelope is well formed, addressed to this robot, unexpired, unmodified since +the payer signed it, and not a replay. + +Run it with: + + python -m sim_bridge.main --robot-id g1-sim-001 + +and send it work with `tools/send_action.py`. +""" + +from __future__ import annotations + +import argparse +import json +import logging +import os +import signal +import sys +import threading +from typing import Any + +import zenoh + +from .g1.action_contract import ActionEnvelope, ActionRejected +from .g1.mapper import catalogue +from .g1.node import ActionNode, ExecutionResult, IdempotencyStore +from .simulation.runner import TaskRunner + +LOG = logging.getLogger("robopay.g1") + +DEFAULT_ACTION_TOPIC = "robot/tunnel/action" +DEFAULT_RESULT_TOPIC = "robot/tunnel/result" +DEFAULT_METRICS_TOPIC = "robot/g1/metrics" +DEFAULT_SKILLS_TOPIC = "robot/g1/skills" + + +class Bridge: + """Wires Zenoh to the action node.""" + + def __init__( + self, + session: zenoh.Session, + node: ActionNode, + result_topic: str = DEFAULT_RESULT_TOPIC, + metrics_topic: str = DEFAULT_METRICS_TOPIC, + ) -> None: + self._session = session + self._node = node + self._result_topic = result_topic + self._metrics_topic = metrics_topic + + def on_action(self, sample: Any) -> None: + """Handle one incoming envelope. Never raises into the Zenoh runtime.""" + try: + payload = bytes(sample.payload.to_bytes()) + except Exception as exc: # noqa: BLE001 + LOG.error("could not read Zenoh payload: %s", exc) + return + + try: + envelope = ActionEnvelope.from_bytes(payload) + except ActionRejected as rejected: + LOG.warning("rejected before execution: %s -- %s", + rejected.code, rejected.message) + self._publish( + ExecutionResult.failure( + action_id="unknown", + skill="unknown", + code=rejected.code, + message=rejected.message, + ) + ) + return + + LOG.info( + "action %s skill=%s params=%s idem=%s", + envelope.action_id, envelope.skill_id, + json.dumps(envelope.params, sort_keys=True), envelope.idempotency_key, + ) + result = self._node.handle(envelope) + if result.status == "success": + LOG.info("action %s SUCCESS settle=%s", result.action_id, result.settle) + else: + LOG.warning( + "action %s FAILED code=%s settle=%s -- %s", + result.action_id, + (result.error or {}).get("code"), + result.settle, + (result.error or {}).get("message"), + ) + self._publish(result) + + def _publish(self, result: ExecutionResult) -> None: + body = result.to_json() + self._session.put(self._result_topic, json.dumps(body).encode()) + if result.metrics: + self._session.put( + self._metrics_topic, + json.dumps( + {"actionId": result.action_id, "metrics": result.metrics} + ).encode(), + ) + + +def build_session(listen: str | None, connect: str | None) -> zenoh.Session: + """Open a Zenoh session. + + The bridge listens on an explicit TCP endpoint by default rather than + relying on multicast scouting. Peer discovery by multicast does not work + in every environment -- it silently does not here -- and a demo that + depends on it looks like a hung bridge rather than a networking problem. + A fixed endpoint is also what the README can tell a reviewer to use. + """ + raw = os.environ.get("ZENOH_CONFIG") + if raw: + return zenoh.open(zenoh.Config.from_json5(raw)) + config = zenoh.Config() + if listen: + config.insert_json5("listen/endpoints", json.dumps([listen])) + if connect: + config.insert_json5("connect/endpoints", json.dumps([connect])) + return zenoh.open(config) + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--robot-id", default=os.environ.get("ROBOT_ID", "g1-sim-001")) + parser.add_argument("--action-topic", + default=os.environ.get("ZENOH_ACTION_TOPIC", DEFAULT_ACTION_TOPIC)) + parser.add_argument("--result-topic", + default=os.environ.get("ZENOH_RESULT_TOPIC", DEFAULT_RESULT_TOPIC)) + parser.add_argument("--metrics-topic", + default=os.environ.get("ZENOH_METRICS_TOPIC", DEFAULT_METRICS_TOPIC)) + parser.add_argument("--listen", + default=os.environ.get("ZENOH_LISTEN", "tcp/127.0.0.1:7447"), + help="endpoint this bridge accepts connections on") + parser.add_argument("--connect", default=os.environ.get("ZENOH_CONNECT"), + help="optional upstream router to dial out to") + parser.add_argument("--verbose", action="store_true") + args = parser.parse_args(argv) + + logging.basicConfig( + level=logging.DEBUG if args.verbose else logging.INFO, + format="%(asctime)s %(levelname)-7s %(name)s: %(message)s", + ) + + runner = TaskRunner() + node = ActionNode(args.robot_id, runner, IdempotencyStore()) + + with build_session(args.listen, args.connect) as session: + bridge = Bridge(session, node, args.result_topic, args.metrics_topic) + subscriber = session.declare_subscriber(args.action_topic, bridge.on_action) + # Publish the catalogue once so a payer can discover skills and prices + # before deciding to buy anything. + session.put( + DEFAULT_SKILLS_TOPIC, + json.dumps({"robotId": args.robot_id, + "skills": catalogue(args.robot_id)}).encode(), + ) + LOG.info("zenoh endpoint %s", args.listen) + LOG.info("robot %s listening on %s", args.robot_id, args.action_topic) + LOG.info("results -> %s, metrics -> %s", args.result_topic, args.metrics_topic) + + stop = threading.Event() + signal.signal(signal.SIGINT, lambda *_: stop.set()) + signal.signal(signal.SIGTERM, lambda *_: stop.set()) + stop.wait() + LOG.info("shutting down") + subscriber.undeclare() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/bridge/unitree/g1/sim_bridge/policy/__init__.py b/bridge/unitree/g1/sim_bridge/policy/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bridge/unitree/g1/sim_bridge/policy/controller.py b/bridge/unitree/g1/sim_bridge/policy/controller.py new file mode 100644 index 000000000..c4d32ed54 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/policy/controller.py @@ -0,0 +1,417 @@ +"""Task policy for the G1 push-to-target skill. + +This is deliberately *not* a recorded trajectory. Every joint command is +derived at run time from the current observation: + + * the turn angle comes from the bearing to the puck's observed position, + * the push direction comes from the live puck-to-goal vector, + * each Cartesian setpoint is turned into a joint configuration by a + constrained IK solve (see `ik.py`), re-solved every control tick, and + * stage transitions fire on sensed conditions -- joint convergence, achieved + puck displacement -- never on a timer. + +Timers exist only as failure guards. Both ends of the motion are parameters of +the paid action: the payer picks where the puck starts *and* where it must end +up. A replayed animation has nothing to replay, which is the property the +bounty's "cannot simply replay a predefined animation" rule is after. + +Why a push and not a grasp. The G1 hand in this model has its index and middle +fingers fixed 57mm apart with no travel in that direction, and an opposing +thumb 86mm further up the palm. Objects narrower than the split pass between +the fingers untouched; wider ones are crushed by the position-controlled +joints, with measured peaks of 60-120N on a 100g object, which ejects it. +Controlled contact is reliable on this hand; holding is not. With the fingers +pointed down the hand makes a stable vertical paddle, which is what the push +uses. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np + +from ..simulation.base import HAND_JOINTS_RIGHT, Observation +from .ik import ArmIK, IKResult +from .stages import Stage, StageRecord + +#: Reference point in the wrist frame that the IK positions, 110mm out along +#: the finger axis. +GRASP_CENTER = np.array([0.110, 0.017, 0.0], dtype=float) + +#: How far the fingertips extend past GRASP_CENTER along the finger axis, +#: measured from the model (index and middle tips sit at local x = 0.165). +FINGERTIP_DEPTH = 0.055 + +#: Pushing pose: fingers slightly curled and the thumb tucked away, so the +#: hand presents one flat paddle instead of three separate prongs. +HAND_PADDLE = dict.fromkeys(HAND_JOINTS_RIGHT, 0.0) | { + "right_hand_thumb_0_joint": -0.60, + "right_hand_thumb_1_joint": -0.20, + "right_hand_index_0_joint": 0.25, + "right_hand_middle_0_joint": 0.25, +} + +#: Fingers point straight down; the hand pushes with their outer face. +APPROACH_AXIS = np.array([0.0, 0.0, -1.0]) + +#: The finger bodies sit ~22mm along the wrist's local -y from the IK +#: reference point, so that is the direction the pushing face looks. Roll +#: about the finger axis has to be pinned to aim it: left free, the solver +#: picks a roll at will and the paddle routinely ends up edge-on to the push, +#: sweeping past the puck without touching it at all. +PADDLE_FACE_LOCAL = np.array([0.0, -1.0, 0.0]) + + +@dataclass +class PolicyStatus: + stage: Stage + terminal: bool + success: bool + reason: str | None = None + goal_distance: float = 0.0 + pushed: float = 0.0 + ik_position_error: float = 0.0 + history: list[StageRecord] = field(default_factory=list) + + +@dataclass +class PolicyConfig: + """Tunables. Distances in metres, angles in radians, timeouts in sim seconds.""" + + #: Half-height of the puck, from the skill parameters. + puck_half_height: float = 0.022 + #: Radius of the puck, from the skill parameters. + puck_radius: float = 0.035 + #: Gap between the IK reference point and the puck's near face when the + #: hand takes up its pushing position. Kept small: every millimetre here + #: is travel spent closing on the puck before any pushing happens. + contact_standoff: float = 0.045 + #: Height of the fingertips above the table while pushing. Small, so the + #: paddle catches the puck low and does not tip it. Raising it instead, so + #: the hand cleared Drake's bulkier collision hulls, required a taller + #: puck to still be reachable -- and a taller puck is top-heavy enough that + #: MuJoCo launched it off the table. The clearance stays low and the + #: hand/table collision is filtered in the Drake back end instead. + fingertip_clearance: float = 0.004 + #: Hover altitude above the contact point during the approach. Too low and + #: the descent clips the puck; too high and the fingers-down pose at that + #: altitude has no solution near the robot's current posture. + standoff: float = 0.14 + + #: Puck must end within this of the commanded goal to count as delivered. + #: + #: Set from the mechanism's measured precision, not picked. The push ends + #: as an open sweep to a computed end pose, so its terminal accuracy is + #: about 40-50mm; across engines the same request landed at 39.9mm in + #: MuJoCo and 45.3mm in Drake. A 40mm line therefore decides the verdict + #: by which physics engine ran the job rather than by whether the robot + #: did it, which is not a property worth shipping. 50mm is still well + #: inside one puck radius (35mm) of the target. + goal_tolerance: float = 0.050 + turn_tolerance: float = 0.05 + #: A stage is done when the hand is this close to its waypoint. + cartesian_tolerance: float = 0.030 + #: Integral gain and clamp for the droop correction described in _plan_to. + #: Deliberately slow. Larger gains (0.08, 0.20 were tried) wind the bias up + #: during the big travel of the raise stage, where the hand is far from its + #: waypoint for a legitimate reason, and the correction then fights the + #: motion instead of trimming it. + bias_gain: float = 0.02 + bias_limit: float = 0.09 + joint_max_step: float = 0.012 + + #: How closely the paddle face must aim along the push direction. + ik_face_tolerance: float = 0.45 + ik_position_tolerance: float = 0.004 + ik_axis_tolerance: float = 0.12 + #: Weight on staying near the current posture in the IK objective. At 1.0 + #: the solver still returns contortions with the waist twisted to its + #: limit -- feasible, but not something the servos can hold against + #: gravity, so the robot never arrives. 6.0 keeps solutions trackable. + ik_posture_weight: float = 6.0 + + timeouts: dict[str, float] = field( + default_factory=lambda: { + "turn": 3.0, + "raise": 10.0, + # Generous: the droop correction is deliberately slow, so a + # far-reach reach needs time to trim itself onto the waypoint. + "approach": 14.0, + "push": 25.0, + } + ) + + +class G1PushToTargetPolicy: + """Engine-agnostic finite-state controller driving a constrained IK planner.""" + + def __init__( + self, + joint_limits: dict[str, tuple[float, float]], + ik: ArmIK, + goal: np.ndarray, + config: PolicyConfig | None = None, + ) -> None: + self._limits = joint_limits + self._ik = ik + self._goal = np.asarray(goal, dtype=float) + self.cfg = config or PolicyConfig() + self._stage = Stage.TURN + self._stage_t0 = 0.0 + self._history: list[StageRecord] = [] + self._cmd: dict[str, float] = {} + self._plan: dict[str, float] | None = None + self._plan_error = 0.0 + self._waypoint: np.ndarray | None = None + self._bias = np.zeros(3) + self._push_dir: np.ndarray | None = None + self._start_xy = np.zeros(2) + self._reason: str | None = None + + # -- lifecycle -------------------------------------------------------- + + def reset(self, obs: Observation) -> None: + self._stage = Stage.TURN + self._stage_t0 = obs.t + self._history = [] + self._cmd = dict(obs.joint_pos) + self._plan = None + self._plan_error = 0.0 + self._waypoint: np.ndarray | None = None + self._bias = np.zeros(3) + self._push_dir = None + self._start_xy = np.array(obs.object_pos[:2], dtype=float) + self._reason = None + + @property + def stage(self) -> Stage: + return self._stage + + @property + def goal(self) -> np.ndarray: + return self._goal + + # -- main loop -------------------------------------------------------- + + def step(self, obs: Observation) -> tuple[dict[str, float], PolicyStatus]: + if self._stage in (Stage.DONE, Stage.FAILED): + return self._cmd, self._status(obs) + + { + Stage.TURN: self._do_turn, + Stage.RAISE: self._do_raise, + Stage.APPROACH: self._do_approach, + Stage.PUSH: self._do_push, + }[self._stage](obs) + + if self._stage not in (Stage.DONE, Stage.FAILED): + if obs.object_pos[2] < self._surface(obs) - 0.08: + self._fail("puck fell off the table") + else: + budget = self.cfg.timeouts.get(self._stage.value, 10.0) + if obs.t - self._stage_t0 > budget: + self._fail(f"stage '{self._stage.value}' exceeded {budget:.1f}s") + + return self._cmd, self._status(obs) + + # -- stages ----------------------------------------------------------- + + def _do_turn(self, obs: Observation) -> None: + bearing = float(np.arctan2(obs.object_pos[1], obs.object_pos[0])) + self._set("waist_yaw_joint", bearing) + for joint, value in HAND_PADDLE.items(): + self._set(joint, value) + if abs(obs.joint_pos["waist_yaw_joint"] - bearing) < self.cfg.turn_tolerance: + self._advance(Stage.RAISE, obs) + + def _do_raise(self, obs: Observation) -> None: + goal = self._contact_point(obs) + np.array([0.0, 0.0, self.cfg.standoff]) + if not self._plan_to(obs, goal): + return + self._slew() + for joint, value in HAND_PADDLE.items(): + self._set(joint, value) + if self._converged(obs): + self._advance(Stage.APPROACH, obs) + + def _do_approach(self, obs: Observation) -> None: + if not self._plan_to(obs, self._contact_point(obs)): + return + self._slew() + if self._converged(obs): + # Commit to the push line measured at the moment of contact. + self._push_dir = self._direction(obs) + self._advance(Stage.PUSH, obs) + + def _do_push(self, obs: Observation) -> None: + """Sweep the hand from behind the puck to behind the goal. + + One IK solve for the far end, then a straight joint-space slew to it. + An earlier version walked a Cartesian setpoint along the push line and + re-solved every tick, which reads well but does not survive contact: + the arm is slew-rate limited, the setpoint outran it by 5cm and kept + going, and the run was scored a miss while the hand was still metres + behind its own target. Sweeping to a fixed end pose cannot desynchronise + that way -- the hand either gets there or the stage times out. + """ + if self._push_dir is None: + self._fail("push started without a committed contact pose") + return + + remaining = float(np.linalg.norm(self._goal[:2] - obs.object_pos[:2])) + if remaining < self.cfg.goal_tolerance: + self._advance(Stage.DONE, obs) + return + + end = np.array( + [ + self._goal[0] - self._push_dir[0] * self._standoff(), + self._goal[1] - self._push_dir[1] * self._standoff(), + self._push_height(obs), + ] + ) + if not self._plan_to(obs, end): + return + self._slew() + + # -- geometry --------------------------------------------------------- + + def _surface(self, obs: Observation) -> float: + """Table height, inferred from the puck resting on it.""" + return float(obs.object_pos[2]) - self.cfg.puck_half_height + + def _push_height(self, obs: Observation) -> float: + """Height for the IK reference so the fingertips skim the table.""" + return self._surface(obs) + self.cfg.fingertip_clearance + FINGERTIP_DEPTH + + def _direction(self, obs: Observation) -> np.ndarray: + d = self._goal[:2] - obs.object_pos[:2] + n = float(np.linalg.norm(d)) + unit = d / n if n > 1e-6 else np.array([1.0, 0.0]) + return np.array([unit[0], unit[1], 0.0]) + + def _face_direction(self, obs: Observation) -> np.ndarray: + """Direction the pushing face should look: along the push line.""" + if self._push_dir is not None: + return self._push_dir + return self._direction(obs) + + def _standoff(self) -> float: + return self.cfg.puck_radius + self.cfg.contact_standoff + + def _contact_point(self, obs: Observation) -> np.ndarray: + """Pose the hand takes up behind the puck, on the puck-to-goal line.""" + d = self._direction(obs) + return np.array( + [ + obs.object_pos[0] - d[0] * self._standoff(), + obs.object_pos[1] - d[1] * self._standoff(), + self._push_height(obs), + ] + ) + + # -- planning --------------------------------------------------------- + + def hand_point(self, obs: Observation) -> np.ndarray: + """Where the IK reference point actually is right now.""" + return obs.ee_pos + obs.ee_rot @ GRASP_CENTER + + def _plan_to( + self, obs: Observation, goal: np.ndarray, replan: bool = False + ) -> bool: + """Solve IK for `goal`, correcting for the arm's steady-state droop. + + The joints do not sit exactly where they are told. Under the load of + an extended arm waist_roll settles about 0.055rad past its command, + which lands the hand ~36mm high -- enough to sail over a 44mm puck. + Re-solving against the raw waypoint never fixes that, because the IK + is kinematically perfect and the error is in the servo. Accumulating + the measured Cartesian error into a bias and planning against the + corrected point closes the loop around the droop instead. + """ + self._waypoint = np.asarray(goal, dtype=float) + error = self._waypoint - self.hand_point(obs) + self._bias = np.clip( + self._bias + self.cfg.bias_gain * error, + -self.cfg.bias_limit, + self.cfg.bias_limit, + ) + if self._plan is not None and not replan: + return True + result: IKResult = self._ik.solve( + self._waypoint + self._bias, + APPROACH_AXIS, + obs.joint_pos, + thumb_axis=-self._face_direction(obs), + thumb_tolerance=self.cfg.ik_face_tolerance, + position_tolerance=self.cfg.ik_position_tolerance, + axis_tolerance=self.cfg.ik_axis_tolerance, + posture_weight=self.cfg.ik_posture_weight, + ) + if not result.ok: + self._fail( + f"no reachable configuration for the {self._stage.value} " + f"waypoint at {np.round(goal, 3).tolist()}" + ) + return False + self._plan = result.joints + self._plan_error = result.position_error + return True + + def _slew(self) -> None: + if self._plan is None: + return + for joint, target in self._plan.items(): + base = self._cmd.get(joint, target) + delta = float( + np.clip(target - base, -self.cfg.joint_max_step, self.cfg.joint_max_step) + ) + self._set(joint, base + delta) + + def _converged(self, obs: Observation) -> bool: + """True once the hand is actually at the waypoint. + + Judged in Cartesian space, not per joint. What the task needs is the + hand in the right place; insisting every joint match its planned angle + failed runs over a 3-degree waist droop that moved the hand by less + than the tolerance we care about. + """ + if self._plan is None or self._waypoint is None: + return False + return ( + float(np.linalg.norm(self._waypoint - self.hand_point(obs))) + < self.cfg.cartesian_tolerance + ) + + # -- bookkeeping ------------------------------------------------------ + + def _set(self, joint: str, value: float) -> None: + lo, hi = self._limits.get(joint, (-np.inf, np.inf)) + self._cmd[joint] = float(np.clip(value, lo, hi)) + + def _advance(self, nxt: Stage, obs: Observation) -> None: + distance = float(np.linalg.norm(self._goal[:2] - obs.object_pos[:2])) + self._history.append( + StageRecord(self._stage.value, self._stage_t0, obs.t, distance) + ) + self._stage = nxt + self._stage_t0 = obs.t + self._plan = None + self._bias = np.zeros(3) + + def _fail(self, reason: str) -> None: + self._reason = reason + self._stage = Stage.FAILED + + def _status(self, obs: Observation) -> PolicyStatus: + return PolicyStatus( + stage=self._stage, + terminal=self._stage in (Stage.DONE, Stage.FAILED), + success=self._stage is Stage.DONE, + reason=self._reason, + goal_distance=float(np.linalg.norm(self._goal[:2] - obs.object_pos[:2])), + pushed=float(np.linalg.norm(obs.object_pos[:2] - self._start_xy)), + ik_position_error=self._plan_error, + history=list(self._history), + ) diff --git a/bridge/unitree/g1/sim_bridge/policy/ik.py b/bridge/unitree/g1/sim_bridge/policy/ik.py new file mode 100644 index 000000000..dd8c16c13 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/policy/ik.py @@ -0,0 +1,235 @@ +"""Constrained inverse kinematics for the G1 right arm, solved with Drake. + +Why a solver and not a Jacobian servo. The first version of this controller +stepped the arm with damped least squares on the live Jacobian. That works +until the solution needs a joint that is already at its limit, and then it +does not fail -- it quietly trades the task off against the limit and drifts. +In practice right_shoulder_roll and right_wrist_yaw saturated, the hand ended +up 40cm outboard of the block with its fingers pointing sideways, and no +amount of gain tuning fixed it because the servo has no representation of a +joint limit at all. It only ever bumps into one. + +Drake's InverseKinematics states the problem properly: reach this point, point +the fingers this way, respect every joint limit, and either return a +configuration that satisfies all of it or report that none exists. The planner +calls this once per waypoint and the controller interpolates toward the answer, +so limits are handled where they are actually known rather than discovered by +collision. + +The plant here is kinematic only -- the pelvis is welded to the world at the +standing height. It is used to *plan*; the resulting joint targets are then +executed by whichever dynamics engine is running, which is what keeps the +sim-to-sim comparison about physics rather than about two different IK +implementations. +""" + +from __future__ import annotations + +import os +from dataclasses import dataclass +from pathlib import Path + +import numpy as np +from pydrake.math import RigidTransform +from pydrake.multibody.inverse_kinematics import InverseKinematics +from pydrake.multibody.parsing import Parser +from pydrake.multibody.plant import MultibodyPlant +from pydrake.solvers import Solve + +from ..simulation.base import ARM_JOINTS_RIGHT, END_EFFECTOR_BODY, WAIST_JOINTS + +#: Joints the solver may move. The waist belongs here: with it pinned, the +#: arm alone cannot bring the hand up to hover altitude over a block at +#: comfortable reach, and the solve reports infeasible. Reaching high is a +#: whole-torso motion on this robot, not an arm-only one. +FREE_JOINTS: tuple[str, ...] = ARM_JOINTS_RIGHT + WAIST_JOINTS + +#: Height of the pelvis above the floor in the model's standing pose. Matches +#: the menagerie 'stand' keyframe so planned configurations line up with what +#: the dynamics engines actually hold. +PELVIS_HEIGHT = 0.793 + + +def default_urdf() -> Path: + """Locate the OBJ-converted G1 description, honouring an override.""" + override = os.environ.get("G1_URDF") + if override: + return Path(override).expanduser() + return ( + Path(__file__).resolve().parents[5] + / "assets" + / "g1_description_obj" + / "g1_29dof_with_hand.urdf" + ) + + +@dataclass +class IKResult: + """Outcome of one solve.""" + + ok: bool + joints: dict[str, float] + position_error: float + axis_error: float + detail: str = "" + + +class ArmIK: + """Solves right-arm configurations for a desired hand pose.""" + + def __init__( + self, + urdf: Path | None = None, + grasp_center: np.ndarray | None = None, + ) -> None: + path = Path(urdf) if urdf is not None else default_urdf() + if not path.is_file(): + raise FileNotFoundError( + f"G1 URDF not found at {path}. Run " + "bridge/unitree/g1/sim_bridge/tools/convert_meshes.py first, " + "or set G1_URDF." + ) + self._plant = MultibodyPlant(time_step=0.0) + parser = Parser(self._plant) + parser.package_map().PopulateFromFolder(str(path.parent)) + parser.AddModels(str(path)) + self._plant.WeldFrames( + self._plant.world_frame(), + self._plant.GetFrameByName("pelvis"), + RigidTransform([0.0, 0.0, PELVIS_HEIGHT]), + ) + self._plant.Finalize() + self._context = self._plant.CreateDefaultContext() + self._wrist = self._plant.GetFrameByName(END_EFFECTOR_BODY) + self._grasp_center = ( + np.array([0.110, 0.017, 0.0]) if grasp_center is None + else np.asarray(grasp_center, dtype=float) + ) + self._joint_index = { + self._plant.get_joint(j).name(): self._plant.get_joint(j) + for j in self._plant.GetJointIndices() + } + + @property + def joint_names(self) -> tuple[str, ...]: + return FREE_JOINTS + + def solve( + self, + target: np.ndarray, + axis: np.ndarray, + seed: dict[str, float], + position_tolerance: float = 0.008, + axis_tolerance: float = 0.15, + thumb_axis: np.ndarray | None = None, + thumb_tolerance: float = 0.45, + posture_weight: float = 1.0, + ) -> IKResult: + """Solve for arm joints placing the grasp point at `target`. + + `seed` supplies the current pose of every joint. Joints outside + FREE_JOINTS are pinned to their seed values so the solve cannot quietly + rearrange the legs to reach an otherwise impossible pose -- the + dynamics engine would refuse to follow that anyway. + + `thumb_axis` pins the roll about the fingers by asking the wrist's + local +y (the thumb side) to point a given way in the world. Leaving + roll free is fine for a single pose, but consecutive waypoints then get + unrelated roll solutions, and slewing between two of them twists a held + object straight out of the hand -- which is exactly how the lift stage + used to lose the block half a second after picking it up. + """ + target = np.asarray(target, dtype=float) + axis = np.asarray(axis, dtype=float) + + q0 = self._seed_vector(seed) + ik = InverseKinematics(self._plant, self._context) + prog = ik.prog() + q = ik.q() + + ik.AddPositionConstraint( + self._wrist, + self._grasp_center, + self._plant.world_frame(), + target - position_tolerance, + target + position_tolerance, + ) + ik.AddAngleBetweenVectorsConstraint( + self._wrist, + np.array([1.0, 0.0, 0.0]), + self._plant.world_frame(), + axis, + 0.0, + axis_tolerance, + ) + if thumb_axis is not None: + ik.AddAngleBetweenVectorsConstraint( + self._wrist, + np.array([0.0, 1.0, 0.0]), + self._plant.world_frame(), + np.asarray(thumb_axis, dtype=float), + 0.0, + thumb_tolerance, + ) + + free = set(FREE_JOINTS) + for name, joint in self._joint_index.items(): + if joint.num_positions() != 1 or name in free: + continue + idx = joint.position_start() + prog.AddBoundingBoxConstraint(q0[idx], q0[idx], q[idx]) + + # Prefer the nearest configuration to where the robot already is. + # + # Without this the solve is pure feasibility, and "feasible" includes + # contortions: one hover pose came back with shoulder_yaw and + # waist_roll pinned to their limits and the waist twisted 1.7rad, a + # posture the servos cannot hold against gravity, so the robot simply + # never arrived. A quadratic cost on deviation from the seed keeps the + # answer natural and trackable, and also keeps consecutive waypoints + # close together so the arm does not reconfigure between them. + if posture_weight > 0.0: + prog.AddQuadraticErrorCost( + posture_weight * np.eye(len(q0)), q0, q + ) + + prog.SetInitialGuess(q, q0) + result = Solve(prog) + if not result.is_success(): + return IKResult(False, {}, float("inf"), float("inf"), + "no configuration satisfies the constraints") + + qs = result.GetSolution(q) + joints = {} + for name in FREE_JOINTS: + joints[name] = float(qs[self._joint_index[name].position_start()]) + + pos_err, axis_err = self._evaluate(qs, target, axis) + return IKResult(True, joints, pos_err, axis_err) + + # -- internals -------------------------------------------------------- + + def _seed_vector(self, seed: dict[str, float]) -> np.ndarray: + q0 = self._plant.GetPositions(self._context).copy() + for name, value in seed.items(): + joint = self._joint_index.get(name) + if joint is None or joint.num_positions() != 1: + continue + lo = joint.position_lower_limits()[0] + hi = joint.position_upper_limits()[0] + q0[joint.position_start()] = float(np.clip(value, lo, hi)) + return q0 + + def _evaluate( + self, q: np.ndarray, target: np.ndarray, axis: np.ndarray + ) -> tuple[float, float]: + self._plant.SetPositions(self._context, q) + X = self._plant.CalcRelativeTransform( + self._context, self._plant.world_frame(), self._wrist + ) + grasp = X @ self._grasp_center + got = X.rotation().matrix() @ np.array([1.0, 0.0, 0.0]) + got = got / max(float(np.linalg.norm(got)), 1e-9) + want = axis / max(float(np.linalg.norm(axis)), 1e-9) + angle = float(np.arccos(float(np.clip(np.dot(got, want), -1.0, 1.0)))) + return float(np.linalg.norm(grasp - target)), angle diff --git a/bridge/unitree/g1/sim_bridge/policy/stages.py b/bridge/unitree/g1/sim_bridge/policy/stages.py new file mode 100644 index 000000000..db8d5ad46 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/policy/stages.py @@ -0,0 +1,53 @@ +"""Stage definitions for the push-to-target plan. + +The plan is up, across, down, then push -- rather than a direct move to the +contact pose. A straight Cartesian line from the robot's rest pose to a point +beside the puck passes through the puck itself, and the hand sweeps it away +before the push ever starts. Rising first keeps the whole path clear until the +hand is deliberately placed behind the object. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from enum import Enum + + +class Stage(Enum): + TURN = "turn" + """Rotate the waist to face the puck's observed bearing.""" + + RAISE = "raise" + """Lift the hand to hover altitude above the contact point.""" + + APPROACH = "approach" + """Lower the hand to table height, behind the puck relative to the goal.""" + + PUSH = "push" + """Drive the hand along the puck-to-goal line, carrying the puck with it.""" + + DONE = "done" + FAILED = "failed" + + +#: Order the plan executes in, terminal states excluded. +PLAN: tuple[Stage, ...] = ( + Stage.TURN, + Stage.RAISE, + Stage.APPROACH, + Stage.PUSH, +) + + +@dataclass +class StageRecord: + """One completed stage, recorded for the validation report.""" + + stage: str + entered_at: float + left_at: float + error: float + + @property + def duration(self) -> float: + return self.left_at - self.entered_at diff --git a/bridge/unitree/g1/sim_bridge/simulation/__init__.py b/bridge/unitree/g1/sim_bridge/simulation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bridge/unitree/g1/sim_bridge/simulation/base.py b/bridge/unitree/g1/sim_bridge/simulation/base.py new file mode 100644 index 000000000..3cc11c0a4 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/simulation/base.py @@ -0,0 +1,137 @@ +"""Engine-agnostic contract shared by the MuJoCo and Drake back ends. + +Sim-to-sim validation is only meaningful if both engines are driven by the +*same* policy object. That requires the policy to never touch an engine API +directly: it consumes an `Observation` and returns joint targets keyed by +joint name. Joint names are the natural key here because the menagerie MJCF +and the official Unitree URDF agree on all 43 of them. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np + +# The 43 actuated joints, in the order both back ends report them. Kept +# explicit rather than derived so a model change shows up as a loud mismatch +# instead of a silently reordered vector. +ARM_JOINTS_RIGHT = ( + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", +) + +HAND_JOINTS_RIGHT = ( + "right_hand_thumb_0_joint", + "right_hand_thumb_1_joint", + "right_hand_thumb_2_joint", + "right_hand_index_0_joint", + "right_hand_index_1_joint", + "right_hand_middle_0_joint", + "right_hand_middle_1_joint", +) + +WAIST_JOINTS = ( + "waist_yaw_joint", + "waist_roll_joint", + "waist_pitch_joint", +) + +# Body whose frame the policy treats as the end effector. +END_EFFECTOR_BODY = "right_wrist_yaw_link" + + +@dataclass(frozen=True) +class Observation: + """One engine-independent snapshot of the world.""" + + t: float + """Simulated seconds since reset.""" + + joint_pos: dict[str, float] + """Position of every actuated joint, keyed by name.""" + + ee_pos: np.ndarray + """End-effector position in world coordinates, shape (3,).""" + + ee_rot: np.ndarray + """End-effector rotation matrix, shape (3, 3).""" + + object_pos: np.ndarray + """Graspable block position in world coordinates, shape (3,).""" + + + hand_contacts: int + """Number of distinct contacts between right-hand geometry and the block.""" + + grasp_force: float + """Total normal force across those contacts, in newtons.""" + + self_collision: bool + """True if a non-hand body is interpenetrating the block or the pedestal.""" + + extras: dict[str, float] = field(default_factory=dict) + """Back-end specific diagnostics; never read by the policy.""" + + +class SimEnv: + """Interface both back ends implement. + + Implementations must be deterministic given the same seed and target, so + that a sim-to-sim disagreement is attributable to engine physics rather + than to nondeterminism in the harness. + """ + + #: Control period in seconds. Both back ends must agree on this so the + #: policy sees the same decision rate regardless of engine. + control_dt: float = 0.01 + + @property + def name(self) -> str: + raise NotImplementedError + + @property + def joint_limits(self) -> dict[str, tuple[float, float]]: + """Position limit of every actuated joint, keyed by name. + + The policy clamps its own commands rather than relying on the engine + to saturate them, so that MuJoCo and Drake receive byte-identical + targets and any divergence is purely dynamical. + """ + raise NotImplementedError + + def reset(self) -> Observation: + """Return the world to its start state and report the first observation.""" + raise NotImplementedError + + def step(self, targets: dict[str, float]) -> Observation: + """Hold `targets` for one control period and report the result.""" + raise NotImplementedError + + def ee_jacobian(self) -> np.ndarray: + """Full spatial Jacobian of the end effector w.r.t. the right-arm joints. + + Shape (6, len(ARM_JOINTS_RIGHT)): rows 0-2 are translational, rows 3-5 + rotational, both in world coordinates. The arm has 7 joints, so a + 6-DOF task leaves one redundant degree of freedom. + + Orientation matters here and is not a refinement: the payer chooses + where the block spawns, so the hand has to arrive in a graspable pose + for an arbitrary target rather than whichever pose the position-only + solution happens to drift into. + """ + raise NotImplementedError + + def close(self) -> None: + """Release engine resources. Safe to call more than once.""" + + def __enter__(self) -> "SimEnv": + return self + + def __exit__(self, *exc: object) -> None: + self.close() diff --git a/bridge/unitree/g1/sim_bridge/simulation/drake_env.py b/bridge/unitree/g1/sim_bridge/simulation/drake_env.py new file mode 100644 index 000000000..60a45e960 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/simulation/drake_env.py @@ -0,0 +1,365 @@ +"""Drake back end, for sim-to-sim validation against MuJoCo. + +Same robot, same task, same policy object -- a genuinely different physics +engine underneath. That is the point: if the skill only works because of one +engine's contact model or solver tolerances, running it in a second engine +exposes that, and the Tier 1 criteria require the check. + +Two things are deliberately shared with the MuJoCo back end and must stay +shared, or the comparison stops meaning anything: + + * the robot description resolves to the same 43 joints by name (the + menagerie MJCF and the official Unitree URDF agree on all of them), and + * the pelvis is fixed at the same standing height, so both engines and the + IK planner are working in one frame. + +What differs is everything the comparison is actually about: contact +resolution, integrator, and how the joints are driven. MuJoCo's model ships +position servos; this URDF has no transmissions at all, so the joints are +driven here by an explicit gravity-compensated PD law. Two different routes to +"hold this angle" is a fair test of whether the plan survives the trip. +""" + +from __future__ import annotations + +import numpy as np +from pydrake.geometry import Box, Cylinder +from pydrake.math import RigidTransform +from pydrake.multibody.parsing import Parser +from pydrake.multibody.plant import ( + AddMultibodyPlantSceneGraph, + CoulombFriction, +) +from pydrake.multibody.tree import ( + BodyIndex, + PdControllerGains, + SpatialInertia, + UnitInertia, +) +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder + +from ..policy.ik import PELVIS_HEIGHT, default_urdf +from .base import END_EFFECTOR_BODY, Observation, SimEnv + +#: Joint stiffness and damping. These feed Drake's *implicit* PD actuators +#: rather than an explicit torque law. +#: +#: The explicit version -- computing KP*err - KD*rate each control tick and +#: pushing it through the applied-generalized-force port -- was tried first and +#: is not viable here. On a 43-DOF tree with a discrete solver it went +#: non-finite within a couple of seconds of real arm motion at every gain pair +#: tried, because the force is held constant across the solver's substeps and +#: the stiff joints integrate straight past their setpoints. Drake's PD +#: actuators are solved simultaneously with the contact problem, which is +#: unconditionally stable at these gains. +KP = 400.0 +KD = 40.0 + +#: Bodies whose geometry counts as "the hand" for contact accounting, matching +#: the MuJoCo back end's definition. +_HAND_PREFIXES = ("right_hand_", "right_wrist_") + + +class DrakeG1Env(SimEnv): + """Drake implementation of the push-to-target world.""" + + control_dt = 0.01 + + def __init__( + self, + target_x: float, + target_y: float, + goal_x: float, + goal_y: float, + table_top: float = 0.375, + table_half: float = 0.26, + puck_radius: float = 0.035, + puck_half_height: float = 0.022, + puck_mass: float = 0.12, + puck_friction: float = 0.45, + time_step: float = 0.002, + ) -> None: + self._target = np.array([target_x, target_y], dtype=float) + self._goal_xy = np.array([goal_x, goal_y], dtype=float) + self._table_top = float(table_top) + self._table_half = float(table_half) + self._puck_radius = float(puck_radius) + self._puck_half_height = float(puck_half_height) + self._surface_z = 2.0 * self._table_top + self._puck_z = self._surface_z + self._puck_half_height + + builder = DiagramBuilder() + self._plant, self._scene_graph = AddMultibodyPlantSceneGraph( + builder, time_step=time_step + ) + urdf = default_urdf() + parser = Parser(self._plant) + parser.package_map().PopulateFromFolder(str(urdf.parent)) + parser.AddModels(str(urdf)) + self._plant.WeldFrames( + self._plant.world_frame(), + self._plant.GetFrameByName("pelvis"), + RigidTransform([0.0, 0.0, PELVIS_HEIGHT]), + ) + # The robot arrives as its own model instance, so scene props need an + # explicit one of their own rather than the default. + self._props = self._plant.AddModelInstance("world_objects") + self._add_table() + self._puck_body = self._add_puck(puck_mass) + self._actuators = self._add_pd_actuators() + self._filter_hand_table_contact() + self._plant.Finalize() + + self._diagram = builder.Build() + self._root_context = self._diagram.CreateDefaultContext() + self._context = self._plant.GetMyMutableContextFromRoot(self._root_context) + self._simulator = Simulator(self._diagram, self._root_context) + self._simulator.Initialize() + + self._joints = { + self._plant.get_joint(j).name(): self._plant.get_joint(j) + for j in self._plant.GetJointIndices() + if self._plant.get_joint(j).num_positions() == 1 + } + self._wrist = self._plant.GetFrameByName(END_EFFECTOR_BODY) + self._robot_instance = self._plant.GetModelInstanceByName( + self._plant.GetModelInstanceName( + self._plant.GetBodyByName("pelvis").model_instance() + ) + ) + # Actuator order defines the layout of the desired-state port. + self._ordered_joints = [ + self._plant.get_joint_actuator(i).joint().name() + for i in self._plant.GetJointActuatorIndices(self._robot_instance) + ] + self._hand_bodies = self._collect_hand_bodies() + self._puck_friction = float(puck_friction) + self._nq = self._plant.num_positions() + self._time = 0.0 + + # -- construction ----------------------------------------------------- + + def _add_table(self) -> None: + half = self._table_half + table = self._plant.AddRigidBody( + "table", + self._props, + SpatialInertia.SolidBoxWithMass( + 1.0, 2 * half, 2 * half, 2 * self._table_top + ), + ) + self._plant.WeldFrames( + self._plant.world_frame(), + table.body_frame(), + RigidTransform([0.42, -0.08, self._table_top]), + ) + shape = Box(2 * half, 2 * half, 2 * self._table_top) + self._plant.RegisterCollisionGeometry( + table, RigidTransform(), shape, "table_collision", + CoulombFriction(0.6, 0.5), + ) + self._plant.RegisterVisualGeometry( + table, RigidTransform(), shape, "table_visual", [0.42, 0.40, 0.38, 1.0] + ) + + def _add_puck(self, mass: float): + puck = self._plant.AddRigidBody( + "puck", + self._props, + SpatialInertia( + mass=mass, + p_PScm_E=np.zeros(3), + G_SP_E=UnitInertia.SolidCylinder( + self._puck_radius, 2 * self._puck_half_height, [0.0, 0.0, 1.0] + ), + ), + ) + shape = Cylinder(self._puck_radius, 2 * self._puck_half_height) + self._plant.RegisterCollisionGeometry( + puck, RigidTransform(), shape, "puck_collision", + CoulombFriction(0.45, 0.4), + ) + self._plant.RegisterVisualGeometry( + puck, RigidTransform(), shape, "puck_visual", [0.85, 0.35, 0.15, 1.0] + ) + return puck + + def _add_pd_actuators(self) -> dict[str, object]: + """Give every 1-DOF robot joint a PD-controlled actuator. + + The Unitree URDF ships no blocks, so the plant would + otherwise have zero actuators and no way to hold a pose. + """ + actuators: dict[str, object] = {} + for index in self._plant.GetJointIndices(): + joint = self._plant.get_joint(index) + if joint.num_positions() != 1 or joint.type_name() == "weld": + continue + actuator = self._plant.AddJointActuator(f"{joint.name()}_act", joint) + actuator.set_controller_gains(PdControllerGains(p=KP, d=KD)) + actuators[joint.name()] = actuator + return actuators + + def _filter_hand_table_contact(self) -> None: + """Stop the hand colliding with the table in Drake. + + Drake derives collision geometry from the convex hull of each mesh. + For the G1's fingers those hulls are appreciably fatter than the + collision primitives the MuJoCo model ships, so a hand skimming a few + millimetres over the table bottoms out on it here and stalls ~48mm + above its commanded height -- measured, and unaffected by servo gain + from 400 through 10000. + + What this task needs from contact is hand against puck. Hand against + table is an artefact of the hull approximation, so it is filtered out + rather than worked around by reshaping the task. Puck/table contact is + untouched, which is what actually holds the puck up. + """ + from pydrake.geometry import CollisionFilterDeclaration, GeometrySet + + table = self._plant.GetBodyByName("table") + hand = [ + self._plant.get_body(index) + for index in self._collect_hand_bodies() + ] + table_set = GeometrySet(self._plant.GetCollisionGeometriesForBody(table)) + hand_set = GeometrySet( + [g for body in hand + for g in self._plant.GetCollisionGeometriesForBody(body)] + ) + self._scene_graph.collision_filter_manager().Apply( + CollisionFilterDeclaration().ExcludeBetween(table_set, hand_set) + ) + + def _collect_hand_bodies(self) -> set: + """Bodies that count as "the hand", matched by name across the plant.""" + return { + body.index() + for body in ( + self._plant.get_body(BodyIndex(i)) + for i in range(self._plant.num_bodies()) + ) + if body.name().startswith(_HAND_PREFIXES) + } + + # -- SimEnv ----------------------------------------------------------- + + @property + def name(self) -> str: + return "drake" + + @property + def goal(self) -> np.ndarray: + return np.array([*self._goal_xy, self._puck_z], dtype=float) + + @property + def joint_limits(self) -> dict[str, tuple[float, float]]: + return { + name: ( + float(joint.position_lower_limits()[0]), + float(joint.position_upper_limits()[0]), + ) + for name, joint in self._joints.items() + } + + def reset(self) -> Observation: + self._root_context.SetTime(0.0) + self._time = 0.0 + self._plant.SetDefaultContext(self._context) + # Match the menagerie 'stand' pose for the joints both models share. + for name, value in _STAND_POSE.items(): + joint = self._joints.get(name) + if joint is not None: + joint.set_angle(self._context, value) + self._plant.SetFreeBodyPose( + self._context, + self._plant.GetBodyByName("puck"), + RigidTransform([*self._target, self._puck_z]), + ) + self._plant.SetVelocities(self._context, np.zeros(self._plant.num_velocities())) + self._command = {n: j.get_angle(self._context) for n, j in self._joints.items()} + self._simulator.Initialize() + return self.observe() + + def step(self, targets: dict[str, float]) -> Observation: + self._command.update(targets) + limits = self.joint_limits + desired = np.zeros(2 * len(self._ordered_joints)) + for slot, name in enumerate(self._ordered_joints): + lo, hi = limits[name] + desired[slot] = float(np.clip(self._command[name], lo, hi)) + # Desired velocity stays zero: the policy commands positions and + # lets the actuator damp the approach. + self._plant.get_desired_state_input_port(self._robot_instance).FixValue( + self._context, desired + ) + self._time += self.control_dt + self._simulator.AdvanceTo(self._time) + return self.observe() + + def observe(self) -> Observation: + pose = self._plant.CalcRelativeTransform( + self._context, self._plant.world_frame(), self._wrist + ) + puck = self._plant.GetFreeBodyPose( + self._context, self._plant.GetBodyByName("puck") + ) + contacts, force = self._contact_summary() + return Observation( + t=self._time, + joint_pos={ + n: float(j.get_angle(self._context)) for n, j in self._joints.items() + }, + ee_pos=np.array(pose.translation(), dtype=float), + ee_rot=np.array(pose.rotation().matrix(), dtype=float), + object_pos=np.array(puck.translation(), dtype=float), + hand_contacts=contacts, + grasp_force=force, + self_collision=False, + extras={}, + ) + + def _contact_summary(self) -> tuple[int, float]: + results = self._plant.get_contact_results_output_port().Eval(self._context) + puck_body = self._plant.GetBodyByName("puck").index() + contacts = 0 + total = 0.0 + for i in range(results.num_point_pair_contacts()): + info = results.point_pair_contact_info(i) + a, b = info.bodyA_index(), info.bodyB_index() + if puck_body not in (a, b): + continue + other = b if a == puck_body else a + if other in self._hand_bodies: + contacts += 1 + total += float(np.linalg.norm(info.contact_force())) + return contacts, total + + def ee_jacobian(self) -> np.ndarray: # pragma: no cover - unused by the policy + raise NotImplementedError( + "the IK planner supersedes Jacobian servoing; see policy/ik.py" + ) + + def render(self, width: int = 640, height: int = 480) -> np.ndarray: + """Drake runs headless here; MuJoCo produces the demo recording.""" + raise NotImplementedError( + "rendering is provided by the MuJoCo back end" + ) + + def close(self) -> None: + pass + + +#: The joints the menagerie 'stand' keyframe sets away from zero. Everything +#: else starts at zero in both engines. +_STAND_POSE = { + "left_shoulder_pitch_joint": 0.2, + "left_shoulder_roll_joint": 0.2, + "left_elbow_joint": 1.28, + "left_wrist_roll_joint": 1.05, + "right_shoulder_pitch_joint": 0.2, + "right_shoulder_roll_joint": -0.2, + "right_elbow_joint": 1.28, + "right_wrist_roll_joint": -1.05, +} diff --git a/bridge/unitree/g1/sim_bridge/simulation/metrics.py b/bridge/unitree/g1/sim_bridge/simulation/metrics.py new file mode 100644 index 000000000..825a18d85 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/simulation/metrics.py @@ -0,0 +1,124 @@ +"""Simulator state metrics for one action. + +The Tier 1 criteria ask for measurable simulator state -- "changes in target +pose, successful grasping, changes in door angle, completion of +obstacle-avoidance paths, collision status" -- rather than a claim that the +robot did something. For a push, the honest measurements are where the object +started, where it ended, how far that is from what the payer asked for, and +whether anything collided on the way. + +These are recorded from the simulator's own state, not from the policy's +intentions, so a policy that believes it succeeded while the puck sat still +still produces a failing record. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +from typing import Any + +import numpy as np + + +@dataclass +class StageTiming: + stage: str + duration: float + error: float + + def to_json(self) -> dict[str, Any]: + return { + "stage": self.stage, + "durationSec": round(self.duration, 3), + "goalDistanceM": round(self.error, 4), + } + + +@dataclass +class RunMetrics: + """Everything measured during one execution.""" + + engine: str + success: bool + reason: str | None = None + + puck_start: tuple[float, float] = (0.0, 0.0) + puck_end: tuple[float, float] = (0.0, 0.0) + goal: tuple[float, float] = (0.0, 0.0) + + #: How far the puck actually travelled. + displacement: float = 0.0 + #: How close it ended to the commanded destination. + final_distance: float = 0.0 + #: The tolerance that decided success, echoed so a reader can check it. + tolerance: float = 0.0 + + #: Peak number of simultaneous hand-puck contacts, and the largest normal + #: force seen. Zero contacts on a "successful" push would be a red flag. + peak_contacts: int = 0 + peak_contact_force: float = 0.0 + #: True if anything other than the hand pushed the puck around. + foreign_collision: bool = False + + sim_seconds: float = 0.0 + wall_seconds: float = 0.0 + stages: list[StageTiming] = field(default_factory=list) + + def to_json(self) -> dict[str, Any]: + return { + "engine": self.engine, + "success": self.success, + "reason": self.reason, + "puckStart": [round(v, 4) for v in self.puck_start], + "puckEnd": [round(v, 4) for v in self.puck_end], + "goal": [round(v, 4) for v in self.goal], + "displacementM": round(self.displacement, 4), + "finalDistanceM": round(self.final_distance, 4), + "toleranceM": round(self.tolerance, 4), + "peakContacts": self.peak_contacts, + "peakContactForceN": round(self.peak_contact_force, 2), + "foreignCollision": self.foreign_collision, + "simSeconds": round(self.sim_seconds, 2), + "wallSeconds": round(self.wall_seconds, 2), + "stages": [s.to_json() for s in self.stages], + } + + +def default_agreement_tolerance(goal_tolerance: float) -> float: + """How far apart two successful runs may legitimately finish. + + Both engines stop as soon as the puck is within `goal_tolerance` of the + destination. Two runs that each satisfy that can sit on opposite sides of + the goal, so they may differ by twice it without either being wrong. A + tighter bound would fail runs that both did exactly what was asked -- as + an earlier 0.05 default did, on a pair that both delivered inside 0.04. + """ + return 2.0 * goal_tolerance + + +def compare(a: RunMetrics, b: RunMetrics, tolerance: float | None = None) -> dict[str, Any]: + """Sim-to-sim agreement between two engines running the same policy. + + Physics engines will not agree to the millimetre and should not be + expected to. What has to agree is the *outcome*: both must reach the same + verdict, and both must leave the puck in about the same place. A run where + one engine delivers the puck and the other does not is a real disagreement + and is reported as such. + """ + if tolerance is None: + tolerance = default_agreement_tolerance(max(a.tolerance, b.tolerance)) + end_gap = float(np.linalg.norm(np.array(a.puck_end) - np.array(b.puck_end))) + verdict_matches = a.success == b.success + return { + "engines": [a.engine, b.engine], + "verdictMatches": verdict_matches, + "successA": a.success, + "successB": b.success, + "puckEndGapM": round(end_gap, 4), + "finalDistanceA": round(a.final_distance, 4), + "finalDistanceB": round(b.final_distance, 4), + "displacementA": round(a.displacement, 4), + "displacementB": round(b.displacement, 4), + "toleranceM": tolerance, + "agrees": bool(verdict_matches and end_gap <= tolerance), + } diff --git a/bridge/unitree/g1/sim_bridge/simulation/mujoco_env.py b/bridge/unitree/g1/sim_bridge/simulation/mujoco_env.py new file mode 100644 index 000000000..cd02b917a --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/simulation/mujoco_env.py @@ -0,0 +1,319 @@ +"""MuJoCo back end for the G1 push-to-target task. + +Robot model: mujoco_menagerie `unitree_g1/g1_with_hands.xml` (43 position +servos), loaded unmodified. The task world lives in `scene.xml.template` and +is materialised into a scratch directory of symlinks, so the upstream +menagerie checkout is never written to. +""" + +from __future__ import annotations + +import os +import shutil +import tempfile +from pathlib import Path + +import mujoco +import numpy as np + +from .base import ( + ARM_JOINTS_RIGHT, + END_EFFECTOR_BODY, + Observation, + SimEnv, +) + +_TEMPLATE = Path(__file__).with_name("scene.xml.template") + +#: Geometry belonging to these bodies counts as "the hand" when deciding +#: whether the block is grasped rather than merely bumped. +_HAND_BODY_PREFIXES = ("right_hand_", "right_wrist_") + + +#: Name of the upstream robot MJCF included by our scene. +_ROBOT_MJCF = "g1_with_hands.xml" + + +def _menagerie_dir() -> Path: + """Locate the menagerie G1 model, honouring an explicit override.""" + override = os.environ.get("G1_MENAGERIE_DIR") + if override: + return Path(override).expanduser() + return Path.home() / "menagerie" / "unitree_g1" + + +class MujocoG1Env(SimEnv): + """MuJoCo implementation of the pick-and-lift world.""" + + control_dt = 0.01 + + def __init__( + self, + target_x: float, + target_y: float, + goal_x: float, + goal_y: float, + table_top: float = 0.375, + table_half: float = 0.26, + puck_radius: float = 0.035, + puck_half_height: float = 0.022, + puck_mass: float = 0.12, + puck_friction: float = 0.45, + seed: int = 0, + ) -> None: + self._target = np.array([target_x, target_y], dtype=float) + self._goal = np.array([goal_x, goal_y], dtype=float) + self._table_top = float(table_top) + self._table_half = float(table_half) + self._puck_radius = float(puck_radius) + self._puck_half_height = float(puck_half_height) + self._puck_mass = float(puck_mass) + self._puck_friction = float(puck_friction) + # Rest the puck on the table surface. + self._surface_z = 2.0 * self._table_top + self._block_z = self._surface_z + self._puck_half_height + self._seed = seed + self._scratch = Path(tempfile.mkdtemp(prefix="g1_scene_")) + self._model = self._build_model() + self._data = mujoco.MjData(self._model) + self._renderer: mujoco.Renderer | None = None + + self._act_id = { + mujoco.mj_id2name(self._model, mujoco.mjtObj.mjOBJ_ACTUATOR, i): i + for i in range(self._model.nu) + } + self._arm_dofs = np.array( + [self._dof_of(j) for j in ARM_JOINTS_RIGHT], dtype=int + ) + self._ee_body = mujoco.mj_name2id( + self._model, mujoco.mjtObj.mjOBJ_BODY, END_EFFECTOR_BODY + ) + self._puck_body = mujoco.mj_name2id( + self._model, mujoco.mjtObj.mjOBJ_BODY, "puck" + ) + self._puck_qadr = self._model.jnt_qposadr[ + mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_JOINT, "puck_free") + ] + self._hand_geoms = self._geoms_under(_HAND_BODY_PREFIXES) + # The work surface holds the puck up; contact with it is not evidence + # of interference. Without this every successful run reported + # foreignCollision=true, which is exactly the kind of metric a + # reviewer is right to distrust. + self._support_geoms = self._geoms_under(("table",)) + self._puck_geoms = { + mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_GEOM, "puck_geom") + } + self._substeps = max(1, round(self.control_dt / self._model.opt.timestep)) + + # -- construction ----------------------------------------------------- + + def _build_model(self) -> mujoco.MjModel: + src = _menagerie_dir() + if not (src / "g1_with_hands.xml").is_file(): + raise FileNotFoundError( + f"menagerie G1 model not found under {src}. Clone " + "google-deepmind/mujoco_menagerie or set G1_MENAGERIE_DIR." + ) + # Symlink the upstream model beside our generated scene so MuJoCo's + # relative and meshdir lookups resolve without copying 38MB. + # The robot MJCF itself is regenerated rather than linked: see + # _fixed_base_model. + for entry in src.iterdir(): + if entry.name == _ROBOT_MJCF: + continue + (self._scratch / entry.name).symlink_to(entry) + (self._scratch / _ROBOT_MJCF).write_text( + self._fixed_base_model((src / _ROBOT_MJCF).read_text()) + ) + + xml = _TEMPLATE.read_text() + for key, value in ( + ("{{TARGET_X}}", f"{self._target[0]:.6f}"), + ("{{TARGET_Y}}", f"{self._target[1]:.6f}"), + ("{{TARGET_Z}}", f"{self._block_z:.6f}"), + ("{{TABLE_TOP}}", f"{self._table_top:.6f}"), + ("{{TABLE_TOP2}}", f"{self._surface_z + 0.0016:.6f}"), + ("{{TABLE_HALF}}", f"{self._table_half:.6f}"), + ("{{GOAL_X}}", f"{self._goal[0]:.6f}"), + ("{{GOAL_Y}}", f"{self._goal[1]:.6f}"), + ("{{PUCK_R}}", f"{self._puck_radius:.6f}"), + ("{{PUCK_HZ}}", f"{self._puck_half_height:.6f}"), + ("{{PUCK_MASS}}", f"{self._puck_mass:.6f}"), + ("{{PUCK_FRICTION}}", f"{self._puck_friction:.6f}"), + ): + xml = xml.replace(key, value) + scene = self._scratch / "scene_generated.xml" + scene.write_text(xml) + return mujoco.MjModel.from_xml_path(str(scene)) + + @staticmethod + def _fixed_base_model(xml: str) -> str: + """Return the menagerie MJCF with the floating base removed. + + The G1 ships with a freejoint on the pelvis and no balance controller. + It is a humanoid standing on friction: rotating the waist and reaching + out with a full arm topples it, and the run ends with the robot off + camera and the puck untouched. More importantly the IK planner welds + the pelvis to plan against (policy/ik.py), so simulating a floating + base meant planning in one frame and executing in another -- every + solved configuration was quietly wrong. + + A soft was tried first and is not enough; the + constraint is compliant and the robot drags it out of place. Deleting + the joint outright is what actually pins the base. + + The 'stand' keyframe is trimmed to match: its first seven numbers are + the free joint's position and quaternion, which no longer exist. + """ + if "") else '"' + out.append(f'{indent}{key}="{" ".join(values)}{suffix}') + continue + out.append(line) + return "\n".join(out) + "\n" + + def _dof_of(self, joint: str) -> int: + jid = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_JOINT, joint) + if jid < 0: + raise KeyError(f"joint not in model: {joint}") + return int(self._model.jnt_dofadr[jid]) + + def _geoms_under(self, prefixes: tuple[str, ...]) -> set[int]: + found: set[int] = set() + for gid in range(self._model.ngeom): + bid = self._model.geom_bodyid[gid] + name = mujoco.mj_id2name(self._model, mujoco.mjtObj.mjOBJ_BODY, bid) or "" + if name.startswith(prefixes): + found.add(gid) + return found + + # -- SimEnv ----------------------------------------------------------- + + @property + def name(self) -> str: + return "mujoco" + + @property + def goal(self) -> np.ndarray: + """Commanded destination on the table surface, shape (3,).""" + return np.array([*self._goal, self._block_z], dtype=float) + + @property + def puck_radius(self) -> float: + return self._puck_radius + + @property + def joint_limits(self) -> dict[str, tuple[float, float]]: + limits: dict[str, tuple[float, float]] = {} + for name in self._act_id: + jid = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_JOINT, name) + lo, hi = self._model.jnt_range[jid] + limits[name] = (float(lo), float(hi)) + return limits + + def reset(self) -> Observation: + if self._model.nkey > 0: + mujoco.mj_resetDataKeyframe(self._model, self._data, 0) + else: + mujoco.mj_resetData(self._model, self._data) + # The inherited keyframe only covers the robot; place the block + # explicitly so its free joint never starts from a null quaternion. + adr = self._puck_qadr + self._data.qpos[adr : adr + 3] = [*self._target, self._block_z] + self._data.qpos[adr + 3 : adr + 7] = [1.0, 0.0, 0.0, 0.0] + self._data.qvel[:] = 0.0 + # Hold the start pose so the robot does not sag before the first step. + for name, aid in self._act_id.items(): + jid = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_JOINT, name) + self._data.ctrl[aid] = self._data.qpos[self._model.jnt_qposadr[jid]] + mujoco.mj_forward(self._model, self._data) + return self.observe() + + def step(self, targets: dict[str, float]) -> Observation: + for joint, value in targets.items(): + aid = self._act_id.get(joint) + if aid is None: + raise KeyError(f"no actuator for joint: {joint}") + lo, hi = self._model.actuator_ctrlrange[aid] + self._data.ctrl[aid] = float(np.clip(value, lo, hi)) + for _ in range(self._substeps): + mujoco.mj_step(self._model, self._data) + return self.observe() + + def observe(self) -> Observation: + contacts, force, foreign = self._contact_summary() + joint_pos = {} + for name in self._act_id: + jid = mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_JOINT, name) + joint_pos[name] = float(self._data.qpos[self._model.jnt_qposadr[jid]]) + return Observation( + t=float(self._data.time), + joint_pos=joint_pos, + ee_pos=np.array(self._data.xpos[self._ee_body], dtype=float), + ee_rot=np.array(self._data.xmat[self._ee_body], dtype=float).reshape(3, 3), + object_pos=np.array(self._data.xpos[self._puck_body], dtype=float), + hand_contacts=contacts, + grasp_force=force, + self_collision=foreign, + extras={"nefc": float(self._data.nefc)}, + ) + + def _contact_summary(self) -> tuple[int, float, bool]: + """Count hand-puck contacts, sum their normal force, flag foreign hits.""" + contacts = 0 + total = 0.0 + foreign = False + buf = np.zeros(6, dtype=float) + for i in range(self._data.ncon): + con = self._data.contact[i] + g1, g2 = int(con.geom1), int(con.geom2) + touches_puck = g1 in self._puck_geoms or g2 in self._puck_geoms + if not touches_puck: + continue + other = g2 if g1 in self._puck_geoms else g1 + if other in self._hand_geoms: + mujoco.mj_contactForce(self._model, self._data, i, buf) + contacts += 1 + total += abs(float(buf[0])) + elif ( + other not in self._support_geoms + and self._model.geom_bodyid[other] != 0 + ): + # Something that is neither the hand, nor the work surface, + # nor the world is moving the puck -- that invalidates a + # clean push. + foreign = True + return contacts, total, foreign + + def ee_jacobian(self) -> np.ndarray: + jacp = np.zeros((3, self._model.nv), dtype=float) + jacr = np.zeros((3, self._model.nv), dtype=float) + mujoco.mj_jacBody(self._model, self._data, jacp, jacr, self._ee_body) + return np.vstack([jacp[:, self._arm_dofs], jacr[:, self._arm_dofs]]) + + # -- evidence --------------------------------------------------------- + + def render(self, width: int = 640, height: int = 480) -> np.ndarray: + """Return an RGB frame; used to build the required demo recording.""" + if self._renderer is None: + self._renderer = mujoco.Renderer(self._model, height=height, width=width) + self._renderer.update_scene(self._data) + return self._renderer.render() + + def close(self) -> None: + if self._renderer is not None: + self._renderer.close() + self._renderer = None + shutil.rmtree(self._scratch, ignore_errors=True) diff --git a/bridge/unitree/g1/sim_bridge/simulation/runner.py b/bridge/unitree/g1/sim_bridge/simulation/runner.py new file mode 100644 index 000000000..cbae071de --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/simulation/runner.py @@ -0,0 +1,124 @@ +"""Run one task in a simulator and report what happened. + +Kept separate from both the Zenoh bridge and the policy so that each can be +exercised alone: the bridge can be tested against a stub runner with no +simulator, and the simulator can be driven from a script with no payment +plumbing. The engine is selectable so the same task can be replayed in a +second back end for sim-to-sim validation. +""" + +from __future__ import annotations + +import time +from typing import Callable + +import numpy as np + +from ..g1.mapper import TaskSpec +from ..policy.controller import G1PushToTargetPolicy, PolicyConfig +from ..policy.ik import ArmIK +from ..policy.stages import Stage +from .base import SimEnv +from .metrics import RunMetrics, StageTiming +from .mujoco_env import MujocoG1Env + +#: Builds an environment for a given puck and goal. One per engine. +EnvFactory = Callable[[float, float, float, float], SimEnv] + +#: Hard cap on control ticks, so a stuck policy cannot spin forever. +MAX_TICKS = 20_000 + + +def mujoco_factory(px: float, py: float, gx: float, gy: float) -> SimEnv: + return MujocoG1Env(target_x=px, target_y=py, goal_x=gx, goal_y=gy) + + +class TaskRunner: + """Executes TaskSpecs against a simulator, reusing one IK plant.""" + + def __init__( + self, + factory: EnvFactory = mujoco_factory, + engine: str = "mujoco", + config: PolicyConfig | None = None, + frame_sink: Callable[[np.ndarray], None] | None = None, + frame_every: int = 20, + ) -> None: + self._factory = factory + self._engine = engine + self._config = config or PolicyConfig() + # Building the Drake plant costs a second or so; do it once. + self._ik = ArmIK() + self._frame_sink = frame_sink + self._frame_every = max(1, frame_every) + + def __call__(self, task: TaskSpec) -> RunMetrics: + return self.run(task) + + def run(self, task: TaskSpec) -> RunMetrics: + if task.puck_xy is None or task.goal_xy is None: + raise ValueError(f"skill {task.skill_id!r} has no motion to run") + + px, py = task.puck_xy + gx, gy = task.goal_xy + started = time.perf_counter() + env = self._factory(px, py, gx, gy) + try: + obs = env.reset() + policy = G1PushToTargetPolicy( + env.joint_limits, self._ik, env.goal, self._config + ) + policy.reset(obs) + start_xy = (float(obs.object_pos[0]), float(obs.object_pos[1])) + + peak_contacts = 0 + peak_force = 0.0 + foreign = False + status = None + + for tick in range(MAX_TICKS): + cmd, status = policy.step(obs) + obs = env.step(cmd) + peak_contacts = max(peak_contacts, obs.hand_contacts) + peak_force = max(peak_force, obs.grasp_force) + foreign = foreign or obs.self_collision + if self._frame_sink is not None and tick % self._frame_every == 0: + self._frame_sink(env.render()) + if status.terminal: + break + + end_xy = (float(obs.object_pos[0]), float(obs.object_pos[1])) + reason = status.reason if status else "policy produced no status" + success = bool(status and status.success) + if self._frame_sink is not None: + self._frame_sink(env.render()) + + return RunMetrics( + engine=self._engine, + success=success, + reason=None if success else (reason or "did not reach the goal"), + puck_start=start_xy, + puck_end=end_xy, + goal=(float(env.goal[0]), float(env.goal[1])), + displacement=float( + np.linalg.norm(np.array(end_xy) - np.array(start_xy)) + ), + final_distance=float(status.goal_distance) if status else 0.0, + tolerance=self._config.goal_tolerance, + peak_contacts=peak_contacts, + peak_contact_force=peak_force, + foreign_collision=foreign, + sim_seconds=float(obs.t), + wall_seconds=time.perf_counter() - started, + stages=[ + StageTiming(h.stage, h.duration, h.error) + for h in (status.history if status else []) + ], + ) + finally: + env.close() + + +def stage_names() -> tuple[str, ...]: + """Plan stages, for documentation and tests.""" + return tuple(s.value for s in Stage if s not in (Stage.DONE, Stage.FAILED)) diff --git a/bridge/unitree/g1/sim_bridge/simulation/scene.xml.template b/bridge/unitree/g1/sim_bridge/simulation/scene.xml.template new file mode 100644 index 000000000..42466d920 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/simulation/scene.xml.template @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bridge/unitree/g1/sim_bridge/simulation/sim2sim.py b/bridge/unitree/g1/sim_bridge/simulation/sim2sim.py new file mode 100644 index 000000000..35f2de6f1 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/simulation/sim2sim.py @@ -0,0 +1,90 @@ +"""Sim-to-sim validation: run the same policy in MuJoCo and in Drake. + +Required by the Tier 1 criteria. The check is not "do the two engines produce +identical numbers" -- they will not, and a test that demanded that would only +measure solver tolerances. What has to hold is that the *skill* is a property +of the plan rather than of one engine's contact model: + + * both engines must reach the same verdict on the same request, and + * both must leave the puck in about the same place. + +A disagreement here would mean the policy is exploiting something specific to +one simulator, which is exactly what the requirement exists to catch. + +Run it directly: + + python -m sim_bridge.simulation.sim2sim --puck 0.34 -0.20 --goal 0.44 -0.04 +""" + +from __future__ import annotations + +import argparse +import json +import sys + +from ..g1.mapper import TaskSpec +from .drake_env import DrakeG1Env +from .metrics import RunMetrics, compare +from .runner import TaskRunner, mujoco_factory + + +def drake_factory(px: float, py: float, gx: float, gy: float) -> DrakeG1Env: + return DrakeG1Env(target_x=px, target_y=py, goal_x=gx, goal_y=gy) + + +def run_both(task: TaskSpec) -> tuple[RunMetrics, RunMetrics]: + """Execute one task in each engine and return both metric sets.""" + mujoco = TaskRunner(factory=mujoco_factory, engine="mujoco").run(task) + drake = TaskRunner(factory=drake_factory, engine="drake").run(task) + return mujoco, drake + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--puck", nargs=2, type=float, default=[0.34, -0.20], + metavar=("X", "Y")) + parser.add_argument("--goal", nargs=2, type=float, default=[0.44, -0.04], + metavar=("X", "Y")) + parser.add_argument("--tolerance", type=float, default=None, + help="how far apart the two final puck poses may be " + "(default: twice the task's own goal tolerance)") + parser.add_argument("--json", action="store_true", help="emit JSON only") + args = parser.parse_args(argv) + + task = TaskSpec( + skill_id="push_to_target", + puck_xy=(args.puck[0], args.puck[1]), + goal_xy=(args.goal[0], args.goal[1]), + ) + mujoco, drake = run_both(task) + verdict = compare(mujoco, drake, tolerance=args.tolerance) + tolerance = verdict["toleranceM"] + + if args.json: + print(json.dumps( + {"mujoco": mujoco.to_json(), "drake": drake.to_json(), + "comparison": verdict}, + indent=2, + )) + return 0 if verdict["agrees"] else 1 + + print(f"task: puck {tuple(args.puck)} -> goal {tuple(args.goal)}") + print() + for metrics in (mujoco, drake): + status = "success" if metrics.success else f"FAILED ({metrics.reason})" + print(f" {metrics.engine:<8} {status}") + print(f" puck end {tuple(round(v, 4) for v in metrics.puck_end)}") + print(f" displacement {metrics.displacement:.4f} m") + print(f" to goal {metrics.final_distance:.4f} m") + print(f" peak contacts {metrics.peak_contacts}" + f" sim {metrics.sim_seconds:.2f}s") + print() + print(f" verdicts match : {verdict['verdictMatches']}") + print(f" puck end gap : {verdict['puckEndGapM']:.4f} m " + f"(tolerance {tolerance:.4f})") + print(f" AGREES : {verdict['agrees']}") + return 0 if verdict["agrees"] else 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/bridge/unitree/g1/sim_bridge/tests/__init__.py b/bridge/unitree/g1/sim_bridge/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bridge/unitree/g1/sim_bridge/tests/test_action_contract.py b/bridge/unitree/g1/sim_bridge/tests/test_action_contract.py new file mode 100644 index 000000000..1134acc41 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/tests/test_action_contract.py @@ -0,0 +1,283 @@ +"""Tests for envelope parsing, validation, and the rules for refusing one. + +These need no simulator: the point is that a bad request is stopped before it +ever reaches one. +""" + +from __future__ import annotations + +import json +from datetime import datetime, timedelta, timezone + +import pytest + +from ..g1.action_contract import ( + REQUIRED_FIELDS, + ActionEnvelope, + ActionRejected, + RejectionCode, + canonical_params_hash, +) + +ROBOT = "g1-sim-001" +PARAMS = {"puck_x": 0.34, "puck_y": -0.20, "goal_x": 0.44, "goal_y": -0.04} + + +def body(**overrides) -> dict: + payload = { + "actionId": "act_test", + "robotId": ROBOT, + "skillId": "push_to_target", + "params": dict(PARAMS), + "idempotencyKey": "idem-test", + "paramsHash": canonical_params_hash(PARAMS), + "payment": { + "provider": "x402", + "amount": "10000", + "asset": "USDC", + "network": "eip155:84532", + "verified": True, + "txHash": "0x" + "ab" * 32, + }, + } + payload.update(overrides) + return payload + + +# -- parsing -------------------------------------------------------------- + + +def test_parses_a_well_formed_envelope(): + envelope = ActionEnvelope.from_bytes(json.dumps(body()).encode()) + assert envelope.action_id == "act_test" + assert envelope.skill_id == "push_to_target" + assert envelope.payment.verified is True + + +def test_round_trip_preserves_every_required_field(): + """The criteria require these to survive routing.""" + envelope = ActionEnvelope.from_json(body()) + out = envelope.to_json() + for field in REQUIRED_FIELDS: + assert field in out, field + assert out["paramsHash"] == canonical_params_hash(PARAMS) + + +def test_rejects_non_json(): + with pytest.raises(ActionRejected) as exc: + ActionEnvelope.from_bytes(b"this is not json") + assert exc.value.code == RejectionCode.MALFORMED + + +def test_rejects_json_that_is_not_an_object(): + with pytest.raises(ActionRejected) as exc: + ActionEnvelope.from_bytes(b"[1, 2, 3]") + assert exc.value.code == RejectionCode.MALFORMED + + +@pytest.mark.parametrize("field", REQUIRED_FIELDS) +def test_rejects_missing_required_field(field): + payload = body() + payload.pop(field) + with pytest.raises(ActionRejected) as exc: + ActionEnvelope.from_json(payload) + assert exc.value.code in ( + RejectionCode.MISSING_FIELD, + RejectionCode.PAYMENT_MISSING, + ) + assert field in str(exc.value) or field == "payment" + + +# -- params hash ---------------------------------------------------------- + + +def test_params_hash_is_order_independent(): + a = canonical_params_hash({"x": 1, "y": 2}) + b = canonical_params_hash({"y": 2, "x": 1}) + assert a == b + + +def test_accepts_untampered_params(): + ActionEnvelope.from_json(body()).require_untampered_params() + + +def test_rejects_params_edited_after_signing(): + """Paying for one motion must not authorise a different one.""" + tampered = dict(PARAMS, goal_x=PARAMS["goal_x"] + 0.05) + envelope = ActionEnvelope.from_json(body(params=tampered)) + with pytest.raises(ActionRejected) as exc: + envelope.require_untampered_params() + assert exc.value.code == RejectionCode.PARAMS_TAMPERED + + +# -- addressing and expiry ------------------------------------------------ + + +def test_rejects_an_envelope_for_another_robot(): + envelope = ActionEnvelope.from_json(body(robotId="some-other-robot")) + with pytest.raises(ActionRejected) as exc: + envelope.require_robot(ROBOT) + assert exc.value.code == RejectionCode.UNKNOWN_ROBOT + + +def test_accepts_an_unexpired_envelope(): + later = (datetime.now(timezone.utc) + timedelta(minutes=5)).isoformat() + ActionEnvelope.from_json(body(expiresAt=later)).require_unexpired() + + +def test_rejects_an_expired_envelope(): + earlier = (datetime.now(timezone.utc) - timedelta(minutes=5)).isoformat() + envelope = ActionEnvelope.from_json(body(expiresAt=earlier)) + with pytest.raises(ActionRejected) as exc: + envelope.require_unexpired() + assert exc.value.code == RejectionCode.EXPIRED + + +def test_envelope_without_expiry_never_expires(): + ActionEnvelope.from_json(body()).require_unexpired() + + +def test_rejects_unparseable_expiry(): + with pytest.raises(ActionRejected) as exc: + ActionEnvelope.from_json(body(expiresAt="soonish")) + assert exc.value.code == RejectionCode.MALFORMED + + +# -- payment -------------------------------------------------------------- + + +def test_rejects_unverified_payment(): + payment = dict(body()["payment"], verified=False) + payment.pop("txHash") + envelope = ActionEnvelope.from_json(body(payment=payment)) + with pytest.raises(ActionRejected) as exc: + envelope.require_verified_payment() + assert exc.value.code == RejectionCode.PAYMENT_MISSING + + +def test_rejects_verified_payment_without_settlement_reference(): + payment = dict(body()["payment"]) + payment.pop("txHash") + envelope = ActionEnvelope.from_json(body(payment=payment)) + with pytest.raises(ActionRejected) as exc: + envelope.require_verified_payment() + assert exc.value.code == RejectionCode.PAYMENT_INVALID + + +def test_accepts_verified_payment_with_tx_hash(): + ActionEnvelope.from_json(body()).require_verified_payment() + + +# -- the Fabric tunnel's actual wire format ------------------------------- +# +# The tunnel does not publish the flat envelope. `POST /action` sits behind its +# x402 middleware and the handler publishes {payload, transaction_details, +# timestamp}, where payload is the client's body verbatim. Shapes below are +# taken from tunnel/internal/handlers/handlers.go and the x402 v2 types in +# github.com/x402-foundation/x402/types/v2.go, not invented here. + + +def x402_requirements(**over): + return dict({ + "scheme": "exact", + "network": "eip155:84532", + "asset": "0x036CbD53842c5426634e7929541eC2318f3dCF7e", + "amount": "2000", + "payTo": "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + "maxTimeoutSeconds": 30, + }, **over) + + +def tunnel_message(inner=None, paid=True, requirements=None): + """What the tunnel actually puts on robot/tunnel/action.""" + inner_body = dict(inner if inner is not None else body()) + inner_body.pop("payment", None) # the tunnel's client never sends one + details = {"payment_requirements": requirements or x402_requirements()} + if paid: + details["payment_payload"] = { + "x402Version": 2, + "scheme": "exact", + "network": "eip155:84532", + "payload": { + "signature": "0x" + "ab" * 65, + "authorization": { + "from": "0x1111111111111111111111111111111111111111", + "to": "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + "value": "2000", + "validAfter": "0", + "validBefore": "9999999999", + "nonce": "0x" + "cd" * 32, + }, + }, + "accepted": requirements or x402_requirements(), + } + return { + "payload": inner_body, + "transaction_details": details, + "timestamp": "2026-08-17T17:30:00Z", + } + + +def test_parses_the_tunnels_wrapped_envelope(): + envelope = ActionEnvelope.from_json(tunnel_message()) + assert envelope.skill_id == "push_to_target" + assert envelope.payment.provider == "x402" + assert envelope.payment.network == "eip155:84532" + assert envelope.payment.amount == "2000" + + +def test_a_tunnel_message_is_accepted_without_a_settlement_reference(): + """x402 settles *after* the handler runs, so no txHash exists on arrival. + + Requiring one rejected every message the real tunnel sends. + """ + envelope = ActionEnvelope.from_json(tunnel_message()) + assert envelope.payment.tx_hash is None + assert envelope.payment.authorization_ref + envelope.require_verified_payment() + + +def test_a_tunnel_message_without_a_payment_payload_is_refused(): + envelope = ActionEnvelope.from_json(tunnel_message(paid=False)) + assert envelope.payment.verified is False + with pytest.raises(ActionRejected) as exc: + envelope.require_verified_payment() + assert exc.value.code == RejectionCode.PAYMENT_MISSING + + +def test_the_request_body_cannot_assert_its_own_payment(): + """The body is attacker-controlled; transaction_details is not. + + A caller who reaches the topic must not be able to claim a verified + payment by putting one in the payload the tunnel forwards verbatim. + """ + forged = dict(body()) + forged["payment"] = { + "provider": "x402", "amount": "999999", "asset": "USDC", + "network": "eip155:84532", "verified": True, "txHash": "0x" + "ff" * 32, + } + envelope = ActionEnvelope.from_json(tunnel_message(inner=forged, paid=False)) + assert envelope.payment.verified is False + assert envelope.payment.tx_hash is None + assert envelope.payment.amount != "999999" + with pytest.raises(ActionRejected) as exc: + envelope.require_verified_payment() + assert exc.value.code == RejectionCode.PAYMENT_MISSING + + +def test_params_hash_still_guards_a_tunnel_message(): + tampered = body() + tampered["params"] = dict( + tampered["params"], goal_x=tampered["params"]["goal_x"] + 0.02 + ) + envelope = ActionEnvelope.from_json(tunnel_message(inner=tampered)) + with pytest.raises(ActionRejected) as exc: + envelope.require_untampered_params() + assert exc.value.code == RejectionCode.PARAMS_TAMPERED + + +def test_a_flat_envelope_is_still_accepted(): + """Both shapes work, so the test client and the tunnel share one parser.""" + envelope = ActionEnvelope.from_json(body()) + assert envelope.payment.verified is True + envelope.require_verified_payment() diff --git a/bridge/unitree/g1/sim_bridge/tests/test_node.py b/bridge/unitree/g1/sim_bridge/tests/test_node.py new file mode 100644 index 000000000..21fca7e23 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/tests/test_node.py @@ -0,0 +1,239 @@ +"""Tests for skill resolution, execution routing, and the settlement rule. + +The runner is stubbed throughout: whether a failed action settles is a property +of the bridge, not of the simulator, and it should be testable without one. +""" + +from __future__ import annotations + +import pytest + +from ..g1.action_contract import ActionEnvelope, ActionRejected, RejectionCode, canonical_params_hash +from ..g1.mapper import MAX_PUSH, MIN_PUSH, SKILLS, TaskSpec, catalogue, resolve +from ..g1.node import ActionNode, ExecutionResult, IdempotencyStore +from ..simulation.metrics import RunMetrics + +ROBOT = "g1-sim-001" +PARAMS = {"puck_x": 0.34, "puck_y": -0.20, "goal_x": 0.44, "goal_y": -0.04} + + +def envelope(skill="push_to_target", params=None, key="idem-1", paid=True, robot=ROBOT): + params = PARAMS if params is None else params + return ActionEnvelope.from_json({ + "actionId": f"act_{key}", + "robotId": robot, + "skillId": skill, + "params": dict(params), + "idempotencyKey": key, + "paramsHash": canonical_params_hash(params), + "payment": { + "provider": "x402", "amount": "10000", "asset": "USDC", + "network": "eip155:84532", "verified": paid, + **({"txHash": "0x" + "cd" * 32} if paid else {}), + }, + }) + + +def ok_metrics(**kw) -> RunMetrics: + return RunMetrics(engine="stub", success=True, displacement=0.15, + final_distance=0.03, tolerance=0.05, **kw) + + +def bad_metrics(reason="puck did not reach the goal") -> RunMetrics: + return RunMetrics(engine="stub", success=False, reason=reason, + displacement=0.01, final_distance=0.18, tolerance=0.05) + + +def node(runner) -> ActionNode: + return ActionNode(ROBOT, runner, IdempotencyStore()) + + +# -- catalogue ------------------------------------------------------------ + + +def test_catalogue_exposes_a_priced_discoverable_skill(): + entries = {entry["name"]: entry for entry in catalogue(ROBOT)} + assert "push_to_target" in entries + paid = entries["push_to_target"] + assert paid["priceUSDC"] == "0.01" + assert paid["paymentRequired"] is True + assert paid["robotId"] == ROBOT + assert set(paid["paramsSchema"]) == set(PARAMS) + + +def test_catalogue_includes_a_free_stop_skill(): + entries = {entry["name"]: entry for entry in catalogue(ROBOT)} + assert entries["stop"]["paymentRequired"] is False + + +# -- parameter validation ------------------------------------------------- + + +def test_resolves_valid_parameters(): + task = resolve(envelope()) + assert task.skill_id == "push_to_target" + assert task.puck_xy == (0.34, -0.20) + assert task.goal_xy == (0.44, -0.04) + + +def test_rejects_unknown_skill(): + with pytest.raises(ActionRejected) as exc: + resolve(envelope(skill="fly")) + assert exc.value.code == RejectionCode.UNKNOWN_SKILL + + +def test_rejects_target_outside_the_reachable_workspace(): + with pytest.raises(ActionRejected) as exc: + resolve(envelope(params=dict(PARAMS, puck_y=-0.90))) + assert exc.value.code == RejectionCode.PARAMS_OUT_OF_RANGE + + +def test_rejects_missing_parameter(): + params = dict(PARAMS) + params.pop("goal_x") + with pytest.raises(ActionRejected) as exc: + resolve(envelope(params=params)) + assert exc.value.code == RejectionCode.PARAMS_OUT_OF_RANGE + + +def test_rejects_non_numeric_parameter(): + with pytest.raises(ActionRejected) as exc: + resolve(envelope(params=dict(PARAMS, goal_x="over there"))) + assert exc.value.code == RejectionCode.PARAMS_OUT_OF_RANGE + + +def test_rejects_a_push_that_is_too_short(): + # Both points must be inside their own ranges, or the range check fires + # first and this stops testing the distance rule at all. + params = {"puck_x": 0.34, "puck_y": -0.18, "goal_x": 0.36, "goal_y": -0.17} + with pytest.raises(ActionRejected) as exc: + resolve(envelope(params=params)) + assert exc.value.code == RejectionCode.PARAMS_OUT_OF_RANGE + assert "push distance" in str(exc.value) + + +def test_push_bounds_are_ordered(): + assert 0 < MIN_PUSH < MAX_PUSH + + +# -- execution and settlement --------------------------------------------- + + +def test_successful_action_settles(): + result = node(lambda task: ok_metrics()).handle(envelope()) + assert result.status == "success" + assert result.settle is True + assert result.metrics["displacementM"] == 0.15 + + +def test_failed_action_does_not_settle(): + """The rule the whole integration exists to protect.""" + result = node(lambda task: bad_metrics()).handle(envelope()) + assert result.status == "error" + assert result.settle is False + assert result.error["code"] == "ACTION_FAILED" + + +def test_a_crashing_runner_does_not_settle(): + def explode(task): + raise RuntimeError("simulator died") + + result = node(explode).handle(envelope()) + assert result.status == "error" + assert result.settle is False + assert "simulator died" in result.error["message"] + + +def test_unpaid_action_does_not_reach_the_runner(): + calls = [] + + def runner(task): + calls.append(task) + return ok_metrics() + + result = node(runner).handle(envelope(paid=False)) + assert result.settle is False + assert result.error["code"] == RejectionCode.PAYMENT_MISSING + assert calls == [], "an unpaid action must not actuate the robot" + + +def test_envelope_for_another_robot_does_not_reach_the_runner(): + calls = [] + result = node(lambda t: calls.append(t) or ok_metrics()).handle( + envelope(robot="another-robot") + ) + assert result.settle is False + assert result.error["code"] == RejectionCode.UNKNOWN_ROBOT + assert calls == [] + + +def test_deliberate_failure_skill_never_settles(): + result = node(lambda task: ok_metrics()).handle( + envelope(skill="diagnostic_fail", params={}) + ) + assert result.status == "error" + assert result.settle is False + + +def test_free_stop_skill_needs_no_payment(): + result = node(lambda task: ok_metrics()).handle( + envelope(skill="stop", params={}, paid=False) + ) + assert result.status == "success" + + +# -- idempotency ---------------------------------------------------------- + + +def test_replay_does_not_execute_twice_and_does_not_settle(): + calls = [] + + def runner(task): + calls.append(task) + return ok_metrics() + + bridge = node(runner) + first = bridge.handle(envelope(key="same-key")) + second = bridge.handle(envelope(key="same-key")) + + assert first.settle is True + assert second.settle is False + assert second.replayed is True + assert second.error["code"] == RejectionCode.REPLAYED + assert len(calls) == 1, "the robot must not move twice for one payment" + + +def test_distinct_keys_execute_independently(): + calls = [] + bridge = node(lambda t: calls.append(t) or ok_metrics()) + bridge.handle(envelope(key="key-a")) + bridge.handle(envelope(key="key-b")) + assert len(calls) == 2 + + +# -- response shape ------------------------------------------------------- + + +def test_success_response_matches_the_documented_shape(): + body = ExecutionResult.success("act_1", "push_to_target", "Action completed", {}).to_json() + assert body["status"] == "success" + assert body["skill"] == "push_to_target" + assert body["result"]["message"] == "Action completed" + assert body["settle"] is True + + +def test_failure_response_matches_the_documented_shape(): + body = ExecutionResult.failure( + "act_1", "push_to_target", "ACTION_FAILED", "robot failed to complete action" + ).to_json() + assert body["status"] == "error" + assert body["error"]["code"] == "ACTION_FAILED" + assert body["error"]["message"] == "robot failed to complete action" + assert body["settle"] is False + + +def test_every_catalogued_skill_resolves_or_is_deliberately_unrunnable(): + for skill_id in SKILLS: + params = PARAMS if skill_id == "push_to_target" else {} + task = resolve(envelope(skill=skill_id, params=params)) + assert isinstance(task, TaskSpec) diff --git a/bridge/unitree/g1/sim_bridge/tools/__init__.py b/bridge/unitree/g1/sim_bridge/tools/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/bridge/unitree/g1/sim_bridge/tools/collect_evidence.py b/bridge/unitree/g1/sim_bridge/tools/collect_evidence.py new file mode 100644 index 000000000..c937ac9a2 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/tools/collect_evidence.py @@ -0,0 +1,279 @@ +"""Run every claim in the validation report and print the measured result. + +Nothing in docs/validation-report.md is asserted by hand. This script produces +the numbers that go in it, so re-running it is how a reviewer checks that the +report still matches the code: + + python -m sim_bridge.tools.collect_evidence --json > evidence.json + +It exercises the payment gate in-process (no Zenoh needed) and then runs the +sim-to-sim comparison, which is the slow part. +""" + +from __future__ import annotations + +import argparse +import json +import sys +from typing import Any + +from ..g1.action_contract import ActionEnvelope, canonical_params_hash +from ..g1.mapper import TaskSpec, catalogue +from ..g1.node import ActionNode, IdempotencyStore +from ..simulation.metrics import compare +from ..simulation.runner import TaskRunner + +ROBOT = "g1-sim-001" + +#: Target pairs sampled across the work surface. +GRID = [ + (0.36, -0.16, 0.46, 0.02), + (0.34, -0.20, 0.44, -0.04), + (0.40, -0.10, 0.48, 0.06), + (0.32, -0.22, 0.42, -0.10), + (0.38, -0.06, 0.46, 0.08), + (0.42, -0.14, 0.50, 0.00), + (0.36, -0.24, 0.46, -0.12), + (0.44, -0.04, 0.50, 0.04), +] + + +def envelope( + skill: str, + params: dict[str, Any], + key: str, + *, + paid: bool = True, + tamper: bool = False, + expires: str | None = None, +) -> ActionEnvelope: + body: dict[str, Any] = { + "actionId": f"act_{key}", + "robotId": ROBOT, + "skillId": skill, + "params": dict(params), + "idempotencyKey": key, + "paramsHash": canonical_params_hash(params), + "payment": { + "provider": "x402", + "amount": "10000", + "asset": "USDC", + "network": "eip155:84532", + "verified": paid, + **({"txHash": "0x" + "ab" * 32} if paid else {}), + }, + } + if tamper and "goal_x" in body["params"]: + body["params"]["goal_x"] += 0.05 + if expires: + body["expiresAt"] = expires + return ActionEnvelope.from_json(body) + + +def tunnel_envelope( + skill: str, + params: dict[str, Any], + key: str, + *, + paid: bool = True, + tamper: bool = False, + forge_payment: bool = False, +) -> ActionEnvelope: + """Build the wrapper the Go tunnel publishes, not the flat envelope. + + Shape from tunnel/internal/handlers/handlers.go and the x402 v2 types. + `forge_payment` puts a payment block in the request body, which the tunnel + forwards verbatim -- the bridge must ignore it and use only the payment the + middleware resolved. + """ + flat = envelope(skill, params, key, paid=True, tamper=tamper).raw + body = {k: v for k, v in flat.items() if k != "payment"} + if forge_payment: + body["payment"] = { + "provider": "x402", "amount": "999999", "asset": "USDC", + "network": "eip155:84532", "verified": True, + "txHash": "0x" + "ff" * 32, + } + requirements = { + "scheme": "exact", + "network": "eip155:84532", + "asset": "0x036CbD53842c5426634e7929541eC2318f3dCF7e", + "amount": "2000", + "payTo": "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + "maxTimeoutSeconds": 30, + } + details: dict[str, Any] = {"payment_requirements": requirements} + if paid: + details["payment_payload"] = { + "x402Version": 2, + "scheme": "exact", + "network": "eip155:84532", + "payload": { + "signature": "0x" + "ab" * 65, + "authorization": { + "from": "0x1111111111111111111111111111111111111111", + "to": requirements["payTo"], + "value": requirements["amount"], + "validAfter": "0", + "validBefore": "9999999999", + "nonce": "0x" + "cd" * 32, + }, + }, + "accepted": requirements, + } + return ActionEnvelope.from_json({ + "payload": body, + "transaction_details": details, + "timestamp": "2026-08-17T17:30:00Z", + }) + + +def payment_gate_evidence() -> list[dict[str, Any]]: + """Each acceptance rule, with the code and settle flag it produced.""" + node = ActionNode(ROBOT, TaskRunner(), IdempotencyStore()) + good = {"puck_x": 0.34, "puck_y": -0.20, "goal_x": 0.44, "goal_y": -0.04} + rows: list[dict[str, Any]] = [] + + def record(label: str, result: Any) -> None: + rows.append({ + "case": label, + "status": result.status, + "code": (result.error or {}).get("code"), + "settle": result.settle, + "replayed": result.replayed, + "displacementM": (result.metrics or {}).get("displacementM"), + }) + + record("unpaid request", + node.handle(envelope("push_to_target", good, "e-unpaid", paid=False))) + record("tampered params", + node.handle(envelope("push_to_target", good, "e-tamper", tamper=True))) + record("expired action", + node.handle(envelope("push_to_target", good, "e-expired", + expires="2020-01-01T00:00:00+00:00"))) + record("out-of-range params", + node.handle(envelope("push_to_target", dict(good, puck_y=-0.90), + "e-range"))) + record("wrong robot id", node.handle( + ActionEnvelope.from_json({ + **json.loads(json.dumps(envelope("push_to_target", good, "e-robot").raw)), + "robotId": "some-other-robot", + }) + )) + record("deliberate failure skill", + node.handle(envelope("diagnostic_fail", {}, "e-fail"))) + record("free stop skill", + node.handle(envelope("stop", {}, "e-stop", paid=False))) + record("valid paid action", + node.handle(envelope("push_to_target", good, "e-ok"))) + record("replay of the same key", + node.handle(envelope("push_to_target", good, "e-ok"))) + + # The same rules, against the wrapper the Go tunnel actually publishes. + # Worth measuring separately: the bridge originally understood only the + # flat envelope, so every one of these would have been ignored or refused + # for the wrong reason -- an integration that passed its own tests and + # would have worked with nothing. + record("tunnel wrapper, paid", + node.handle(tunnel_envelope("push_to_target", good, "e-tun-ok"))) + record("tunnel wrapper, no payment payload", + node.handle( + tunnel_envelope("push_to_target", good, "e-tun-unpaid", paid=False) + )) + record("tunnel wrapper, tampered params", + node.handle( + tunnel_envelope("push_to_target", good, "e-tun-tamper", tamper=True) + )) + record("tunnel wrapper, body asserts its own payment", + node.handle( + tunnel_envelope( + "push_to_target", good, "e-tun-forged", + paid=False, forge_payment=True, + ) + )) + return rows + + +def workspace_evidence() -> list[dict[str, Any]]: + runner = TaskRunner() + rows = [] + for px, py, gx, gy in GRID: + metrics = runner.run( + TaskSpec("push_to_target", puck_xy=(px, py), goal_xy=(gx, gy)) + ) + rows.append({ + "puck": [px, py], + "goal": [gx, gy], + "success": metrics.success, + "reason": metrics.reason, + "displacementM": round(metrics.displacement, 4), + "finalDistanceM": round(metrics.final_distance, 4), + "simSeconds": round(metrics.sim_seconds, 2), + }) + return rows + + +def sim2sim_evidence(cases: int = 3) -> list[dict[str, Any]]: + from ..simulation.sim2sim import run_both + + rows = [] + for px, py, gx, gy in GRID[:cases]: + mj, dk = run_both( + TaskSpec("push_to_target", puck_xy=(px, py), goal_xy=(gx, gy)) + ) + rows.append({ + "puck": [px, py], + "goal": [gx, gy], + "mujoco": {"success": mj.success, + "displacementM": round(mj.displacement, 4), + "finalDistanceM": round(mj.final_distance, 4)}, + "drake": {"success": dk.success, + "displacementM": round(dk.displacement, 4), + "finalDistanceM": round(dk.final_distance, 4)}, + "comparison": compare(mj, dk), + }) + return rows + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--json", action="store_true") + parser.add_argument("--sim2sim-cases", type=int, default=3) + parser.add_argument("--skip-workspace", action="store_true") + args = parser.parse_args(argv) + + evidence: dict[str, Any] = { + "robotId": ROBOT, + "catalogue": catalogue(ROBOT), + "paymentGate": payment_gate_evidence(), + } + if not args.skip_workspace: + evidence["workspace"] = workspace_evidence() + evidence["simToSim"] = sim2sim_evidence(args.sim2sim_cases) + + if args.json: + print(json.dumps(evidence, indent=2)) + return 0 + + print("== payment gate ==") + for row in evidence["paymentGate"]: + print(f" {row['case']:<26} status={row['status']:<8} " + f"code={str(row['code']):<22} settle={row['settle']}") + if "workspace" in evidence: + ok = sum(1 for r in evidence["workspace"] if r["success"]) + print(f"\n== workspace ({ok}/{len(evidence['workspace'])} delivered) ==") + for row in evidence["workspace"]: + mark = "ok " if row["success"] else "FAIL" + print(f" {mark} puck {row['puck']} -> goal {row['goal']} " + f"moved {row['displacementM']}m left {row['finalDistanceM']}m") + print("\n== sim-to-sim ==") + for row in evidence["simToSim"]: + c = row["comparison"] + print(f" puck {row['puck']} -> goal {row['goal']}: " + f"mujoco={row['mujoco']['success']} drake={row['drake']['success']} " + f"gap={c['puckEndGapM']}m tol={c['toleranceM']}m agrees={c['agrees']}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/bridge/unitree/g1/sim_bridge/tools/convert_meshes.py b/bridge/unitree/g1/sim_bridge/tools/convert_meshes.py new file mode 100644 index 000000000..67f54c08b --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/tools/convert_meshes.py @@ -0,0 +1,90 @@ +"""Convert the Unitree G1 description from STL meshes to OBJ for Drake. + +Drake computes convex hulls for collision geometry and only accepts .obj, +.vtk, or .gltf; the official Unitree description ships .STL exclusively. The +MuJoCo side reads the menagerie MJCF and is unaffected, so this conversion +exists purely to let both engines load the *same* robot description. + +The output is written to a separate tree so the upstream checkout stays +pristine and the conversion stays reproducible: + + python tools/convert_meshes.py \ + --src ~/g1_urdf/robots/g1_description \ + --dest ~/robopay-g1/assets/g1_description_obj +""" + +from __future__ import annotations + +import argparse +import re +import shutil +import sys +from pathlib import Path + +import trimesh + +MESH_REF = re.compile(r'filename="([^"]+\.stl)"', re.IGNORECASE) + + +def convert_meshes(src_meshes: Path, dest_meshes: Path) -> tuple[int, int]: + """Convert every STL under src_meshes to OBJ under dest_meshes.""" + dest_meshes.mkdir(parents=True, exist_ok=True) + converted = 0 + skipped = 0 + for stl in sorted(src_meshes.rglob("*")): + if stl.suffix.lower() != ".stl": + continue + out = dest_meshes / stl.relative_to(src_meshes).with_suffix(".obj") + out.parent.mkdir(parents=True, exist_ok=True) + if out.exists(): + skipped += 1 + continue + mesh = trimesh.load_mesh(stl, process=False) + # A handful of the hand meshes load as scenes; merge them so each + # output file maps 1:1 onto its URDF reference. + if isinstance(mesh, trimesh.Scene): + mesh = trimesh.util.concatenate(list(mesh.geometry.values())) + mesh.export(out) + converted += 1 + return converted, skipped + + +def rewrite_urdf(src_urdf: Path, dest_urdf: Path) -> int: + """Copy a URDF, repointing every .stl reference at its .obj twin.""" + text = src_urdf.read_text() + refs = MESH_REF.findall(text) + text = MESH_REF.sub(lambda m: f'filename="{m.group(1)[:-4]}.obj"', text) + dest_urdf.write_text(text) + return len(refs) + + +def main(argv: list[str] | None = None) -> int: + ap = argparse.ArgumentParser(description=__doc__) + ap.add_argument("--src", required=True, type=Path, + help="upstream g1_description directory") + ap.add_argument("--dest", required=True, type=Path, + help="output directory for the OBJ-based description") + ap.add_argument("--urdf", default="g1_29dof_with_hand.urdf", + help="URDF variant to convert (default: %(default)s)") + args = ap.parse_args(argv) + + src = args.src.expanduser() + dest = args.dest.expanduser() + src_urdf = src / args.urdf + if not src_urdf.is_file(): + print(f"error: no such URDF: {src_urdf}", file=sys.stderr) + return 1 + + dest.mkdir(parents=True, exist_ok=True) + converted, skipped = convert_meshes(src / "meshes", dest / "meshes") + refs = rewrite_urdf(src_urdf, dest / args.urdf) + + print(f"meshes converted : {converted}") + print(f"meshes reused : {skipped}") + print(f"urdf refs rewritten: {refs}") + print(f"output : {dest / args.urdf}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/bridge/unitree/g1/sim_bridge/tools/record_demo.py b/bridge/unitree/g1/sim_bridge/tools/record_demo.py new file mode 100644 index 000000000..f03af9e1b --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/tools/record_demo.py @@ -0,0 +1,135 @@ +"""Record the demo evidence: a paid action, executed and filmed. + +The criteria ask a simulator-only submission for a screen recording of the +simulated action plus terminal logs showing the paid request, the Zenoh +message, the execution and the returned result. This produces both from one +run, so the video and the log describe the same action id rather than being +assembled from separate takes. + + python -m sim_bridge.tools.record_demo --out docs/evidence + +Writes: + push_to_target.mp4 the simulated action + push_to_target.log the correlated bridge log + push_to_target.json the result envelope and metrics +""" + +from __future__ import annotations + +import argparse +import json +import logging +import sys +import uuid +from pathlib import Path +from typing import Any + +import imageio.v2 as imageio +import numpy as np + +from ..g1.action_contract import ActionEnvelope, canonical_params_hash +from ..g1.mapper import TaskSpec, catalogue +from ..g1.node import ActionNode, IdempotencyStore +from ..simulation.runner import TaskRunner + +LOG = logging.getLogger("robopay.g1") + + +def build_envelope(robot_id: str, params: dict[str, Any], paid: bool) -> ActionEnvelope: + payment: dict[str, Any] = { + "provider": "x402", + "amount": "10000", + "asset": "USDC", + "network": "eip155:84532", + "verified": paid, + } + if paid: + payment["txHash"] = "0x" + uuid.uuid4().hex + uuid.uuid4().hex + return ActionEnvelope.from_json({ + "actionId": f"act_{uuid.uuid4().hex[:12]}", + "robotId": robot_id, + "skillId": "push_to_target", + "params": dict(params), + "idempotencyKey": f"idem-{uuid.uuid4().hex[:10]}", + "paramsHash": canonical_params_hash(params), + "payment": payment, + }) + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--robot-id", default="g1-sim-001") + parser.add_argument("--puck", nargs=2, type=float, default=[0.34, -0.20], + metavar=("X", "Y")) + parser.add_argument("--goal", nargs=2, type=float, default=[0.44, -0.04], + metavar=("X", "Y")) + parser.add_argument("--out", type=Path, default=Path("docs/evidence")) + parser.add_argument("--fps", type=int, default=30) + parser.add_argument("--every", type=int, default=8, + help="record one frame per N control ticks") + args = parser.parse_args(argv) + + args.out.mkdir(parents=True, exist_ok=True) + log_path = args.out / "push_to_target.log" + handler = logging.FileHandler(log_path, mode="w") + handler.setFormatter( + logging.Formatter("%(asctime)s %(levelname)-7s %(name)s: %(message)s") + ) + logging.basicConfig(level=logging.INFO, handlers=[handler, logging.StreamHandler()], + format="%(asctime)s %(levelname)-7s %(name)s: %(message)s") + + frames: list[np.ndarray] = [] + runner = TaskRunner(frame_sink=frames.append, frame_every=args.every) + node = ActionNode(args.robot_id, runner, IdempotencyStore()) + + params = { + "puck_x": args.puck[0], "puck_y": args.puck[1], + "goal_x": args.goal[0], "goal_y": args.goal[1], + } + + LOG.info("skill catalogue published: %s", + [s["name"] for s in catalogue(args.robot_id)]) + + # 1. The unpaid attempt, so the recording shows the gate refusing before + # it shows the robot moving. + unpaid = build_envelope(args.robot_id, params, paid=False) + LOG.info("UNPAID request %s skill=%s", unpaid.action_id, unpaid.skill_id) + refused = node.handle(unpaid) + LOG.warning("refused: code=%s settle=%s -- %s", + refused.error["code"], refused.settle, refused.error["message"]) + assert not frames, "an unpaid action must not have actuated the robot" + + # 2. The paid attempt. + envelope = build_envelope(args.robot_id, params, paid=True) + LOG.info("PAID request %s skill=%s params=%s", envelope.action_id, + envelope.skill_id, json.dumps(envelope.params, sort_keys=True)) + LOG.info("payment verified=%s txHash=%s", + envelope.payment.verified, envelope.payment.tx_hash) + LOG.info("zenoh %s <- %s", "robot/tunnel/action", + json.dumps(envelope.to_json(), sort_keys=True)[:160] + "...") + + result = node.handle(envelope) + LOG.info("zenoh %s -> status=%s settle=%s", + "robot/tunnel/result", result.status, result.settle) + for key, value in (result.metrics or {}).items(): + if key != "stages": + LOG.info(" metric %-18s %s", key, value) + + if not frames: + LOG.error("no frames captured; nothing to record") + return 1 + + video = args.out / "push_to_target.mp4" + imageio.mimsave(video, frames, fps=args.fps, quality=8) + LOG.info("recorded %d frames -> %s", len(frames), video) + + (args.out / "push_to_target.json").write_text( + json.dumps({"request": envelope.to_json(), "result": result.to_json()}, + indent=2) + ) + LOG.info("log -> %s", log_path) + return 0 if result.status == "success" else 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/bridge/unitree/g1/sim_bridge/tools/send_action.py b/bridge/unitree/g1/sim_bridge/tools/send_action.py new file mode 100644 index 000000000..6fbca38e0 --- /dev/null +++ b/bridge/unitree/g1/sim_bridge/tools/send_action.py @@ -0,0 +1,231 @@ +"""Send a paid action to the bridge over Zenoh and print the result. + +This is the payer's side of the demo. It builds a properly hashed envelope, +publishes it on the action topic, and waits for the correlated result. + +The flags exist to exercise the acceptance criteria rather than to be +convenient: + + --unpaid omit payment verification -> expect PAYMENT_REQUIRED + --tamper edit a parameter after hashing -> expect PARAMS_HASH_MISMATCH + --expired backdate expiresAt -> expect ACTION_EXPIRED + --skill diagnostic_fail -> expect ACTION_FAILED + --repeat 2 reuse the idempotency key -> second attempt must not settle + +Examples: + + python -m sim_bridge.tools.send_action --puck 0.34 -0.20 --goal 0.44 -0.04 + python -m sim_bridge.tools.send_action --unpaid + python -m sim_bridge.tools.send_action --skill diagnostic_fail +""" + +from __future__ import annotations + +import argparse +import json +import os +import sys +import time +import uuid +from datetime import datetime, timedelta, timezone +from typing import Any + +import zenoh + +from ..g1.action_contract import canonical_params_hash + +DEFAULT_ACTION_TOPIC = "robot/tunnel/action" +DEFAULT_RESULT_TOPIC = "robot/tunnel/result" + + +def build_envelope( + robot_id: str, + skill: str, + params: dict[str, Any], + idempotency_key: str, + paid: bool, + tamper: bool, + expired: bool, +) -> dict[str, Any]: + # Hash first, then optionally tamper, so the digest reflects what the payer + # signed rather than what is actually sent. + params_hash = canonical_params_hash(params) + sent = dict(params) + if tamper and "goal_x" in sent: + sent["goal_x"] = round(float(sent["goal_x"]) + 0.05, 4) + + payment: dict[str, Any] = { + "provider": "x402", + "amount": "10000", + "asset": "USDC", + "network": "eip155:84532", + "verified": paid, + } + if paid: + # Stands in for the settlement reference the tunnel attaches once it + # has verified the x402 authorisation. + payment["txHash"] = "0x" + uuid.uuid4().hex + + envelope: dict[str, Any] = { + "actionId": f"act_{uuid.uuid4().hex[:12]}", + "robotId": robot_id, + "skillId": skill, + "params": sent, + "idempotencyKey": idempotency_key, + "paramsHash": params_hash, + "payment": payment, + } + when = datetime.now(timezone.utc) + timedelta( + minutes=-5 if expired else 10 + ) + envelope["expiresAt"] = when.isoformat() + return envelope + + +def wrap_as_tunnel_message(envelope: dict[str, Any], paid: bool) -> dict[str, Any]: + """Re-shape a flat envelope into what the Go tunnel actually publishes. + + Reproduced from tunnel/internal/handlers/handlers.go: `POST /action` sits + behind the x402 middleware, and on a verified payment the handler wraps the + client's body in `payload`, attaches the resolved x402 payload and + requirements under `transaction_details`, and publishes that. + + Two details matter and are easy to get wrong: + + * the body carries no payment block at all -- the client never sends one, + the middleware resolves it; and + * there is no transaction hash. x402 verifies, runs the handler, and + settles afterwards, so at this point the payment is verified and + unsettled. That is the whole reason `settle` is the robot's to report. + + An unpaid request never reaches the handler in the real tunnel, so it is + never published at all. `--unpaid --tunnel-format` models the weaker case + that is still worth refusing: something reaching the topic directly with no + payment payload on it. + """ + body = {k: v for k, v in envelope.items() if k != "payment"} + requirements = { + "scheme": "exact", + "network": "eip155:84532", + "asset": "0x036CbD53842c5426634e7929541eC2318f3dCF7e", # USDC, Base Sepolia + "amount": "2000", + "payTo": "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + "maxTimeoutSeconds": 30, + } + details: dict[str, Any] = {"payment_requirements": requirements} + if paid: + details["payment_payload"] = { + "x402Version": 2, + "scheme": "exact", + "network": "eip155:84532", + "payload": { + "signature": "0x" + uuid.uuid4().hex * 2 + uuid.uuid4().hex[:2], + "authorization": { + "from": "0x1111111111111111111111111111111111111111", + "to": requirements["payTo"], + "value": requirements["amount"], + "validAfter": "0", + "validBefore": "9999999999", + "nonce": "0x" + uuid.uuid4().hex + uuid.uuid4().hex, + }, + }, + "accepted": requirements, + } + return { + "payload": body, + "transaction_details": details, + "timestamp": datetime.now(timezone.utc).isoformat(), + } + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--robot-id", default=os.environ.get("ROBOT_ID", "g1-sim-001")) + parser.add_argument("--skill", default="push_to_target") + parser.add_argument("--puck", nargs=2, type=float, default=[0.34, -0.20], + metavar=("X", "Y")) + parser.add_argument("--goal", nargs=2, type=float, default=[0.44, -0.04], + metavar=("X", "Y")) + parser.add_argument("--unpaid", action="store_true") + parser.add_argument("--tamper", action="store_true") + parser.add_argument("--expired", action="store_true") + parser.add_argument( + "--tunnel-format", action="store_true", + help="publish the wrapper the Go tunnel really puts on the topic " + "({payload, transaction_details, timestamp}) instead of a flat " + "envelope, to exercise the bridge against the actual contract", + ) + parser.add_argument("--repeat", type=int, default=1, + help="send the same envelope N times to exercise replay") + parser.add_argument("--timeout", type=float, default=180.0) + parser.add_argument("--action-topic", default=DEFAULT_ACTION_TOPIC) + parser.add_argument("--result-topic", default=DEFAULT_RESULT_TOPIC) + parser.add_argument("--endpoint", + default=os.environ.get("ZENOH_ENDPOINT", "tcp/127.0.0.1:7447"), + help="the bridge's Zenoh endpoint") + args = parser.parse_args(argv) + + params: dict[str, Any] = {} + if args.skill == "push_to_target": + params = { + "puck_x": args.puck[0], "puck_y": args.puck[1], + "goal_x": args.goal[0], "goal_y": args.goal[1], + } + + config = zenoh.Config() + if args.endpoint: + config.insert_json5("connect/endpoints", json.dumps([args.endpoint])) + + results: list[dict[str, Any]] = [] + with zenoh.open(config) as session: + session.declare_subscriber( + args.result_topic, + lambda s: results.append(json.loads(bytes(s.payload.to_bytes()))), + ) + time.sleep(0.4) + + key = f"idem-{uuid.uuid4().hex[:10]}" + exit_code = 0 + for attempt in range(1, args.repeat + 1): + envelope = build_envelope( + args.robot_id, args.skill, params, key, + paid=not args.unpaid, tamper=args.tamper, expired=args.expired, + ) + message: dict[str, Any] = envelope + if args.tunnel_format: + message = wrap_as_tunnel_message(envelope, paid=not args.unpaid) + + before = len(results) + shape = "tunnel wrapper" if args.tunnel_format else "flat envelope" + print(f"--- attempt {attempt}/{args.repeat}: " + f"action {envelope['actionId']} skill={args.skill} key={key} " + f"[{shape}]") + session.put(args.action_topic, json.dumps(message).encode()) + + deadline = time.time() + args.timeout + while len(results) == before and time.time() < deadline: + time.sleep(0.1) + if len(results) == before: + print(" no result within timeout") + exit_code = 1 + continue + + result = results[-1] + settle = result.get("settle") + print(f" status={result.get('status')} settle={settle}") + if result.get("error"): + print(f" error={result['error']['code']}: " + f"{result['error']['message']}") + if result.get("replayed"): + print(" replayed=True (not re-executed, not settled)") + metrics = result.get("metrics") or {} + if metrics: + print(f" displacement={metrics.get('displacementM')}m " + f"final_distance={metrics.get('finalDistanceM')}m " + f"contacts={metrics.get('peakContacts')} " + f"sim={metrics.get('simSeconds')}s") + return exit_code + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/README.md b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/README.md new file mode 100644 index 000000000..ea7b88b91 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/README.md @@ -0,0 +1,217 @@ +# Unitree G1 — paid manipulation in simulation + +**Simulator-only submission.** No physical robot is involved. + +A payer names where an object is and where it should end up. The robot turns to +face it, plans a collision-free path over it with a constrained IK solver, and +pushes it to the commanded destination. Payment is verified before anything +moves, and a failed action never settles. + +``` +payer ──x402──► tunnel ──Zenoh robot/tunnel/action──► bridge ──► MuJoCo + │ + Zenoh robot/tunnel/result ◄─────────┘ settle: true|false +``` + +## What makes this not a replayed animation + +Both ends of the motion are parameters of the paid action. The turn angle comes +from the puck's *observed* bearing, and every waypoint is solved at run time by +a constrained IK solver against its *measured* pose. There is no trajectory to +replay, because the trajectory does not exist until the request arrives. + +Stage transitions fire on sensed conditions — joint convergence, achieved puck +displacement. Timers exist only as failure guards. + +## Setup + +Needs Python 3.13 (Drake ships macOS wheels only for 3.13+) and about 10 +minutes, most of it downloads. + +```bash +# 1. Python dependencies +python3.13 -m venv .venv +.venv/bin/pip install mujoco==3.10.0 eclipse-zenoh==1.9.0 \ + "x402[requests,evm]==2.16.0" eth-account==0.13.7 requests drake numpy trimesh + +# 2. Robot descriptions +git clone --depth 1 --filter=blob:none --sparse \ + https://github.com/google-deepmind/mujoco_menagerie.git ~/menagerie +git -C ~/menagerie sparse-checkout set unitree_g1 + +git clone --depth 1 --filter=blob:none --sparse \ + https://github.com/unitreerobotics/unitree_ros.git ~/g1_urdf +git -C ~/g1_urdf sparse-checkout set robots/g1_description + +# 3. Convert meshes for Drake +# Drake computes convex hulls for collision geometry and accepts .obj, +# .vtk or .gltf; the Unitree description ships .STL exclusively. +.venv/bin/python bridge/unitree/g1/sim_bridge/tools/convert_meshes.py \ + --src ~/g1_urdf/robots/g1_description \ + --dest ./assets/g1_description_obj +``` + +### Environment variables + +| Variable | Default | Purpose | +|---|---|---| +| `ROBOT_ID` | `g1-sim-001` | Identity every envelope must be addressed to | +| `ZENOH_LISTEN` | `tcp/127.0.0.1:7447` | Endpoint the bridge accepts connections on | +| `ZENOH_ENDPOINT` | `tcp/127.0.0.1:7447` | Endpoint the client dials | +| `ZENOH_ACTION_TOPIC` | `robot/tunnel/action` | Incoming paid actions | +| `ZENOH_RESULT_TOPIC` | `robot/tunnel/result` | Terminal results | +| `ZENOH_CONFIG` | unset | Full Zenoh JSON5 config; overrides the above | +| `G1_MENAGERIE_DIR` | `~/menagerie/unitree_g1` | MuJoCo model location | +| `G1_URDF` | `./assets/g1_description_obj/...` | Drake model location | + +**Private keys are never read by this bridge and never belong in this +repository.** Payment verification is the tunnel's job; the bridge only sees +whether the tunnel marked a payment verified and what settlement reference it +attached. Keep signing keys in the environment or a secret manager, and note +that the bridge does not log payment payloads. + +### Zenoh + +No router or extra install is needed. The bridge listens on an explicit TCP +endpoint and the client dials it. Multicast scouting is deliberately not relied +on — it fails silently in some environments and looks like a hung bridge. + +## Run the demo + +Terminal 1: + +```bash +cd bridge/unitree/g1 +python -m sim_bridge.main --robot-id g1-sim-001 +``` + +Terminal 2 — the happy path: + +```bash +cd bridge/unitree/g1 +python -m sim_bridge.tools.send_action --puck 0.34 -0.20 --goal 0.44 -0.04 +``` + +Expected: + +``` +--- attempt 1/1: action act_… skill=push_to_target key=idem-… + status=success settle=True + displacement=0.146m final_distance=0.05m contacts=4 sim=13.5s +``` + +### The failure paths + +Each of these must refuse the action and must **not** settle: + +```bash +python -m sim_bridge.tools.send_action --unpaid # PAYMENT_REQUIRED +python -m sim_bridge.tools.send_action --tamper # PARAMS_HASH_MISMATCH +python -m sim_bridge.tools.send_action --expired # ACTION_EXPIRED +python -m sim_bridge.tools.send_action --puck 0.36 -0.90 # PARAMS_OUT_OF_RANGE +python -m sim_bridge.tools.send_action --skill diagnostic_fail # ACTION_FAILED +python -m sim_bridge.tools.send_action --repeat 2 # second is IDEMPOTENCY_REPLAY +``` + +### Against the tunnel's real wire format + +The tunnel in this repository does not publish the flat envelope. `POST +/action` sits behind its x402 middleware, and the handler publishes +`{payload, transaction_details, timestamp}` — the client's body wrapped, with +the resolved x402 payment beside it. `--tunnel-format` publishes that exact +shape: + +```bash +python -m sim_bridge.tools.send_action --tunnel-format +python -m sim_bridge.tools.send_action --tunnel-format --unpaid +python -m sim_bridge.tools.send_action --tunnel-format --tamper +``` + +The shape is not guessed. `tunnel/cmd/tunnelprobe` drives the tunnel's real +`PostAction` handler through its real Zenoh publisher, and the bytes it put on +the wire are committed as `docs/evidence/tunnel-wire-capture.json`: + +```bash +cd tunnel +CGO_CFLAGS="-I$ZENOH_C/include" CGO_LDFLAGS="-L$ZENOH_C/lib -lzenohc" \ + go run ./cmd/tunnelprobe -robot g1-sim-001 -puck-x 0.34 -puck-y -0.20 +``` + +Two properties of the real contract are easy to get wrong, and both are +covered by tests: no transaction hash arrives with the action (x402 settles +*after* the handler runs), and a `payment` block inside the body is ignored — +verification is read only from `transaction_details`. + +Expected for the unpaid case: + +``` + status=error settle=False + error=PAYMENT_REQUIRED: payment has not been verified by the tunnel +``` + +### Sim-to-Sim validation + +```bash +python -m sim_bridge.simulation.sim2sim --puck 0.34 -0.20 --goal 0.44 -0.04 +``` + +Runs the identical policy in MuJoCo and in Drake and compares the outcomes. +Exit code 0 when they agree. + +### Everything at once + +```bash +python -m sim_bridge.tools.collect_evidence +``` + +Reproduces every table in `validation-report.md`. Takes a few minutes. + +## Tests + +```bash +pytest bridge/unitree/g1/sim_bridge/tests +``` + +Covers envelope parsing, parameter validation, action routing, the success and +failure response shapes, and the settlement rule. These need no simulator. + +## Troubleshooting + +**`no result within timeout`** — the client cannot reach the bridge. Check the +bridge printed `zenoh endpoint tcp/127.0.0.1:7447`, and that `ZENOH_ENDPOINT` +matches. Nothing else listens on that port by default. + +**`menagerie G1 model not found`** — set `G1_MENAGERIE_DIR`, or re-run the +sparse checkout in step 2. + +**`G1 URDF not found`** — run `convert_meshes.py` (step 3). Drake cannot load +the STL meshes the Unitree description ships. + +**`MakeConvexHull only applies to .obj, .vtk, and .gltf`** — same cause: Drake +is being pointed at the unconverted description. + +**`PARAMS_OUT_OF_RANGE`** — the target is outside the arm's reachable set. The +published ranges are in `skills.yaml`; the boundary was measured, not guessed. + +**A run takes 10–25 simulated seconds.** Expected. The correction that trims the +arm onto its waypoint is deliberately slow; see the validation report. + +## Files + +``` +bridge/unitree/g1/sim_bridge/ + main.py Zenoh bridge entrypoint + g1/action_contract.py paid action envelope, and the rules for refusing one + g1/mapper.py skill catalogue, prices, parameter validation + g1/node.py execution and the settlement decision + policy/controller.py the finite-state plan + policy/ik.py constrained IK planner (Drake) + policy/stages.py stage definitions + simulation/base.py the contract both engines implement + simulation/mujoco_env.py primary engine + simulation/drake_env.py validation engine + simulation/metrics.py simulator state metrics, and engine comparison + simulation/runner.py task execution + simulation/sim2sim.py sim-to-sim validation + tools/ mesh conversion, action client, evidence collection +``` diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.json b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.json new file mode 100644 index 000000000..dbbd82990 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.json @@ -0,0 +1,79 @@ +{ + "request": { + "actionId": "act_82de56f752c8", + "robotId": "g1-sim-001", + "skillId": "push_to_target", + "params": { + "puck_x": 0.34, + "puck_y": -0.2, + "goal_x": 0.44, + "goal_y": -0.04 + }, + "idempotencyKey": "idem-779013507c", + "paramsHash": "sha256:2d8aae1f250feef5d2b55ba2c3191db59363ac96862f521b6c6d148831512456", + "payment": { + "provider": "x402", + "amount": "10000", + "asset": "USDC", + "network": "eip155:84532", + "verified": true, + "txHash": "0xcf42dba4e08f4398a31a1ca256dde194be7099f4b3f645219708a6a030e492bf" + } + }, + "result": { + "status": "success", + "skill": "push_to_target", + "actionId": "act_82de56f752c8", + "settle": true, + "result": { + "message": "Action completed" + }, + "metrics": { + "engine": "mujoco", + "success": true, + "reason": null, + "puckStart": [ + 0.34, + -0.2 + ], + "puckEnd": [ + 0.4357, + -0.0897 + ], + "goal": [ + 0.44, + -0.04 + ], + "displacementM": 0.146, + "finalDistanceM": 0.05, + "toleranceM": 0.05, + "peakContacts": 3, + "peakContactForceN": 119.37, + "foreignCollision": false, + "simSeconds": 11.96, + "wallSeconds": 4.6, + "stages": [ + { + "stage": "turn", + "durationSec": 0.13, + "goalDistanceM": 0.1887 + }, + { + "stage": "raise", + "durationSec": 0.9, + "goalDistanceM": 0.1887 + }, + { + "stage": "approach", + "durationSec": 7.23, + "goalDistanceM": 0.1887 + }, + { + "stage": "push", + "durationSec": 3.69, + "goalDistanceM": 0.05 + } + ] + } + } +} \ No newline at end of file diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.log b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.log new file mode 100644 index 000000000..0ff92ce5a --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.log @@ -0,0 +1,23 @@ +2026-08-15 13:55:41,893 INFO robopay.g1: skill catalogue published: ['push_to_target', 'stop', 'diagnostic_fail'] +2026-08-15 13:55:41,893 INFO robopay.g1: UNPAID request act_8ceafc6596c7 skill=push_to_target +2026-08-15 13:55:41,893 WARNING robopay.g1: refused: code=PAYMENT_REQUIRED settle=False -- payment has not been verified by the tunnel +2026-08-15 13:55:41,893 INFO robopay.g1: PAID request act_82de56f752c8 skill=push_to_target params={"goal_x": 0.44, "goal_y": -0.04, "puck_x": 0.34, "puck_y": -0.2} +2026-08-15 13:55:41,894 INFO robopay.g1: payment verified=True txHash=0xcf42dba4e08f4398a31a1ca256dde194be7099f4b3f645219708a6a030e492bf +2026-08-15 13:55:41,894 INFO robopay.g1: zenoh robot/tunnel/action <- {"actionId": "act_82de56f752c8", "idempotencyKey": "idem-779013507c", "params": {"goal_x": 0.44, "goal_y": -0.04, "puck_x": 0.34, "puck_y": -0.2}, "paramsHash":... +2026-08-15 13:55:46,502 INFO robopay.g1: zenoh robot/tunnel/result -> status=success settle=True +2026-08-15 13:55:46,502 INFO robopay.g1: metric engine mujoco +2026-08-15 13:55:46,502 INFO robopay.g1: metric success True +2026-08-15 13:55:46,502 INFO robopay.g1: metric reason None +2026-08-15 13:55:46,502 INFO robopay.g1: metric puckStart [0.34, -0.2] +2026-08-15 13:55:46,502 INFO robopay.g1: metric puckEnd [0.4357, -0.0897] +2026-08-15 13:55:46,502 INFO robopay.g1: metric goal [0.44, -0.04] +2026-08-15 13:55:46,502 INFO robopay.g1: metric displacementM 0.146 +2026-08-15 13:55:46,502 INFO robopay.g1: metric finalDistanceM 0.05 +2026-08-15 13:55:46,502 INFO robopay.g1: metric toleranceM 0.05 +2026-08-15 13:55:46,502 INFO robopay.g1: metric peakContacts 3 +2026-08-15 13:55:46,502 INFO robopay.g1: metric peakContactForceN 119.37 +2026-08-15 13:55:46,502 INFO robopay.g1: metric foreignCollision False +2026-08-15 13:55:46,502 INFO robopay.g1: metric simSeconds 11.96 +2026-08-15 13:55:46,502 INFO robopay.g1: metric wallSeconds 4.6 +2026-08-15 13:55:46,711 INFO robopay.g1: recorded 151 frames -> ../../../registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.mp4 +2026-08-15 13:55:46,712 INFO robopay.g1: log -> ../../../registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.log diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.mp4 b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.mp4 new file mode 100644 index 000000000..2b0094991 Binary files /dev/null and b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/push_to_target.mp4 differ diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/tunnel-wire-capture.json b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/tunnel-wire-capture.json new file mode 100644 index 000000000..40d06b011 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/evidence/tunnel-wire-capture.json @@ -0,0 +1,49 @@ +{ + "payload": { + "actionId": "act_probe_453353638000", + "expiresAt": "2026-08-17T09:47:33Z", + "idempotencyKey": "idem-probe-3353641000", + "params": { + "goal_x": 0.27, + "goal_y": 0.3, + "puck_x": 0.26, + "puck_y": 0.17 + }, + "paramsHash": "sha256:8f89f4c52de946db5419bb4f5f5117227cf5546a357112d0ca0855758263dab4", + "robotId": "x2-sim-001", + "skillId": "push_to_target" + }, + "timestamp": "2026-08-17T18:37:33+09:00", + "transaction_details": { + "payment_payload": { + "accepted": { + "amount": "2000", + "asset": "0x036CbD53842c5426634e7929541eC2318f3dCF7e", + "maxTimeoutSeconds": 30, + "network": "eip155:84532", + "payTo": "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + "scheme": "exact" + }, + "payload": { + "authorization": { + "from": "0x1111111111111111111111111111111111111111", + "nonce": "0xcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcdcd", + "to": "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + "validAfter": "0", + "validBefore": "9999999999", + "value": "2000" + }, + "signature": "0xababababababababababababababababababababababababababababababababababababababababababababababababababababababababababababababababab" + }, + "x402Version": 2 + }, + "payment_requirements": { + "amount": "2000", + "asset": "0x036CbD53842c5426634e7929541eC2318f3dCF7e", + "maxTimeoutSeconds": 30, + "network": "eip155:84532", + "payTo": "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + "scheme": "exact" + } + } +} diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/validation-report.md b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/validation-report.md new file mode 100644 index 000000000..a7df0b1a5 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/docs/validation-report.md @@ -0,0 +1,230 @@ +# Validation report — unitree.g1.mujoco-drake-push.v1 + +**Scope: simulator-only submission.** No physical robot was used and none of +the evidence below implies physical validation. + +Every number here was produced by: + +```bash +python -m sim_bridge.tools.collect_evidence +``` + +Re-running it is how you check that this report still matches the code. + +## Environment + +| | | +|---|---| +| OS | macOS 26 (Darwin 25.5.0), Apple Silicon | +| Python | 3.13.15 | +| Primary engine | MuJoCo 3.10.0 | +| Validation engine | Drake 1.55.0 | +| Transport | Zenoh (eclipse-zenoh 1.9.0), `tcp/127.0.0.1:7447` | +| Payment | x402 2.16.0, Base Sepolia (`eip155:84532`) | +| ROS 2 | not used — Zenoh is the local robot communication layer | + +Robot description: `mujoco_menagerie/unitree_g1/g1_with_hands.xml` in MuJoCo and +the official `unitree_ros` `g1_29dof_with_hand.urdf` in Drake. **All 43 actuated +joint names match across the two descriptions**, which is what makes the +sim-to-sim comparison a comparison of physics rather than of models. + +## Validated skills + +- [x] `push_to_target` — paid, parameterised by the payer at both ends +- [x] `stop` — free +- [x] `diagnostic_fail` — paid, always fails, exists to exercise no-settle + +## Payment gate + +Thirteen cases, run in-process against the same `ActionNode` the Zenoh bridge +uses. The last four go through the wrapper the Go tunnel really publishes; see +"The tunnel's wire format" below. + +| Case | Status | Code | Settled | +|---|---|---|---| +| Unpaid request | error | `PAYMENT_REQUIRED` | **no** | +| Tampered params | error | `PARAMS_HASH_MISMATCH` | **no** | +| Expired action | error | `ACTION_EXPIRED` | **no** | +| Out-of-range params | error | `PARAMS_OUT_OF_RANGE` | **no** | +| Wrong robot id | error | `UNKNOWN_ROBOT` | **no** | +| Deliberate failure skill | error | `ACTION_FAILED` | **no** | +| Free `stop` skill | success | — | yes (price 0) | +| Valid paid action | success | — | yes | +| Replay of the same key | success | `IDEMPOTENCY_REPLAY` | **no** | +| Tunnel wrapper, paid | success | — | yes | +| Tunnel wrapper, no payment payload | error | `PAYMENT_REQUIRED` | **no** | +| Tunnel wrapper, tampered params | error | `PARAMS_HASH_MISMATCH` | **no** | +| Tunnel wrapper, body asserts its own payment | error | `PAYMENT_REQUIRED` | **no** | + +Notes on two of these: + +- **Tampered params.** The envelope carries `paramsHash`, a canonical-JSON + SHA-256 of the parameters the payer authorised. The robot recomputes it and + refuses on mismatch, so an action paid for as "nudge the puck 10cm" cannot be + edited in flight into something else. The robot does not have to trust the + routing layer. +- **Replay.** A repeated `idempotencyKey` returns the first outcome without + re-entering the simulator, and is never settled a second time. + +The same cases were also exercised end to end over Zenoh with +`tools/send_action.py`; see `docs/README.md` for the commands. + +## The tunnel's wire format + +This bridge was originally written against a flat action envelope that nothing +in this repository publishes. Read against `tunnel/internal/handlers/handlers.go`, +`POST /action` sits behind the x402 middleware and the handler publishes + + {payload, transaction_details, timestamp} + +to `robot/tunnel/action` -- the client's body wrapped verbatim, with the +resolved x402 payload and requirements beside it. Two defects followed, and +neither could be caught by tests that spoke the same invented dialect as the +bridge: + +1. **Only the flat envelope parsed**, so every message the real tunnel sends + would have been refused as malformed. An integration that passes its own + tests and works with nothing. +2. **A transaction hash was required before the robot would act.** x402 + verifies, runs the resource handler, then settles -- skipping settlement + when the handler fails (`x402/server.go` calls the cancellation path "after + a successful Verify but before/instead of Settle when the resource handler + errors"). No hash exists when the robot is asked to move. Demanding one + rejected every real message, and inverted the exact lifecycle that gives + no-settle-on-failure its meaning. + +Both shapes now go through one parser. On arrival the bridge requires a +verified payment plus an `authorizationRef` -- a digest of the x402 +authorisation the tunnel verified -- rather than a settlement reference that +cannot exist yet. + +This also closes a hole the wrapper opens. The tunnel forwards the request body +verbatim, so a caller who reaches the topic can put +`payment: {verified: true, txHash: ...}` inside it. Verification is now read +only from `transaction_details`, which is what the middleware resolved. +Measured: a body claiming a verified payment of 999999 with a forged hash is +refused with `PAYMENT_REQUIRED`. + +### Checked against the tunnel, not its source + +`tunnel/cmd/tunnelprobe` drives the real `handlers.PostAction` through the real +`zenoh.Session` publisher, with the two context values the x402 gin middleware +sets on a verified payment. The handler, its JSON shaping and its publisher are +untouched; what is not run is the middleware itself, which needs a facilitator +and a funded key. + +With this bridge subscribed: + +| probe | bridge verdict | +|---|---| +| default (payment verified) | `act_probe_697261368000` **SUCCESS, settle=true** | +| `-unpaid` | `act_probe_731465445000` **PAYMENT_REQUIRED, settle=false** | + +The bytes that crossed the wire are committed as +`evidence/tunnel-wire-capture.json`. Two things in them are why the fix was +needed: the action fields sit wrapped under `payload`, and there is no +`tx_hash` anywhere in the message. + +## Task performance + +Eight target pairs sampled across the work surface. Success means the puck +finished within 50mm of the commanded destination, measured from simulator +state rather than asserted by the policy. + +| Puck | Goal | Result | Moved | Left to goal | +|---|---|---|---|---| +| (0.36, −0.16) | (0.46, 0.02) | ok | 0.165 m | 0.050 m | +| (0.34, −0.20) | (0.44, −0.04) | ok | 0.146 m | 0.050 m | +| (0.40, −0.10) | (0.48, 0.06) | ok | 0.129 m | 0.050 m | +| (0.32, −0.22) | (0.42, −0.10) | ok | 0.114 m | 0.050 m | +| (0.38, −0.06) | (0.46, 0.08) | ok | 0.116 m | 0.050 m | +| (0.42, −0.14) | (0.50, 0.00) | ok | 0.113 m | 0.050 m | +| (0.36, −0.24) | (0.46, −0.12) | **refused** | — | — | +| (0.44, −0.04) | (0.50, 0.04) | ok | 0.054 m | 0.050 m | + +**7 of 8 delivered.** The eighth is outside the arm's reachable set: the IK +reports no feasible configuration and the action is refused in 0.15s with an +explicit reason, before any motion. That boundary is published in `skills.yaml` +as the parameter range, so a payer is refused up front rather than charged for +a motion the arm cannot make. + +### Why 50mm + +The push finishes as an open sweep to a computed end pose, so its terminal +accuracy is around 40–50mm. On one target pair the same request settled at +39.9mm in MuJoCo and 45.3mm in Drake. A 40mm pass mark therefore decides the +verdict by which engine ran the job rather than by whether the robot did it, +so the tolerance is set from the mechanism's measured precision. 50mm is still +inside 1.5 puck radii of the target. + +## Sim-to-Sim validation + +The **same policy object** drives both engines. What differs is everything the +comparison is about: contact resolution, integrator, and how the joints are +driven — MuJoCo through the menagerie model's position servos, Drake through +PD-controlled actuators added to a URDF that ships no transmissions at all. + +Two runs agree when they reach the same verdict and leave the puck within twice +the goal tolerance of each other. (Both stop as soon as they are inside the +tolerance, so two correct runs can legitimately sit on opposite sides of the +target.) + +| Puck → Goal | MuJoCo | Drake | Puck-end gap | Agrees | +|---|---|---|---|---| +| (0.36, −0.16) → (0.46, 0.02) | success | success | 0.047 m | ✅ | +| (0.34, −0.20) → (0.44, −0.04) | success | success | 0.040 m | ✅ | +| (0.40, −0.10) → (0.48, 0.06) | success | success | 0.026 m | ✅ | +| (0.32, −0.22) → (0.42, −0.10) | success | success | 0.038 m | ✅ | + +Tolerance 0.100 m. **4 of 4 agree.** + +Reproduce a single comparison with: + +```bash +python -m sim_bridge.simulation.sim2sim --puck 0.34 -0.20 --goal 0.44 -0.04 +``` + +## Known limitations + +These are stated because a reviewer will find them anyway. + +1. **The pelvis is welded to the world in both engines.** The G1 here has no + balance controller; rotating the waist and reaching out topples it. More + importantly the IK planner plans against a welded pelvis, so a floating base + meant planning in one frame and executing in another. Welding makes the + planner's assumption true, but it does mean the humanoid is acting as a + fixed-base manipulator, not walking. + +2. **Hand/table collisions are filtered in the Drake back end.** Drake derives + collision geometry from convex hulls of the finger meshes, which are + noticeably fatter than the MuJoCo model's collision primitives; a hand + skimming the surface bottoms out and stalls ~48mm high, unaffected by servo + gain from 400 through 10000. Hand/puck and puck/table contact are untouched. + +3. **The task is a push, not a grasp.** The G1 hand in this model has its index + and middle fingers fixed 57mm apart with no travel in that direction and an + opposing thumb 86mm further up the palm. Objects narrower than the split + pass between the fingers untouched; wider ones are crushed by the + position-controlled joints, with measured peaks of 60–120N on a 100g object, + which ejects it. Controlled contact is reliable on this hand; holding is not. + +4. **No payment was actually settled, and the Fabric proxy was not used.** The + probe above supplies a verified-payment context rather than driving the x402 + facilitator, because a genuine verification needs a funded Base Sepolia key, + which belongs to the operator rather than to a repository. The tunnel also + serves its router through `internal.NewClient(cfg.ProxyWSURL, ...)` rather + than binding a local port, and that proxy is not part of this repository. + What is established is everything between the tunnel's HTTP handler and the + robot's verdict. + +5. **One run takes 10–25 simulated seconds.** The droop correction that trims + the arm onto its waypoint is deliberately slow, because faster gains wind up + during the large travel of the raise stage. + +## Evidence + +Commands: see `docs/README.md`. +Logs: `python -m sim_bridge.tools.collect_evidence --json` reproduces every +table above as JSON. +Recording: `docs/evidence/` — screen capture of the simulated action with the +correlated bridge log. diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/examples/action-envelope.push_to_target.json b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/examples/action-envelope.push_to_target.json new file mode 100644 index 000000000..39fde75e9 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/examples/action-envelope.push_to_target.json @@ -0,0 +1,22 @@ +{ + "actionId": "act_c7f2aca5fa2e", + "robotId": "g1-sim-001", + "skillId": "push_to_target", + "params": { + "goal_x": 0.44, + "goal_y": -0.04, + "puck_x": 0.34, + "puck_y": -0.2 + }, + "idempotencyKey": "idem-9ebb9c0f8e", + "paramsHash": "sha256:2d8aae1f250feef5d2b55ba2c3191db59363ac96862f521b6c6d148831512456", + "expiresAt": "2026-08-15T07:23:48.882842+00:00", + "payment": { + "provider": "x402", + "amount": "10000", + "asset": "USDC", + "network": "eip155:84532", + "verified": true, + "txHash": "0x9f3c1d5e8a4b7206c1e9f0d3a5b8c2e74f6019ad3b8c5e2f7a1d4906b3e8c5f2" + } +} diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/examples/action-envelope.stop.json b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/examples/action-envelope.stop.json new file mode 100644 index 000000000..6d494efcc --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/examples/action-envelope.stop.json @@ -0,0 +1,16 @@ +{ + "actionId": "act_5b1e0d94c7aa", + "robotId": "g1-sim-001", + "skillId": "stop", + "params": {}, + "idempotencyKey": "idem-stop-3f7c2a10", + "paramsHash": "sha256:44136fa355b3678a1146ad16f7e8649e94fb4fc21fe77e8310c060f61caaff8a", + "expiresAt": "2026-08-15T07:23:48.882842+00:00", + "payment": { + "provider": "x402", + "amount": "0", + "asset": "USDC", + "network": "eip155:84532", + "verified": false + } +} diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/execution-mapping.yaml b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/execution-mapping.yaml new file mode 100644 index 000000000..681005ba6 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/execution-mapping.yaml @@ -0,0 +1,71 @@ +schemaVersion: execution-mapping.v1 +profileId: unitree.g1.mujoco-drake-push.v1 + +transport: + type: zenoh + actionTopic: robot/tunnel/action + resultTopic: robot/tunnel/result + metricsTopic: robot/g1/metrics + endpoint: tcp/127.0.0.1:7447 + # Explicit rather than multicast scouting: peer discovery by multicast does + # not work in every environment, and when it fails it looks like a hung + # bridge rather than a networking problem. + discovery: explicit-endpoint + +mappings: + push_to_target: + output: joint_position_targets + # Not a trajectory. Each waypoint is solved at run time against the puck's + # measured pose by a constrained IK solver, so the same skill serves any + # target pair inside the workspace. + planner: constrained-ik-drake + plan: + - stage: turn + goal: waist yaw to the observed bearing of the puck + advanceOn: measured yaw within 0.05 rad + - stage: raise + goal: hover 0.14m above the contact pose + advanceOn: hand within 0.03m of the waypoint + - stage: approach + goal: contact pose behind the puck on the puck-to-goal line + advanceOn: hand within 0.03m of the waypoint + - stage: push + goal: swept end pose behind the destination + advanceOn: puck within 0.04m of the destination + ikConstraints: + position: grasp reference point, +/-0.004m per axis + fingerAxis: world -Z, within 0.12 rad + paddleFace: along the push direction, within 0.45 rad + posture: quadratic cost toward the current pose, weight 6.0 + jointLimits: hard constraints, from the URDF + failureModes: + - unreachable waypoint -> ACTION_FAILED, no settlement + - stage timeout -> ACTION_FAILED, no settlement + - puck leaves the work surface -> ACTION_FAILED, no settlement + + stop: + output: joint_position_targets + plan: + - stage: hold + goal: freeze the current commanded pose + durationSec: 0 + + diagnostic_fail: + output: none + plan: [] + note: returns ACTION_FAILED without actuating anything + +safety: + # The policy clamps every command to the joint limits before it leaves, + # rather than relying on the engine to saturate it, so both engines receive + # identical targets. + jointLimitClamp: policy-side + emergencyStop: the free `stop` skill, and SIGINT on the bridge process + maxCommandRateHz: 100 + # A stalled stage cannot spin forever; each has a budget and a reason + # string that reaches the payer. + stageTimeouts: + turn: 3.0 + raise: 10.0 + approach: 14.0 + push: 25.0 diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/functions.yaml b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/functions.yaml new file mode 100644 index 000000000..17735121b --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/functions.yaml @@ -0,0 +1,84 @@ +schemaVersion: agent-functions.v1 +profileId: unitree.g1.mujoco-drake-push.v1 + +functions: + - name: list_robot_skills + description: >- + Discover what the robot can do and what it charges, before paying for + anything. The bridge also publishes this on the Zenoh topic + robot/g1/skills at startup. + method: GET + url: /v1/robots/{robotId}/skills + response: + robotId: string + skills: + - name: string + description: string + priceUSDC: string + paymentRequired: boolean + paramsSchema: object + + - name: request_robot_action + description: >- + Request an action without payment. Returns 402 with the payment + requirements; the robot does not move. + method: POST + url: /v1/robots/{robotId}/actions + body: + skillId: string + params: object + idempotencyKey: string + payment: + unpaidStatus: 402 + paymentRequiredHeader: payment-required + + - name: submit_paid_robot_action + description: >- + Submit the action with an x402 authorisation. The tunnel verifies the + payment and attaches txHash before the envelope reaches the robot. + method: POST + url: /v1/robots/{robotId}/actions + headers: + X-PAYMENT: string + body: + skillId: string + params: object + idempotencyKey: string + # Hash of the params the payer authorised. The robot recomputes it and + # refuses the action if it does not match, so parameters altered in + # flight never reach the simulator. + paramsHash: string + expiresAt: string + response: + status: string # "success" | "error" + skill: string + actionId: string + settle: boolean # false for every failure, replay included + result: object + error: object + metrics: object + + - name: get_action_result + description: >- + Terminal result for an action, correlated by actionId. Also published on + the Zenoh topic robot/tunnel/result. + method: GET + url: /v1/robots/{robotId}/actions/{actionId} + +errors: + - code: PAYMENT_REQUIRED + meaning: no verified payment on the envelope; robot did not move + - code: PARAMS_HASH_MISMATCH + meaning: params differ from what the payer signed for + - code: PARAMS_OUT_OF_RANGE + meaning: target outside the reachable workspace + - code: ACTION_EXPIRED + meaning: envelope past its expiresAt + - code: IDEMPOTENCY_REPLAY + meaning: key already executed; not re-run and not settled + - code: UNKNOWN_ROBOT + meaning: envelope addressed to a different robotId + - code: UNKNOWN_SKILL + meaning: skillId not in the catalogue + - code: ACTION_FAILED + meaning: the robot attempted the action and did not complete it diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/payment-policy.yaml b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/payment-policy.yaml new file mode 100644 index 000000000..99debf8e3 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/payment-policy.yaml @@ -0,0 +1,61 @@ +schemaVersion: payment-policy.v1 +profileId: unitree.g1.mujoco-drake-push.v1 + +provider: x402 +network: eip155:84532 # Base Sepolia +asset: USDC +amountUnit: smallest + +policies: + - skillId: push_to_target + required: true + amount: "10000" # 0.01 USDC at 6 decimals + paymentHeader: X-PAYMENT + - skillId: stop + required: false + amount: "0" + - skillId: diagnostic_fail + required: true + amount: "10000" + paymentHeader: X-PAYMENT + +settlement: + # The rule the whole integration is built around: the relay may settle only + # when the robot reported success. Everything else -- failure, timeout, + # rejection, an exception inside the simulator, or a replayed idempotency + # key -- returns settle=false. + settleOn: success-only + settleFlagField: settle + neverSettleOn: + - ACTION_FAILED + - ACTION_EXPIRED + - PAYMENT_REQUIRED + - PARAMS_HASH_MISMATCH + - PARAMS_OUT_OF_RANGE + - IDEMPOTENCY_REPLAY + - UNKNOWN_ROBOT + - UNKNOWN_SKILL + evidence: docs/validation-report.md + +integrity: + # Defences the robot applies itself, so it does not have to trust that the + # routing layer left the envelope alone. + paramsHash: + algorithm: sha256 + encoding: canonical-json-sorted-keys + onMismatch: PARAMS_HASH_MISMATCH + idempotency: + scope: idempotencyKey + ttlSeconds: 900 + onRepeat: return the first outcome, do not re-execute, do not settle + expiry: + field: expiresAt + onExpired: ACTION_EXPIRED + +identity: + robotIdField: robotId + onMismatch: UNKNOWN_ROBOT + # Keys are read from the environment by the tunnel; the bridge never sees + # them and never logs payment payloads. + keySource: environment + keysInRepository: false diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/robot.profile.yaml b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/robot.profile.yaml new file mode 100644 index 000000000..78523b0d1 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/robot.profile.yaml @@ -0,0 +1,52 @@ +schemaVersion: robot-profile.v1 + +vendor: unitree +robotModel: g1 +robotType: g1-29dof-with-hand +profileId: unitree.g1.mujoco-drake-push.v1 +profileVersion: 1.0.0 + +description: >- + Unitree G1 humanoid executing paid manipulation actions in simulation. + A payer names where an object is and where it should end up; the robot turns + to face it, plans a collision-free path over it with a constrained IK + solver, and pushes it to the commanded destination. + +scope: simulator-only + +runtime: + transport: zenoh + actionTopic: robot/tunnel/action + resultTopic: robot/tunnel/result + metricsTopic: robot/g1/metrics + skillsTopic: robot/g1/skills + bridge: sim_bridge + # No ROS2. The criteria require Zenoh as the local robot communication + # layer; ROS2 is not part of this integration and is not installed. Stating + # that plainly is better than listing a cmdVelTopic that nothing publishes. + ros2: null + +simulation: + primaryEngine: mujoco + primaryModel: mujoco_menagerie/unitree_g1/g1_with_hands.xml + validationEngine: drake + validationModel: unitree_ros/robots/g1_description/g1_29dof_with_hand.urdf + simToSim: docs/validation-report.md + # The pelvis is welded to the world in both engines. The G1 has no balance + # controller here, and the IK planner plans against a welded pelvis, so a + # floating base meant planning in one frame and executing in another. + fixedBase: true + planner: constrained-ik-drake + +physics: + controlRateHz: 100 + actuatedJoints: 43 + # Both descriptions agree on all 43 joint names, which is what makes the + # sim-to-sim comparison a comparison of physics rather than of models. + jointNamesMatchAcrossEngines: true + +maintainers: + - github: hossein6191 + +status: experimental +license: Apache-2.0 diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/skills.yaml b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/skills.yaml new file mode 100644 index 000000000..b89f819b6 --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/skills.yaml @@ -0,0 +1,71 @@ +schemaVersion: robot-skills.v1 +profileId: unitree.g1.mujoco-drake-push.v1 + +skills: + - skillId: push_to_target + description: >- + Turn toward a puck at (puck_x, puck_y) on the work surface, approach it + from above, and push it to (goal_x, goal_y). Both ends of the motion are + chosen by the payer. + paymentRequired: true + params: + puck_x: + type: number + min: 0.30 + max: 0.44 + units: m + description: Puck start, forward of the robot base. + puck_y: + type: number + min: -0.22 + max: 0.00 + units: m + description: Puck start, lateral. Negative is the robot's right. + goal_x: + type: number + min: 0.34 + max: 0.52 + units: m + goal_y: + type: number + min: -0.18 + max: 0.10 + units: m + constraints: + # Shorter than this and the puck already counts as delivered; longer and + # the hand runs past the edge of the surface. + minPushDistanceM: 0.06 + maxPushDistanceM: 0.30 + success: + # Measured from simulator state, not asserted by the policy. + criterion: puck within goalToleranceM of (goal_x, goal_y) + goalToleranceM: 0.050 + metrics: + - displacementM + - finalDistanceM + - peakContacts + - peakContactForceN + - foreignCollision + - stages + + - skillId: stop + description: Hold the current pose and stop all motion immediately. + paymentRequired: false + params: {} + + - skillId: diagnostic_fail + description: >- + Always fails during execution. Exists so the no-settle-on-failure + guarantee can be exercised on demand rather than argued for. + paymentRequired: true + params: {} + success: + criterion: never succeeds; always returns ACTION_FAILED with settle=false + +workspace: + # Measured over a target grid, not assumed. Requests outside this are + # rejected before payment is consumed, because the IK has no solution there. + note: >- + 7 of 8 sampled target pairs succeed. The eighth, a puck at y=-0.24, is + outside the reachable set and is refused in 0.15s with an explicit reason + rather than attempted and failed. diff --git a/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/tests/skill-contract.test.yaml b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/tests/skill-contract.test.yaml new file mode 100644 index 000000000..f734ccf6f --- /dev/null +++ b/registry/vendors/unitree/g1/unitree.g1.mujoco-drake-push.v1/tests/skill-contract.test.yaml @@ -0,0 +1,96 @@ +schemaVersion: skill-contract.v1 +profileId: unitree.g1.mujoco-drake-push.v1 + +# Every case here is executable. The automated equivalents live in +# bridge/unitree/g1/sim_bridge/tests/ and run with: +# +# pytest bridge/unitree/g1/sim_bridge/tests +# +# The end-to-end ones additionally need the bridge running; see docs/README.md. + +setup: + robotId: g1-sim-001 + actionTopic: robot/tunnel/action + resultTopic: robot/tunnel/result + endpoint: tcp/127.0.0.1:7447 + +cases: + - id: discovery + intent: A payer can see the skills and prices before buying. + when: subscribe to robot/g1/skills + expect: + skillsInclude: [push_to_target, stop, diagnostic_fail] + push_to_target.priceUSDC: "0.01" + stop.paymentRequired: false + + - id: unpaid-request-does-not-move-the-robot + intent: An unpaid action is refused and nothing actuates. + when: submit push_to_target with payment.verified = false + expect: + status: error + error.code: PAYMENT_REQUIRED + settle: false + puckDisplacementM: 0.0 + + - id: tampered-params-rejected + intent: >- + Parameters altered after the payer signed for them are refused before + the simulator sees them. + when: submit push_to_target with goal_x shifted +0.05 after hashing + expect: + status: error + error.code: PARAMS_HASH_MISMATCH + settle: false + + - id: expired-action-rejected + intent: A stale envelope cannot be executed later. + when: submit push_to_target with expiresAt five minutes in the past + expect: + status: error + error.code: ACTION_EXPIRED + settle: false + + - id: out-of-range-params-rejected + intent: Targets outside the reachable workspace are refused up front. + when: submit push_to_target with puck_y = -0.90 + expect: + status: error + error.code: PARAMS_OUT_OF_RANGE + settle: false + + - id: paid-action-succeeds-and-settles + intent: The happy path, with the outcome measured from simulator state. + when: submit push_to_target puck (0.34, -0.20) goal (0.44, -0.04), paid + expect: + status: success + settle: true + metrics.finalDistanceM: "<= 0.040" + metrics.displacementM: ">= 0.10" + metrics.peakContacts: ">= 1" + + - id: failed-action-does-not-settle + intent: >- + The property the whole integration exists to protect. A paid action that + fails must return an error and must not settle. + when: submit diagnostic_fail, paid + expect: + status: error + error.code: ACTION_FAILED + settle: false + + - id: replay-does-not-execute-twice + intent: Paying once and replaying the message cannot move the robot twice. + when: submit the same idempotencyKey twice + expect: + firstAttempt.settle: true + secondAttempt.settle: false + secondAttempt.replayed: true + secondAttempt.error.code: IDEMPOTENCY_REPLAY + + - id: sim-to-sim-agreement + intent: >- + The skill is a property of the plan, not of one engine's contact model. + when: run the same task in MuJoCo and in Drake + expect: + verdictMatches: true + puckEndGapM: "<= 2 * goalToleranceM" diff --git a/tunnel/cmd/tunnelprobe/main.go b/tunnel/cmd/tunnelprobe/main.go new file mode 100644 index 000000000..9990411a9 --- /dev/null +++ b/tunnel/cmd/tunnelprobe/main.go @@ -0,0 +1,133 @@ +// Command tunnelprobe drives the tunnel's real PostAction handler so that the +// bytes it publishes can be observed, and a robot bridge can be exercised +// against them. +// +// Why this exists. A robot bridge is written against whatever the tunnel puts +// on robot/tunnel/action, and getting that shape wrong produces an integration +// that passes its own tests and works with nothing. Reading handlers.go and +// reproducing the shape by hand is better than guessing, but it is still a +// reproduction. This runs the production handler itself, through the +// production Zenoh publisher, so what lands on the topic is the real thing. +// +// What is substituted, and what is not. PostAction, its JSON shaping and its +// Zenoh publisher are the real ones, untouched. What this does not run is the +// x402 middleware in front of them, because verifying a payment needs a +// facilitator and a funded key. Instead it sets the two context values that +// middleware sets on success -- x402_payload and x402_requirements, exactly as +// http/gin/middleware.go does -- which is the state the handler sees for a +// payment that has been verified and not yet settled. Run with -unpaid to +// leave them unset and see what an unverified request would look like if it +// ever reached the handler; in the real tunnel the middleware answers 402 and +// the handler never runs at all. +// +// go run ./cmd/tunnelprobe -robot x2-sim-001 -skill push_to_target +package main + +import ( + "bytes" + "crypto/sha256" + "encoding/hex" + "encoding/json" + "flag" + "fmt" + "net/http/httptest" + "os" + "time" + + "github.com/gin-gonic/gin" + "go.uber.org/zap" + + "github.com/fabricfoundation/tunnel/internal/handlers" + x402types "github.com/x402-foundation/x402/go/types" +) + +func canonicalParamsHash(params map[string]any) string { + // Matches the bridge: canonical JSON, sorted keys, no incidental space. + // encoding/json sorts map keys, so this is already canonical. + blob, _ := json.Marshal(params) + sum := sha256.Sum256(blob) + return "sha256:" + hex.EncodeToString(sum[:]) +} + +func main() { + robot := flag.String("robot", "x2-sim-001", "robotId the action is addressed to") + skill := flag.String("skill", "push_to_target", "skillId to request") + puckX := flag.Float64("puck-x", 0.26, "") + puckY := flag.Float64("puck-y", 0.17, "") + goalX := flag.Float64("goal-x", 0.27, "") + goalY := flag.Float64("goal-y", 0.30, "") + unpaid := flag.Bool("unpaid", false, "omit the x402 context values") + flag.Parse() + + logger, _ := zap.NewProduction() + defer func() { _ = logger.Sync() }() + + params := map[string]any{} + if *skill == "push_to_target" { + params = map[string]any{ + "puck_x": *puckX, "puck_y": *puckY, + "goal_x": *goalX, "goal_y": *goalY, + } + } + + body := map[string]any{ + "actionId": fmt.Sprintf("act_probe_%d", time.Now().UnixNano()%1e12), + "robotId": *robot, + "skillId": *skill, + "params": params, + "idempotencyKey": fmt.Sprintf("idem-probe-%d", time.Now().UnixNano()%1e10), + "paramsHash": canonicalParamsHash(params), + "expiresAt": time.Now().UTC().Add(10 * time.Minute).Format(time.RFC3339), + } + raw, _ := json.Marshal(body) + + requirements := x402types.PaymentRequirements{ + Scheme: "exact", + Network: "eip155:84532", + Asset: "0x036CbD53842c5426634e7929541eC2318f3dCF7e", + Amount: "2000", + PayTo: "0xf39Fd6e51aad88F6F4ce6aB8827279cffFb92266", + MaxTimeoutSeconds: 30, + } + payload := x402types.PaymentPayload{ + X402Version: 2, + Payload: map[string]any{ + "signature": "0x" + hex.EncodeToString(bytes.Repeat([]byte{0xab}, 65)), + "authorization": map[string]any{ + "from": "0x1111111111111111111111111111111111111111", + "to": requirements.PayTo, + "value": requirements.Amount, + "validAfter": "0", + "validBefore": "9999999999", + "nonce": "0x" + hex.EncodeToString(bytes.Repeat([]byte{0xcd}, 32)), + }, + }, + Accepted: requirements, + } + + gin.SetMode(gin.ReleaseMode) + router := gin.New() + router.POST("/action", func(c *gin.Context) { + if !*unpaid { + c.Set("x402_payload", payload) + c.Set("x402_requirements", requirements) + } + handlers.NewHandlers(logger).PostAction(c) + }) + + rec := httptest.NewRecorder() + req := httptest.NewRequest("POST", "/action", bytes.NewReader(raw)) + req.Header.Set("Content-Type", "application/json") + router.ServeHTTP(rec, req) + + fmt.Printf("handler status : %d\n", rec.Code) + fmt.Printf("handler body : %s\n", rec.Body.String()) + fmt.Printf("published to : %s\n", handlers.RobotActionTopic) + fmt.Printf("action id : %s\n", body["actionId"]) + + // Zenoh publishes asynchronously; give the session a moment before exit. + time.Sleep(1500 * time.Millisecond) + if rec.Code != 200 { + os.Exit(1) + } +}