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,35 @@
|
|
|
1
|
+
"""Scene presets. Robot elements and scene elements are kept separate."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from typing import Callable
|
|
6
|
+
|
|
7
|
+
import mujoco
|
|
8
|
+
|
|
9
|
+
|
|
10
|
+
def _empty(spec: mujoco.MjSpec) -> None:
|
|
11
|
+
pass
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
def _plain_floor(spec: mujoco.MjSpec) -> None:
|
|
15
|
+
# Scene geoms collide with robot geoms (contype 1) but not with each other.
|
|
16
|
+
spec.worldbody.add_geom(
|
|
17
|
+
name="floor",
|
|
18
|
+
type=mujoco.mjtGeom.mjGEOM_PLANE,
|
|
19
|
+
size=[5.0, 5.0, 0.1],
|
|
20
|
+
contype=0,
|
|
21
|
+
conaffinity=1,
|
|
22
|
+
)
|
|
23
|
+
spec.worldbody.add_light(name="light", pos=[0.0, 0.0, 3.0], dir=[0.0, 0.0, -1.0])
|
|
24
|
+
|
|
25
|
+
|
|
26
|
+
SCENES: dict[str, Callable[[mujoco.MjSpec], None]] = {
|
|
27
|
+
"empty": _empty,
|
|
28
|
+
"plain_floor": _plain_floor,
|
|
29
|
+
}
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
def apply_scene(spec: mujoco.MjSpec, name: str) -> None:
|
|
33
|
+
if name not in SCENES:
|
|
34
|
+
raise ValueError(f"unknown scene {name!r}; available: {sorted(SCENES)}")
|
|
35
|
+
SCENES[name](spec)
|
|
@@ -0,0 +1,174 @@
|
|
|
1
|
+
"""Batched simulation core: N independent ``MjData`` sharing one ``MjModel``.
|
|
2
|
+
|
|
3
|
+
Float64 throughout; conversion to float32 tensors happens in the env layer. The core owns no random
|
|
4
|
+
number generator: all randomness (noise, targets) lives in the env layer.
|
|
5
|
+
"""
|
|
6
|
+
|
|
7
|
+
from __future__ import annotations
|
|
8
|
+
|
|
9
|
+
import mujoco
|
|
10
|
+
import numpy as np
|
|
11
|
+
import torch
|
|
12
|
+
|
|
13
|
+
from ...config.kinematics import pose_to_matrix
|
|
14
|
+
from .compile import CompiledRobot
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
def _quat_mul(a: np.ndarray, b: np.ndarray) -> np.ndarray:
|
|
18
|
+
"""Hamilton product of quaternions (w, x, y, z), broadcasting over leading dimensions."""
|
|
19
|
+
aw, ax, ay, az = np.moveaxis(a, -1, 0)
|
|
20
|
+
bw, bx, by, bz = np.moveaxis(b, -1, 0)
|
|
21
|
+
return np.stack(
|
|
22
|
+
[
|
|
23
|
+
aw * bw - ax * bx - ay * by - az * bz,
|
|
24
|
+
aw * bx + ax * bw + ay * bz - az * by,
|
|
25
|
+
aw * by - ax * bz + ay * bw + az * bx,
|
|
26
|
+
aw * bz + ax * by - ay * bx + az * bw,
|
|
27
|
+
],
|
|
28
|
+
axis=-1,
|
|
29
|
+
)
|
|
30
|
+
|
|
31
|
+
|
|
32
|
+
class BatchedSim:
|
|
33
|
+
"""Implements ``tasks.base.SimView`` (``device``, ``base_frame``, ``body_pose``, ``site_pos``, ``write_qpos``,
|
|
34
|
+
``host_data``) next to its numpy accessors."""
|
|
35
|
+
|
|
36
|
+
device = torch.device("cpu")
|
|
37
|
+
|
|
38
|
+
def __init__(self, compiled: CompiledRobot, num_envs: int, n_substeps: int):
|
|
39
|
+
if num_envs < 1:
|
|
40
|
+
raise ValueError("num_envs must be >= 1")
|
|
41
|
+
if n_substeps < 1:
|
|
42
|
+
raise ValueError("n_substeps must be >= 1")
|
|
43
|
+
self.model = compiled.model
|
|
44
|
+
self.iface = compiled.interface
|
|
45
|
+
self.num_envs = num_envs
|
|
46
|
+
self.n_substeps = n_substeps
|
|
47
|
+
self.data: list[mujoco.MjData] = [mujoco.MjData(self.model) for _ in range(num_envs)]
|
|
48
|
+
T = pose_to_matrix(self.iface.mount)
|
|
49
|
+
self._base_R = T[:3, :3]
|
|
50
|
+
self._base_pos = T[:3, 3]
|
|
51
|
+
q = np.asarray(self.iface.mount.quat, dtype=float)
|
|
52
|
+
self._base_quat_inv = np.array([q[0], -q[1], -q[2], -q[3]]) / float(q @ q)
|
|
53
|
+
self.reset()
|
|
54
|
+
|
|
55
|
+
@property
|
|
56
|
+
def control_dt(self) -> float:
|
|
57
|
+
return self.n_substeps * self.model.opt.timestep
|
|
58
|
+
|
|
59
|
+
def reset(self, mask=None, qpos=None, qvel=None) -> None:
|
|
60
|
+
"""Reset the masked envs (all when ``mask`` is None) to ``qpos`` / ``qvel`` or home / rest.
|
|
61
|
+
|
|
62
|
+
``qpos`` and ``qvel`` are ``(N, n)``; only rows selected by the mask are read. Control, time and
|
|
63
|
+
warm-start state are cleared for the masked envs; the others are not touched.
|
|
64
|
+
"""
|
|
65
|
+
N, n, iface = self.num_envs, self.iface.n, self.iface
|
|
66
|
+
mask = np.ones(N, dtype=bool) if mask is None else np.asarray(mask, dtype=bool)
|
|
67
|
+
if mask.shape != (N,):
|
|
68
|
+
raise ValueError(f"mask must have shape ({N},), got {mask.shape}")
|
|
69
|
+
for name, arr in (("qpos", qpos), ("qvel", qvel)):
|
|
70
|
+
if arr is not None and np.shape(arr) != (N, n):
|
|
71
|
+
raise ValueError(f"{name} must have shape ({N}, {n}), got {np.shape(arr)}")
|
|
72
|
+
for i in np.flatnonzero(mask):
|
|
73
|
+
d = self.data[i]
|
|
74
|
+
mujoco.mj_resetData(self.model, d)
|
|
75
|
+
if iface.reset_qpos is not None: # an external robot: the model's own state first, the driven joints on top
|
|
76
|
+
d.qpos[:] = iface.reset_qpos
|
|
77
|
+
d.qpos[iface.qpos_adr] = iface.home_qpos if qpos is None else qpos[i]
|
|
78
|
+
if iface.root_qpos_adr is not None:
|
|
79
|
+
a = iface.root_qpos_adr
|
|
80
|
+
d.qpos[a : a + 7] = [*iface.home_root.pos, *iface.home_root.quat]
|
|
81
|
+
if iface.held_ctrl is not None:
|
|
82
|
+
d.ctrl[:] = iface.held_ctrl
|
|
83
|
+
if qvel is not None:
|
|
84
|
+
d.qvel[iface.qvel_adr] = qvel[i]
|
|
85
|
+
mujoco.mj_forward(self.model, d)
|
|
86
|
+
|
|
87
|
+
def set_ctrl(self, ctrl) -> None:
|
|
88
|
+
ctrl = np.asarray(ctrl, dtype=np.float64)
|
|
89
|
+
if self.iface.held_ctrl is not None and ctrl.shape == (self.num_envs, self.iface.n):
|
|
90
|
+
# An external robot: commands of the driven joints in joint order; the other actuators keep their ctrl.
|
|
91
|
+
full = np.tile(self.iface.held_ctrl, (self.num_envs, 1))
|
|
92
|
+
full[:, self.iface.actuator_ids] = ctrl
|
|
93
|
+
ctrl = full
|
|
94
|
+
if ctrl.shape != (self.num_envs, self.model.nu):
|
|
95
|
+
raise ValueError(f"ctrl must have shape ({self.num_envs}, {self.model.nu}), got {ctrl.shape}")
|
|
96
|
+
for d, c in zip(self.data, ctrl):
|
|
97
|
+
d.ctrl[:] = c
|
|
98
|
+
|
|
99
|
+
def step(self) -> None:
|
|
100
|
+
"""Advance ``n_substeps`` physics steps in every env with the current control."""
|
|
101
|
+
for d in self.data:
|
|
102
|
+
mujoco.mj_step(self.model, d, nstep=self.n_substeps)
|
|
103
|
+
|
|
104
|
+
@property
|
|
105
|
+
def qpos(self) -> np.ndarray:
|
|
106
|
+
return np.stack([d.qpos[self.iface.qpos_adr] for d in self.data])
|
|
107
|
+
|
|
108
|
+
@property
|
|
109
|
+
def qvel(self) -> np.ndarray:
|
|
110
|
+
return np.stack([d.qvel[self.iface.qvel_adr] for d in self.data])
|
|
111
|
+
|
|
112
|
+
@property
|
|
113
|
+
def gripper_opening(self) -> np.ndarray:
|
|
114
|
+
"""The gap between the finger surfaces, ``(N, 1)`` metres (the sum of the two finger slides)."""
|
|
115
|
+
adr = self.iface.gripper_qpos_adr
|
|
116
|
+
return np.stack([d.qpos[adr].sum(keepdims=True) for d in self.data])
|
|
117
|
+
|
|
118
|
+
@property
|
|
119
|
+
def actuator_force(self) -> np.ndarray:
|
|
120
|
+
return np.stack([d.actuator_force[self.iface.actuator_ids] for d in self.data])
|
|
121
|
+
|
|
122
|
+
@property
|
|
123
|
+
def time(self) -> np.ndarray:
|
|
124
|
+
return np.array([d.time for d in self.data])
|
|
125
|
+
|
|
126
|
+
def ee_pos_base(self) -> np.ndarray:
|
|
127
|
+
"""End-effector positions in the base frame, ``(N, 3)``."""
|
|
128
|
+
if self.iface.ee_site_id is None:
|
|
129
|
+
raise ValueError("this robot has no end effector")
|
|
130
|
+
world = np.stack([d.site_xpos[self.iface.ee_site_id] for d in self.data])
|
|
131
|
+
return (world - self._base_pos) @ self._base_R
|
|
132
|
+
|
|
133
|
+
# --- SimView -------------------------------------------------------------------------
|
|
134
|
+
|
|
135
|
+
def base_frame(self) -> tuple[np.ndarray, np.ndarray]:
|
|
136
|
+
"""``(pos (3,), R (3, 3))`` of the robot's base in the world, float64."""
|
|
137
|
+
return self._base_pos, self._base_R
|
|
138
|
+
|
|
139
|
+
@staticmethod
|
|
140
|
+
def _check_frame(frame: str) -> None:
|
|
141
|
+
if frame not in ("world", "base"):
|
|
142
|
+
raise ValueError(f"frame must be 'world' or 'base', got {frame!r}")
|
|
143
|
+
|
|
144
|
+
def body_pose(self, body_id: int, frame: str = "world") -> tuple[torch.Tensor, torch.Tensor]:
|
|
145
|
+
"""Position ``(N, 3)`` and quaternion ``(N, 4)`` (w x y z) of a body, float32."""
|
|
146
|
+
self._check_frame(frame)
|
|
147
|
+
pos = np.stack([d.xpos[body_id] for d in self.data])
|
|
148
|
+
quat = np.stack([d.xquat[body_id] for d in self.data])
|
|
149
|
+
if frame == "base":
|
|
150
|
+
pos, quat = (pos - self._base_pos) @ self._base_R, _quat_mul(self._base_quat_inv, quat)
|
|
151
|
+
return torch.from_numpy(pos).to(torch.float32), torch.from_numpy(quat).to(torch.float32)
|
|
152
|
+
|
|
153
|
+
def site_pos(self, site_id: int, frame: str = "world") -> torch.Tensor:
|
|
154
|
+
"""``(N, 3)`` float32."""
|
|
155
|
+
self._check_frame(frame)
|
|
156
|
+
pos = np.stack([d.site_xpos[site_id] for d in self.data])
|
|
157
|
+
if frame == "base":
|
|
158
|
+
pos = (pos - self._base_pos) @ self._base_R
|
|
159
|
+
return torch.from_numpy(pos).to(torch.float32)
|
|
160
|
+
|
|
161
|
+
def write_qpos(self, adr: int, values, mask) -> None:
|
|
162
|
+
"""Set ``qpos[adr : adr + k]`` of the envs where ``mask`` is true to ``values`` ``(N, k)``, then recompute
|
|
163
|
+
the derived quantities of those envs."""
|
|
164
|
+
mask = torch.as_tensor(mask, dtype=torch.bool).cpu().numpy()
|
|
165
|
+
values = torch.as_tensor(values).to("cpu", torch.float64).numpy()
|
|
166
|
+
if mask.shape != (self.num_envs,) or values.ndim != 2 or len(values) != self.num_envs:
|
|
167
|
+
raise ValueError(f"mask must have shape ({self.num_envs},) and values (N, k)")
|
|
168
|
+
for i in np.flatnonzero(mask):
|
|
169
|
+
self.data[i].qpos[adr : adr + values.shape[1]] = values[i]
|
|
170
|
+
mujoco.mj_forward(self.model, self.data[i])
|
|
171
|
+
|
|
172
|
+
def host_data(self, env_index: int) -> mujoco.MjData:
|
|
173
|
+
"""The live ``MjData`` of one env. Read-only for the caller."""
|
|
174
|
+
return self.data[env_index]
|
|
@@ -0,0 +1,100 @@
|
|
|
1
|
+
"""MuJoCo Warp (GPU) simulator. Importing this module never imports warp, torch or mujoco."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from typing import Any
|
|
6
|
+
|
|
7
|
+
from ..base import Capabilities, Problem
|
|
8
|
+
|
|
9
|
+
|
|
10
|
+
def _installed(dist: str) -> str | None:
|
|
11
|
+
from importlib import metadata
|
|
12
|
+
|
|
13
|
+
try:
|
|
14
|
+
return metadata.version(dist)
|
|
15
|
+
except metadata.PackageNotFoundError:
|
|
16
|
+
return None
|
|
17
|
+
|
|
18
|
+
|
|
19
|
+
class WarpSimulator:
|
|
20
|
+
name = "warp"
|
|
21
|
+
|
|
22
|
+
def capabilities(self) -> Capabilities:
|
|
23
|
+
# What the CPU-versus-Warp parity tests (tests/test_warp_parity.py) have shown to match.
|
|
24
|
+
return Capabilities(
|
|
25
|
+
control_modes=frozenset({"joint_pos_abs", "joint_pos_delta", "joint_torque", "joint_vel"}),
|
|
26
|
+
devices=frozenset({"cuda"}),
|
|
27
|
+
robot_kinds=frozenset({"parametric", "external"}),
|
|
28
|
+
integrators=frozenset({"euler", "implicit", "implicitfast", "rk4"}),
|
|
29
|
+
)
|
|
30
|
+
|
|
31
|
+
def validate(self, cfg: Any) -> list[Problem]:
|
|
32
|
+
problems: list[Problem] = []
|
|
33
|
+
if type(cfg.robot).__name__ == "ExternalRobot":
|
|
34
|
+
problems.extend(self._external_problems(cfg))
|
|
35
|
+
if getattr(cfg.robot, "gripper", None) is not None:
|
|
36
|
+
problems.append(
|
|
37
|
+
Problem("robot.gripper", "the warp simulator does not build grippers yet; use the mujoco simulator")
|
|
38
|
+
)
|
|
39
|
+
mujoco_version, warp_version = _installed("mujoco"), _installed("mujoco-warp")
|
|
40
|
+
if warp_version is None:
|
|
41
|
+
problems.append(
|
|
42
|
+
Problem("sim.simulator", "mujoco_warp is not installed; install the extra: pip install 'robotenv[warp]'")
|
|
43
|
+
)
|
|
44
|
+
elif mujoco_version is not None and warp_version != mujoco_version:
|
|
45
|
+
problems.append(
|
|
46
|
+
Problem(
|
|
47
|
+
"sim.simulator",
|
|
48
|
+
f"mujoco-warp {warp_version} does not match mujoco {mujoco_version}; install one release of both",
|
|
49
|
+
)
|
|
50
|
+
)
|
|
51
|
+
try:
|
|
52
|
+
from ..mujoco.scenes import SCENES
|
|
53
|
+
except ImportError:
|
|
54
|
+
problems.append(Problem("sim.scene", "cannot check the scene: mujoco is not installed"))
|
|
55
|
+
else:
|
|
56
|
+
if cfg.sim.scene not in SCENES:
|
|
57
|
+
problems.append(Problem("sim.scene", f"unknown scene {cfg.sim.scene!r}; available: {sorted(SCENES)}"))
|
|
58
|
+
problems.extend(self._device_problems(cfg.sim.device))
|
|
59
|
+
return problems
|
|
60
|
+
|
|
61
|
+
@staticmethod
|
|
62
|
+
def _external_problems(cfg: Any) -> list[Problem]:
|
|
63
|
+
"""What the installed MuJoCo Warp does not run in the model of an external robot, at the path ``robot``. The
|
|
64
|
+
model is the robot provider's, attached to the scene preset: the task's own assets are checked at build time."""
|
|
65
|
+
try:
|
|
66
|
+
from ..mujoco.compile import compile_robot
|
|
67
|
+
from ..mujoco.external import problems_of
|
|
68
|
+
from .sim import check_model_supported
|
|
69
|
+
except ImportError:
|
|
70
|
+
return [] # a missing library is reported below
|
|
71
|
+
opened = problems_of(cfg) # the provider's checks, as for the mujoco simulator
|
|
72
|
+
if opened:
|
|
73
|
+
return opened
|
|
74
|
+
try:
|
|
75
|
+
model = compile_robot(cfg).model
|
|
76
|
+
except Exception as e: # not a feature of the robot: the other checks say what is wrong
|
|
77
|
+
return [Problem("robot", f"the model could not be compiled: {type(e).__name__}: {e}")]
|
|
78
|
+
return [Problem("robot", p.message) for p in check_model_supported(model)]
|
|
79
|
+
|
|
80
|
+
@staticmethod
|
|
81
|
+
def _device_problems(device: str) -> list[Problem]:
|
|
82
|
+
if not device.startswith("cuda"):
|
|
83
|
+
return [] # reported by the capability check
|
|
84
|
+
try:
|
|
85
|
+
import torch
|
|
86
|
+
except ImportError:
|
|
87
|
+
return [Problem("sim.device", f"cannot use {device!r}: torch is not installed")]
|
|
88
|
+
if not torch.cuda.is_available():
|
|
89
|
+
return [Problem("sim.device", f"CUDA is not available, so device {device!r} cannot be used")]
|
|
90
|
+
index, count = int(device.split(":")[1]), torch.cuda.device_count()
|
|
91
|
+
if index >= count:
|
|
92
|
+
return [Problem("sim.device", f"device {device!r} is not visible; {count} CUDA device(s) found")]
|
|
93
|
+
return []
|
|
94
|
+
|
|
95
|
+
def make_env(self, cfg: Any):
|
|
96
|
+
from ...config.providers import get_task_provider
|
|
97
|
+
from .env import WarpEnv
|
|
98
|
+
|
|
99
|
+
provider = get_task_provider(cfg.task.params.provider)
|
|
100
|
+
return WarpEnv(cfg, provider.build(cfg, self.name))
|
|
@@ -0,0 +1,235 @@
|
|
|
1
|
+
"""The MuJoCo Warp simulator's environment: the GPU twin of ``MujocoEnv``.
|
|
2
|
+
|
|
3
|
+
Same specs, keys, control law, masked reset and seeding rules, on ``cfg.sim.device``, driven by ``WarpBatchedSim`` and
|
|
4
|
+
any task written against ``SimView``. Both random streams are CPU generators seeded exactly as in ``MujocoEnv``; the
|
|
5
|
+
draws are moved to the device, so the same seed and ``num_envs`` give the same draws as the CPU simulator.
|
|
6
|
+
"""
|
|
7
|
+
|
|
8
|
+
from __future__ import annotations
|
|
9
|
+
|
|
10
|
+
import numpy as np
|
|
11
|
+
import torch
|
|
12
|
+
from tensordict import TensorDict, TensorDictBase
|
|
13
|
+
from torchrl.data import Binary, Bounded, Composite, Unbounded
|
|
14
|
+
from torchrl.envs import EnvBase
|
|
15
|
+
|
|
16
|
+
from ...config import ConfigError, EnvConfig
|
|
17
|
+
from ...tasks.base import mount_site_of as _mount_site
|
|
18
|
+
from ...envs.spaces import SpacesMixin
|
|
19
|
+
from ..base import task_seed
|
|
20
|
+
from ..mujoco.compile import compile_robot
|
|
21
|
+
from .sim import WarpBatchedSim, check_model_supported
|
|
22
|
+
|
|
23
|
+
|
|
24
|
+
class WarpEnv(SpacesMixin, EnvBase):
|
|
25
|
+
"""Batched TorchRL environment: ``cfg.robot`` under ``cfg.control`` doing ``task`` in MuJoCo Warp."""
|
|
26
|
+
|
|
27
|
+
batch_locked = True
|
|
28
|
+
|
|
29
|
+
def __init__(self, cfg: EnvConfig, task):
|
|
30
|
+
cfg = cfg.resolve()
|
|
31
|
+
self.cfg = cfg
|
|
32
|
+
self.robot_hash = cfg.robot.config_hash
|
|
33
|
+
self.env_hash = cfg.env_hash
|
|
34
|
+
N = cfg.sim.num_envs
|
|
35
|
+
device = torch.device(cfg.sim.device)
|
|
36
|
+
super().__init__(device=device, batch_size=torch.Size([N]))
|
|
37
|
+
|
|
38
|
+
self._task = task
|
|
39
|
+
self._compiled = compile_robot(cfg, scene_hook=task.attach, mount_site=_mount_site(task))
|
|
40
|
+
self._iface = self._compiled.interface
|
|
41
|
+
self.world_mount = self._iface.mount # the base in the world, read from the compiled model; recorded with env_hash
|
|
42
|
+
self._sim = WarpBatchedSim(self._compiled, N, cfg.sim.n_substeps, cfg.sim.device, cfg.sim.options)
|
|
43
|
+
self._renderer = None # created by the first render()
|
|
44
|
+
n = self._iface.n
|
|
45
|
+
self._n = n
|
|
46
|
+
task.bind(self._sim)
|
|
47
|
+
problems = check_model_supported(self._sim.model)
|
|
48
|
+
if problems:
|
|
49
|
+
lines = "\n".join(f" - {p}" for p in problems)
|
|
50
|
+
raise ConfigError("", f"simulator 'warp' cannot run this model ({len(problems)} problems):\n{lines}")
|
|
51
|
+
self._sim.finalize()
|
|
52
|
+
|
|
53
|
+
f32 = dict(dtype=torch.float32, device=device)
|
|
54
|
+
self._mode = cfg.control.mode
|
|
55
|
+
self._range = torch.as_tensor(self._iface.joint_range, **f32)
|
|
56
|
+
self._q_target = torch.zeros(N, n, **f32)
|
|
57
|
+
if self._mode in ("joint_pos_delta", "joint_vel"):
|
|
58
|
+
self._scale = torch.full((n,), float(cfg.control.action_scale), **f32)
|
|
59
|
+
self._offset = torch.zeros(n, **f32)
|
|
60
|
+
elif self._mode == "joint_pos_abs":
|
|
61
|
+
# [-1, 1] spans the actuator's ctrlrange for an external robot (its own servo), the joint range otherwise
|
|
62
|
+
rng = self._iface.joint_range if self._iface.ctrl_range is None else self._iface.ctrl_range
|
|
63
|
+
lo, hi = rng[:, 0], rng[:, 1]
|
|
64
|
+
finite = np.isfinite(lo) & np.isfinite(hi)
|
|
65
|
+
lo, hi = np.where(finite, lo, 0.0), np.where(finite, hi, 0.0) # no inf - inf below
|
|
66
|
+
self._scale = torch.as_tensor(np.where(finite, (hi - lo) / 2.0, np.pi), **f32)
|
|
67
|
+
self._offset = torch.as_tensor(np.where(finite, (hi + lo) / 2.0, 0.0), **f32)
|
|
68
|
+
else: # joint_torque
|
|
69
|
+
scale = self._iface.effort_limit.copy()
|
|
70
|
+
if self._iface.ctrl_range is not None: # an external robot: the ends of the actuator's ctrlrange
|
|
71
|
+
top = np.abs(self._iface.ctrl_range).max(axis=1)
|
|
72
|
+
scale = np.where(np.isfinite(top), top, scale)
|
|
73
|
+
self._scale = torch.as_tensor(scale, **f32)
|
|
74
|
+
self._offset = torch.zeros(n, **f32)
|
|
75
|
+
|
|
76
|
+
params = cfg.task.params
|
|
77
|
+
self._max_episode_steps = cfg.task.max_episode_steps
|
|
78
|
+
self._reset_noise = float(getattr(params, "reset_noise", 0.0))
|
|
79
|
+
self._terminate_on_success = bool(getattr(params, "terminate_on_success", False))
|
|
80
|
+
|
|
81
|
+
# Two seeded CPU streams, as in MujocoEnv: the robot's reset noise, and the task's own draws.
|
|
82
|
+
self._gen = torch.Generator(device="cpu")
|
|
83
|
+
self._gen.manual_seed(cfg.sim.seed)
|
|
84
|
+
self._task_gen = torch.Generator(device="cpu")
|
|
85
|
+
self._task_gen.manual_seed(task_seed(cfg.sim.seed))
|
|
86
|
+
self._episode_step = torch.zeros(N, 1, dtype=torch.int64, device=device)
|
|
87
|
+
|
|
88
|
+
f32 = torch.float32
|
|
89
|
+
self.observation_spec = Composite(
|
|
90
|
+
task=task.observation_spec(N).to(device),
|
|
91
|
+
proprio=Composite(
|
|
92
|
+
qpos=Unbounded(shape=(N, n), dtype=f32, device=device),
|
|
93
|
+
qvel=Unbounded(shape=(N, n), dtype=f32, device=device),
|
|
94
|
+
effort=Unbounded(shape=(N, n), dtype=f32, device=device),
|
|
95
|
+
shape=(N,),
|
|
96
|
+
device=device,
|
|
97
|
+
),
|
|
98
|
+
success=Binary(shape=(N, 1), dtype=torch.bool, device=device),
|
|
99
|
+
episode_step=Unbounded(shape=(N, 1), dtype=torch.int64, device=device),
|
|
100
|
+
shape=(N,),
|
|
101
|
+
device=device,
|
|
102
|
+
)
|
|
103
|
+
self.action_spec = Bounded(low=-1.0, high=1.0, shape=(N, n), dtype=f32, device=device)
|
|
104
|
+
self.reward_spec = Unbounded(shape=(N, 1), dtype=f32, device=device)
|
|
105
|
+
self.done_spec = Composite(
|
|
106
|
+
done=Binary(shape=(N, 1), dtype=torch.bool, device=device),
|
|
107
|
+
terminated=Binary(shape=(N, 1), dtype=torch.bool, device=device),
|
|
108
|
+
truncated=Binary(shape=(N, 1), dtype=torch.bool, device=device),
|
|
109
|
+
shape=(N,),
|
|
110
|
+
device=device,
|
|
111
|
+
)
|
|
112
|
+
|
|
113
|
+
@property
|
|
114
|
+
def q_target(self) -> np.ndarray:
|
|
115
|
+
"""Position targets, ``(N, n)`` float64 on the host; meaningful in ``joint_pos_delta`` mode."""
|
|
116
|
+
return self._q_target.detach().cpu().numpy().astype(np.float64)
|
|
117
|
+
|
|
118
|
+
@property
|
|
119
|
+
def interface(self):
|
|
120
|
+
return self._iface
|
|
121
|
+
|
|
122
|
+
@property
|
|
123
|
+
def task(self):
|
|
124
|
+
"""The engine side of the task (``tasks.base.TaskImpl``)."""
|
|
125
|
+
return self._task
|
|
126
|
+
|
|
127
|
+
# --- sampling -------------------------------------------------------------------
|
|
128
|
+
|
|
129
|
+
def _rand(self, *shape, dtype=torch.float64) -> torch.Tensor:
|
|
130
|
+
return torch.rand(*shape, generator=self._gen, dtype=dtype)
|
|
131
|
+
|
|
132
|
+
def _task_rand(self, *shape, dtype=torch.float64) -> torch.Tensor:
|
|
133
|
+
return torch.rand(*shape, generator=self._task_gen, dtype=dtype)
|
|
134
|
+
|
|
135
|
+
# --- observations ---------------------------------------------------------------
|
|
136
|
+
|
|
137
|
+
def _observe(self) -> tuple[TensorDict, dict]:
|
|
138
|
+
s = self._sim
|
|
139
|
+
task_obs = self._task.observe(s)
|
|
140
|
+
out = TensorDict(
|
|
141
|
+
{
|
|
142
|
+
"task": TensorDict(dict(task_obs), batch_size=self.batch_size, device=self.device),
|
|
143
|
+
"proprio": TensorDict(
|
|
144
|
+
{"qpos": s.qpos, "qvel": s.qvel, "effort": s.actuator_force},
|
|
145
|
+
batch_size=self.batch_size,
|
|
146
|
+
device=self.device,
|
|
147
|
+
),
|
|
148
|
+
"success": self._task.success(s, task_obs),
|
|
149
|
+
"episode_step": self._episode_step.clone(),
|
|
150
|
+
},
|
|
151
|
+
batch_size=self.batch_size,
|
|
152
|
+
device=self.device,
|
|
153
|
+
)
|
|
154
|
+
return out, task_obs
|
|
155
|
+
|
|
156
|
+
# --- EnvBase interface ----------------------------------------------------------
|
|
157
|
+
|
|
158
|
+
def _reset(self, tensordict: TensorDictBase | None = None, **kwargs) -> TensorDictBase:
|
|
159
|
+
N, n = self.batch_size[0], self._n
|
|
160
|
+
mask = torch.ones(N, dtype=torch.bool, device=self.device)
|
|
161
|
+
if tensordict is not None and tensordict.get("_reset", None) is not None:
|
|
162
|
+
mask = tensordict.get("_reset").reshape(N).to(self.device, torch.bool)
|
|
163
|
+
|
|
164
|
+
def injected(key):
|
|
165
|
+
if tensordict is not None and tensordict.get(key, None) is not None:
|
|
166
|
+
return tensordict.get(key)
|
|
167
|
+
return None
|
|
168
|
+
|
|
169
|
+
# Random draws always cover the whole batch so that rng use does not depend on the mask. The arithmetic is the
|
|
170
|
+
# CPU env's (float64 on the host); only the result is stored as float32 on the device.
|
|
171
|
+
noise = (self._rand(N, n) * 2.0 - 1.0) * self._reset_noise
|
|
172
|
+
qpos = torch.from_numpy(self._iface.home_qpos).expand(N, n) + noise
|
|
173
|
+
qpos = torch.from_numpy(np.clip(qpos.numpy(), self._iface.joint_range[:, 0], self._iface.joint_range[:, 1]))
|
|
174
|
+
qpos_in, qvel_in = injected("qpos"), injected("qvel")
|
|
175
|
+
if qpos_in is not None:
|
|
176
|
+
qpos = qpos_in.to(torch.float64)
|
|
177
|
+
|
|
178
|
+
self._sim.reset(mask=mask, qpos=qpos, qvel=qvel_in)
|
|
179
|
+
if self._mode == "joint_pos_delta":
|
|
180
|
+
self._q_target[mask] = self._sim.qpos[mask]
|
|
181
|
+
self._task.reset(self._sim, mask, self._task_rand, injected)
|
|
182
|
+
self._episode_step[mask] = 0
|
|
183
|
+
|
|
184
|
+
out, _ = self._observe()
|
|
185
|
+
for key in ("done", "terminated", "truncated"):
|
|
186
|
+
prev = injected(key)
|
|
187
|
+
flags = torch.zeros(N, 1, dtype=torch.bool, device=self.device)
|
|
188
|
+
if prev is not None:
|
|
189
|
+
flags = prev.to(self.device, torch.bool).clone()
|
|
190
|
+
flags[mask] = False
|
|
191
|
+
out.set(key, flags)
|
|
192
|
+
return out
|
|
193
|
+
|
|
194
|
+
def _step(self, tensordict: TensorDictBase) -> TensorDictBase:
|
|
195
|
+
action = tensordict.get("action").to(self.device, torch.float32).clamp(-1.0, 1.0)
|
|
196
|
+
if self._mode == "joint_pos_delta":
|
|
197
|
+
lo, hi = self._range[:, 0], self._range[:, 1]
|
|
198
|
+
self._q_target = torch.minimum(torch.maximum(self._q_target + action * self._scale, lo), hi)
|
|
199
|
+
self._sim.set_ctrl(self._q_target)
|
|
200
|
+
else:
|
|
201
|
+
self._sim.set_ctrl(action * self._scale + self._offset)
|
|
202
|
+
self._sim.step()
|
|
203
|
+
self._sim.check_overflow()
|
|
204
|
+
self._episode_step += 1
|
|
205
|
+
|
|
206
|
+
out, task_obs = self._observe()
|
|
207
|
+
success = out["success"]
|
|
208
|
+
truncated = self._episode_step >= self._max_episode_steps
|
|
209
|
+
terminated = success.clone() if self._terminate_on_success else torch.zeros_like(success)
|
|
210
|
+
out.set("reward", self._task.reward(self._sim, task_obs))
|
|
211
|
+
out.set("terminated", terminated)
|
|
212
|
+
out.set("truncated", truncated)
|
|
213
|
+
out.set("done", terminated | truncated)
|
|
214
|
+
return out
|
|
215
|
+
|
|
216
|
+
def _set_seed(self, seed: int | None):
|
|
217
|
+
if seed is not None:
|
|
218
|
+
self._gen.manual_seed(int(seed))
|
|
219
|
+
self._task_gen.manual_seed(task_seed(int(seed)))
|
|
220
|
+
return seed
|
|
221
|
+
|
|
222
|
+
# --- rendering (needs an OpenGL context: MUJOCO_GL=egl on headless machines) ----------------------------
|
|
223
|
+
|
|
224
|
+
def render(self, env_index: int = 0, *, camera=None, size: tuple[int, int] = (240, 320), overlays: bool = True):
|
|
225
|
+
"""Render one env to ``uint8 (H, W, 3)`` from a host copy of its state; see ``MujocoEnv.render``."""
|
|
226
|
+
from ..mujoco.render import get_renderer
|
|
227
|
+
|
|
228
|
+
return get_renderer(self).render(env_index, camera=camera, size=size, overlays=overlays)
|
|
229
|
+
|
|
230
|
+
def close(self, *, raise_if_closed: bool = True):
|
|
231
|
+
renderer, self._renderer = getattr(self, "_renderer", None), None
|
|
232
|
+
if renderer is not None:
|
|
233
|
+
renderer.close()
|
|
234
|
+
self._sim.close()
|
|
235
|
+
super().close(raise_if_closed=raise_if_closed)
|