a2a-robots-sharpa 0.1.1__py3-none-any.whl
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/__init__.py +16 -0
- a2a_robots_sharpa/__main__.py +7 -0
- a2a_robots_sharpa/model.py +88 -0
- a2a_robots_sharpa/mujoco_backend.py +310 -0
- a2a_robots_sharpa/planner.py +157 -0
- a2a_robots_sharpa-0.1.1.dist-info/METADATA +61 -0
- a2a_robots_sharpa-0.1.1.dist-info/RECORD +8 -0
- a2a_robots_sharpa-0.1.1.dist-info/WHEEL +4 -0
|
@@ -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")
|
|
@@ -0,0 +1,157 @@
|
|
|
1
|
+
"""A joint-space planner that uses a MuJoCo model of the robot and its scene.
|
|
2
|
+
|
|
3
|
+
For each hand it knows a chain: the joints that carry the hand and the body
|
|
4
|
+
whose pose ``move_to`` targets. It solves inverse kinematics, interpolates in
|
|
5
|
+
joint space, and rejects a path on which the hand would hit something. Objects
|
|
6
|
+
the hand already touches at the start (such as one it holds) are not obstacles.
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
from collections.abc import Mapping, Sequence
|
|
12
|
+
from dataclasses import dataclass
|
|
13
|
+
|
|
14
|
+
import mujoco
|
|
15
|
+
import numpy as np
|
|
16
|
+
|
|
17
|
+
from a2a_robots import FailureCode, Pose
|
|
18
|
+
from a2a_robots.planning import JointTrajectory, PlanError, Planner, interpolate
|
|
19
|
+
|
|
20
|
+
|
|
21
|
+
@dataclass(frozen=True, slots=True)
|
|
22
|
+
class Chain:
|
|
23
|
+
"""``joints`` move ``body``; ``move_to`` targets the pose of ``body``.
|
|
24
|
+
|
|
25
|
+
Six joints reach any nearby pose; a seven-joint arm works too.
|
|
26
|
+
"""
|
|
27
|
+
|
|
28
|
+
body: str
|
|
29
|
+
joints: tuple[str, ...]
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
@dataclass(frozen=True, slots=True)
|
|
33
|
+
class _Chain:
|
|
34
|
+
joints: tuple[str, ...]
|
|
35
|
+
body: int
|
|
36
|
+
qpos: np.ndarray
|
|
37
|
+
dofs: np.ndarray
|
|
38
|
+
lower: np.ndarray
|
|
39
|
+
upper: np.ndarray
|
|
40
|
+
max_speeds: np.ndarray
|
|
41
|
+
geoms: frozenset[int]
|
|
42
|
+
|
|
43
|
+
|
|
44
|
+
class MujocoPlanner(Planner):
|
|
45
|
+
def __init__(
|
|
46
|
+
self,
|
|
47
|
+
model: mujoco.MjModel,
|
|
48
|
+
chains: Mapping[str, Chain],
|
|
49
|
+
*,
|
|
50
|
+
max_speed_m_s: float = 0.05,
|
|
51
|
+
max_speed_rad_s: float = 0.5,
|
|
52
|
+
penetration_m: float = 1e-3,
|
|
53
|
+
) -> None:
|
|
54
|
+
"""``max_speed_*`` cap slide and hinge joint speeds along the path.
|
|
55
|
+
A contact deeper than ``penetration_m`` counts as a collision."""
|
|
56
|
+
self._model = model
|
|
57
|
+
self._data = mujoco.MjData(model)
|
|
58
|
+
self._penetration = penetration_m
|
|
59
|
+
self._chains = {
|
|
60
|
+
hand: self._index(chain, max_speed_m_s, max_speed_rad_s)
|
|
61
|
+
for hand, chain in chains.items()
|
|
62
|
+
}
|
|
63
|
+
|
|
64
|
+
def set_scene(self, qpos: Sequence[float]) -> None:
|
|
65
|
+
"""Copy the full simulator state the next plan checks collisions against."""
|
|
66
|
+
self._data.qpos[:] = qpos
|
|
67
|
+
|
|
68
|
+
def plan(
|
|
69
|
+
self, hand: str, start: Mapping[str, float], target: Pose, dt: float
|
|
70
|
+
) -> JointTrajectory:
|
|
71
|
+
chain = self._chains[hand]
|
|
72
|
+
data = self._data
|
|
73
|
+
q0 = np.array([start[name] for name in chain.joints])
|
|
74
|
+
data.qpos[chain.qpos] = q0
|
|
75
|
+
goal = self._solve(chain, target)
|
|
76
|
+
if goal is None:
|
|
77
|
+
raise PlanError(FailureCode.UNREACHABLE, "no joint solution reaches the target")
|
|
78
|
+
points = interpolate(q0, goal, chain.max_speeds, dt)
|
|
79
|
+
|
|
80
|
+
data.qpos[chain.qpos] = q0
|
|
81
|
+
touching = self._contacts(chain, depth=0.0)
|
|
82
|
+
for point in points:
|
|
83
|
+
data.qpos[chain.qpos] = point
|
|
84
|
+
hit = self._contacts(chain, depth=self._penetration) - touching
|
|
85
|
+
if hit:
|
|
86
|
+
names = ", ".join(sorted(self._name(g) for g in hit))
|
|
87
|
+
raise PlanError(FailureCode.UNREACHABLE, f"the path collides with {names}")
|
|
88
|
+
return JointTrajectory(chain.joints, tuple(points), dt)
|
|
89
|
+
|
|
90
|
+
def _solve(self, chain: _Chain, target: Pose) -> np.ndarray | None:
|
|
91
|
+
"""Joint values that put the chain body at ``target`` (damped least squares)."""
|
|
92
|
+
m, data = self._model, self._data
|
|
93
|
+
goal_pos = np.asarray(target.position, dtype=float)
|
|
94
|
+
goal_quat = np.asarray(target.orientation, dtype=float)
|
|
95
|
+
jacp = np.zeros((3, m.nv))
|
|
96
|
+
jacr = np.zeros((3, m.nv))
|
|
97
|
+
error = np.zeros(6)
|
|
98
|
+
for _ in range(100):
|
|
99
|
+
mujoco.mj_kinematics(m, data)
|
|
100
|
+
mujoco.mj_comPos(m, data)
|
|
101
|
+
error[:3] = goal_pos - data.xpos[chain.body]
|
|
102
|
+
mujoco.mju_subQuat(error[3:], goal_quat, data.xquat[chain.body])
|
|
103
|
+
if np.linalg.norm(error[:3]) < 1e-5 and np.linalg.norm(error[3:]) < 1e-5:
|
|
104
|
+
return data.qpos[chain.qpos].copy()
|
|
105
|
+
mujoco.mj_jacBody(m, data, jacp, jacr, chain.body)
|
|
106
|
+
jac = np.vstack((jacp, jacr))[:, chain.dofs]
|
|
107
|
+
step = jac.T @ np.linalg.solve(jac @ jac.T + 1e-6 * np.eye(6), error)
|
|
108
|
+
data.qpos[chain.qpos] = np.clip(data.qpos[chain.qpos] + step, chain.lower, chain.upper)
|
|
109
|
+
return None
|
|
110
|
+
|
|
111
|
+
def _contacts(self, chain: _Chain, depth: float) -> set[int]:
|
|
112
|
+
"""Geoms outside the hand that it touches deeper than ``depth``."""
|
|
113
|
+
mujoco.mj_fwdPosition(self._model, self._data)
|
|
114
|
+
outside = set()
|
|
115
|
+
for i in range(self._data.ncon):
|
|
116
|
+
contact = self._data.contact[i]
|
|
117
|
+
g1, g2 = contact.geom1, contact.geom2
|
|
118
|
+
if (g1 in chain.geoms) != (g2 in chain.geoms) and contact.dist <= -depth:
|
|
119
|
+
outside.add(g2 if g1 in chain.geoms else g1)
|
|
120
|
+
return outside
|
|
121
|
+
|
|
122
|
+
def _name(self, geom: int) -> str:
|
|
123
|
+
name = self._model.geom(geom).name
|
|
124
|
+
return name or self._model.body(self._model.geom_bodyid[geom]).name
|
|
125
|
+
|
|
126
|
+
def _index(self, chain: Chain, max_m: float, max_rad: float) -> _Chain:
|
|
127
|
+
m = self._model
|
|
128
|
+
joints = [_id(m, mujoco.mjtObj.mjOBJ_JOINT, name) for name in chain.joints]
|
|
129
|
+
body = _id(m, mujoco.mjtObj.mjOBJ_BODY, chain.body)
|
|
130
|
+
# Every geom attached below the first chain joint moves with the hand.
|
|
131
|
+
root = m.jnt_bodyid[joints[0]]
|
|
132
|
+
geoms = set()
|
|
133
|
+
for g in range(m.ngeom):
|
|
134
|
+
b = m.geom_bodyid[g]
|
|
135
|
+
while b not in (root, 0):
|
|
136
|
+
b = m.body_parentid[b]
|
|
137
|
+
if b == root:
|
|
138
|
+
geoms.add(g)
|
|
139
|
+
slide = m.jnt_type[joints] == mujoco.mjtJoint.mjJNT_SLIDE
|
|
140
|
+
limited = m.jnt_limited[joints].astype(bool)
|
|
141
|
+
return _Chain(
|
|
142
|
+
joints=tuple(chain.joints),
|
|
143
|
+
body=body,
|
|
144
|
+
qpos=m.jnt_qposadr[joints],
|
|
145
|
+
dofs=m.jnt_dofadr[joints],
|
|
146
|
+
lower=np.where(limited, m.jnt_range[joints, 0], -np.inf),
|
|
147
|
+
upper=np.where(limited, m.jnt_range[joints, 1], np.inf),
|
|
148
|
+
max_speeds=np.where(slide, max_m, max_rad),
|
|
149
|
+
geoms=frozenset(geoms),
|
|
150
|
+
)
|
|
151
|
+
|
|
152
|
+
|
|
153
|
+
def _id(model: mujoco.MjModel, kind: mujoco.mjtObj, name: str) -> int:
|
|
154
|
+
index = mujoco.mj_name2id(model, kind, name)
|
|
155
|
+
if index < 0:
|
|
156
|
+
raise ValueError(f"model has no {kind.name[6:].lower()} named {name!r}")
|
|
157
|
+
return index
|
|
@@ -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,8 @@
|
|
|
1
|
+
a2a_robots_sharpa/__init__.py,sha256=C4X9bgL14HDxiFVG3tHw-icp6GRpkeNaD9XwrH1KL5Q,329
|
|
2
|
+
a2a_robots_sharpa/__main__.py,sha256=6v2-7IeLMYlPbIxbWL3ZFAyPYddiv3R1tXQw6BLUCIQ,198
|
|
3
|
+
a2a_robots_sharpa/model.py,sha256=3BfFWrp57kevSyLZFClXHjmWHkV9O4PiZchJYNlZ4eY,3113
|
|
4
|
+
a2a_robots_sharpa/mujoco_backend.py,sha256=viCMC_bsbGZnsm2hfwzfbjc_nVWRr3hmnn6s9ULriOo,12786
|
|
5
|
+
a2a_robots_sharpa/planner.py,sha256=rCB9IMKoq1_wqT90ciTcZH6AdWieQKUeO6IkZdrhhbg,6145
|
|
6
|
+
a2a_robots_sharpa-0.1.1.dist-info/METADATA,sha256=Wrnnpm_9P1x1PSi247yN-N-d7ZCAxzPHIJdyvWcYAOw,3922
|
|
7
|
+
a2a_robots_sharpa-0.1.1.dist-info/WHEEL,sha256=2iZrJkDIkeDh0k9SikKMqFuRP-ua6mb4Ir1AsER4BEw,87
|
|
8
|
+
a2a_robots_sharpa-0.1.1.dist-info/RECORD,,
|