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.
- a2a_robots_sharpa-0.1.1/.gitignore +8 -0
- a2a_robots_sharpa-0.1.1/CHANGELOG.md +17 -0
- a2a_robots_sharpa-0.1.1/PKG-INFO +61 -0
- a2a_robots_sharpa-0.1.1/README.md +39 -0
- a2a_robots_sharpa-0.1.1/examples/serve_mujoco.py +29 -0
- a2a_robots_sharpa-0.1.1/pyproject.toml +37 -0
- a2a_robots_sharpa-0.1.1/src/a2a_robots_sharpa/__init__.py +16 -0
- a2a_robots_sharpa-0.1.1/src/a2a_robots_sharpa/__main__.py +7 -0
- a2a_robots_sharpa-0.1.1/src/a2a_robots_sharpa/model.py +88 -0
- a2a_robots_sharpa-0.1.1/src/a2a_robots_sharpa/mujoco_backend.py +310 -0
- a2a_robots_sharpa-0.1.1/src/a2a_robots_sharpa/planner.py +157 -0
- a2a_robots_sharpa-0.1.1/tests/mini_dual_hand.xml +123 -0
- a2a_robots_sharpa-0.1.1/tests/test_contract.py +8 -0
- a2a_robots_sharpa-0.1.1/tests/test_model.py +67 -0
- a2a_robots_sharpa-0.1.1/tests/test_mujoco_backend.py +210 -0
- a2a_robots_sharpa-0.1.1/tests/test_planner.py +135 -0
- a2a_robots_sharpa-0.1.1/uv.lock +1541 -0
|
@@ -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,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")
|