robotenv 0.2.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.
- robotenv/__init__.py +21 -0
- robotenv/_canon.py +136 -0
- robotenv/config/__init__.py +93 -0
- robotenv/config/base_env.py +557 -0
- robotenv/config/control.py +161 -0
- robotenv/config/defaults.py +29 -0
- robotenv/config/env.py +107 -0
- robotenv/config/kinematics.py +121 -0
- robotenv/config/mount.py +172 -0
- robotenv/config/providers.py +171 -0
- robotenv/config/robot_providers.py +140 -0
- robotenv/config/sim.py +152 -0
- robotenv/config/specs.py +111 -0
- robotenv/config/sweep.py +100 -0
- robotenv/config/task.py +183 -0
- robotenv/envs/__init__.py +34 -0
- robotenv/envs/spaces.py +38 -0
- robotenv/errors.py +35 -0
- robotenv/robot/__init__.py +30 -0
- robotenv/robot/arm.py +319 -0
- robotenv/robot/external.py +234 -0
- robotenv/robot/generators.py +138 -0
- robotenv/robot/gripper.py +82 -0
- robotenv/robot/pose.py +49 -0
- robotenv/robot/providers/__init__.py +5 -0
- robotenv/robot/providers/menagerie/__init__.py +127 -0
- robotenv/robot/providers/menagerie/_describe.py +130 -0
- robotenv/robot/providers/menagerie/cards.json +37 -0
- robotenv/simulators/__init__.py +51 -0
- robotenv/simulators/base.py +70 -0
- robotenv/simulators/mujoco/__init__.py +38 -0
- robotenv/simulators/mujoco/arm_spec.py +137 -0
- robotenv/simulators/mujoco/compile.py +242 -0
- robotenv/simulators/mujoco/env.py +257 -0
- robotenv/simulators/mujoco/external.py +443 -0
- robotenv/simulators/mujoco/mount.py +65 -0
- robotenv/simulators/mujoco/render.py +158 -0
- robotenv/simulators/mujoco/scenes.py +35 -0
- robotenv/simulators/mujoco/sim.py +174 -0
- robotenv/simulators/warp/__init__.py +100 -0
- robotenv/simulators/warp/env.py +235 -0
- robotenv/simulators/warp/sim.py +332 -0
- robotenv/tasks/__init__.py +10 -0
- robotenv/tasks/base.py +147 -0
- robotenv/tasks/empty/__init__.py +42 -0
- robotenv/tasks/empty/params.py +33 -0
- robotenv/tasks/empty/task.py +27 -0
- robotenv/tasks/robosuite/__init__.py +133 -0
- robotenv/tasks/robosuite/_compat.py +165 -0
- robotenv/tasks/robosuite/_introspect.py +65 -0
- robotenv/tasks/robosuite/params.py +152 -0
- robotenv/tasks/robosuite/scene.py +140 -0
- robotenv/tasks/robosuite/task.py +145 -0
- robotenv/viz/__init__.py +22 -0
- robotenv/viz/_deps.py +16 -0
- robotenv/viz/camera.py +98 -0
- robotenv/viz/clip.py +180 -0
- robotenv/viz/frames.py +93 -0
- robotenv/viz/record.py +141 -0
- robotenv-0.2.1.dist-info/METADATA +136 -0
- robotenv-0.2.1.dist-info/RECORD +63 -0
- robotenv-0.2.1.dist-info/WHEEL +4 -0
- robotenv-0.2.1.dist-info/licenses/LICENSE +339 -0
robotenv/config/task.py
ADDED
|
@@ -0,0 +1,183 @@
|
|
|
1
|
+
"""Task description: the provider's params object, an episode length and, for goal-defined tasks, a target region
|
|
2
|
+
and tolerance.
|
|
3
|
+
|
|
4
|
+
The ``params`` object is the task: its class names the provider (class attribute ``provider``, a namespace of
|
|
5
|
+
``config.providers``) and its ``name`` field names the task within it. ``TaskConfig.id`` is the derived
|
|
6
|
+
``<provider>/<name>``. This module knows no provider and no task. Never mentions the number of joints or link lengths.
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
import warnings
|
|
12
|
+
from dataclasses import dataclass
|
|
13
|
+
from typing import Any, Mapping
|
|
14
|
+
|
|
15
|
+
from .._canon import as_float, as_int, as_vector, check_keys, within
|
|
16
|
+
from ..errors import ConfigError
|
|
17
|
+
from .mount import MobileMount, Mount, check_mount, mount_from_dict
|
|
18
|
+
from .providers import get_task_provider, known_task_ids, list_task_providers
|
|
19
|
+
|
|
20
|
+
|
|
21
|
+
@dataclass(frozen=True, slots=True)
|
|
22
|
+
class FixedBox:
|
|
23
|
+
"""Targets uniform in an axis-aligned box, in the base frame."""
|
|
24
|
+
|
|
25
|
+
low: tuple[float, float, float]
|
|
26
|
+
high: tuple[float, float, float]
|
|
27
|
+
|
|
28
|
+
def __post_init__(self):
|
|
29
|
+
low = as_vector(self.low, 3, "low")
|
|
30
|
+
high = as_vector(self.high, 3, "high")
|
|
31
|
+
for i, (a, b) in enumerate(zip(low, high)):
|
|
32
|
+
if a > b:
|
|
33
|
+
raise ConfigError(f"low[{i}]", f"low must be <= high ({a} > {b})")
|
|
34
|
+
object.__setattr__(self, "low", low)
|
|
35
|
+
object.__setattr__(self, "high", high)
|
|
36
|
+
|
|
37
|
+
def to_dict(self) -> dict[str, Any]:
|
|
38
|
+
return {"kind": "fixed_box", "low": list(self.low), "high": list(self.high)}
|
|
39
|
+
|
|
40
|
+
@classmethod
|
|
41
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "FixedBox":
|
|
42
|
+
check_keys(d, {"kind", "low", "high"}, "")
|
|
43
|
+
return cls(low=d["low"], high=d["high"])
|
|
44
|
+
|
|
45
|
+
|
|
46
|
+
@dataclass(frozen=True, slots=True)
|
|
47
|
+
class OwnReachable:
|
|
48
|
+
"""Targets are end-effector positions of uniformly sampled joint configurations."""
|
|
49
|
+
|
|
50
|
+
def to_dict(self) -> dict[str, Any]:
|
|
51
|
+
return {"kind": "own_reachable"}
|
|
52
|
+
|
|
53
|
+
@classmethod
|
|
54
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "OwnReachable":
|
|
55
|
+
check_keys(d, {"kind"}, "")
|
|
56
|
+
return cls()
|
|
57
|
+
|
|
58
|
+
|
|
59
|
+
TargetRegion = FixedBox | OwnReachable
|
|
60
|
+
_REGIONS = {"fixed_box": FixedBox, "own_reachable": OwnReachable}
|
|
61
|
+
|
|
62
|
+
|
|
63
|
+
def region_from_dict(d: Mapping[str, Any]) -> TargetRegion:
|
|
64
|
+
if not isinstance(d, Mapping) or "kind" not in d:
|
|
65
|
+
raise ConfigError("", "expected a mapping with a 'kind' key")
|
|
66
|
+
cls = _REGIONS.get(d["kind"])
|
|
67
|
+
if cls is None:
|
|
68
|
+
raise ConfigError("kind", f"unknown target region {d['kind']!r}; expected one of {sorted(_REGIONS)}")
|
|
69
|
+
return cls.from_dict(d)
|
|
70
|
+
|
|
71
|
+
|
|
72
|
+
@dataclass(frozen=True, slots=True)
|
|
73
|
+
class TaskConfig:
|
|
74
|
+
"""``target_region`` and ``success_tolerance`` define the goal of tasks whose ``params`` class sets
|
|
75
|
+
``uses_goal = True``; they must be ``None`` for tasks that define their own goal.
|
|
76
|
+
|
|
77
|
+
``params`` is an instance of the provider's params class: that class has a class attribute ``provider`` (the
|
|
78
|
+
namespace) and a field ``name`` (the task within the provider). Besides ``to_dict`` / ``from_dict`` the
|
|
79
|
+
environment reads ``reset_noise`` (rad, uniform noise on the initial joint positions) and
|
|
80
|
+
``terminate_on_success`` from it when they exist.
|
|
81
|
+
|
|
82
|
+
``mount`` places the robot base relative to the scene's mount frame (``TaskProvider.mount_frame(params)``):
|
|
83
|
+
``Mount(offset=Pose)`` for a fixed-base arm, ``MobileMount(spawn, heading)`` for a floating-base robot; the world
|
|
84
|
+
pose is ``world_mount(cfg)``. The robot itself has no pose, so ``robot_hash`` identifies the embodiment alone.
|
|
85
|
+
Target regions are in the robot's base frame and follow the mount.
|
|
86
|
+
"""
|
|
87
|
+
|
|
88
|
+
params: Any
|
|
89
|
+
max_episode_steps: int
|
|
90
|
+
success_tolerance: float | None
|
|
91
|
+
target_region: TargetRegion | None
|
|
92
|
+
mount: Mount | MobileMount
|
|
93
|
+
|
|
94
|
+
def __post_init__(self):
|
|
95
|
+
provider = getattr(self.params, "provider", None)
|
|
96
|
+
name = getattr(self.params, "name", None)
|
|
97
|
+
if not isinstance(provider, str) or not isinstance(name, str):
|
|
98
|
+
raise ConfigError(
|
|
99
|
+
"params",
|
|
100
|
+
f"expected a task provider's params object (a class attribute 'provider' and a field 'name'), "
|
|
101
|
+
f"got {type(self.params).__name__}",
|
|
102
|
+
)
|
|
103
|
+
params_cls = _params_class(provider, name, "params", "params.name")
|
|
104
|
+
if not isinstance(self.params, params_cls):
|
|
105
|
+
raise ConfigError("params", f"task {self.id!r} requires {params_cls.__name__}")
|
|
106
|
+
check_mount(self.mount)
|
|
107
|
+
max_episode_steps = as_int(self.max_episode_steps, "max_episode_steps")
|
|
108
|
+
if max_episode_steps < 1:
|
|
109
|
+
raise ConfigError("max_episode_steps", "must be >= 1")
|
|
110
|
+
object.__setattr__(self, "max_episode_steps", max_episode_steps)
|
|
111
|
+
if getattr(params_cls, "uses_goal", False):
|
|
112
|
+
tol = as_float(self.success_tolerance, "success_tolerance")
|
|
113
|
+
if tol <= 0.0:
|
|
114
|
+
raise ConfigError("success_tolerance", "must be > 0")
|
|
115
|
+
object.__setattr__(self, "success_tolerance", tol)
|
|
116
|
+
if not isinstance(self.target_region, (FixedBox, OwnReachable)):
|
|
117
|
+
raise ConfigError("target_region", "expected FixedBox or OwnReachable")
|
|
118
|
+
else:
|
|
119
|
+
if self.success_tolerance is not None:
|
|
120
|
+
raise ConfigError(
|
|
121
|
+
"success_tolerance", f"must be None for task {self.id!r}, which defines its own success"
|
|
122
|
+
)
|
|
123
|
+
if self.target_region is not None:
|
|
124
|
+
raise ConfigError("target_region", f"must be None for task {self.id!r}, which defines its own goal")
|
|
125
|
+
|
|
126
|
+
@property
|
|
127
|
+
def id(self) -> str:
|
|
128
|
+
"""``"<provider>/<name>"``, for messages and logs; derived from ``params``, never stored."""
|
|
129
|
+
return f"{self.params.provider}/{self.params.name}"
|
|
130
|
+
|
|
131
|
+
def to_dict(self) -> dict[str, Any]:
|
|
132
|
+
return {
|
|
133
|
+
"provider": self.params.provider,
|
|
134
|
+
"params": self.params.to_dict(),
|
|
135
|
+
"max_episode_steps": self.max_episode_steps,
|
|
136
|
+
"success_tolerance": self.success_tolerance,
|
|
137
|
+
"target_region": None if self.target_region is None else self.target_region.to_dict(),
|
|
138
|
+
"mount": self.mount.to_dict(),
|
|
139
|
+
}
|
|
140
|
+
|
|
141
|
+
@classmethod
|
|
142
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "TaskConfig":
|
|
143
|
+
d = migrate_task_dict(d)
|
|
144
|
+
check_keys(d, {"provider", "params", "max_episode_steps", "success_tolerance", "target_region", "mount"}, "")
|
|
145
|
+
raw = d["params"]
|
|
146
|
+
if not isinstance(raw, Mapping):
|
|
147
|
+
raise ConfigError("params", f"expected a mapping, got {type(raw).__name__}")
|
|
148
|
+
if "name" not in raw:
|
|
149
|
+
raise ConfigError("params.name", "missing")
|
|
150
|
+
params_cls = _params_class(d["provider"], raw["name"], "provider", "params.name")
|
|
151
|
+
region = d["target_region"]
|
|
152
|
+
return cls(
|
|
153
|
+
params=within("params", params_cls.from_dict, raw),
|
|
154
|
+
max_episode_steps=d["max_episode_steps"],
|
|
155
|
+
success_tolerance=d["success_tolerance"],
|
|
156
|
+
target_region=None if region is None else within("target_region", region_from_dict, region),
|
|
157
|
+
mount=within("mount", mount_from_dict, d["mount"]),
|
|
158
|
+
)
|
|
159
|
+
|
|
160
|
+
|
|
161
|
+
def migrate_task_dict(d: Any) -> Any:
|
|
162
|
+
"""Read the old key ``horizon`` as ``max_episode_steps`` (Gymnasium's name for the time limit), with a
|
|
163
|
+
``DeprecationWarning``. Giving both keys is an error. Other values pass through unchanged."""
|
|
164
|
+
if not isinstance(d, Mapping) or "horizon" not in d:
|
|
165
|
+
return d
|
|
166
|
+
if "max_episode_steps" in d:
|
|
167
|
+
raise ConfigError("horizon", "is the old name of max_episode_steps; give only max_episode_steps")
|
|
168
|
+
warnings.warn(
|
|
169
|
+
"the task key 'horizon' is deprecated: it is now 'max_episode_steps' (Gymnasium's name for the time limit)",
|
|
170
|
+
DeprecationWarning, stacklevel=3,
|
|
171
|
+
)
|
|
172
|
+
return {("max_episode_steps" if k == "horizon" else k): v for k, v in d.items()}
|
|
173
|
+
|
|
174
|
+
|
|
175
|
+
def _params_class(provider: str, name: str, provider_path: str, name_path: str | None = None) -> type:
|
|
176
|
+
"""The params class of task ``name`` of ``provider``; errors say where the bad value was written."""
|
|
177
|
+
name_path = name_path or provider_path
|
|
178
|
+
if not isinstance(provider, str) or provider not in list_task_providers():
|
|
179
|
+
raise ConfigError(provider_path, f"unknown task provider {provider!r}; registered: {list(list_task_providers())}")
|
|
180
|
+
cls = get_task_provider(provider).params_class(name) if isinstance(name, str) else None
|
|
181
|
+
if cls is None:
|
|
182
|
+
raise ConfigError(name_path, f"unknown task '{provider}/{name}'; known: {known_task_ids()}")
|
|
183
|
+
return cls
|
|
@@ -0,0 +1,34 @@
|
|
|
1
|
+
"""Environment construction. Importing this module never imports mujoco, torch or torchrl."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from typing import Any
|
|
6
|
+
|
|
7
|
+
from ..simulators import check_capabilities, get_simulator
|
|
8
|
+
from ..config import ConfigError, EnvConfig
|
|
9
|
+
from ..config.providers import get_task_provider
|
|
10
|
+
|
|
11
|
+
|
|
12
|
+
def make_env(cfg: EnvConfig) -> Any:
|
|
13
|
+
"""Build the environment of ``cfg``: the simulator named by ``cfg.sim.simulator`` running the task ``cfg.task.id``.
|
|
14
|
+
|
|
15
|
+
``cfg`` is complete data (robot, control, task, sim), so ``env = make_env(cfg)`` is the only step. Every
|
|
16
|
+
incompatibility, of the config with the simulator and of the task with the simulator, is collected before raising,
|
|
17
|
+
so one error lists all of them.
|
|
18
|
+
"""
|
|
19
|
+
simulator = get_simulator(cfg.sim.simulator)
|
|
20
|
+
provider = get_task_provider(cfg.task.params.provider)
|
|
21
|
+
problems = check_capabilities(cfg, simulator.capabilities(), simulator.name)
|
|
22
|
+
problems += simulator.validate(cfg)
|
|
23
|
+
problems += provider.validate(cfg, simulator.name)
|
|
24
|
+
if problems:
|
|
25
|
+
lines = "\n".join(f" - {p}" for p in problems)
|
|
26
|
+
raise ConfigError(
|
|
27
|
+
"",
|
|
28
|
+
f"simulator {simulator.name!r} cannot run this config ({len(problems)} problem"
|
|
29
|
+
f"{'s' if len(problems) != 1 else ''}):\n{lines}",
|
|
30
|
+
)
|
|
31
|
+
return simulator.make_env(cfg)
|
|
32
|
+
|
|
33
|
+
|
|
34
|
+
__all__ = ["make_env"]
|
robotenv/envs/spaces.py
ADDED
|
@@ -0,0 +1,38 @@
|
|
|
1
|
+
"""``observation_space`` and ``action_space`` for any batched TorchRL environment of this package. Needs torch.
|
|
2
|
+
|
|
3
|
+
Plain shapes, derived from the env's own ``observation_spec`` / ``action_spec`` in one place, so every simulator's
|
|
4
|
+
environment gets them by inheriting :class:`SpacesMixin` (no per-simulator code). Gymnasium users look for these
|
|
5
|
+
two names, but they return the *shape*, not a ``gymnasium.Space``; dtype and bounds stay in the specs.
|
|
6
|
+
"""
|
|
7
|
+
|
|
8
|
+
from __future__ import annotations
|
|
9
|
+
|
|
10
|
+
from typing import Any
|
|
11
|
+
|
|
12
|
+
import torch
|
|
13
|
+
|
|
14
|
+
__all__ = ["SpacesMixin"]
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
def _shapes(spec: Any, skip: int) -> Any:
|
|
18
|
+
"""A nested dict of ``torch.Size`` mirroring a (nested) ``Composite``, without its first ``skip`` batch dims."""
|
|
19
|
+
if hasattr(spec, "items") and hasattr(spec, "keys"):
|
|
20
|
+
return {key: _shapes(sub, skip) for key, sub in spec.items()}
|
|
21
|
+
return torch.Size(spec.shape[skip:])
|
|
22
|
+
|
|
23
|
+
|
|
24
|
+
class SpacesMixin:
|
|
25
|
+
"""Mix in before ``EnvBase``. ``self.batch_size`` is the number of envs; the spaces are per env."""
|
|
26
|
+
|
|
27
|
+
@property
|
|
28
|
+
def action_space(self) -> torch.Size:
|
|
29
|
+
"""The shape of one env's action, ``torch.Size([n])``: the batched action has the shape
|
|
30
|
+
``env.batch_size + env.action_space``. For the observation nested dict see :attr:`observation_space`."""
|
|
31
|
+
return torch.Size(self.action_spec.shape[len(self.batch_size):])
|
|
32
|
+
|
|
33
|
+
@property
|
|
34
|
+
def observation_space(self) -> dict[str, Any]:
|
|
35
|
+
"""The shapes of one env's observation, as a nested dict with the same keys as ``observation_spec``
|
|
36
|
+
(``{"task": {...}, "proprio": {"qpos": torch.Size([n]), ...}, "success": torch.Size([1]), ...}``). Nested
|
|
37
|
+
observations are primary, so nothing is flattened."""
|
|
38
|
+
return _shapes(self.observation_spec, len(self.batch_size))
|
robotenv/errors.py
ADDED
|
@@ -0,0 +1,35 @@
|
|
|
1
|
+
"""Validation errors that carry the path of the offending field."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from dataclasses import dataclass
|
|
6
|
+
|
|
7
|
+
|
|
8
|
+
class ConfigError(ValueError):
|
|
9
|
+
"""Invalid configuration. The message starts with the path of the offending field."""
|
|
10
|
+
|
|
11
|
+
def __init__(self, path: str, message: str):
|
|
12
|
+
self.path = path
|
|
13
|
+
self.reason = message
|
|
14
|
+
super().__init__(f"{path}: {message}" if path else message)
|
|
15
|
+
|
|
16
|
+
def prefixed(self, prefix: str) -> "ConfigError":
|
|
17
|
+
"""The same error, one level further from the root."""
|
|
18
|
+
if not self.path:
|
|
19
|
+
path = prefix
|
|
20
|
+
elif self.path.startswith("["):
|
|
21
|
+
path = prefix + self.path
|
|
22
|
+
else:
|
|
23
|
+
path = f"{prefix}.{self.path}"
|
|
24
|
+
return ConfigError(path, self.reason)
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
@dataclass(frozen=True)
|
|
28
|
+
class Problem:
|
|
29
|
+
"""One reason a config cannot be run, with the path of the field to change (for example ``control.mode``)."""
|
|
30
|
+
|
|
31
|
+
path: str
|
|
32
|
+
message: str
|
|
33
|
+
|
|
34
|
+
def __str__(self) -> str:
|
|
35
|
+
return f"{self.path}: {self.message}"
|
|
@@ -0,0 +1,30 @@
|
|
|
1
|
+
"""Robot embodiments as frozen, validated, hashable data (numpy only; no simulator is imported)."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from typing import Any, Mapping
|
|
6
|
+
|
|
7
|
+
from ..errors import ConfigError
|
|
8
|
+
from .arm import Capsule, JointSpec, LinkSpec, ParametricArm, Segment, ToolSpec
|
|
9
|
+
from .external import ACTUATOR_KINDS, EndEffectorRef, ExternalJoint, ExternalRobot
|
|
10
|
+
from .generators import chain
|
|
11
|
+
from .gripper import TwoFingerGripper
|
|
12
|
+
from .pose import Pose
|
|
13
|
+
|
|
14
|
+
__all__ = [
|
|
15
|
+
"ACTUATOR_KINDS", "Capsule", "EndEffectorRef", "ExternalJoint", "ExternalRobot", "JointSpec", "LinkSpec",
|
|
16
|
+
"ParametricArm", "Pose", "Segment", "ToolSpec", "TwoFingerGripper", "chain", "robot_from_dict",
|
|
17
|
+
]
|
|
18
|
+
|
|
19
|
+
_KINDS = {"parametric_arm": ParametricArm, "external_robot": ExternalRobot}
|
|
20
|
+
|
|
21
|
+
|
|
22
|
+
def robot_from_dict(d: Mapping[str, Any]) -> ParametricArm | ExternalRobot:
|
|
23
|
+
"""The robot ``d`` describes, dispatching on its ``kind`` (``parametric_arm`` or ``external_robot``)."""
|
|
24
|
+
if not isinstance(d, Mapping):
|
|
25
|
+
raise ConfigError("", f"expected a mapping, got {type(d).__name__}")
|
|
26
|
+
if "kind" not in d:
|
|
27
|
+
raise ConfigError("kind", f"missing; expected one of {sorted(_KINDS)}")
|
|
28
|
+
if d["kind"] not in _KINDS:
|
|
29
|
+
raise ConfigError("kind", f"unknown robot kind {d['kind']!r}; expected one of {sorted(_KINDS)}")
|
|
30
|
+
return _KINDS[d["kind"]].from_dict(d)
|
robotenv/robot/arm.py
ADDED
|
@@ -0,0 +1,319 @@
|
|
|
1
|
+
"""Robot embodiments as frozen, validated, hashable data. No simulator is imported here."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import math
|
|
6
|
+
from dataclasses import dataclass
|
|
7
|
+
from typing import Any, Mapping
|
|
8
|
+
|
|
9
|
+
from .._canon import (
|
|
10
|
+
HASH_DISPLAY_LENGTH,
|
|
11
|
+
as_float,
|
|
12
|
+
as_vector,
|
|
13
|
+
canonical_json,
|
|
14
|
+
check_keys,
|
|
15
|
+
sha256_hex,
|
|
16
|
+
within,
|
|
17
|
+
)
|
|
18
|
+
from ..errors import ConfigError
|
|
19
|
+
from .gripper import TwoFingerGripper
|
|
20
|
+
from .pose import Pose, _normalize_quat
|
|
21
|
+
|
|
22
|
+
SUPPORTED_SCHEMA_VERSIONS = frozenset({1})
|
|
23
|
+
|
|
24
|
+
|
|
25
|
+
@dataclass(frozen=True, slots=True)
|
|
26
|
+
class Capsule:
|
|
27
|
+
"""Capsule running from the link origin to ``tip_offset``."""
|
|
28
|
+
|
|
29
|
+
radius: float
|
|
30
|
+
|
|
31
|
+
def __post_init__(self):
|
|
32
|
+
r = as_float(self.radius, "radius")
|
|
33
|
+
if r <= 0.0:
|
|
34
|
+
raise ConfigError("radius", "must be > 0")
|
|
35
|
+
object.__setattr__(self, "radius", r)
|
|
36
|
+
|
|
37
|
+
def to_dict(self) -> dict[str, Any]:
|
|
38
|
+
return {"kind": "capsule", "radius": self.radius}
|
|
39
|
+
|
|
40
|
+
@classmethod
|
|
41
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "Capsule":
|
|
42
|
+
check_keys(d, {"kind", "radius"}, "")
|
|
43
|
+
if d["kind"] != "capsule":
|
|
44
|
+
raise ConfigError("kind", f"unsupported geometry {d['kind']!r}")
|
|
45
|
+
return cls(radius=d["radius"])
|
|
46
|
+
|
|
47
|
+
|
|
48
|
+
@dataclass(frozen=True, slots=True)
|
|
49
|
+
class JointSpec:
|
|
50
|
+
"""Hinge joint. ``axis`` is in the joint frame; ``range`` is ``None`` (unlimited) or ``(lo, hi)`` in rad."""
|
|
51
|
+
|
|
52
|
+
axis: tuple[float, float, float]
|
|
53
|
+
range: tuple[float, float] | None
|
|
54
|
+
damping: float
|
|
55
|
+
armature: float
|
|
56
|
+
frictionloss: float
|
|
57
|
+
effort_limit: float
|
|
58
|
+
|
|
59
|
+
def __post_init__(self):
|
|
60
|
+
axis = as_vector(self.axis, 3, "axis")
|
|
61
|
+
norm = math.sqrt(sum(c * c for c in axis))
|
|
62
|
+
if norm == 0.0:
|
|
63
|
+
raise ConfigError("axis", "must be non-zero")
|
|
64
|
+
object.__setattr__(self, "axis", tuple(c / norm for c in axis))
|
|
65
|
+
|
|
66
|
+
if self.range is not None:
|
|
67
|
+
lo, hi = as_vector(self.range, 2, "range")
|
|
68
|
+
if not lo < hi:
|
|
69
|
+
raise ConfigError("range", "lo must be < hi")
|
|
70
|
+
object.__setattr__(self, "range", (lo, hi))
|
|
71
|
+
|
|
72
|
+
for name in ("damping", "armature", "frictionloss"):
|
|
73
|
+
v = as_float(getattr(self, name), name)
|
|
74
|
+
if v < 0.0:
|
|
75
|
+
raise ConfigError(name, "must be >= 0")
|
|
76
|
+
object.__setattr__(self, name, v)
|
|
77
|
+
effort = as_float(self.effort_limit, "effort_limit")
|
|
78
|
+
if effort <= 0.0:
|
|
79
|
+
raise ConfigError("effort_limit", "must be > 0")
|
|
80
|
+
object.__setattr__(self, "effort_limit", effort)
|
|
81
|
+
|
|
82
|
+
def to_dict(self) -> dict[str, Any]:
|
|
83
|
+
return {
|
|
84
|
+
"axis": list(self.axis),
|
|
85
|
+
"range": None if self.range is None else list(self.range),
|
|
86
|
+
"damping": self.damping,
|
|
87
|
+
"armature": self.armature,
|
|
88
|
+
"frictionloss": self.frictionloss,
|
|
89
|
+
"effort_limit": self.effort_limit,
|
|
90
|
+
}
|
|
91
|
+
|
|
92
|
+
@classmethod
|
|
93
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "JointSpec":
|
|
94
|
+
check_keys(d, {"axis", "range", "damping", "armature", "frictionloss", "effort_limit"}, "")
|
|
95
|
+
return cls(**{k: d[k] for k in d})
|
|
96
|
+
|
|
97
|
+
|
|
98
|
+
@dataclass(frozen=True, slots=True)
|
|
99
|
+
class LinkSpec:
|
|
100
|
+
"""A rigid link.
|
|
101
|
+
|
|
102
|
+
``joint_frame`` is the pose of the joint frame relative to the previous link's end frame (the
|
|
103
|
+
base frame for the first segment). ``tip_offset`` is the position of the link's end frame in the
|
|
104
|
+
link frame; the end frame has the link frame's orientation. ``inertia`` is ``None`` (derived from
|
|
105
|
+
geometry and mass) or the three principal moments about the centre of mass.
|
|
106
|
+
"""
|
|
107
|
+
|
|
108
|
+
joint_frame: Pose
|
|
109
|
+
tip_offset: tuple[float, float, float]
|
|
110
|
+
mass: float
|
|
111
|
+
geometry: Capsule
|
|
112
|
+
inertia: tuple[float, float, float] | None
|
|
113
|
+
|
|
114
|
+
def __post_init__(self):
|
|
115
|
+
if not isinstance(self.joint_frame, Pose):
|
|
116
|
+
raise ConfigError("joint_frame", "expected a Pose")
|
|
117
|
+
object.__setattr__(self, "tip_offset", as_vector(self.tip_offset, 3, "tip_offset"))
|
|
118
|
+
m = as_float(self.mass, "mass")
|
|
119
|
+
if m <= 0.0:
|
|
120
|
+
raise ConfigError("mass", "must be > 0")
|
|
121
|
+
object.__setattr__(self, "mass", m)
|
|
122
|
+
if not isinstance(self.geometry, Capsule):
|
|
123
|
+
raise ConfigError("geometry", "expected a Capsule")
|
|
124
|
+
if self.inertia is not None:
|
|
125
|
+
inertia = as_vector(self.inertia, 3, "inertia")
|
|
126
|
+
if any(v <= 0.0 for v in inertia):
|
|
127
|
+
raise ConfigError("inertia", "principal moments must be > 0")
|
|
128
|
+
object.__setattr__(self, "inertia", inertia)
|
|
129
|
+
|
|
130
|
+
def to_dict(self) -> dict[str, Any]:
|
|
131
|
+
return {
|
|
132
|
+
"joint_frame": self.joint_frame.to_dict(),
|
|
133
|
+
"tip_offset": list(self.tip_offset),
|
|
134
|
+
"mass": self.mass,
|
|
135
|
+
"geometry": self.geometry.to_dict(),
|
|
136
|
+
"inertia": None if self.inertia is None else list(self.inertia),
|
|
137
|
+
}
|
|
138
|
+
|
|
139
|
+
@classmethod
|
|
140
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "LinkSpec":
|
|
141
|
+
check_keys(d, {"joint_frame", "tip_offset", "mass", "geometry", "inertia"}, "")
|
|
142
|
+
return cls(
|
|
143
|
+
joint_frame=within("joint_frame", Pose.from_dict, d["joint_frame"]),
|
|
144
|
+
tip_offset=d["tip_offset"],
|
|
145
|
+
mass=d["mass"],
|
|
146
|
+
geometry=within("geometry", Capsule.from_dict, d["geometry"]),
|
|
147
|
+
inertia=d["inertia"],
|
|
148
|
+
)
|
|
149
|
+
|
|
150
|
+
|
|
151
|
+
@dataclass(frozen=True, slots=True)
|
|
152
|
+
class Segment:
|
|
153
|
+
joint: JointSpec
|
|
154
|
+
link: LinkSpec
|
|
155
|
+
|
|
156
|
+
def __post_init__(self):
|
|
157
|
+
if not isinstance(self.joint, JointSpec):
|
|
158
|
+
raise ConfigError("joint", "expected a JointSpec")
|
|
159
|
+
if not isinstance(self.link, LinkSpec):
|
|
160
|
+
raise ConfigError("link", "expected a LinkSpec")
|
|
161
|
+
|
|
162
|
+
def to_dict(self) -> dict[str, Any]:
|
|
163
|
+
return {"joint": self.joint.to_dict(), "link": self.link.to_dict()}
|
|
164
|
+
|
|
165
|
+
@classmethod
|
|
166
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "Segment":
|
|
167
|
+
check_keys(d, {"joint", "link"}, "")
|
|
168
|
+
return cls(
|
|
169
|
+
joint=within("joint", JointSpec.from_dict, d["joint"]),
|
|
170
|
+
link=within("link", LinkSpec.from_dict, d["link"]),
|
|
171
|
+
)
|
|
172
|
+
|
|
173
|
+
|
|
174
|
+
@dataclass(frozen=True, slots=True)
|
|
175
|
+
class ToolSpec:
|
|
176
|
+
"""A gripper mounted on the end effector.
|
|
177
|
+
|
|
178
|
+
Part of the robot's identity: it is stored and hashed. The MuJoCo simulator does not build a gripper, so it has no
|
|
179
|
+
effect on the simulation yet.
|
|
180
|
+
"""
|
|
181
|
+
|
|
182
|
+
gripper: str
|
|
183
|
+
mount_pos: tuple[float, float, float]
|
|
184
|
+
mount_quat: tuple[float, float, float, float]
|
|
185
|
+
|
|
186
|
+
def __post_init__(self):
|
|
187
|
+
if not isinstance(self.gripper, str) or not self.gripper:
|
|
188
|
+
raise ConfigError("gripper", "must be a non-empty string")
|
|
189
|
+
object.__setattr__(self, "mount_pos", as_vector(self.mount_pos, 3, "mount_pos"))
|
|
190
|
+
quat = as_vector(self.mount_quat, 4, "mount_quat")
|
|
191
|
+
object.__setattr__(self, "mount_quat", _normalize_quat(quat, "mount_quat"))
|
|
192
|
+
|
|
193
|
+
def to_dict(self) -> dict[str, Any]:
|
|
194
|
+
return {
|
|
195
|
+
"gripper": self.gripper,
|
|
196
|
+
"mount_pos": list(self.mount_pos),
|
|
197
|
+
"mount_quat": list(self.mount_quat),
|
|
198
|
+
}
|
|
199
|
+
|
|
200
|
+
@classmethod
|
|
201
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "ToolSpec":
|
|
202
|
+
check_keys(d, {"gripper", "mount_pos", "mount_quat"}, "")
|
|
203
|
+
return cls(**{k: d[k] for k in d})
|
|
204
|
+
|
|
205
|
+
|
|
206
|
+
@dataclass(frozen=True, slots=True)
|
|
207
|
+
class ParametricArm:
|
|
208
|
+
"""A serial chain of hinge joints and capsule links on a fixed base.
|
|
209
|
+
|
|
210
|
+
The MuJoCo simulator names the elements ``base``, ``link_{i}`` and ``joint_{i}``; the end effector is the ``ee``
|
|
211
|
+
site.
|
|
212
|
+
``gripper`` is a parametric two-finger gripper mounted on the end effector (built by the MuJoCo simulator, adds one
|
|
213
|
+
action dimension); ``tool`` is a named gripper that the simulator ignores. At most one of them is set.
|
|
214
|
+
``label`` is metadata and does not enter the hash.
|
|
215
|
+
"""
|
|
216
|
+
|
|
217
|
+
schema_version: int
|
|
218
|
+
segments: tuple[Segment, ...]
|
|
219
|
+
home_qpos: tuple[float, ...]
|
|
220
|
+
end_effector: Pose
|
|
221
|
+
tool: ToolSpec | None
|
|
222
|
+
label: str = ""
|
|
223
|
+
gripper: TwoFingerGripper | None = None
|
|
224
|
+
|
|
225
|
+
def __post_init__(self):
|
|
226
|
+
if isinstance(self.schema_version, bool) or self.schema_version not in SUPPORTED_SCHEMA_VERSIONS:
|
|
227
|
+
raise ConfigError(
|
|
228
|
+
"schema_version",
|
|
229
|
+
f"unsupported {self.schema_version!r}; supported: {sorted(SUPPORTED_SCHEMA_VERSIONS)}",
|
|
230
|
+
)
|
|
231
|
+
segments = tuple(self.segments)
|
|
232
|
+
if len(segments) < 1:
|
|
233
|
+
raise ConfigError("segments", "need at least one segment (n >= 1)")
|
|
234
|
+
for i, s in enumerate(segments):
|
|
235
|
+
if not isinstance(s, Segment):
|
|
236
|
+
raise ConfigError(f"segments[{i}]", "expected a Segment")
|
|
237
|
+
object.__setattr__(self, "segments", segments)
|
|
238
|
+
|
|
239
|
+
for name in ("end_effector",):
|
|
240
|
+
if not isinstance(getattr(self, name), Pose):
|
|
241
|
+
raise ConfigError(name, "expected a Pose")
|
|
242
|
+
if self.tool is not None and not isinstance(self.tool, ToolSpec):
|
|
243
|
+
raise ConfigError("tool", "expected a ToolSpec or None")
|
|
244
|
+
if self.gripper is not None and not isinstance(self.gripper, TwoFingerGripper):
|
|
245
|
+
raise ConfigError("gripper", "expected a TwoFingerGripper or None")
|
|
246
|
+
if self.gripper is not None and self.tool is not None:
|
|
247
|
+
raise ConfigError("gripper", "give a gripper or a tool, not both")
|
|
248
|
+
|
|
249
|
+
home = as_vector(self.home_qpos, len(segments), "home_qpos")
|
|
250
|
+
for i, (q, s) in enumerate(zip(home, segments)):
|
|
251
|
+
rng = s.joint.range
|
|
252
|
+
if rng is not None and not rng[0] <= q <= rng[1]:
|
|
253
|
+
raise ConfigError(f"home_qpos[{i}]", f"{q} is outside the joint range {list(rng)}")
|
|
254
|
+
object.__setattr__(self, "home_qpos", home)
|
|
255
|
+
|
|
256
|
+
if not isinstance(self.label, str):
|
|
257
|
+
raise ConfigError("label", "must be a string")
|
|
258
|
+
|
|
259
|
+
@property
|
|
260
|
+
def n(self) -> int:
|
|
261
|
+
return len(self.segments)
|
|
262
|
+
|
|
263
|
+
# --- serialization and hashing -------------------------------------------------
|
|
264
|
+
|
|
265
|
+
def to_dict(self) -> dict[str, Any]:
|
|
266
|
+
d = {
|
|
267
|
+
"kind": "parametric_arm",
|
|
268
|
+
"schema_version": self.schema_version,
|
|
269
|
+
"segments": [s.to_dict() for s in self.segments],
|
|
270
|
+
"home_qpos": list(self.home_qpos),
|
|
271
|
+
"end_effector": self.end_effector.to_dict(),
|
|
272
|
+
"tool": None if self.tool is None else self.tool.to_dict(),
|
|
273
|
+
"label": self.label,
|
|
274
|
+
}
|
|
275
|
+
if self.gripper is not None: # absent when None, so the hash of an arm without a gripper is unchanged
|
|
276
|
+
d["gripper"] = self.gripper.to_dict()
|
|
277
|
+
return d
|
|
278
|
+
|
|
279
|
+
@classmethod
|
|
280
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "ParametricArm":
|
|
281
|
+
check_keys(
|
|
282
|
+
d,
|
|
283
|
+
{"kind", "schema_version", "segments", "home_qpos", "end_effector", "tool"},
|
|
284
|
+
"",
|
|
285
|
+
optional={"label", "gripper"},
|
|
286
|
+
)
|
|
287
|
+
if d["kind"] != "parametric_arm":
|
|
288
|
+
raise ConfigError("kind", f"expected 'parametric_arm', got {d['kind']!r}")
|
|
289
|
+
segs = d["segments"]
|
|
290
|
+
if isinstance(segs, (str, bytes)) or not hasattr(segs, "__iter__"):
|
|
291
|
+
raise ConfigError("segments", "expected a sequence")
|
|
292
|
+
tool = d["tool"]
|
|
293
|
+
gripper = d.get("gripper")
|
|
294
|
+
return cls(
|
|
295
|
+
schema_version=d["schema_version"],
|
|
296
|
+
segments=tuple(within(f"segments[{i}]", Segment.from_dict, s) for i, s in enumerate(segs)),
|
|
297
|
+
home_qpos=d["home_qpos"],
|
|
298
|
+
end_effector=within("end_effector", Pose.from_dict, d["end_effector"]),
|
|
299
|
+
tool=None if tool is None else within("tool", ToolSpec.from_dict, tool),
|
|
300
|
+
label=d.get("label", ""),
|
|
301
|
+
gripper=None if gripper is None else within("gripper", TwoFingerGripper.from_dict, gripper),
|
|
302
|
+
)
|
|
303
|
+
|
|
304
|
+
def hash_dict(self) -> dict[str, Any]:
|
|
305
|
+
"""The dict that is hashed: everything except the label."""
|
|
306
|
+
d = self.to_dict()
|
|
307
|
+
del d["label"]
|
|
308
|
+
return d
|
|
309
|
+
|
|
310
|
+
def canonical_json(self) -> str:
|
|
311
|
+
return canonical_json(self.hash_dict())
|
|
312
|
+
|
|
313
|
+
@property
|
|
314
|
+
def full_hash(self) -> str:
|
|
315
|
+
return sha256_hex(self.canonical_json())
|
|
316
|
+
|
|
317
|
+
@property
|
|
318
|
+
def config_hash(self) -> str:
|
|
319
|
+
return self.full_hash[:HASH_DISPLAY_LENGTH]
|