a2a-robots-sharpa 0.1.1__tar.gz

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,8 @@
1
+ __pycache__/
2
+ *.py[cod]
3
+ .venv/
4
+ build/
5
+ dist/
6
+ *.egg-info/
7
+ .pytest_cache/
8
+ .ruff_cache/
@@ -0,0 +1,17 @@
1
+ # Changelog
2
+
3
+ ## [0.1.1](https://github.com/hybroai/a2a-robots/compare/a2a-robots-sharpa-v0.1.0...a2a-robots-sharpa-v0.1.1) (2026-11-18)
4
+
5
+
6
+ ### Miscellaneous Chores
7
+
8
+ * **sharpa:** bootstrap first PyPI release ([d7ffa98](https://github.com/hybroai/a2a-robots/commit/d7ffa981df11c32d409a5295cb93d73853e734fd))
9
+
10
+ ## Changelog
11
+
12
+ All notable changes to this project will be documented in this file.
13
+
14
+ Entries are generated by
15
+ [release-please](https://github.com/googleapis/release-please) from
16
+ Conventional Commits. This project adheres to
17
+ [Semantic Versioning](https://semver.org/).
@@ -0,0 +1,61 @@
1
+ Metadata-Version: 2.5
2
+ Name: a2a-robots-sharpa
3
+ Version: 0.1.1
4
+ Summary: Sharpa Wave hands for a2a-robots, simulated in MuJoCo
5
+ Project-URL: Repository, https://github.com/hybroai/a2a-robots
6
+ Author-email: HYBRO AI <info@hybro.ai>
7
+ License-Expression: Apache-2.0
8
+ Classifier: Development Status :: 2 - Pre-Alpha
9
+ Classifier: Programming Language :: Python :: 3
10
+ Classifier: Programming Language :: Python :: 3.11
11
+ Classifier: Programming Language :: Python :: 3.12
12
+ Classifier: Programming Language :: Python :: 3.13
13
+ Classifier: Topic :: Scientific/Engineering :: Artificial Intelligence
14
+ Requires-Python: >=3.11
15
+ Requires-Dist: a2a-robots
16
+ Requires-Dist: mujoco<4,>=3.3
17
+ Requires-Dist: numpy>=1.26
18
+ Provides-Extra: dev
19
+ Requires-Dist: pytest-asyncio>=0.23; extra == 'dev'
20
+ Requires-Dist: pytest>=8; extra == 'dev'
21
+ Description-Content-Type: text/markdown
22
+
23
+ # a2a-robots-sharpa
24
+
25
+ [Sharpa Wave](https://github.com/sharpa-robotics) hands for [a2a-robots](../../README.md). This release has the MuJoCo backend; the Sharpa SDK backend for the real hands will be added here once the SDK is available.
26
+
27
+ Install from PyPI: `pip install a2a-robots-sharpa`.
28
+
29
+ ## MuJoCo backend
30
+
31
+ `SharpaMujocoBackend` runs `dual_sharpa_wave.xml` from [sharpa-robotics/sharpa-urdf-usd-xml](https://github.com/sharpa-robotics/sharpa-urdf-usd-xml) (Apache-2.0): two hands, each on a 6-DoF floating base, with no arm or body model.
32
+
33
+ - The hand pose is the palm frame (`<hand>_hand_C_MC`) in `world`. `move_to` asks the planner for a joint trajectory for the base joints and streams it to the model's position actuators, one target per control step, the way a real arm takes joint targets from a host-side planner.
34
+ - The default planner is `MujocoPlanner` on the same model. It solves inverse kinematics, interpolates the joints at up to 5 cm/s and 0.5 rad/s with smooth start and stop, and checks every point against the current scene. A path into an object or the other hand returns `unreachable` with the object's name, before anything moves. Objects the hand already touches, such as one it holds, are not treated as obstacles. Pass `planner=` to use another `a2a_robots.planning.Planner`.
35
+ - A target outside the base joint range, or one the hand does not settle at, returns `unreachable`.
36
+ - `close` bends the flexion joints to `grasp_closure` (default 0.6) of their range and keeps abduction at zero; `open` straightens the fingers. `contact` is the normal force in newtons on each finger (and `palm`) from anything outside the hand. `held` means the thumb and another finger are both in contact. A close that touches something but ends without holding it returns `slipped`.
37
+ - The hand bodies have gravity compensation, as if mounted on an arm, so the base does not sag. Objects in the scene still fall.
38
+ - The simulation advances only while a skill runs, paced to wall-clock time unless `realtime=False`. Every control step (20 ms) checks the stop flag.
39
+
40
+ The meshes are about 24 MB, so they are not committed here. `fetch_model()` downloads the model at a pinned commit on first use, checks its digest, and caches it under `~/.cache/a2a-robots/`.
41
+
42
+ ```bash
43
+ uv run --directory packages/a2a-robots-sharpa python examples/serve_mujoco.py # http://127.0.0.1:9100/
44
+ ```
45
+
46
+ To add objects, edit the spec before compiling, as the example does:
47
+
48
+ ```python
49
+ spec = mujoco.MjSpec.from_file(str(fetch_model()))
50
+ # spec.worldbody.add_geom(...) or add_body(...)
51
+ backend = SharpaMujocoBackend(load_model(spec))
52
+ ```
53
+
54
+ ## Tests
55
+
56
+ The unit tests use `tests/mini_dual_hand.xml`, a mesh-free model with the same joint and body names, so they need no download. `test_real_sharpa_model_moves_and_grasps` runs against the real model when `SHARPA_MODEL_DIR` is set:
57
+
58
+ ```bash
59
+ uv run --directory packages/a2a-robots-sharpa python -m a2a_robots_sharpa ~/.cache/a2a-robots/sharpa-wave
60
+ SHARPA_MODEL_DIR=~/.cache/a2a-robots/sharpa-wave uv run --directory packages/a2a-robots-sharpa --extra dev pytest
61
+ ```
@@ -0,0 +1,39 @@
1
+ # a2a-robots-sharpa
2
+
3
+ [Sharpa Wave](https://github.com/sharpa-robotics) hands for [a2a-robots](../../README.md). This release has the MuJoCo backend; the Sharpa SDK backend for the real hands will be added here once the SDK is available.
4
+
5
+ Install from PyPI: `pip install a2a-robots-sharpa`.
6
+
7
+ ## MuJoCo backend
8
+
9
+ `SharpaMujocoBackend` runs `dual_sharpa_wave.xml` from [sharpa-robotics/sharpa-urdf-usd-xml](https://github.com/sharpa-robotics/sharpa-urdf-usd-xml) (Apache-2.0): two hands, each on a 6-DoF floating base, with no arm or body model.
10
+
11
+ - The hand pose is the palm frame (`<hand>_hand_C_MC`) in `world`. `move_to` asks the planner for a joint trajectory for the base joints and streams it to the model's position actuators, one target per control step, the way a real arm takes joint targets from a host-side planner.
12
+ - The default planner is `MujocoPlanner` on the same model. It solves inverse kinematics, interpolates the joints at up to 5 cm/s and 0.5 rad/s with smooth start and stop, and checks every point against the current scene. A path into an object or the other hand returns `unreachable` with the object's name, before anything moves. Objects the hand already touches, such as one it holds, are not treated as obstacles. Pass `planner=` to use another `a2a_robots.planning.Planner`.
13
+ - A target outside the base joint range, or one the hand does not settle at, returns `unreachable`.
14
+ - `close` bends the flexion joints to `grasp_closure` (default 0.6) of their range and keeps abduction at zero; `open` straightens the fingers. `contact` is the normal force in newtons on each finger (and `palm`) from anything outside the hand. `held` means the thumb and another finger are both in contact. A close that touches something but ends without holding it returns `slipped`.
15
+ - The hand bodies have gravity compensation, as if mounted on an arm, so the base does not sag. Objects in the scene still fall.
16
+ - The simulation advances only while a skill runs, paced to wall-clock time unless `realtime=False`. Every control step (20 ms) checks the stop flag.
17
+
18
+ The meshes are about 24 MB, so they are not committed here. `fetch_model()` downloads the model at a pinned commit on first use, checks its digest, and caches it under `~/.cache/a2a-robots/`.
19
+
20
+ ```bash
21
+ uv run --directory packages/a2a-robots-sharpa python examples/serve_mujoco.py # http://127.0.0.1:9100/
22
+ ```
23
+
24
+ To add objects, edit the spec before compiling, as the example does:
25
+
26
+ ```python
27
+ spec = mujoco.MjSpec.from_file(str(fetch_model()))
28
+ # spec.worldbody.add_geom(...) or add_body(...)
29
+ backend = SharpaMujocoBackend(load_model(spec))
30
+ ```
31
+
32
+ ## Tests
33
+
34
+ The unit tests use `tests/mini_dual_hand.xml`, a mesh-free model with the same joint and body names, so they need no download. `test_real_sharpa_model_moves_and_grasps` runs against the real model when `SHARPA_MODEL_DIR` is set:
35
+
36
+ ```bash
37
+ uv run --directory packages/a2a-robots-sharpa python -m a2a_robots_sharpa ~/.cache/a2a-robots/sharpa-wave
38
+ SHARPA_MODEL_DIR=~/.cache/a2a-robots/sharpa-wave uv run --directory packages/a2a-robots-sharpa --extra dev pytest
39
+ ```
@@ -0,0 +1,29 @@
1
+ """Serve the simulated Sharpa Wave hands on http://127.0.0.1:9100/.
2
+
3
+ Downloads the model on first run. A can stands in front of the right hand, so
4
+ ``close`` on the right hand grasps it.
5
+ """
6
+
7
+ import mujoco
8
+
9
+ from a2a_robots import RobotAdapter, SafetyLimits, Workspace, serve_robot
10
+ from a2a_robots_sharpa import SharpaMujocoBackend, fetch_model, load_model
11
+
12
+ spec = mujoco.MjSpec.from_file(str(fetch_model()))
13
+ can = spec.worldbody.add_geom()
14
+ can.name = "can"
15
+ can.type = mujoco.mjtGeom.mjGEOM_CYLINDER
16
+ can.size = [0.02, 0.04, 0]
17
+ can.pos = [0.05, 0.18, 0.11]
18
+ can.quat = [0.7071068, 0.7071068, 0, 0]
19
+
20
+ limits = SafetyLimits(workspace=Workspace("world", (-0.3, -0.5, -0.3), (0.3, 0.5, 0.4)))
21
+ adapter = RobotAdapter(
22
+ SharpaMujocoBackend(load_model(spec)),
23
+ limits,
24
+ name="Sharpa Wave (MuJoCo)",
25
+ description="Two simulated Sharpa Wave hands on floating bases.",
26
+ )
27
+
28
+ if __name__ == "__main__":
29
+ serve_robot(adapter, port=9100)
@@ -0,0 +1,37 @@
1
+ [build-system]
2
+ requires = ["hatchling>=1.24"]
3
+ build-backend = "hatchling.build"
4
+
5
+ [project]
6
+ name = "a2a-robots-sharpa"
7
+ version = "0.1.1"
8
+ description = "Sharpa Wave hands for a2a-robots, simulated in MuJoCo"
9
+ readme = "README.md"
10
+ license = "Apache-2.0"
11
+ authors = [{ name = "HYBRO AI", email = "info@hybro.ai" }]
12
+ requires-python = ">=3.11"
13
+ classifiers = [
14
+ "Development Status :: 2 - Pre-Alpha",
15
+ "Programming Language :: Python :: 3",
16
+ "Programming Language :: Python :: 3.11",
17
+ "Programming Language :: Python :: 3.12",
18
+ "Programming Language :: Python :: 3.13",
19
+ "Topic :: Scientific/Engineering :: Artificial Intelligence",
20
+ ]
21
+ dependencies = ["a2a-robots", "mujoco>=3.3,<4", "numpy>=1.26"]
22
+
23
+ [project.optional-dependencies]
24
+ dev = ["pytest>=8", "pytest-asyncio>=0.23"]
25
+
26
+ [project.urls]
27
+ Repository = "https://github.com/hybroai/a2a-robots"
28
+
29
+ [tool.uv.sources]
30
+ a2a-robots = { path = "../..", editable = true }
31
+
32
+ [tool.hatch.build.targets.wheel]
33
+ packages = ["src/a2a_robots_sharpa"]
34
+
35
+ [tool.pytest.ini_options]
36
+ asyncio_mode = "auto"
37
+ asyncio_default_fixture_loop_scope = "function"
@@ -0,0 +1,16 @@
1
+ """Sharpa Wave hands for a2a-robots."""
2
+
3
+ __version__ = "0.1.1"
4
+
5
+ from .model import fetch_model
6
+ from .mujoco_backend import SharpaMujocoBackend, load_model
7
+ from .planner import Chain, MujocoPlanner
8
+
9
+ __all__ = [
10
+ "Chain",
11
+ "MujocoPlanner",
12
+ "SharpaMujocoBackend",
13
+ "__version__",
14
+ "fetch_model",
15
+ "load_model",
16
+ ]
@@ -0,0 +1,7 @@
1
+ """``python -m a2a_robots_sharpa [cache_dir]``: download the model and print its path."""
2
+
3
+ import sys
4
+
5
+ from .model import fetch_model
6
+
7
+ print(fetch_model(sys.argv[1] if len(sys.argv) > 1 else None))
@@ -0,0 +1,88 @@
1
+ """Download the Sharpa Wave MuJoCo model (Apache-2.0) at a pinned commit.
2
+
3
+ The meshes are about 24 MB, so they are fetched on first use and cached
4
+ instead of being committed to this repository.
5
+ """
6
+
7
+ from __future__ import annotations
8
+
9
+ import hashlib
10
+ import io
11
+ import shutil
12
+ import tarfile
13
+ import tempfile
14
+ import urllib.request
15
+ from pathlib import Path
16
+
17
+ REPO = "sharpa-robotics/sharpa-urdf-usd-xml"
18
+ COMMIT = "0d19cac602f46456b819e4b6a2c09a74982c9a3e"
19
+ # sha256 over the sorted "path sha256" lines of every extracted file.
20
+ DIGEST = "e6e654c254a8d954e5574da66d77067f78a3962e5a1ef4dd86f4af6321429816"
21
+ MODEL_FILE = "dual_sharpa_wave.xml"
22
+
23
+ _KEEP = (
24
+ "LICENSE.txt",
25
+ "NOTICE.txt",
26
+ "wave_01/left_sharpa_wave/meshes/",
27
+ "wave_01/right_sharpa_wave/meshes/",
28
+ )
29
+ _XML = "wave_01/dual_sharpa_wave/dual_sharpa_wave.xml"
30
+
31
+
32
+ def default_cache_dir() -> Path:
33
+ return Path.home() / ".cache" / "a2a-robots" / f"sharpa-wave-{COMMIT[:12]}"
34
+
35
+
36
+ def fetch_model(cache_dir: str | Path | None = None) -> Path:
37
+ """Return the path to ``dual_sharpa_wave.xml``, downloading it if needed.
38
+
39
+ The XML is placed next to the ``left_sharpa_wave`` and ``right_sharpa_wave``
40
+ mesh folders so its relative mesh paths resolve.
41
+ """
42
+ root = Path(cache_dir) if cache_dir is not None else default_cache_dir()
43
+ xml = root / MODEL_FILE
44
+ if xml.is_file() and _digest(root) == DIGEST:
45
+ return xml
46
+ url = f"https://codeload.github.com/{REPO}/tar.gz/{COMMIT}"
47
+ with urllib.request.urlopen(url, timeout=120) as response:
48
+ archive = response.read()
49
+ root.parent.mkdir(parents=True, exist_ok=True)
50
+ staging = Path(tempfile.mkdtemp(dir=root.parent, prefix=".sharpa-"))
51
+ try:
52
+ _extract(archive, staging)
53
+ digest = _digest(staging)
54
+ if digest != DIGEST:
55
+ raise RuntimeError(f"Sharpa model digest mismatch: got {digest}, expected {DIGEST}")
56
+ shutil.rmtree(root, ignore_errors=True)
57
+ staging.rename(root)
58
+ finally:
59
+ shutil.rmtree(staging, ignore_errors=True)
60
+ return xml
61
+
62
+
63
+ def _extract(archive: bytes, dest: Path) -> None:
64
+ with tarfile.open(fileobj=io.BytesIO(archive), mode="r:gz") as tar:
65
+ for member in tar.getmembers():
66
+ if not member.isfile():
67
+ continue
68
+ # Strip the "<repo>-<commit>/" prefix GitHub adds.
69
+ path = member.name.split("/", 1)[1] if "/" in member.name else ""
70
+ if path == _XML:
71
+ target = dest / MODEL_FILE
72
+ elif path.startswith(_KEEP) and ".." not in path.split("/"):
73
+ target = dest / path.removeprefix("wave_01/")
74
+ else:
75
+ continue
76
+ source = tar.extractfile(member)
77
+ assert source is not None
78
+ target.parent.mkdir(parents=True, exist_ok=True)
79
+ target.write_bytes(source.read())
80
+
81
+
82
+ def _digest(root: Path) -> str:
83
+ lines = sorted(
84
+ f"{p.relative_to(root).as_posix()} {hashlib.sha256(p.read_bytes()).hexdigest()}"
85
+ for p in root.rglob("*")
86
+ if p.is_file()
87
+ )
88
+ return hashlib.sha256("\n".join(lines).encode()).hexdigest()
@@ -0,0 +1,310 @@
1
+ """Sharpa Wave hands simulated in MuJoCo.
2
+
3
+ Works with ``dual_sharpa_wave.xml`` from sharpa-robotics/sharpa-urdf-usd-xml and
4
+ with any model that uses the same names: each hand ``<hand>`` has a floating
5
+ base driven by the position actuators ``<hand>_{x,y,z,roll,pitch,yaw}_joint_ctrl``,
6
+ a palm body ``<hand>_hand_C_MC``, and finger joints ``<hand>_<finger>_*``, each
7
+ with a ``<joint>_ctrl`` position actuator.
8
+
9
+ The hand pose is the palm body frame in world coordinates. ``move_to`` asks a
10
+ planner for a joint trajectory and streams it one target per control step, the
11
+ way a real arm takes joint targets from a host-side planner. The simulation only
12
+ advances while a motion runs, so the scene is frozen between skills.
13
+ """
14
+
15
+ from __future__ import annotations
16
+
17
+ import threading
18
+ import time
19
+ from collections.abc import Iterable
20
+ from dataclasses import dataclass
21
+ from pathlib import Path
22
+
23
+ import mujoco
24
+ import numpy as np
25
+
26
+ from a2a_robots import FailureCode, HandState, MotionOutcome, Pose, RobotBackend
27
+ from a2a_robots.planning import PlanError, Planner
28
+
29
+ from .planner import Chain, MujocoPlanner
30
+
31
+ BASE_JOINTS = ("x", "y", "z", "roll", "pitch", "yaw")
32
+ PALM = "palm"
33
+
34
+
35
+ @dataclass(frozen=True, slots=True)
36
+ class _Hand:
37
+ palm_body: int
38
+ base_joints: tuple[str, ...]
39
+ base_qpos: np.ndarray
40
+ base_dofs: np.ndarray
41
+ base_actuators: np.ndarray
42
+ finger_qpos: np.ndarray
43
+ finger_dofs: np.ndarray
44
+ finger_actuators: np.ndarray
45
+ open_targets: np.ndarray
46
+ close_targets: np.ndarray
47
+ geom_part: dict[int, str]
48
+
49
+
50
+ class SharpaMujocoBackend(RobotBackend):
51
+ def __init__(
52
+ self,
53
+ model: mujoco.MjModel,
54
+ *,
55
+ hands: Iterable[str] = ("left", "right"),
56
+ frame: str = "world",
57
+ realtime: bool = True,
58
+ control_dt: float = 0.02,
59
+ settle_time_s: float = 3.0,
60
+ position_tolerance_m: float = 0.002,
61
+ orientation_tolerance_rad: float = 0.02,
62
+ grasp_closure: float = 0.6,
63
+ planner: Planner | None = None,
64
+ ) -> None:
65
+ """``realtime`` paces the simulation to wall-clock time; tests turn it off.
66
+ ``grasp_closure`` is how far ``close`` bends the flexion joints, as a
67
+ fraction of their range. ``planner`` defaults to a ``MujocoPlanner`` on
68
+ the same model, which checks the path against the simulated scene."""
69
+ self._model = model
70
+ self._data = mujoco.MjData(model)
71
+ self._frame = frame
72
+ self._realtime = realtime
73
+ self._substeps = max(1, round(control_dt / model.opt.timestep))
74
+ self._control_dt = self._substeps * model.opt.timestep
75
+ self._settle_steps = max(1, round(settle_time_s / self._control_dt))
76
+ self._pos_tol = position_tolerance_m
77
+ self._rot_tol = orientation_tolerance_rad
78
+ self._hands = {name: self._index_hand(name, grasp_closure) for name in hands}
79
+ self._planner = planner or MujocoPlanner(
80
+ model,
81
+ {name: Chain(f"{name}_hand_C_MC", h.base_joints) for name, h in self._hands.items()},
82
+ )
83
+ self._lock = threading.Lock()
84
+ with self._lock:
85
+ mujoco.mj_forward(self._model, self._data)
86
+ self._hold_locked()
87
+
88
+ @property
89
+ def hands(self) -> frozenset[str]:
90
+ return frozenset(self._hands)
91
+
92
+ def observe(self) -> dict[str, HandState]:
93
+ with self._lock:
94
+ return {name: self._state(hand) for name, hand in self._hands.items()}
95
+
96
+ def move_to(self, hand: str, target: Pose, stop: threading.Event) -> MotionOutcome:
97
+ h = self._hands[hand]
98
+ with self._lock:
99
+ start = dict(zip(h.base_joints, self._data.qpos[h.base_qpos], strict=True))
100
+ if isinstance(self._planner, MujocoPlanner):
101
+ self._planner.set_scene(self._data.qpos)
102
+ try:
103
+ trajectory = self._planner.plan(hand, start, target, self._control_dt)
104
+ except PlanError as error:
105
+ return MotionOutcome(error.failure_code, error.detail)
106
+ if trajectory.joints != h.base_joints:
107
+ raise ValueError(f"planner returned joints {trajectory.joints}, not {h.base_joints}")
108
+ points = iter(trajectory.points)
109
+
110
+ def track() -> bool:
111
+ # Stream the next target each control step, then wait for the hand to settle.
112
+ point = next(points, None)
113
+ if point is not None:
114
+ self._data.ctrl[h.base_actuators] = point
115
+ return False
116
+ pos_err, rot_err = self._pose_error(h, target)
117
+ speed = np.abs(self._data.qvel[h.base_dofs]).max()
118
+ return pos_err <= self._pos_tol and rot_err <= self._rot_tol and speed < 1e-2
119
+
120
+ if self._run(stop, track, extra_steps=len(trajectory.points)):
121
+ return MotionOutcome()
122
+ if stop.is_set():
123
+ return MotionOutcome(FailureCode.STOPPED, "stopped before reaching the target")
124
+ return MotionOutcome(FailureCode.UNREACHABLE, "the hand did not settle at the target")
125
+
126
+ def open(self, hand: str, stop: threading.Event) -> MotionOutcome:
127
+ return self._move_fingers(self._hands[hand], self._hands[hand].open_targets, stop)
128
+
129
+ def close(self, hand: str, stop: threading.Event) -> MotionOutcome:
130
+ h = self._hands[hand]
131
+ touched = False
132
+
133
+ def watch() -> None:
134
+ nonlocal touched
135
+ touched = touched or bool(self._contact(h))
136
+
137
+ outcome = self._move_fingers(h, h.close_targets, stop, watch)
138
+ if outcome.failure_code is not None:
139
+ return outcome
140
+ with self._lock:
141
+ held = self._held(h)
142
+ if touched and not held:
143
+ return MotionOutcome(FailureCode.SLIPPED, "object slipped during the grasp")
144
+ return MotionOutcome()
145
+
146
+ def hold(self) -> None:
147
+ with self._lock:
148
+ self._hold_locked()
149
+
150
+ def shutdown(self) -> None:
151
+ """Nothing to release: the model and data are freed with the backend."""
152
+
153
+ # Motion
154
+
155
+ def _move_fingers(self, h: _Hand, targets: np.ndarray, stop: threading.Event, watch=None):
156
+ with self._lock:
157
+ self._data.ctrl[h.finger_actuators] = targets
158
+ best = np.inf
159
+ stalled = 0
160
+
161
+ def settled() -> bool:
162
+ # Fingers stopped by an object or by each other never reach their
163
+ # targets, so treat them as settled once the error stops shrinking.
164
+ nonlocal best, stalled
165
+ if watch is not None:
166
+ watch()
167
+ error = np.abs(self._data.qpos[h.finger_qpos] - targets).max(initial=0.0)
168
+ if error < best - 5e-3:
169
+ best, stalled = error, 0
170
+ else:
171
+ stalled += 1
172
+ return error < 1e-2 or stalled * self._control_dt >= 0.2
173
+
174
+ if self._run(stop, settled):
175
+ return MotionOutcome()
176
+ if stop.is_set():
177
+ return MotionOutcome(FailureCode.STOPPED, "stopped while moving the fingers")
178
+ return MotionOutcome(FailureCode.TIMEOUT, "the fingers did not settle")
179
+
180
+ def _run(self, stop: threading.Event, done, extra_steps: int = 0) -> bool:
181
+ """Step one control period at a time until ``done()`` or stop.
182
+
183
+ ``done()`` runs after each step and may set the next targets. Returns True
184
+ when it became true within ``extra_steps`` plus the settle time.
185
+ """
186
+ start = time.monotonic()
187
+ for i in range(1, extra_steps + self._settle_steps + 1):
188
+ if stop.is_set():
189
+ return False
190
+ with self._lock:
191
+ mujoco.mj_step(self._model, self._data, nstep=self._substeps)
192
+ if done():
193
+ return True
194
+ if self._realtime:
195
+ remaining = start + i * self._control_dt - time.monotonic()
196
+ if remaining > 0 and stop.wait(remaining):
197
+ return False
198
+ return False
199
+
200
+ def _hold_locked(self) -> None:
201
+ for h in self._hands.values():
202
+ self._data.ctrl[h.base_actuators] = self._data.qpos[h.base_qpos]
203
+ self._data.ctrl[h.finger_actuators] = self._data.qpos[h.finger_qpos]
204
+ self._data.qvel[h.base_dofs] = 0.0
205
+ self._data.qvel[h.finger_dofs] = 0.0
206
+
207
+ # Kinematics
208
+
209
+ def _pose_error(self, h: _Hand, target: Pose) -> tuple[float, float]:
210
+ pos_err = np.linalg.norm(np.asarray(target.position) - self._data.xpos[h.palm_body])
211
+ rot = np.zeros(3)
212
+ mujoco.mju_subQuat(
213
+ rot, np.asarray(target.orientation, dtype=float), self._data.xquat[h.palm_body]
214
+ )
215
+ return float(pos_err), float(np.linalg.norm(rot))
216
+
217
+ # State
218
+
219
+ def _state(self, h: _Hand) -> HandState:
220
+ position = tuple(float(v) for v in self._data.xpos[h.palm_body])
221
+ quat = self._data.xquat[h.palm_body]
222
+ orientation = tuple(float(v) for v in (quat if quat[0] >= 0 else -quat))
223
+ return HandState(Pose(self._frame, position, orientation), self._contact(h), self._held(h))
224
+
225
+ def _contact(self, h: _Hand) -> dict[str, float]:
226
+ """Normal force in newtons on each part of the hand from anything outside it."""
227
+ forces: dict[str, float] = {}
228
+ wrench = np.zeros(6)
229
+ for i in range(self._data.ncon):
230
+ contact = self._data.contact[i]
231
+ part1 = h.geom_part.get(contact.geom1)
232
+ part2 = h.geom_part.get(contact.geom2)
233
+ if (part1 is None) == (part2 is None):
234
+ continue
235
+ mujoco.mj_contactForce(self._model, self._data, i, wrench)
236
+ part = part1 or part2
237
+ forces[part] = forces.get(part, 0.0) + float(abs(wrench[0]))
238
+ return {part: round(force, 4) for part, force in forces.items() if force > 1e-6}
239
+
240
+ def _held(self, h: _Hand) -> bool:
241
+ contact = self._contact(h)
242
+ return "thumb" in contact and any(p not in ("thumb", PALM) for p in contact)
243
+
244
+ # Model indexing
245
+
246
+ def _index_hand(self, name: str, grasp_closure: float) -> _Hand:
247
+ m = self._model
248
+ base_joints = [_id(m, mujoco.mjtObj.mjOBJ_JOINT, f"{name}_{j}_joint") for j in BASE_JOINTS]
249
+ palm = _id(m, mujoco.mjtObj.mjOBJ_BODY, f"{name}_hand_C_MC")
250
+ base_set = set(base_joints)
251
+ fingers = [
252
+ j for j in range(m.njnt) if m.joint(j).name.startswith(f"{name}_") and j not in base_set
253
+ ]
254
+ if not fingers:
255
+ raise ValueError(f"model has no finger joints for hand {name!r}")
256
+ ranges = m.jnt_range[fingers]
257
+ open_targets = np.clip(0.0, ranges[:, 0], ranges[:, 1])
258
+ flexion = np.array(["_AA" not in m.joint(j).name for j in fingers])
259
+ close_targets = np.where(
260
+ flexion, ranges[:, 0] + grasp_closure * (ranges[:, 1] - ranges[:, 0]), open_targets
261
+ )
262
+ geom_part = {}
263
+ for g in range(m.ngeom):
264
+ body = m.body(m.geom_bodyid[g]).name
265
+ if not body.startswith(f"{name}_"):
266
+ continue
267
+ part = body.split("_")[1]
268
+ geom_part[g] = PALM if part in ("hand", "root") else part
269
+ return _Hand(
270
+ palm_body=palm,
271
+ base_joints=tuple(m.joint(j).name for j in base_joints),
272
+ base_qpos=m.jnt_qposadr[base_joints],
273
+ base_dofs=m.jnt_dofadr[base_joints],
274
+ base_actuators=np.array([_actuator(m, j) for j in base_joints]),
275
+ finger_qpos=m.jnt_qposadr[fingers],
276
+ finger_dofs=m.jnt_dofadr[fingers],
277
+ finger_actuators=np.array([_actuator(m, j) for j in fingers]),
278
+ open_targets=open_targets,
279
+ close_targets=close_targets,
280
+ geom_part=geom_part,
281
+ )
282
+
283
+
284
+ def load_model(
285
+ source: str | Path | mujoco.MjSpec, hands: Iterable[str] = ("left", "right")
286
+ ) -> mujoco.MjModel:
287
+ """Compile a model for ``SharpaMujocoBackend`` from an MJCF path, or from a
288
+ spec the caller has edited (for example to add objects to the scene).
289
+
290
+ The arm a hand is mounted on carries its weight, so the hand bodies get
291
+ gravity compensation and the floating base does not sag. Objects in the
292
+ scene still fall. MuJoCo only applies this when it is set before compiling.
293
+ """
294
+ spec = source if isinstance(source, mujoco.MjSpec) else mujoco.MjSpec.from_file(str(source))
295
+ prefixes = tuple(f"{hand}_" for hand in hands)
296
+ for body in spec.bodies:
297
+ if body.name.startswith(prefixes):
298
+ body.gravcomp = 1.0
299
+ return spec.compile()
300
+
301
+
302
+ def _id(model: mujoco.MjModel, kind: mujoco.mjtObj, name: str) -> int:
303
+ index = mujoco.mj_name2id(model, kind, name)
304
+ if index < 0:
305
+ raise ValueError(f"model has no {kind.name[6:].lower()} named {name!r}")
306
+ return index
307
+
308
+
309
+ def _actuator(model: mujoco.MjModel, joint: int) -> int:
310
+ return _id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{model.joint(joint).name}_ctrl")