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
|
@@ -0,0 +1,234 @@
|
|
|
1
|
+
"""A robot that exists as a model somewhere else: a reference plus the numbers a config needs. No simulator is imported.
|
|
2
|
+
|
|
3
|
+
An :class:`ExternalRobot` does not hold the model. It names a *robot provider* (the owner of a namespace such as
|
|
4
|
+
``menagerie``), the robot and model file within it, and a content id (``source``) the provider vouches for. It carries
|
|
5
|
+
the facts the rest of the config layer needs without opening the model: which joints a controller drives, their
|
|
6
|
+
ranges and effort limits, the home posture and the end effector. A provider's ``describe`` produces these numbers from
|
|
7
|
+
the model; a config can also be written by hand. ``source`` is hashed, so a changed model is a different robot.
|
|
8
|
+
"""
|
|
9
|
+
|
|
10
|
+
from __future__ import annotations
|
|
11
|
+
|
|
12
|
+
from collections.abc import Mapping
|
|
13
|
+
from dataclasses import dataclass
|
|
14
|
+
from typing import Any
|
|
15
|
+
|
|
16
|
+
from .._canon import (
|
|
17
|
+
HASH_DISPLAY_LENGTH,
|
|
18
|
+
as_float,
|
|
19
|
+
as_str,
|
|
20
|
+
as_vector,
|
|
21
|
+
canonical_json,
|
|
22
|
+
check_keys,
|
|
23
|
+
sha256_hex,
|
|
24
|
+
within,
|
|
25
|
+
)
|
|
26
|
+
from ..errors import ConfigError
|
|
27
|
+
from .pose import Pose
|
|
28
|
+
|
|
29
|
+
ACTUATOR_KINDS = ("position", "torque", "velocity", "mixed")
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
@dataclass(frozen=True, slots=True)
|
|
33
|
+
class ExternalJoint:
|
|
34
|
+
"""A joint of the model that a controller drives. ``name`` is as in the model, without any prefix the compile adds;
|
|
35
|
+
``range`` is ``None`` (unlimited) or ``(lo, hi)`` in rad or m; ``effort_limit`` is N·m or N."""
|
|
36
|
+
|
|
37
|
+
name: str
|
|
38
|
+
range: tuple[float, float] | None
|
|
39
|
+
effort_limit: float
|
|
40
|
+
|
|
41
|
+
def __post_init__(self):
|
|
42
|
+
as_str(self.name, "name")
|
|
43
|
+
if self.range is not None:
|
|
44
|
+
lo, hi = as_vector(self.range, 2, "range")
|
|
45
|
+
if not lo < hi:
|
|
46
|
+
raise ConfigError("range", "lo must be < hi")
|
|
47
|
+
object.__setattr__(self, "range", (lo, hi))
|
|
48
|
+
effort = as_float(self.effort_limit, "effort_limit")
|
|
49
|
+
if effort <= 0.0:
|
|
50
|
+
raise ConfigError("effort_limit", "must be > 0")
|
|
51
|
+
object.__setattr__(self, "effort_limit", effort)
|
|
52
|
+
|
|
53
|
+
def to_dict(self) -> dict[str, Any]:
|
|
54
|
+
return {
|
|
55
|
+
"name": self.name,
|
|
56
|
+
"range": None if self.range is None else list(self.range),
|
|
57
|
+
"effort_limit": self.effort_limit,
|
|
58
|
+
}
|
|
59
|
+
|
|
60
|
+
@classmethod
|
|
61
|
+
def from_dict(cls, d: Mapping[str, Any]) -> ExternalJoint:
|
|
62
|
+
check_keys(d, {"name", "range", "effort_limit"}, "")
|
|
63
|
+
return cls(name=d["name"], range=d["range"], effort_limit=d["effort_limit"])
|
|
64
|
+
|
|
65
|
+
|
|
66
|
+
@dataclass(frozen=True, slots=True)
|
|
67
|
+
class EndEffectorRef:
|
|
68
|
+
"""The end effector of the model: an existing ``site``, or a ``body`` plus an ``offset`` in the body's frame.
|
|
69
|
+
Exactly one of ``site`` and ``body`` is given; ``offset`` is the identity when ``site`` is given."""
|
|
70
|
+
|
|
71
|
+
site: str | None
|
|
72
|
+
body: str | None
|
|
73
|
+
offset: Pose
|
|
74
|
+
|
|
75
|
+
def __post_init__(self):
|
|
76
|
+
if (self.site is None) == (self.body is None):
|
|
77
|
+
raise ConfigError("", "give exactly one of site and body")
|
|
78
|
+
for name in ("site", "body"):
|
|
79
|
+
if getattr(self, name) is not None:
|
|
80
|
+
as_str(getattr(self, name), name)
|
|
81
|
+
if not isinstance(self.offset, Pose):
|
|
82
|
+
raise ConfigError("offset", "expected a Pose")
|
|
83
|
+
if self.site is not None and self.offset != Pose.identity():
|
|
84
|
+
raise ConfigError("offset", "must be the identity when a site is given: the site already is a frame")
|
|
85
|
+
|
|
86
|
+
def to_dict(self) -> dict[str, Any]:
|
|
87
|
+
return {"site": self.site, "body": self.body, "offset": self.offset.to_dict()}
|
|
88
|
+
|
|
89
|
+
@classmethod
|
|
90
|
+
def from_dict(cls, d: Mapping[str, Any]) -> EndEffectorRef:
|
|
91
|
+
check_keys(d, {"site", "body", "offset"}, "")
|
|
92
|
+
return cls(site=d["site"], body=d["body"], offset=within("offset", Pose.from_dict, d["offset"]))
|
|
93
|
+
|
|
94
|
+
|
|
95
|
+
@dataclass(frozen=True, slots=True)
|
|
96
|
+
class ExternalRobot:
|
|
97
|
+
"""A reference to a robot model of a provider, with the numbers the config layer needs.
|
|
98
|
+
|
|
99
|
+
``provider`` is a registered robot provider (see ``config.robot_providers``); ``name`` the robot within it and
|
|
100
|
+
``entry`` the model file within the robot. ``source`` is a content id the provider vouches for (for MuJoCo
|
|
101
|
+
Menagerie: the git tree id of the model directory). ``joints`` are the joints a ``ControlConfig`` drives, in
|
|
102
|
+
order: ``n = len(joints)``. ``home_qpos`` has one value per joint, inside its range. ``home_root`` is the root
|
|
103
|
+
body at home in the model's world frame for a model with a free joint, else ``None``. ``keyframe`` names the model
|
|
104
|
+
keyframe that sets the state of the joints that are not driven (finger joints). ``end_effector`` is ``None`` for a
|
|
105
|
+
robot without one. ``actuator_kind`` describes the model's own actuators on ``joints``: ``position``, ``torque``,
|
|
106
|
+
``velocity`` or ``mixed``. ``label`` is metadata and does not enter the hash.
|
|
107
|
+
"""
|
|
108
|
+
|
|
109
|
+
provider: str
|
|
110
|
+
name: str
|
|
111
|
+
entry: str
|
|
112
|
+
source: str
|
|
113
|
+
joints: tuple[ExternalJoint, ...]
|
|
114
|
+
home_qpos: tuple[float, ...]
|
|
115
|
+
home_root: Pose | None
|
|
116
|
+
keyframe: str | None
|
|
117
|
+
end_effector: EndEffectorRef | None
|
|
118
|
+
actuator_kind: str
|
|
119
|
+
label: str = ""
|
|
120
|
+
|
|
121
|
+
def __post_init__(self):
|
|
122
|
+
for name in ("provider", "name", "entry", "source"):
|
|
123
|
+
as_str(getattr(self, name), name)
|
|
124
|
+
# Imported here: the registry is in the config package, which imports this one.
|
|
125
|
+
from ..config.robot_providers import list_robot_providers
|
|
126
|
+
|
|
127
|
+
if self.provider not in list_robot_providers():
|
|
128
|
+
raise ConfigError(
|
|
129
|
+
"provider", f"unknown robot provider {self.provider!r}; registered: {list(list_robot_providers())}"
|
|
130
|
+
)
|
|
131
|
+
|
|
132
|
+
joints = self.joints
|
|
133
|
+
if isinstance(joints, (str, bytes)) or not hasattr(joints, "__iter__"):
|
|
134
|
+
raise ConfigError("joints", "expected a sequence of ExternalJoint")
|
|
135
|
+
joints = tuple(joints)
|
|
136
|
+
if not joints:
|
|
137
|
+
raise ConfigError("joints", "need at least one joint")
|
|
138
|
+
for i, j in enumerate(joints):
|
|
139
|
+
if not isinstance(j, ExternalJoint):
|
|
140
|
+
raise ConfigError(f"joints[{i}]", "expected an ExternalJoint")
|
|
141
|
+
names = [j.name for j in joints]
|
|
142
|
+
for i, name in enumerate(names):
|
|
143
|
+
if name in names[:i]:
|
|
144
|
+
raise ConfigError(f"joints[{i}].name", f"duplicate joint name {name!r}")
|
|
145
|
+
object.__setattr__(self, "joints", joints)
|
|
146
|
+
|
|
147
|
+
home = as_vector(self.home_qpos, len(joints), "home_qpos")
|
|
148
|
+
for i, (q, j) in enumerate(zip(home, joints)):
|
|
149
|
+
if j.range is not None and not j.range[0] <= q <= j.range[1]:
|
|
150
|
+
raise ConfigError(f"home_qpos[{i}]", f"{q} is outside the joint range {list(j.range)}")
|
|
151
|
+
object.__setattr__(self, "home_qpos", home)
|
|
152
|
+
|
|
153
|
+
if self.home_root is not None and not isinstance(self.home_root, Pose):
|
|
154
|
+
raise ConfigError("home_root", "expected a Pose or None")
|
|
155
|
+
if self.keyframe is not None:
|
|
156
|
+
as_str(self.keyframe, "keyframe")
|
|
157
|
+
if self.end_effector is not None and not isinstance(self.end_effector, EndEffectorRef):
|
|
158
|
+
raise ConfigError("end_effector", "expected an EndEffectorRef or None")
|
|
159
|
+
if self.actuator_kind not in ACTUATOR_KINDS:
|
|
160
|
+
raise ConfigError(
|
|
161
|
+
"actuator_kind", f"unknown kind {self.actuator_kind!r}; expected one of {list(ACTUATOR_KINDS)}"
|
|
162
|
+
)
|
|
163
|
+
if not isinstance(self.label, str):
|
|
164
|
+
raise ConfigError("label", "must be a string")
|
|
165
|
+
|
|
166
|
+
@property
|
|
167
|
+
def n(self) -> int:
|
|
168
|
+
return len(self.joints)
|
|
169
|
+
|
|
170
|
+
# --- serialization and hashing -------------------------------------------------
|
|
171
|
+
|
|
172
|
+
def to_dict(self) -> dict[str, Any]:
|
|
173
|
+
return {
|
|
174
|
+
"kind": "external_robot",
|
|
175
|
+
"provider": self.provider,
|
|
176
|
+
"name": self.name,
|
|
177
|
+
"entry": self.entry,
|
|
178
|
+
"source": self.source,
|
|
179
|
+
"joints": [j.to_dict() for j in self.joints],
|
|
180
|
+
"home_qpos": list(self.home_qpos),
|
|
181
|
+
"home_root": None if self.home_root is None else self.home_root.to_dict(),
|
|
182
|
+
"keyframe": self.keyframe,
|
|
183
|
+
"end_effector": None if self.end_effector is None else self.end_effector.to_dict(),
|
|
184
|
+
"actuator_kind": self.actuator_kind,
|
|
185
|
+
"label": self.label,
|
|
186
|
+
}
|
|
187
|
+
|
|
188
|
+
@classmethod
|
|
189
|
+
def from_dict(cls, d: Mapping[str, Any]) -> ExternalRobot:
|
|
190
|
+
check_keys(
|
|
191
|
+
d,
|
|
192
|
+
{
|
|
193
|
+
"kind", "provider", "name", "entry", "source", "joints", "home_qpos", "home_root", "keyframe",
|
|
194
|
+
"end_effector", "actuator_kind",
|
|
195
|
+
},
|
|
196
|
+
"",
|
|
197
|
+
optional={"label"},
|
|
198
|
+
)
|
|
199
|
+
if d["kind"] != "external_robot":
|
|
200
|
+
raise ConfigError("kind", f"expected 'external_robot', got {d['kind']!r}")
|
|
201
|
+
joints = d["joints"]
|
|
202
|
+
if isinstance(joints, (str, bytes)) or not hasattr(joints, "__iter__"):
|
|
203
|
+
raise ConfigError("joints", "expected a sequence")
|
|
204
|
+
root, ee = d["home_root"], d["end_effector"]
|
|
205
|
+
return cls(
|
|
206
|
+
provider=d["provider"],
|
|
207
|
+
name=d["name"],
|
|
208
|
+
entry=d["entry"],
|
|
209
|
+
source=d["source"],
|
|
210
|
+
joints=tuple(within(f"joints[{i}]", ExternalJoint.from_dict, j) for i, j in enumerate(joints)),
|
|
211
|
+
home_qpos=d["home_qpos"],
|
|
212
|
+
home_root=None if root is None else within("home_root", Pose.from_dict, root),
|
|
213
|
+
keyframe=d["keyframe"],
|
|
214
|
+
end_effector=None if ee is None else within("end_effector", EndEffectorRef.from_dict, ee),
|
|
215
|
+
actuator_kind=d["actuator_kind"],
|
|
216
|
+
label=d.get("label", ""),
|
|
217
|
+
)
|
|
218
|
+
|
|
219
|
+
def hash_dict(self) -> dict[str, Any]:
|
|
220
|
+
"""The dict that is hashed: everything except the label."""
|
|
221
|
+
d = self.to_dict()
|
|
222
|
+
del d["label"]
|
|
223
|
+
return d
|
|
224
|
+
|
|
225
|
+
def canonical_json(self) -> str:
|
|
226
|
+
return canonical_json(self.hash_dict())
|
|
227
|
+
|
|
228
|
+
@property
|
|
229
|
+
def full_hash(self) -> str:
|
|
230
|
+
return sha256_hex(self.canonical_json())
|
|
231
|
+
|
|
232
|
+
@property
|
|
233
|
+
def config_hash(self) -> str:
|
|
234
|
+
return self.full_hash[:HASH_DISPLAY_LENGTH]
|
|
@@ -0,0 +1,138 @@
|
|
|
1
|
+
"""Generators that build ``ParametricArm`` objects. ``n`` is an argument here, not a config field."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import numbers
|
|
6
|
+
from typing import Any, Sequence
|
|
7
|
+
|
|
8
|
+
from .._canon import as_float
|
|
9
|
+
from ..errors import ConfigError
|
|
10
|
+
from .arm import Capsule, JointSpec, LinkSpec, ParametricArm, Pose, Segment, ToolSpec
|
|
11
|
+
from .gripper import TwoFingerGripper
|
|
12
|
+
|
|
13
|
+
_AXIS_LETTERS = {"x": (1.0, 0.0, 0.0), "y": (0.0, 1.0, 0.0), "z": (0.0, 0.0, 1.0)}
|
|
14
|
+
|
|
15
|
+
|
|
16
|
+
def _is_number(x: Any) -> bool:
|
|
17
|
+
return isinstance(x, numbers.Real) and not isinstance(x, bool)
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
def _per_link(value: Any, n: int, name: str) -> list[float]:
|
|
21
|
+
"""A scalar is repeated ``n`` times; a sequence must have length ``n``."""
|
|
22
|
+
if _is_number(value):
|
|
23
|
+
return [as_float(value, name)] * n
|
|
24
|
+
try:
|
|
25
|
+
items = list(value)
|
|
26
|
+
except TypeError:
|
|
27
|
+
raise ConfigError(name, "expected a number or a sequence of numbers") from None
|
|
28
|
+
if len(items) != n:
|
|
29
|
+
raise ConfigError(name, f"expected {n} values, got {len(items)}")
|
|
30
|
+
return [as_float(v, f"{name}[{i}]") for i, v in enumerate(items)]
|
|
31
|
+
|
|
32
|
+
|
|
33
|
+
def _split(single: Any, single_name: str, total: Any, total_name: str, n: int) -> list[float]:
|
|
34
|
+
"""Resolve an exactly-one-of (per-link, total) pair into per-link values."""
|
|
35
|
+
if (single is None) == (total is None):
|
|
36
|
+
raise ConfigError(f"{single_name}/{total_name}", f"give exactly one of {single_name} and {total_name}")
|
|
37
|
+
if single is not None:
|
|
38
|
+
return _per_link(single, n, single_name)
|
|
39
|
+
return [as_float(total, total_name) / n] * n
|
|
40
|
+
|
|
41
|
+
|
|
42
|
+
def _axes(axes: Any, n: int) -> list[tuple[float, ...]]:
|
|
43
|
+
items = list(axes)
|
|
44
|
+
if len(items) != n:
|
|
45
|
+
raise ConfigError("axes", f"expected {n} axes, got {len(items)}")
|
|
46
|
+
out = []
|
|
47
|
+
for i, a in enumerate(items):
|
|
48
|
+
if isinstance(a, str):
|
|
49
|
+
if a not in _AXIS_LETTERS:
|
|
50
|
+
raise ConfigError(f"axes[{i}]", f"unknown axis letter {a!r}; use x, y or z")
|
|
51
|
+
out.append(_AXIS_LETTERS[a])
|
|
52
|
+
else:
|
|
53
|
+
out.append(tuple(a))
|
|
54
|
+
return out
|
|
55
|
+
|
|
56
|
+
|
|
57
|
+
def _ranges(range_: Any, n: int) -> list[tuple[float, float] | None]:
|
|
58
|
+
if range_ is None:
|
|
59
|
+
return [None] * n
|
|
60
|
+
items = list(range_)
|
|
61
|
+
if len(items) == 2 and all(_is_number(v) for v in items):
|
|
62
|
+
return [tuple(items)] * n
|
|
63
|
+
if len(items) != n:
|
|
64
|
+
raise ConfigError("range", f"expected None, (lo, hi) or {n} entries, got {len(items)}")
|
|
65
|
+
return [None if r is None else tuple(r) for r in items]
|
|
66
|
+
|
|
67
|
+
|
|
68
|
+
def chain(
|
|
69
|
+
n: int,
|
|
70
|
+
axes: str | Sequence[Any],
|
|
71
|
+
*,
|
|
72
|
+
link_length: float | Sequence[float] | None = None,
|
|
73
|
+
total_length: float | None = None,
|
|
74
|
+
mass: float | Sequence[float] | None = None,
|
|
75
|
+
total_mass: float | None = None,
|
|
76
|
+
radius: float | Sequence[float],
|
|
77
|
+
damping: float | Sequence[float],
|
|
78
|
+
armature: float | Sequence[float],
|
|
79
|
+
frictionloss: float | Sequence[float],
|
|
80
|
+
effort_limit: float | Sequence[float],
|
|
81
|
+
range: Any = None,
|
|
82
|
+
home_qpos: Sequence[float] | None = None,
|
|
83
|
+
end_effector: Pose | None = None,
|
|
84
|
+
tool: ToolSpec | None = None,
|
|
85
|
+
gripper: TwoFingerGripper | None = None,
|
|
86
|
+
label: str = "",
|
|
87
|
+
) -> ParametricArm:
|
|
88
|
+
"""An ``n``-joint serial arm, straight along +x of the base frame at ``q = 0``.
|
|
89
|
+
|
|
90
|
+
``axes`` is a string of x/y/z letters or a sequence of vectors, one per joint (no cycling).
|
|
91
|
+
Exactly one of ``link_length`` / ``total_length`` and one of ``mass`` / ``total_mass`` is given;
|
|
92
|
+
totals are split uniformly. The remaining physical parameters are scalars or per-joint sequences.
|
|
93
|
+
"""
|
|
94
|
+
if isinstance(n, bool) or not isinstance(n, numbers.Integral) or n < 1:
|
|
95
|
+
raise ConfigError("n", "must be an integer >= 1")
|
|
96
|
+
n = int(n)
|
|
97
|
+
axis_list = _axes(axes, n)
|
|
98
|
+
lengths = _split(link_length, "link_length", total_length, "total_length", n)
|
|
99
|
+
masses = _split(mass, "mass", total_mass, "total_mass", n)
|
|
100
|
+
for i, L in enumerate(lengths):
|
|
101
|
+
if L <= 0.0:
|
|
102
|
+
raise ConfigError(f"link_length[{i}]", "must be > 0")
|
|
103
|
+
radii = _per_link(radius, n, "radius")
|
|
104
|
+
dampings = _per_link(damping, n, "damping")
|
|
105
|
+
armatures = _per_link(armature, n, "armature")
|
|
106
|
+
frictions = _per_link(frictionloss, n, "frictionloss")
|
|
107
|
+
efforts = _per_link(effort_limit, n, "effort_limit")
|
|
108
|
+
ranges = _ranges(range, n)
|
|
109
|
+
|
|
110
|
+
segments = tuple(
|
|
111
|
+
Segment(
|
|
112
|
+
joint=JointSpec(
|
|
113
|
+
axis=axis_list[i],
|
|
114
|
+
range=ranges[i],
|
|
115
|
+
damping=dampings[i],
|
|
116
|
+
armature=armatures[i],
|
|
117
|
+
frictionloss=frictions[i],
|
|
118
|
+
effort_limit=efforts[i],
|
|
119
|
+
),
|
|
120
|
+
link=LinkSpec(
|
|
121
|
+
joint_frame=Pose.identity(),
|
|
122
|
+
tip_offset=(lengths[i], 0.0, 0.0),
|
|
123
|
+
mass=masses[i],
|
|
124
|
+
geometry=Capsule(radius=radii[i]),
|
|
125
|
+
inertia=None,
|
|
126
|
+
),
|
|
127
|
+
)
|
|
128
|
+
for i, _ in enumerate(axis_list)
|
|
129
|
+
)
|
|
130
|
+
return ParametricArm(
|
|
131
|
+
schema_version=1,
|
|
132
|
+
segments=segments,
|
|
133
|
+
home_qpos=(0.0,) * n if home_qpos is None else tuple(home_qpos),
|
|
134
|
+
end_effector=Pose.identity() if end_effector is None else end_effector,
|
|
135
|
+
tool=tool,
|
|
136
|
+
label=label,
|
|
137
|
+
gripper=gripper,
|
|
138
|
+
)
|
|
@@ -0,0 +1,82 @@
|
|
|
1
|
+
"""A parametric two-finger parallel gripper. numpy only; the MuJoCo simulator builds it (``simulators/mujoco/arm_spec.py``)."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from dataclasses import dataclass
|
|
6
|
+
from typing import Any, Mapping
|
|
7
|
+
|
|
8
|
+
from .._canon import as_float, check_keys, within
|
|
9
|
+
from ..errors import ConfigError
|
|
10
|
+
from .pose import Pose
|
|
11
|
+
|
|
12
|
+
|
|
13
|
+
@dataclass(frozen=True, slots=True)
|
|
14
|
+
class TwoFingerGripper:
|
|
15
|
+
"""Two capsule fingers on a palm, mounted on the arm's end effector.
|
|
16
|
+
|
|
17
|
+
Gripper frame: ``mount`` relative to the end-effector frame; fingers point along +x and open along ±y. The palm is a
|
|
18
|
+
box ``palm_length`` long (x); the fingers start at its far end. Each finger slides on a prismatic joint from ``0``
|
|
19
|
+
(closed: the finger surfaces touch) to ``stroke`` (open); the two joints are coupled, so the gripper has one degree of
|
|
20
|
+
freedom, the opening ``w`` in ``[0, 2 * stroke]`` (the gap between the finger surfaces). The simulator drives it
|
|
21
|
+
with one position servo (``kp``, ``kd``, ``force_limit`` on the opening) and the env adds one action dimension.
|
|
22
|
+
``tcp_offset`` of the tool-centre point from the gripper frame is ``(palm_length + finger_length, 0, 0)``.
|
|
23
|
+
"""
|
|
24
|
+
|
|
25
|
+
finger_length: float
|
|
26
|
+
finger_radius: float
|
|
27
|
+
stroke: float
|
|
28
|
+
finger_mass: float
|
|
29
|
+
palm_length: float
|
|
30
|
+
palm_mass: float
|
|
31
|
+
kp: float
|
|
32
|
+
kd: float
|
|
33
|
+
force_limit: float
|
|
34
|
+
mount: Pose
|
|
35
|
+
|
|
36
|
+
_POSITIVE = (
|
|
37
|
+
"finger_length", "finger_radius", "stroke", "finger_mass", "palm_length", "palm_mass", "force_limit",
|
|
38
|
+
)
|
|
39
|
+
|
|
40
|
+
def __post_init__(self):
|
|
41
|
+
for name in self._POSITIVE:
|
|
42
|
+
v = as_float(getattr(self, name), name)
|
|
43
|
+
if v <= 0.0:
|
|
44
|
+
raise ConfigError(name, "must be > 0")
|
|
45
|
+
object.__setattr__(self, name, v)
|
|
46
|
+
for name in ("kp", "kd"):
|
|
47
|
+
v = as_float(getattr(self, name), name)
|
|
48
|
+
if v < 0.0:
|
|
49
|
+
raise ConfigError(name, "must be >= 0")
|
|
50
|
+
object.__setattr__(self, name, v)
|
|
51
|
+
if not isinstance(self.mount, Pose):
|
|
52
|
+
raise ConfigError("mount", "expected a Pose")
|
|
53
|
+
|
|
54
|
+
@classmethod
|
|
55
|
+
def default(cls, **overrides: Any) -> "TwoFingerGripper":
|
|
56
|
+
"""A small gripper (6 cm fingers, 4 cm stroke per finger); keyword arguments replace fields."""
|
|
57
|
+
values: dict[str, Any] = dict(
|
|
58
|
+
finger_length=0.06, finger_radius=0.006, stroke=0.04, finger_mass=0.03, palm_length=0.03,
|
|
59
|
+
palm_mass=0.1, kp=200.0, kd=10.0, force_limit=20.0, mount=Pose.identity(),
|
|
60
|
+
)
|
|
61
|
+
values.update(overrides)
|
|
62
|
+
return cls(**values)
|
|
63
|
+
|
|
64
|
+
@property
|
|
65
|
+
def max_opening(self) -> float:
|
|
66
|
+
"""The largest gap between the finger surfaces, metres."""
|
|
67
|
+
return 2.0 * self.stroke
|
|
68
|
+
|
|
69
|
+
@property
|
|
70
|
+
def tcp_offset(self) -> tuple[float, float, float]:
|
|
71
|
+
return (self.palm_length + self.finger_length, 0.0, 0.0)
|
|
72
|
+
|
|
73
|
+
def to_dict(self) -> dict[str, Any]:
|
|
74
|
+
d = {k: getattr(self, k) for k in (*self._POSITIVE, "kp", "kd")}
|
|
75
|
+
d["mount"] = self.mount.to_dict()
|
|
76
|
+
return d
|
|
77
|
+
|
|
78
|
+
@classmethod
|
|
79
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "TwoFingerGripper":
|
|
80
|
+
check_keys(d, {*cls._POSITIVE, "kp", "kd", "mount"}, "")
|
|
81
|
+
values = {k: d[k] for k in d if k != "mount"}
|
|
82
|
+
return cls(mount=within("mount", Pose.from_dict, d["mount"]), **values)
|
robotenv/robot/pose.py
ADDED
|
@@ -0,0 +1,49 @@
|
|
|
1
|
+
"""Rigid poses: position (m) and quaternion ``(w, x, y, z)``."""
|
|
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 as_vector, check_keys
|
|
10
|
+
from ..errors import ConfigError
|
|
11
|
+
|
|
12
|
+
|
|
13
|
+
def _normalize_quat(q: tuple[float, ...], path: str) -> tuple[float, ...]:
|
|
14
|
+
norm = math.sqrt(sum(c * c for c in q))
|
|
15
|
+
if norm == 0.0:
|
|
16
|
+
raise ConfigError(path, "must be non-zero")
|
|
17
|
+
q = tuple(c / norm for c in q)
|
|
18
|
+
# q and -q are the same rotation: make the first non-zero component positive.
|
|
19
|
+
for c in q:
|
|
20
|
+
if c != 0.0:
|
|
21
|
+
if c < 0.0:
|
|
22
|
+
q = tuple(-v for v in q)
|
|
23
|
+
break
|
|
24
|
+
return q
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
@dataclass(frozen=True, slots=True)
|
|
28
|
+
class Pose:
|
|
29
|
+
"""Position (m) and orientation as a quaternion ``(w, x, y, z)``."""
|
|
30
|
+
|
|
31
|
+
pos: tuple[float, float, float]
|
|
32
|
+
quat: tuple[float, float, float, float]
|
|
33
|
+
|
|
34
|
+
def __post_init__(self):
|
|
35
|
+
object.__setattr__(self, "pos", as_vector(self.pos, 3, "pos"))
|
|
36
|
+
quat = as_vector(self.quat, 4, "quat")
|
|
37
|
+
object.__setattr__(self, "quat", _normalize_quat(quat, "quat"))
|
|
38
|
+
|
|
39
|
+
@classmethod
|
|
40
|
+
def identity(cls) -> "Pose":
|
|
41
|
+
return cls(pos=(0.0, 0.0, 0.0), quat=(1.0, 0.0, 0.0, 0.0))
|
|
42
|
+
|
|
43
|
+
def to_dict(self) -> dict[str, Any]:
|
|
44
|
+
return {"pos": list(self.pos), "quat": list(self.quat)}
|
|
45
|
+
|
|
46
|
+
@classmethod
|
|
47
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "Pose":
|
|
48
|
+
check_keys(d, {"pos", "quat"}, "")
|
|
49
|
+
return cls(pos=d["pos"], quat=d["quat"])
|
|
@@ -0,0 +1,5 @@
|
|
|
1
|
+
"""Robot providers shipped with robotenv: one subpackage per namespace, each exposing ``PROVIDER``.
|
|
2
|
+
|
|
3
|
+
See :mod:`robotenv.config.robot_providers` for the contract and the other ways a provider is found. The core
|
|
4
|
+
names no provider; the subpackages here are found by name, as those of ``robotenv.tasks`` are.
|
|
5
|
+
"""
|
|
@@ -0,0 +1,127 @@
|
|
|
1
|
+
"""The robot provider ``menagerie``: robots of MuJoCo Menagerie, through the ``mujoco-menagerie`` library.
|
|
2
|
+
|
|
3
|
+
Importing this module imports no engine and not ``mujoco_menagerie``; each method imports what it needs when it is
|
|
4
|
+
called. No model file is copied into this repository: the library resolves and caches the models (its cache directory
|
|
5
|
+
is ``MENAGERIE_CACHE_DIR``, or ``MENAGERIE_ROOT`` points at a checkout) and the licences differ per model, so
|
|
6
|
+
:meth:`MenagerieRobots.license` reports the registry's licence of each.
|
|
7
|
+
|
|
8
|
+
``describe`` reads a model and returns an :class:`~robotenv.robot.ExternalRobot`; ``cards.json`` holds, per robot,
|
|
9
|
+
what cannot be derived (driven joints where a finger is a joint actuator, an end effector where the model has no
|
|
10
|
+
suitable site, a keyframe that is not called ``home``).
|
|
11
|
+
"""
|
|
12
|
+
|
|
13
|
+
from __future__ import annotations
|
|
14
|
+
|
|
15
|
+
import json
|
|
16
|
+
from importlib import resources
|
|
17
|
+
from typing import Any
|
|
18
|
+
|
|
19
|
+
from ....errors import ConfigError, Problem
|
|
20
|
+
|
|
21
|
+
__all__ = ["PROVIDER", "MenagerieRobots", "cards"]
|
|
22
|
+
|
|
23
|
+
DEFAULT = object() # "use the model's keyframe called home, if it has one"
|
|
24
|
+
EXTRA = "pip install 'robotenv[menagerie]'"
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
def cards() -> dict[str, dict[str, Any]]:
|
|
28
|
+
"""The cards of ``cards.json`` by robot name."""
|
|
29
|
+
data = json.loads(resources.files(__package__).joinpath("cards.json").read_text())
|
|
30
|
+
return {k: v for k, v in data.items() if not k.startswith("_")}
|
|
31
|
+
|
|
32
|
+
|
|
33
|
+
def _library():
|
|
34
|
+
try:
|
|
35
|
+
import mujoco_menagerie
|
|
36
|
+
except ImportError as e:
|
|
37
|
+
raise ConfigError("provider", f"the menagerie provider needs the mujoco-menagerie library: {EXTRA}") from e
|
|
38
|
+
return mujoco_menagerie
|
|
39
|
+
|
|
40
|
+
|
|
41
|
+
def _robot_meta(name: str):
|
|
42
|
+
"""The library's record of robot ``name`` (no download); ``ConfigError`` at ``name`` with the near match."""
|
|
43
|
+
lib = _library()
|
|
44
|
+
try:
|
|
45
|
+
return lib.get(name)
|
|
46
|
+
except lib.UnknownRobotError as e:
|
|
47
|
+
raise ConfigError("name", str(e)) from e
|
|
48
|
+
|
|
49
|
+
|
|
50
|
+
def _entry_meta(meta, entry: str):
|
|
51
|
+
try:
|
|
52
|
+
return meta.entry(entry)
|
|
53
|
+
except Exception as e: # UnknownEntryPointError
|
|
54
|
+
raise ConfigError("entry", str(e)) from e
|
|
55
|
+
|
|
56
|
+
|
|
57
|
+
class MenagerieRobots:
|
|
58
|
+
"""``RobotProvider`` of MuJoCo Menagerie."""
|
|
59
|
+
|
|
60
|
+
def names(self) -> tuple[str, ...]:
|
|
61
|
+
try:
|
|
62
|
+
return tuple(_library().names())
|
|
63
|
+
except ConfigError:
|
|
64
|
+
return ()
|
|
65
|
+
|
|
66
|
+
def license(self, name: str) -> str:
|
|
67
|
+
"""The licence string of the registry for robot ``name``."""
|
|
68
|
+
return _robot_meta(name).license
|
|
69
|
+
|
|
70
|
+
def describe(
|
|
71
|
+
self,
|
|
72
|
+
name: str,
|
|
73
|
+
entry: str | None = None,
|
|
74
|
+
*,
|
|
75
|
+
controlled: tuple[str, ...] | list[str] | None = None,
|
|
76
|
+
end_effector: Any = None,
|
|
77
|
+
keyframe: Any = DEFAULT,
|
|
78
|
+
effort_limit: float | dict[str, float] | None = None,
|
|
79
|
+
):
|
|
80
|
+
"""Open the model of robot ``name`` and return its :class:`ExternalRobot`.
|
|
81
|
+
|
|
82
|
+
``entry`` defaults to the card's, else the library's default model; a scene entry is refused. ``controlled``
|
|
83
|
+
names the driven joints (default: the card's, else every joint with an actuator, in actuator order; free joints
|
|
84
|
+
are never driven). ``end_effector`` is an ``EndEffectorRef`` (default: the card's, else a site with a
|
|
85
|
+
conventional name, else none). ``keyframe`` defaults to the card's, else ``home`` if the model has it, else none.
|
|
86
|
+
``effort_limit`` supplies the limit of joints whose model has none: a number, or ``{joint name: limit}``.
|
|
87
|
+
"""
|
|
88
|
+
_library() # the library first: its absence is a ConfigError that names the extra
|
|
89
|
+
from ._describe import describe
|
|
90
|
+
|
|
91
|
+
return describe(self, name, entry, controlled, end_effector, keyframe, effort_limit)
|
|
92
|
+
|
|
93
|
+
def spec(self, robot):
|
|
94
|
+
"""The editable ``mujoco.MjSpec`` of ``robot``, after checking ``robot.source`` against the library."""
|
|
95
|
+
try:
|
|
96
|
+
meta = _robot_meta(robot.name)
|
|
97
|
+
_entry_meta(meta, robot.entry)
|
|
98
|
+
except ConfigError as e:
|
|
99
|
+
raise e.prefixed("robot") from None
|
|
100
|
+
if meta.oid != robot.source:
|
|
101
|
+
raise ConfigError("robot.source", STALE)
|
|
102
|
+
try:
|
|
103
|
+
return meta.spec(robot.entry)
|
|
104
|
+
except Exception as e: # a download that failed, a model that does not open
|
|
105
|
+
raise ConfigError("robot", f"the model of menagerie/{robot.name} could not be opened: {type(e).__name__}: {e}") from e
|
|
106
|
+
|
|
107
|
+
def validate(self, robot, simulator: str) -> list[Problem]:
|
|
108
|
+
problems: list[Problem] = []
|
|
109
|
+
try:
|
|
110
|
+
meta = _robot_meta(robot.name)
|
|
111
|
+
if _entry_meta(meta, robot.entry).kind == "scene":
|
|
112
|
+
problems.append(Problem("robot.entry", f"{robot.entry!r} is a scene: the compile brings floor and lights"))
|
|
113
|
+
if meta.oid != robot.source:
|
|
114
|
+
problems.append(Problem("robot.source", STALE))
|
|
115
|
+
except ConfigError as e:
|
|
116
|
+
e = e.prefixed("robot")
|
|
117
|
+
problems.append(Problem(e.path, e.reason))
|
|
118
|
+
return problems
|
|
119
|
+
|
|
120
|
+
def kinematics(self, robot):
|
|
121
|
+
from ....simulators.mujoco.external import MujocoKinematics
|
|
122
|
+
|
|
123
|
+
return MujocoKinematics(robot, self.spec(robot))
|
|
124
|
+
|
|
125
|
+
|
|
126
|
+
STALE = "the model changed since this config was written"
|
|
127
|
+
PROVIDER = MenagerieRobots()
|