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,161 @@
|
|
|
1
|
+
"""Control interface configuration."""
|
|
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
|
+
|
|
11
|
+
CONTROL_MODES = (
|
|
12
|
+
"joint_pos_abs",
|
|
13
|
+
"joint_pos_delta",
|
|
14
|
+
"joint_torque",
|
|
15
|
+
"joint_vel",
|
|
16
|
+
"ee_pos_delta",
|
|
17
|
+
"ee_pose_delta",
|
|
18
|
+
)
|
|
19
|
+
_SCALED_MODES = frozenset({"joint_pos_delta", "joint_vel"})
|
|
20
|
+
# the control modes that drive the model's own actuators of each kind (NativeGains); "mixed" fits none
|
|
21
|
+
NATIVE_MODES = {
|
|
22
|
+
"position": ("joint_pos_abs", "joint_pos_delta"),
|
|
23
|
+
"torque": ("joint_torque",),
|
|
24
|
+
"velocity": ("joint_vel",),
|
|
25
|
+
"mixed": (),
|
|
26
|
+
}
|
|
27
|
+
|
|
28
|
+
|
|
29
|
+
def _gain(x: Any, name: str) -> float | tuple[float, ...]:
|
|
30
|
+
try:
|
|
31
|
+
items = tuple(x)
|
|
32
|
+
except TypeError:
|
|
33
|
+
v = as_float(x, name)
|
|
34
|
+
if v < 0.0:
|
|
35
|
+
raise ConfigError(name, "must be >= 0") from None
|
|
36
|
+
return v
|
|
37
|
+
out = tuple(as_float(v, f"{name}[{i}]") for i, v in enumerate(items))
|
|
38
|
+
if not out:
|
|
39
|
+
raise ConfigError(name, "must not be empty")
|
|
40
|
+
for i, v in enumerate(out):
|
|
41
|
+
if v < 0.0:
|
|
42
|
+
raise ConfigError(f"{name}[{i}]", "must be >= 0")
|
|
43
|
+
return out
|
|
44
|
+
|
|
45
|
+
|
|
46
|
+
@dataclass(frozen=True, slots=True)
|
|
47
|
+
class FixedGains:
|
|
48
|
+
"""Actuator gains. Scalars apply to every joint; sequences are per joint."""
|
|
49
|
+
|
|
50
|
+
kp: float | tuple[float, ...]
|
|
51
|
+
kd: float | tuple[float, ...]
|
|
52
|
+
|
|
53
|
+
def __post_init__(self):
|
|
54
|
+
object.__setattr__(self, "kp", _gain(self.kp, "kp"))
|
|
55
|
+
object.__setattr__(self, "kd", _gain(self.kd, "kd"))
|
|
56
|
+
|
|
57
|
+
def to_dict(self) -> dict[str, Any]:
|
|
58
|
+
return {
|
|
59
|
+
"kind": "fixed",
|
|
60
|
+
"kp": list(self.kp) if isinstance(self.kp, tuple) else self.kp,
|
|
61
|
+
"kd": list(self.kd) if isinstance(self.kd, tuple) else self.kd,
|
|
62
|
+
}
|
|
63
|
+
|
|
64
|
+
@classmethod
|
|
65
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "FixedGains":
|
|
66
|
+
check_keys(d, {"kind", "kp", "kd"}, "")
|
|
67
|
+
if d["kind"] != "fixed":
|
|
68
|
+
raise ConfigError("kind", f"unsupported gains {d['kind']!r}")
|
|
69
|
+
return cls(kp=d["kp"], kd=d["kd"])
|
|
70
|
+
|
|
71
|
+
|
|
72
|
+
@dataclass(frozen=True, slots=True)
|
|
73
|
+
class InertiaScaledGains:
|
|
74
|
+
"""Gains derived from the inertia each joint sees, evaluated at ``home_qpos`` by the simulator.
|
|
75
|
+
|
|
76
|
+
``kp_i = bandwidth^2 * M_ii``, ``kd_i = max(2 * damping_ratio * sqrt(kp_i * M_ii) - damping_i, 0)``,
|
|
77
|
+
with ``M`` the joint-space mass matrix of the compiled model (armature included). An approximation:
|
|
78
|
+
``M`` changes with configuration.
|
|
79
|
+
"""
|
|
80
|
+
|
|
81
|
+
bandwidth: float # rad/s
|
|
82
|
+
damping_ratio: float # dimensionless
|
|
83
|
+
|
|
84
|
+
def __post_init__(self):
|
|
85
|
+
for name in ("bandwidth", "damping_ratio"):
|
|
86
|
+
v = as_float(getattr(self, name), name)
|
|
87
|
+
if v <= 0.0:
|
|
88
|
+
raise ConfigError(name, "must be > 0")
|
|
89
|
+
object.__setattr__(self, name, v)
|
|
90
|
+
|
|
91
|
+
def to_dict(self) -> dict[str, Any]:
|
|
92
|
+
return {"kind": "inertia_scaled", "bandwidth": self.bandwidth, "damping_ratio": self.damping_ratio}
|
|
93
|
+
|
|
94
|
+
@classmethod
|
|
95
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "InertiaScaledGains":
|
|
96
|
+
check_keys(d, {"kind", "bandwidth", "damping_ratio"}, "")
|
|
97
|
+
if d["kind"] != "inertia_scaled":
|
|
98
|
+
raise ConfigError("kind", f"expected 'inertia_scaled', got {d['kind']!r}")
|
|
99
|
+
return cls(bandwidth=d["bandwidth"], damping_ratio=d["damping_ratio"])
|
|
100
|
+
|
|
101
|
+
|
|
102
|
+
@dataclass(frozen=True, slots=True)
|
|
103
|
+
class NativeGains:
|
|
104
|
+
"""Drive the model with its own actuators and gains: nothing is replaced. Only an ``ExternalRobot`` takes it, and
|
|
105
|
+
an ``ExternalRobot`` takes nothing else until re-actuation exists."""
|
|
106
|
+
|
|
107
|
+
def to_dict(self) -> dict[str, Any]:
|
|
108
|
+
return {"kind": "native"}
|
|
109
|
+
|
|
110
|
+
@classmethod
|
|
111
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "NativeGains":
|
|
112
|
+
check_keys(d, {"kind"}, "")
|
|
113
|
+
if d["kind"] != "native":
|
|
114
|
+
raise ConfigError("kind", f"expected 'native', got {d['kind']!r}")
|
|
115
|
+
return cls()
|
|
116
|
+
|
|
117
|
+
|
|
118
|
+
Gains = FixedGains | InertiaScaledGains | NativeGains
|
|
119
|
+
|
|
120
|
+
|
|
121
|
+
def gains_from_dict(d: Mapping[str, Any]) -> Gains:
|
|
122
|
+
if not isinstance(d, Mapping) or "kind" not in d:
|
|
123
|
+
raise ConfigError("", "expected a mapping with a 'kind' key")
|
|
124
|
+
kinds = {"fixed": FixedGains, "inertia_scaled": InertiaScaledGains, "native": NativeGains}
|
|
125
|
+
if d["kind"] not in kinds:
|
|
126
|
+
raise ConfigError("kind", f"unknown gains {d['kind']!r}; expected one of {sorted(kinds)}")
|
|
127
|
+
return kinds[d["kind"]].from_dict(d)
|
|
128
|
+
|
|
129
|
+
|
|
130
|
+
@dataclass(frozen=True, slots=True)
|
|
131
|
+
class ControlConfig:
|
|
132
|
+
mode: str
|
|
133
|
+
gains: Gains
|
|
134
|
+
action_scale: float | None
|
|
135
|
+
|
|
136
|
+
def __post_init__(self):
|
|
137
|
+
if self.mode not in CONTROL_MODES:
|
|
138
|
+
raise ConfigError("mode", f"unknown mode {self.mode!r}; expected one of {list(CONTROL_MODES)}")
|
|
139
|
+
if not isinstance(self.gains, (FixedGains, InertiaScaledGains, NativeGains)):
|
|
140
|
+
raise ConfigError("gains", "expected FixedGains, InertiaScaledGains or NativeGains")
|
|
141
|
+
if self.mode in _SCALED_MODES:
|
|
142
|
+
if self.action_scale is None:
|
|
143
|
+
raise ConfigError("action_scale", f"required for mode {self.mode!r}")
|
|
144
|
+
scale = as_float(self.action_scale, "action_scale")
|
|
145
|
+
if scale <= 0.0:
|
|
146
|
+
raise ConfigError("action_scale", "must be > 0")
|
|
147
|
+
object.__setattr__(self, "action_scale", scale)
|
|
148
|
+
elif self.action_scale is not None:
|
|
149
|
+
raise ConfigError("action_scale", f"must be None for mode {self.mode!r}")
|
|
150
|
+
|
|
151
|
+
def to_dict(self) -> dict[str, Any]:
|
|
152
|
+
return {"mode": self.mode, "gains": self.gains.to_dict(), "action_scale": self.action_scale}
|
|
153
|
+
|
|
154
|
+
@classmethod
|
|
155
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "ControlConfig":
|
|
156
|
+
check_keys(d, {"mode", "gains", "action_scale"}, "")
|
|
157
|
+
return cls(
|
|
158
|
+
mode=d["mode"],
|
|
159
|
+
gains=within("gains", gains_from_dict, d["gains"]),
|
|
160
|
+
action_scale=d["action_scale"],
|
|
161
|
+
)
|
|
@@ -0,0 +1,29 @@
|
|
|
1
|
+
"""The defaults of ``BaseEnvConfig``, in one table. Nothing else in the config layer has a default value.
|
|
2
|
+
|
|
3
|
+
``BaseEnvConfig()`` is a one-link arm in the ``empty`` scene doing the ``empty/idle`` task on the CPU MuJoCo simulator.
|
|
4
|
+
The numbers are plain choices, not recommendations: a change of any of them changes the ``env_hash`` of every config
|
|
5
|
+
that relied on it, so ``DEFAULTS_VERSION`` is raised with it. Store ``resolve().to_dict()`` with every run: it holds
|
|
6
|
+
every value and reproduces after the defaults change. The simulator's options are the ``default()`` of its options
|
|
7
|
+
class (see ``config.sim``).
|
|
8
|
+
"""
|
|
9
|
+
|
|
10
|
+
from __future__ import annotations
|
|
11
|
+
|
|
12
|
+
DEFAULTS_VERSION = 2 # 2: WarpOptions.default() turned the CUDA graph on (the options are part of the env_hash)
|
|
13
|
+
|
|
14
|
+
DEFAULTS: dict[str, dict] = {
|
|
15
|
+
# the arm: one yaw joint, 0.5 m long, 1 kg (the arguments of ``chain``, see ``specs.Chain``)
|
|
16
|
+
"robot": dict(
|
|
17
|
+
n=1, axes="z", total_length=0.5, total_mass=1.0, radius=0.03, damping=0.5, armature=0.01, frictionloss=0.0,
|
|
18
|
+
effort_limit=50.0, range=(-2.5, 2.5),
|
|
19
|
+
),
|
|
20
|
+
# the README quick start's control: absolute joint positions, kp 60, kd 6 on every joint
|
|
21
|
+
"control": dict(mode="joint_pos_abs", kp=60.0, kd=6.0, action_scale=None),
|
|
22
|
+
# the placeholder task of the package; ``name`` is also the task whose default mount is used
|
|
23
|
+
"task": dict(provider="empty", name="idle", max_episode_steps=200),
|
|
24
|
+
# 500 Hz physics, 50 Hz control (the quick start's numbers), one env, Euler
|
|
25
|
+
"sim": dict(
|
|
26
|
+
simulator="mujoco", num_envs=1, device="cpu", dt=0.002, integrator="euler", control_freq=50.0, seed=0,
|
|
27
|
+
scene="empty",
|
|
28
|
+
),
|
|
29
|
+
}
|
robotenv/config/env.py
ADDED
|
@@ -0,0 +1,107 @@
|
|
|
1
|
+
"""The complete, declarative description of one environment, with one hash."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from dataclasses import dataclass
|
|
6
|
+
from typing import Any, Mapping
|
|
7
|
+
|
|
8
|
+
from .._canon import HASH_DISPLAY_LENGTH, canonical_json, check_keys, sha256_hex, within
|
|
9
|
+
from .control import NATIVE_MODES, ControlConfig, NativeGains
|
|
10
|
+
from ..errors import ConfigError
|
|
11
|
+
from ..robot import ExternalRobot, ParametricArm, robot_from_dict
|
|
12
|
+
from .mount import check_mount_fits_robot
|
|
13
|
+
from .sim import SimConfig
|
|
14
|
+
from .task import TaskConfig
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
@dataclass(frozen=True, slots=True)
|
|
18
|
+
class EnvConfig:
|
|
19
|
+
robot: ParametricArm | ExternalRobot
|
|
20
|
+
control: ControlConfig
|
|
21
|
+
task: TaskConfig
|
|
22
|
+
sim: SimConfig
|
|
23
|
+
|
|
24
|
+
def __post_init__(self):
|
|
25
|
+
for name, cls in (
|
|
26
|
+
("robot", (ParametricArm, ExternalRobot)),
|
|
27
|
+
("control", ControlConfig),
|
|
28
|
+
("task", TaskConfig),
|
|
29
|
+
("sim", SimConfig),
|
|
30
|
+
):
|
|
31
|
+
if not isinstance(getattr(self, name), cls):
|
|
32
|
+
names = " or ".join(c.__name__ for c in (cls if isinstance(cls, tuple) else (cls,)))
|
|
33
|
+
raise ConfigError(name, f"expected {names}")
|
|
34
|
+
|
|
35
|
+
n = self.robot.n
|
|
36
|
+
for name in ("kp", "kd"):
|
|
37
|
+
v = getattr(self.control.gains, name, None)
|
|
38
|
+
if isinstance(v, tuple) and len(v) != n:
|
|
39
|
+
raise ConfigError(f"control.gains.{name}", f"expected {n} per-joint values, got {len(v)}")
|
|
40
|
+
self._check_gains_against_robot()
|
|
41
|
+
check_mount_fits_robot(self.task.mount, self.robot)
|
|
42
|
+
|
|
43
|
+
def _check_gains_against_robot(self) -> None:
|
|
44
|
+
gains = self.control.gains
|
|
45
|
+
if isinstance(self.robot, ExternalRobot):
|
|
46
|
+
# FixedGains and InertiaScaledGains replace the model's actuators with ours, so any mode fits; NativeGains
|
|
47
|
+
# drives the model's own actuators, so the mode must fit their kind.
|
|
48
|
+
if isinstance(gains, NativeGains):
|
|
49
|
+
fits = NATIVE_MODES[self.robot.actuator_kind]
|
|
50
|
+
if self.control.mode not in fits:
|
|
51
|
+
hint = (
|
|
52
|
+
f"the modes that fit are {list(fits)}" if fits else
|
|
53
|
+
"no mode fits: restrict robot.joints to joints with one kind of actuator, or use FixedGains"
|
|
54
|
+
)
|
|
55
|
+
raise ConfigError(
|
|
56
|
+
"control.mode",
|
|
57
|
+
f"mode {self.control.mode!r} does not fit the model's {self.robot.actuator_kind!r} actuators; {hint}",
|
|
58
|
+
)
|
|
59
|
+
elif isinstance(gains, NativeGains):
|
|
60
|
+
raise ConfigError("control.gains", "NativeGains drives a model's own actuators: it needs an ExternalRobot")
|
|
61
|
+
|
|
62
|
+
# --- resolution and hashing ----------------------------------------------------
|
|
63
|
+
|
|
64
|
+
def resolve(self) -> "EnvConfig":
|
|
65
|
+
"""A concrete config with region policies resolved into numbers. Idempotent.
|
|
66
|
+
|
|
67
|
+
Nothing needs resolving yet; later policies (family-reachable regions) will be turned into
|
|
68
|
+
numbers here, before hashing.
|
|
69
|
+
"""
|
|
70
|
+
return self
|
|
71
|
+
|
|
72
|
+
def hash_dict(self) -> dict[str, Any]:
|
|
73
|
+
r = self.resolve()
|
|
74
|
+
return {
|
|
75
|
+
"robot_hash": r.robot.full_hash,
|
|
76
|
+
"control": r.control.to_dict(),
|
|
77
|
+
"task": r.task.to_dict(),
|
|
78
|
+
"sim": r.sim.to_dict(),
|
|
79
|
+
}
|
|
80
|
+
|
|
81
|
+
@property
|
|
82
|
+
def full_env_hash(self) -> str:
|
|
83
|
+
return sha256_hex(canonical_json(self.hash_dict()))
|
|
84
|
+
|
|
85
|
+
@property
|
|
86
|
+
def env_hash(self) -> str:
|
|
87
|
+
return self.full_env_hash[:HASH_DISPLAY_LENGTH]
|
|
88
|
+
|
|
89
|
+
# --- serialization -------------------------------------------------------------
|
|
90
|
+
|
|
91
|
+
def to_dict(self) -> dict[str, Any]:
|
|
92
|
+
return {
|
|
93
|
+
"robot": self.robot.to_dict(),
|
|
94
|
+
"control": self.control.to_dict(),
|
|
95
|
+
"task": self.task.to_dict(),
|
|
96
|
+
"sim": self.sim.to_dict(),
|
|
97
|
+
}
|
|
98
|
+
|
|
99
|
+
@classmethod
|
|
100
|
+
def from_dict(cls, d: Mapping[str, Any]) -> "EnvConfig":
|
|
101
|
+
check_keys(d, {"robot", "control", "task", "sim"}, "")
|
|
102
|
+
return cls(
|
|
103
|
+
robot=within("robot", robot_from_dict, d["robot"]),
|
|
104
|
+
control=within("control", ControlConfig.from_dict, d["control"]),
|
|
105
|
+
task=within("task", TaskConfig.from_dict, d["task"]),
|
|
106
|
+
sim=within("sim", SimConfig.from_dict, d["sim"]),
|
|
107
|
+
)
|
|
@@ -0,0 +1,121 @@
|
|
|
1
|
+
"""Pure-numpy forward kinematics of a ``ParametricArm``: the engine-independent oracle.
|
|
2
|
+
|
|
3
|
+
A robot of kind ``ExternalRobot`` has no model here: ``forward_ee_batch`` and ``sample_joint_positions`` ask its robot
|
|
4
|
+
provider for an oracle (engine side, imported lazily) and fail with a ``ConfigError`` when the engine is missing.
|
|
5
|
+
|
|
6
|
+
Convention: ``T_0 = base`` (the robot base in the world: the task's ``mount``; identity when not given); for each segment ``F_i = T_i . joint_frame_i``,
|
|
7
|
+
``L_i = F_i . Rot(axis_i, q_i)``, ``tip_i = L_i . Pose(tip_offset_i, identity)``,
|
|
8
|
+
``T_{i+1} = tip_i``; ``ee = T_n . end_effector``.
|
|
9
|
+
"""
|
|
10
|
+
|
|
11
|
+
from __future__ import annotations
|
|
12
|
+
|
|
13
|
+
from dataclasses import dataclass
|
|
14
|
+
|
|
15
|
+
import numpy as np
|
|
16
|
+
|
|
17
|
+
from ..errors import ConfigError
|
|
18
|
+
from ..robot import ExternalRobot, ParametricArm, Pose
|
|
19
|
+
|
|
20
|
+
|
|
21
|
+
@dataclass(frozen=True)
|
|
22
|
+
class FK:
|
|
23
|
+
link_frames: np.ndarray # (n, 4, 4) world poses of the link frames (after the joint rotation)
|
|
24
|
+
tip_frames: np.ndarray # (n, 4, 4) world poses of each link's end frame
|
|
25
|
+
ee: np.ndarray # (4, 4) end-effector pose
|
|
26
|
+
|
|
27
|
+
|
|
28
|
+
def quat_to_matrix(q) -> np.ndarray:
|
|
29
|
+
w, x, y, z = q
|
|
30
|
+
return np.array(
|
|
31
|
+
[
|
|
32
|
+
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
|
33
|
+
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
|
34
|
+
[2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
|
|
35
|
+
]
|
|
36
|
+
)
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
def pose_to_matrix(pose: Pose) -> np.ndarray:
|
|
40
|
+
T = np.eye(4)
|
|
41
|
+
T[:3, :3] = quat_to_matrix(pose.quat)
|
|
42
|
+
T[:3, 3] = pose.pos
|
|
43
|
+
return T
|
|
44
|
+
|
|
45
|
+
|
|
46
|
+
def axis_angle_matrices(axis, angles: np.ndarray) -> np.ndarray:
|
|
47
|
+
"""Rotations about a unit ``axis`` by each of ``angles`` (B,) as (B, 4, 4) transforms."""
|
|
48
|
+
ax = np.asarray(axis, dtype=float)
|
|
49
|
+
K = np.array([[0, -ax[2], ax[1]], [ax[2], 0, -ax[0]], [-ax[1], ax[0], 0]])
|
|
50
|
+
s = np.sin(angles)[:, None, None]
|
|
51
|
+
c = np.cos(angles)[:, None, None]
|
|
52
|
+
T = np.zeros((len(angles), 4, 4))
|
|
53
|
+
T[:, :3, :3] = np.eye(3) + s * K + (1 - c) * (K @ K)
|
|
54
|
+
T[:, 3, 3] = 1.0
|
|
55
|
+
return T
|
|
56
|
+
|
|
57
|
+
|
|
58
|
+
def _forward_batch(arm: ParametricArm, q: np.ndarray, base: Pose | None = None):
|
|
59
|
+
q = np.asarray(q, dtype=float)
|
|
60
|
+
if q.ndim != 2 or q.shape[1] != arm.n:
|
|
61
|
+
raise ValueError(f"q must have shape (B, {arm.n}), got {q.shape}")
|
|
62
|
+
B = q.shape[0]
|
|
63
|
+
T = np.broadcast_to(np.eye(4) if base is None else pose_to_matrix(base), (B, 4, 4))
|
|
64
|
+
links, tips = [], []
|
|
65
|
+
for i, seg in enumerate(arm.segments):
|
|
66
|
+
F = T @ pose_to_matrix(seg.link.joint_frame)
|
|
67
|
+
L = F @ axis_angle_matrices(seg.joint.axis, q[:, i])
|
|
68
|
+
tip_local = np.eye(4)
|
|
69
|
+
tip_local[:3, 3] = seg.link.tip_offset
|
|
70
|
+
T = L @ tip_local
|
|
71
|
+
links.append(L)
|
|
72
|
+
tips.append(T)
|
|
73
|
+
ee = T @ pose_to_matrix(arm.end_effector)
|
|
74
|
+
return np.stack(links, axis=1), np.stack(tips, axis=1), ee
|
|
75
|
+
|
|
76
|
+
|
|
77
|
+
def _oracle(robot: ExternalRobot):
|
|
78
|
+
from .robot_providers import get_robot_provider
|
|
79
|
+
|
|
80
|
+
try:
|
|
81
|
+
return get_robot_provider(robot.provider).kinematics(robot)
|
|
82
|
+
except ImportError as e:
|
|
83
|
+
raise ConfigError(
|
|
84
|
+
"robot",
|
|
85
|
+
f"the kinematics of robot {robot.provider}/{robot.name} need the simulation engine "
|
|
86
|
+
f"({e.name or type(e).__name__} is missing): install the extra of robot provider {robot.provider!r} "
|
|
87
|
+
"or robotenv[env]",
|
|
88
|
+
) from e
|
|
89
|
+
|
|
90
|
+
|
|
91
|
+
def forward(arm: ParametricArm, q, base: Pose | None = None) -> FK:
|
|
92
|
+
"""Forward kinematics for one joint configuration ``q`` of shape ``(n,)``. Poses are in the frame the robot base
|
|
93
|
+
``base`` is given in (the task's ``mount``: the world); the base frame itself when ``base`` is ``None``.
|
|
94
|
+
``ParametricArm`` only: an ``ExternalRobot`` has no link frames here."""
|
|
95
|
+
if not isinstance(arm, ParametricArm):
|
|
96
|
+
raise TypeError(f"forward() needs a ParametricArm, got {type(arm).__name__}: it has no link frames here")
|
|
97
|
+
q = np.asarray(q, dtype=float)
|
|
98
|
+
if q.shape != (arm.n,):
|
|
99
|
+
raise ValueError(f"q must have shape ({arm.n},), got {q.shape}")
|
|
100
|
+
links, tips, ee = _forward_batch(arm, q[None], base)
|
|
101
|
+
return FK(link_frames=links[0], tip_frames=tips[0], ee=ee[0])
|
|
102
|
+
|
|
103
|
+
|
|
104
|
+
def forward_ee_batch(arm: ParametricArm | ExternalRobot, q_batch, base: Pose | None = None) -> np.ndarray:
|
|
105
|
+
"""End-effector positions, ``(B, n) -> (B, 3)``: in the world frame when ``base`` is the mount of the robot, in the
|
|
106
|
+
robot's base frame when it is ``None``. An ``ExternalRobot`` answers through its robot provider."""
|
|
107
|
+
if isinstance(arm, ExternalRobot):
|
|
108
|
+
return np.asarray(_oracle(arm).forward_ee_batch(q_batch, base), dtype=float)
|
|
109
|
+
return _forward_batch(arm, q_batch, base)[2][:, :3, 3]
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
def sample_joint_positions(arm: ParametricArm | ExternalRobot, num: int, rng: np.random.Generator) -> np.ndarray:
|
|
113
|
+
"""``(num, n)`` samples, uniform in each joint range and in ``[-pi, pi]`` for unlimited joints. An
|
|
114
|
+
``ExternalRobot`` answers through its robot provider."""
|
|
115
|
+
if isinstance(arm, ExternalRobot):
|
|
116
|
+
return np.asarray(_oracle(arm).sample_joint_positions(num, rng), dtype=float)
|
|
117
|
+
lo = np.empty(arm.n)
|
|
118
|
+
hi = np.empty(arm.n)
|
|
119
|
+
for i, seg in enumerate(arm.segments):
|
|
120
|
+
lo[i], hi[i] = (-np.pi, np.pi) if seg.joint.range is None else seg.joint.range
|
|
121
|
+
return rng.uniform(lo, hi, size=(num, arm.n))
|
robotenv/config/mount.py
ADDED
|
@@ -0,0 +1,172 @@
|
|
|
1
|
+
"""Where the robot base stands in a scene: a mount relative to the scene's mount frame. Numpy only.
|
|
2
|
+
|
|
3
|
+
The scene owns a *mount frame* (``TaskProvider.mount_frame(params)``, a world pose). A task adds only what differs
|
|
4
|
+
from it:
|
|
5
|
+
|
|
6
|
+
- :class:`Mount` for a fixed-base arm: ``offset``, a :class:`Pose` in the mount frame (identity: snapped exactly);
|
|
7
|
+
- :class:`MobileMount` for a robot with a floating base: ``spawn`` (x, y[, z]) in the mount frame and ``heading``
|
|
8
|
+
(yaw, rad, about the frame's +z; 0 faces the frame's +x). ``z`` is added to the robot's own home base height.
|
|
9
|
+
|
|
10
|
+
``world_mount`` is the only function that composes them: ``T_world_base = T_frame @ T_mount``.
|
|
11
|
+
"""
|
|
12
|
+
|
|
13
|
+
from __future__ import annotations
|
|
14
|
+
|
|
15
|
+
import math
|
|
16
|
+
from collections.abc import Mapping
|
|
17
|
+
from dataclasses import dataclass
|
|
18
|
+
from typing import Any
|
|
19
|
+
|
|
20
|
+
from .._canon import as_float, as_vector, check_keys, within
|
|
21
|
+
from ..errors import ConfigError
|
|
22
|
+
from ..robot import ExternalRobot, Pose
|
|
23
|
+
|
|
24
|
+
__all__ = ["MobileMount", "Mount", "is_mobile", "mount_from_dict", "mount_in", "mount_pose", "scene_frame", "snapped", "world_mount"]
|
|
25
|
+
|
|
26
|
+
_MIGRATION = (
|
|
27
|
+
"mounts are relative to the scene's mount frame now: write Mount(offset=Pose(...)) for a fixed-base arm "
|
|
28
|
+
"(Mount() snaps it onto the frame) or MobileMount(spawn=(x, y), heading=...) for a mobile robot"
|
|
29
|
+
)
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
@dataclass(frozen=True, slots=True)
|
|
33
|
+
class Mount:
|
|
34
|
+
"""A fixed-base robot: ``offset`` is the base relative to the scene's mount frame (identity: snapped exactly)."""
|
|
35
|
+
|
|
36
|
+
offset: Pose = Pose.identity()
|
|
37
|
+
|
|
38
|
+
def __post_init__(self):
|
|
39
|
+
if not isinstance(self.offset, Pose):
|
|
40
|
+
raise ConfigError("offset", "expected a Pose: the robot base in the scene's mount frame")
|
|
41
|
+
|
|
42
|
+
def to_dict(self) -> dict[str, Any]:
|
|
43
|
+
return {"kind": "mount", "offset": self.offset.to_dict()}
|
|
44
|
+
|
|
45
|
+
@classmethod
|
|
46
|
+
def from_dict(cls, d: Mapping[str, Any]) -> Mount:
|
|
47
|
+
check_keys(d, {"offset"}, "", optional={"kind"})
|
|
48
|
+
return cls(offset=within("offset", Pose.from_dict, d["offset"]))
|
|
49
|
+
|
|
50
|
+
|
|
51
|
+
@dataclass(frozen=True, slots=True)
|
|
52
|
+
class MobileMount:
|
|
53
|
+
"""A robot with a floating base: it spawns at ``spawn`` (m, in the mount frame; ``z`` is added to the robot's own
|
|
54
|
+
home base height, default 0) facing ``heading`` (rad, yaw about the frame's +z, 0 = the frame's +x)."""
|
|
55
|
+
|
|
56
|
+
spawn: tuple[float, ...] = (0.0, 0.0, 0.0)
|
|
57
|
+
heading: float = 0.0
|
|
58
|
+
|
|
59
|
+
def __post_init__(self):
|
|
60
|
+
spawn = self.spawn
|
|
61
|
+
if isinstance(spawn, (str, bytes)) or not hasattr(spawn, "__len__") or len(spawn) not in (2, 3):
|
|
62
|
+
raise ConfigError("spawn", "expected (x, y) or (x, y, z) in metres")
|
|
63
|
+
spawn = as_vector(spawn, len(spawn), "spawn")
|
|
64
|
+
object.__setattr__(self, "spawn", spawn if len(spawn) == 3 else (*spawn, 0.0))
|
|
65
|
+
object.__setattr__(self, "heading", as_float(self.heading, "heading"))
|
|
66
|
+
|
|
67
|
+
def to_dict(self) -> dict[str, Any]:
|
|
68
|
+
return {"kind": "mobile_mount", "spawn": list(self.spawn), "heading": self.heading}
|
|
69
|
+
|
|
70
|
+
@classmethod
|
|
71
|
+
def from_dict(cls, d: Mapping[str, Any]) -> MobileMount:
|
|
72
|
+
check_keys(d, {"spawn", "heading"}, "", optional={"kind"})
|
|
73
|
+
return cls(spawn=d["spawn"], heading=d["heading"])
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
def mount_from_dict(d: Any) -> Mount | MobileMount:
|
|
77
|
+
"""Inverse of ``to_dict``. The old absolute form (``pos`` / ``quat``) is an error that shows the replacement."""
|
|
78
|
+
if isinstance(d, Pose) or (isinstance(d, Mapping) and ({"pos", "quat"} & set(d))):
|
|
79
|
+
raise ConfigError("", f"an absolute pose is no longer a mount: {_MIGRATION}")
|
|
80
|
+
if not isinstance(d, Mapping):
|
|
81
|
+
raise ConfigError("", f"expected a mapping, got {type(d).__name__}")
|
|
82
|
+
kind = d.get("kind", "mount" if "offset" in d else None)
|
|
83
|
+
if kind == "mount":
|
|
84
|
+
return Mount.from_dict(d)
|
|
85
|
+
if kind == "mobile_mount":
|
|
86
|
+
return MobileMount.from_dict(d)
|
|
87
|
+
raise ConfigError("kind", f"unknown mount kind {kind!r}; expected 'mount' or 'mobile_mount'")
|
|
88
|
+
|
|
89
|
+
|
|
90
|
+
def check_mount(mount: Any) -> None:
|
|
91
|
+
"""Raise a ``ConfigError`` unless ``mount`` is a :class:`Mount` or a :class:`MobileMount`."""
|
|
92
|
+
if isinstance(mount, Pose):
|
|
93
|
+
raise ConfigError("mount", f"got a Pose: {_MIGRATION}")
|
|
94
|
+
if not isinstance(mount, (Mount, MobileMount)):
|
|
95
|
+
raise ConfigError("mount", f"expected Mount or MobileMount, got {type(mount).__name__}")
|
|
96
|
+
|
|
97
|
+
|
|
98
|
+
def is_mobile(robot: Any) -> bool:
|
|
99
|
+
"""Whether ``robot`` has a floating base (a model with a free joint, ``home_root`` set). A planar base built from
|
|
100
|
+
slide joints is part of the model's own joints and is not detected."""
|
|
101
|
+
return isinstance(robot, ExternalRobot) and robot.home_root is not None
|
|
102
|
+
|
|
103
|
+
|
|
104
|
+
def snapped(robot: Any) -> Mount | MobileMount:
|
|
105
|
+
"""The mount that snaps ``robot`` onto the scene's mount frame: ``Mount()`` for a fixed-base robot,
|
|
106
|
+
``MobileMount()`` (spawn at the frame, heading 0) for a floating-base one."""
|
|
107
|
+
return MobileMount() if is_mobile(robot) else Mount()
|
|
108
|
+
|
|
109
|
+
|
|
110
|
+
def check_mount_fits_robot(mount: Mount | MobileMount, robot: Any) -> None:
|
|
111
|
+
"""A fixed-base robot takes a :class:`Mount`, a floating-base one a :class:`MobileMount`."""
|
|
112
|
+
if is_mobile(robot) and isinstance(mount, Mount):
|
|
113
|
+
raise ConfigError(
|
|
114
|
+
"task.mount", f"robot {robot.name!r} has a floating base: use MobileMount(spawn=(x, y), heading=...)"
|
|
115
|
+
)
|
|
116
|
+
if not is_mobile(robot) and isinstance(mount, MobileMount):
|
|
117
|
+
raise ConfigError("task.mount", "MobileMount is for a robot with a floating base; a fixed-base robot takes Mount(offset=...)")
|
|
118
|
+
|
|
119
|
+
|
|
120
|
+
def _qmul(a, b):
|
|
121
|
+
aw, ax, ay, az = a
|
|
122
|
+
bw, bx, by, bz = b
|
|
123
|
+
return (
|
|
124
|
+
aw * bw - ax * bx - ay * by - az * bz,
|
|
125
|
+
aw * bx + ax * bw + ay * bz - az * by,
|
|
126
|
+
aw * by - ax * bz + ay * bw + az * bx,
|
|
127
|
+
aw * bz + ax * by - ay * bx + az * bw,
|
|
128
|
+
)
|
|
129
|
+
|
|
130
|
+
|
|
131
|
+
def _qrot(q, v):
|
|
132
|
+
w, x, y, z = q
|
|
133
|
+
t = (2 * (y * v[2] - z * v[1]), 2 * (z * v[0] - x * v[2]), 2 * (x * v[1] - y * v[0]))
|
|
134
|
+
return (
|
|
135
|
+
v[0] + w * t[0] + (y * t[2] - z * t[1]),
|
|
136
|
+
v[1] + w * t[1] + (z * t[0] - x * t[2]),
|
|
137
|
+
v[2] + w * t[2] + (x * t[1] - y * t[0]),
|
|
138
|
+
)
|
|
139
|
+
|
|
140
|
+
|
|
141
|
+
def compose(a: Pose, b: Pose) -> Pose:
|
|
142
|
+
"""``a @ b``: the pose ``b`` expressed in the frame ``a``, as a world pose."""
|
|
143
|
+
p = _qrot(a.quat, b.pos)
|
|
144
|
+
return Pose(tuple(a.pos[i] + p[i] for i in range(3)), _qmul(a.quat, b.quat))
|
|
145
|
+
|
|
146
|
+
|
|
147
|
+
def mount_pose(mount: Mount | MobileMount) -> Pose:
|
|
148
|
+
"""The mount as a pose in the scene's mount frame."""
|
|
149
|
+
if isinstance(mount, Mount):
|
|
150
|
+
return mount.offset
|
|
151
|
+
h = 0.5 * mount.heading
|
|
152
|
+
return Pose(mount.spawn, (math.cos(h), 0.0, 0.0, math.sin(h)))
|
|
153
|
+
|
|
154
|
+
|
|
155
|
+
def scene_frame(cfg: Any) -> Pose:
|
|
156
|
+
"""The scene's mount frame, as the task provider names it (a world pose). ``cfg`` is an ``EnvConfig`` or a ``TaskConfig``."""
|
|
157
|
+
from .providers import get_task_provider
|
|
158
|
+
|
|
159
|
+
task = getattr(cfg, "task", cfg)
|
|
160
|
+
return get_task_provider(task.params.provider).mount_frame(task.params)
|
|
161
|
+
|
|
162
|
+
|
|
163
|
+
def mount_in(frame: Pose, mount: Mount | MobileMount) -> Pose:
|
|
164
|
+
"""The robot base in the world, given the mount frame (the one the engine read from the model, or the provider's)."""
|
|
165
|
+
return compose(frame, mount_pose(mount))
|
|
166
|
+
|
|
167
|
+
|
|
168
|
+
def world_mount(cfg: Any) -> Pose:
|
|
169
|
+
"""The robot base in the world: the scene's mount frame composed with the mount. ``cfg`` is an ``EnvConfig`` or a
|
|
170
|
+
``TaskConfig``. This is the config-side pose (what ``viz`` and the kinematics oracle use); the simulator reads the
|
|
171
|
+
frame from the model's marker and checks that it agrees (``simulators.mujoco.mount``)."""
|
|
172
|
+
return mount_in(scene_frame(cfg), getattr(cfg, "task", cfg).mount)
|