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,242 @@
|
|
|
1
|
+
"""Compile a ``ParametricArm`` into a MuJoCo model and a simulator-neutral ``RobotInterface``."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
from dataclasses import dataclass
|
|
6
|
+
|
|
7
|
+
import mujoco
|
|
8
|
+
import numpy as np
|
|
9
|
+
|
|
10
|
+
from ...config import EnvConfig, FixedGains, InertiaScaledGains, Pose
|
|
11
|
+
from .arm_spec import add_arm, add_gripper
|
|
12
|
+
from .mount import resolve_world_mount
|
|
13
|
+
from .scenes import apply_scene
|
|
14
|
+
|
|
15
|
+
_INTEGRATORS = {
|
|
16
|
+
"euler": mujoco.mjtIntegrator.mjINT_EULER,
|
|
17
|
+
"implicit": mujoco.mjtIntegrator.mjINT_IMPLICIT,
|
|
18
|
+
"implicitfast": mujoco.mjtIntegrator.mjINT_IMPLICITFAST,
|
|
19
|
+
"rk4": mujoco.mjtIntegrator.mjINT_RK4,
|
|
20
|
+
}
|
|
21
|
+
_LIMITED = mujoco.mjtLimited.mjLIMITED_TRUE
|
|
22
|
+
_UNLIMITED = mujoco.mjtLimited.mjLIMITED_FALSE
|
|
23
|
+
|
|
24
|
+
|
|
25
|
+
@dataclass(frozen=True)
|
|
26
|
+
class RobotInterface:
|
|
27
|
+
"""What tasks and wrappers need to know about the compiled robot. Nothing engine-specific by name."""
|
|
28
|
+
|
|
29
|
+
n: int
|
|
30
|
+
joint_names: tuple[str, ...]
|
|
31
|
+
qpos_adr: np.ndarray # (n,) int
|
|
32
|
+
qvel_adr: np.ndarray # (n,) int
|
|
33
|
+
actuator_ids: np.ndarray # (n,) int
|
|
34
|
+
ee_site_id: int | None # None for an external robot without an end effector
|
|
35
|
+
joint_range: np.ndarray # (n, 2); -inf / +inf where unlimited
|
|
36
|
+
effort_limit: np.ndarray # (n,)
|
|
37
|
+
home_qpos: np.ndarray # (n,)
|
|
38
|
+
mount: Pose # the robot base in the world: the scene's mount marker (or frame) composed with the task's mount
|
|
39
|
+
kp: np.ndarray | None # (n,) effective position gains; None for torque control
|
|
40
|
+
kd: np.ndarray | None # (n,) effective damping / velocity gains; None for torque control
|
|
41
|
+
# Gripper (``arm.gripper``); ``None`` / 0.0 when the arm has none. The actuator is the last one (``act_gripper``).
|
|
42
|
+
gripper_actuator_id: int | None = None
|
|
43
|
+
gripper_qpos_adr: np.ndarray | None = None # (2,) int, the two finger joints
|
|
44
|
+
gripper_max_opening: float = 0.0 # metres, the control range of the actuator is [0, gripper_max_opening]
|
|
45
|
+
tcp_site_id: int | None = None
|
|
46
|
+
# External robots (``ExternalRobot``); ``None`` for a ``ParametricArm``. The Warp simulator ignores them.
|
|
47
|
+
ctrl_range: np.ndarray | None = None # (n, 2) ctrlrange of the driven actuators; -inf / +inf where unlimited
|
|
48
|
+
root_qpos_adr: int | None = None # first of the 7 qpos of a free root joint
|
|
49
|
+
home_root: Pose | None = None # the root body at home in the world: mount composed with ``ExternalRobot.home_root``
|
|
50
|
+
reset_qpos: np.ndarray | None = None # (nq,) the state of the model at reset: the keyframe, mapped onto this model
|
|
51
|
+
held_ctrl: np.ndarray | None = None # (nu,) ctrl of the actuators that are not driven (the keyframe's), 0 where driven
|
|
52
|
+
|
|
53
|
+
@property
|
|
54
|
+
def has_gripper(self) -> bool:
|
|
55
|
+
return self.gripper_actuator_id is not None
|
|
56
|
+
|
|
57
|
+
@property
|
|
58
|
+
def action_dim(self) -> int:
|
|
59
|
+
"""Joint commands plus one gripper command when there is a gripper."""
|
|
60
|
+
return self.n + (1 if self.has_gripper else 0)
|
|
61
|
+
|
|
62
|
+
|
|
63
|
+
@dataclass(frozen=True)
|
|
64
|
+
class CompiledRobot:
|
|
65
|
+
model: mujoco.MjModel
|
|
66
|
+
spec: mujoco.MjSpec
|
|
67
|
+
interface: RobotInterface
|
|
68
|
+
|
|
69
|
+
|
|
70
|
+
def _per_joint(value, n: int) -> list[float]:
|
|
71
|
+
if isinstance(value, tuple):
|
|
72
|
+
return list(value)
|
|
73
|
+
return [float(value)] * n
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
SUPPORTED_MODES = ("joint_pos_abs", "joint_pos_delta", "joint_torque", "joint_vel")
|
|
77
|
+
|
|
78
|
+
|
|
79
|
+
def inertia_scaled_gains(model: mujoco.MjModel, iface_home: np.ndarray, qpos_adr, qvel_adr, joint_damping, gains):
|
|
80
|
+
"""``kp_i = w^2 M_ii``, ``kd_i = max(2 zeta sqrt(kp_i M_ii) - damping_i, 0)`` with ``M`` at home."""
|
|
81
|
+
data = mujoco.MjData(model)
|
|
82
|
+
data.qpos[qpos_adr] = iface_home
|
|
83
|
+
mujoco.mj_forward(model, data)
|
|
84
|
+
M = np.zeros((model.nv, model.nv))
|
|
85
|
+
try: # newer MuJoCo: mj_fullM(model, data, dst)
|
|
86
|
+
mujoco.mj_fullM(model, data, M)
|
|
87
|
+
except TypeError: # older MuJoCo: mj_fullM(model, dst, qM)
|
|
88
|
+
mujoco.mj_fullM(model, M, data.qM)
|
|
89
|
+
m_diag = M[qvel_adr, qvel_adr]
|
|
90
|
+
kp = gains.bandwidth**2 * m_diag
|
|
91
|
+
kd = np.maximum(2.0 * gains.damping_ratio * np.sqrt(kp * m_diag) - joint_damping, 0.0)
|
|
92
|
+
return kp, kd
|
|
93
|
+
|
|
94
|
+
|
|
95
|
+
def _build_spec(cfg: EnvConfig, kp: list[float], kd: list[float], scene_hook=None, mount: Pose | None = None) -> mujoco.MjSpec:
|
|
96
|
+
arm = cfg.robot
|
|
97
|
+
control = cfg.control
|
|
98
|
+
|
|
99
|
+
spec = mujoco.MjSpec()
|
|
100
|
+
spec.compiler.degree = False # MjSpec reads angles as degrees by default; all angles here are radians
|
|
101
|
+
spec.option.timestep = cfg.sim.dt
|
|
102
|
+
spec.option.integrator = _INTEGRATORS[cfg.sim.integrator]
|
|
103
|
+
apply_scene(spec, cfg.sim.scene)
|
|
104
|
+
if scene_hook is not None: # scene elements the task adds (arena, objects), before the robot
|
|
105
|
+
scene_hook(spec)
|
|
106
|
+
|
|
107
|
+
self_collision = bool(getattr(cfg.sim.options, "self_collision", False))
|
|
108
|
+
conaffinity = 1 if self_collision else 0
|
|
109
|
+
|
|
110
|
+
base = spec.worldbody.add_body(
|
|
111
|
+
name="base", pos=list(mount.pos), quat=list(mount.quat)
|
|
112
|
+
)
|
|
113
|
+
|
|
114
|
+
parent, prev_tip = add_arm(base, arm, conaffinity=conaffinity)
|
|
115
|
+
if arm.gripper is not None:
|
|
116
|
+
add_gripper(parent, arm, prev_tip, conaffinity=conaffinity)
|
|
117
|
+
|
|
118
|
+
# Pose(tip_offset, identity) composed with end_effector.
|
|
119
|
+
parent.add_site(
|
|
120
|
+
name="ee",
|
|
121
|
+
pos=list(prev_tip + np.asarray(arm.end_effector.pos)),
|
|
122
|
+
quat=list(arm.end_effector.quat),
|
|
123
|
+
)
|
|
124
|
+
|
|
125
|
+
for i, seg in enumerate(arm.segments):
|
|
126
|
+
effort = seg.joint.effort_limit
|
|
127
|
+
act = spec.add_actuator(
|
|
128
|
+
name=f"act_{i}", target=f"joint_{i}", trntype=mujoco.mjtTrn.mjTRN_JOINT
|
|
129
|
+
)
|
|
130
|
+
act.forcelimited = _LIMITED
|
|
131
|
+
act.forcerange = [-effort, effort]
|
|
132
|
+
act.ctrllimited = _LIMITED
|
|
133
|
+
rng = seg.joint.range
|
|
134
|
+
if control.mode in ("joint_pos_abs", "joint_pos_delta"):
|
|
135
|
+
act.gainprm[0] = kp[i]
|
|
136
|
+
act.biasprm[:3] = [0.0, -kp[i], -kd[i]]
|
|
137
|
+
act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
|
|
138
|
+
if rng is not None:
|
|
139
|
+
act.ctrlrange = list(rng)
|
|
140
|
+
elif control.mode == "joint_pos_abs":
|
|
141
|
+
act.ctrlrange = [-np.pi, np.pi]
|
|
142
|
+
else: # a delta target on an unlimited joint is not bounded
|
|
143
|
+
act.ctrllimited = _UNLIMITED
|
|
144
|
+
elif control.mode == "joint_vel":
|
|
145
|
+
# Velocity servo: force = kd * (ctrl - qvel).
|
|
146
|
+
act.gainprm[0] = kd[i]
|
|
147
|
+
act.biasprm[:3] = [0.0, 0.0, -kd[i]]
|
|
148
|
+
act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
|
|
149
|
+
act.ctrllimited = _UNLIMITED
|
|
150
|
+
else: # joint_torque
|
|
151
|
+
act.ctrlrange = [-effort, effort]
|
|
152
|
+
|
|
153
|
+
g = arm.gripper
|
|
154
|
+
if g is not None:
|
|
155
|
+
# One degree of freedom: the two finger slides are tied together and driven through their sum (the opening).
|
|
156
|
+
eq = spec.add_equality(name="fingers_coupled")
|
|
157
|
+
eq.type = mujoco.mjtEq.mjEQ_JOINT
|
|
158
|
+
eq.name1, eq.name2 = "finger_joint_1", "finger_joint_0"
|
|
159
|
+
eq.data[:5] = [0.0, 1.0, 0.0, 0.0, 0.0]
|
|
160
|
+
tendon = spec.add_tendon(name="gripper_width")
|
|
161
|
+
tendon.wrap_joint("finger_joint_0", 1.0)
|
|
162
|
+
tendon.wrap_joint("finger_joint_1", 1.0)
|
|
163
|
+
act = spec.add_actuator(name="act_gripper", target="gripper_width", trntype=mujoco.mjtTrn.mjTRN_TENDON)
|
|
164
|
+
act.gainprm[0] = g.kp
|
|
165
|
+
act.biasprm[:3] = [0.0, -g.kp, -g.kd]
|
|
166
|
+
act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
|
|
167
|
+
act.ctrllimited = _LIMITED
|
|
168
|
+
act.ctrlrange = [0.0, g.max_opening]
|
|
169
|
+
act.forcelimited = _LIMITED
|
|
170
|
+
act.forcerange = [-g.force_limit, g.force_limit]
|
|
171
|
+
return spec
|
|
172
|
+
|
|
173
|
+
|
|
174
|
+
def compile_robot(cfg: EnvConfig, scene_hook=None, mount_site: str | None = None) -> CompiledRobot:
|
|
175
|
+
"""Compile ``cfg.robot`` into a model. ``scene_hook(spec)``, if given, adds the task's scene elements to the spec;
|
|
176
|
+
``mount_site`` names the marker it adds for the scene's mount frame (``TaskImpl.mount_site()``), where the base is snapped."""
|
|
177
|
+
if type(cfg.robot).__name__ == "ExternalRobot":
|
|
178
|
+
from .external import compile_external
|
|
179
|
+
|
|
180
|
+
return compile_external(cfg, scene_hook, mount_site)
|
|
181
|
+
arm = cfg.robot
|
|
182
|
+
n = arm.n
|
|
183
|
+
control = cfg.control
|
|
184
|
+
if control.mode not in SUPPORTED_MODES:
|
|
185
|
+
raise NotImplementedError(f"control mode {control.mode!r} is not compiled")
|
|
186
|
+
|
|
187
|
+
mount = resolve_world_mount(cfg, scene_hook, mount_site)
|
|
188
|
+
gains = control.gains
|
|
189
|
+
if isinstance(gains, FixedGains):
|
|
190
|
+
kp, kd = _per_joint(gains.kp, n), _per_joint(gains.kd, n)
|
|
191
|
+
else:
|
|
192
|
+
# Two passes: compile with placeholder gains, read M at home, then build with the final gains.
|
|
193
|
+
assert isinstance(gains, InertiaScaledGains)
|
|
194
|
+
first = _build_spec(cfg, [0.0] * n, [0.0] * n, scene_hook, mount).compile()
|
|
195
|
+
ids = [mujoco.mj_name2id(first, mujoco.mjtObj.mjOBJ_JOINT, f"joint_{i}") for i in range(n)]
|
|
196
|
+
kp_arr, kd_arr = inertia_scaled_gains(
|
|
197
|
+
first,
|
|
198
|
+
np.array(arm.home_qpos),
|
|
199
|
+
first.jnt_qposadr[ids],
|
|
200
|
+
first.jnt_dofadr[ids],
|
|
201
|
+
np.array([s.joint.damping for s in arm.segments]),
|
|
202
|
+
gains,
|
|
203
|
+
)
|
|
204
|
+
kp, kd = list(kp_arr), list(kd_arr)
|
|
205
|
+
|
|
206
|
+
spec = _build_spec(cfg, kp, kd, scene_hook, mount)
|
|
207
|
+
model = spec.compile()
|
|
208
|
+
|
|
209
|
+
def jid(i):
|
|
210
|
+
return mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, f"joint_{i}")
|
|
211
|
+
|
|
212
|
+
joint_ids = [jid(i) for i in range(n)]
|
|
213
|
+
joint_range = np.empty((n, 2))
|
|
214
|
+
for i, seg in enumerate(arm.segments):
|
|
215
|
+
joint_range[i] = (-np.inf, np.inf) if seg.joint.range is None else seg.joint.range
|
|
216
|
+
gripper_fields = {}
|
|
217
|
+
if arm.gripper is not None:
|
|
218
|
+
fids = [mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, f"finger_joint_{i}") for i in (0, 1)]
|
|
219
|
+
gripper_fields = dict(
|
|
220
|
+
gripper_actuator_id=mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, "act_gripper"),
|
|
221
|
+
gripper_qpos_adr=model.jnt_qposadr[fids].astype(int),
|
|
222
|
+
gripper_max_opening=arm.gripper.max_opening,
|
|
223
|
+
tcp_site_id=mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "tcp"),
|
|
224
|
+
)
|
|
225
|
+
interface = RobotInterface(
|
|
226
|
+
n=n,
|
|
227
|
+
joint_names=tuple(f"joint_{i}" for i in range(n)),
|
|
228
|
+
qpos_adr=model.jnt_qposadr[joint_ids].astype(int),
|
|
229
|
+
qvel_adr=model.jnt_dofadr[joint_ids].astype(int),
|
|
230
|
+
actuator_ids=np.array(
|
|
231
|
+
[mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"act_{i}") for i in range(n)], dtype=int
|
|
232
|
+
),
|
|
233
|
+
ee_site_id=mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee"),
|
|
234
|
+
joint_range=joint_range,
|
|
235
|
+
effort_limit=np.array([s.joint.effort_limit for s in arm.segments]),
|
|
236
|
+
home_qpos=np.array(arm.home_qpos),
|
|
237
|
+
mount=mount,
|
|
238
|
+
kp=None if control.mode == "joint_torque" else np.array(kp),
|
|
239
|
+
kd=None if control.mode == "joint_torque" else np.array(kd),
|
|
240
|
+
**gripper_fields,
|
|
241
|
+
)
|
|
242
|
+
return CompiledRobot(model=model, spec=spec, interface=interface)
|
|
@@ -0,0 +1,257 @@
|
|
|
1
|
+
"""The CPU MuJoCo simulator's environment: any task of any provider, on any robot of the config.
|
|
2
|
+
|
|
3
|
+
Owns what is the same for every task: the compiled robot, the batched simulation, the control law, resets with
|
|
4
|
+
a mask, episode bookkeeping and rendering. The task (``tasks.base.TaskImpl``) adds its scene assets before the model
|
|
5
|
+
is compiled and supplies the ``task`` observation group, success and reward. Never autoresets; honours
|
|
6
|
+
``td["_reset"]``.
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
11
|
+
import numpy as np
|
|
12
|
+
import torch
|
|
13
|
+
from tensordict import TensorDict, TensorDictBase
|
|
14
|
+
from torchrl.data import Binary, Bounded, Composite, Unbounded
|
|
15
|
+
from torchrl.envs import EnvBase
|
|
16
|
+
|
|
17
|
+
from ...config import EnvConfig
|
|
18
|
+
from ...tasks.base import mount_site_of as _mount_site
|
|
19
|
+
from ...envs.spaces import SpacesMixin
|
|
20
|
+
from ..base import task_seed
|
|
21
|
+
from .compile import compile_robot
|
|
22
|
+
from .sim import BatchedSim
|
|
23
|
+
|
|
24
|
+
class MujocoEnv(SpacesMixin, EnvBase):
|
|
25
|
+
"""Batched TorchRL environment: ``cfg.robot`` under ``cfg.control`` doing ``task`` in MuJoCo."""
|
|
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
|
+
super().__init__(device=torch.device("cpu"), batch_size=torch.Size([N]))
|
|
36
|
+
|
|
37
|
+
self._task = task
|
|
38
|
+
self._compiled = compile_robot(cfg, scene_hook=task.attach, mount_site=_mount_site(task))
|
|
39
|
+
self._iface = self._compiled.interface
|
|
40
|
+
self.world_mount = self._iface.mount # the base in the world, read from the compiled model; recorded with env_hash
|
|
41
|
+
self._sim = BatchedSim(self._compiled, N, cfg.sim.n_substeps)
|
|
42
|
+
self._renderer = None # created by the first render()
|
|
43
|
+
n = self._iface.n
|
|
44
|
+
self._n = n
|
|
45
|
+
self._has_gripper = self._iface.has_gripper
|
|
46
|
+
m = self._iface.action_dim # n joint commands, plus the gripper opening when there is a gripper
|
|
47
|
+
task.bind(self._sim)
|
|
48
|
+
|
|
49
|
+
# Action [-1, 1] -> actuator control: ctrl = action * scale + offset.
|
|
50
|
+
# Delta mode integrates the action into a target first (see _step); velocity mode scales it.
|
|
51
|
+
self._mode = cfg.control.mode
|
|
52
|
+
self._range = self._iface.joint_range
|
|
53
|
+
self._q_target = np.zeros((N, n))
|
|
54
|
+
if self._mode in ("joint_pos_delta", "joint_vel"):
|
|
55
|
+
self._scale = np.full(n, cfg.control.action_scale)
|
|
56
|
+
self._offset = np.zeros(n)
|
|
57
|
+
elif self._mode == "joint_pos_abs":
|
|
58
|
+
# [-1, 1] spans the actuator's ctrlrange (an external robot's own servo, which need not equal the joint range)
|
|
59
|
+
rng = self._iface.joint_range if self._iface.ctrl_range is None else self._iface.ctrl_range
|
|
60
|
+
lo, hi = rng[:, 0], rng[:, 1]
|
|
61
|
+
finite = np.isfinite(lo) & np.isfinite(hi)
|
|
62
|
+
lo, hi = np.where(finite, lo, 0.0), np.where(finite, hi, 0.0) # no inf - inf below
|
|
63
|
+
self._scale = np.where(finite, (hi - lo) / 2.0, np.pi)
|
|
64
|
+
self._offset = np.where(finite, (hi + lo) / 2.0, 0.0)
|
|
65
|
+
else: # joint_torque
|
|
66
|
+
self._scale = self._iface.effort_limit.copy()
|
|
67
|
+
if self._iface.ctrl_range is not None: # an external robot: the ends of the actuator's ctrlrange
|
|
68
|
+
top = np.abs(self._iface.ctrl_range).max(axis=1)
|
|
69
|
+
self._scale = np.where(np.isfinite(top), top, self._scale)
|
|
70
|
+
self._offset = np.zeros(n)
|
|
71
|
+
|
|
72
|
+
params = cfg.task.params
|
|
73
|
+
self._max_episode_steps = cfg.task.max_episode_steps
|
|
74
|
+
self._reset_noise = float(getattr(params, "reset_noise", 0.0))
|
|
75
|
+
self._terminate_on_success = bool(getattr(params, "terminate_on_success", False))
|
|
76
|
+
|
|
77
|
+
# Two seeded streams. ``_gen`` draws the reset noise of the robot (its shape depends on the number of joints).
|
|
78
|
+
# ``_task_gen`` is for the task alone: what the task draws (an object placement, a goal) is then the same for
|
|
79
|
+
# every embodiment that runs it with the same seed and ``num_envs``, whatever the robot.
|
|
80
|
+
self._gen = torch.Generator(device="cpu")
|
|
81
|
+
self._gen.manual_seed(cfg.sim.seed)
|
|
82
|
+
self._task_gen = torch.Generator(device="cpu")
|
|
83
|
+
self._task_gen.manual_seed(task_seed(cfg.sim.seed))
|
|
84
|
+
self._episode_step = torch.zeros(N, 1, dtype=torch.int64)
|
|
85
|
+
|
|
86
|
+
f32 = torch.float32
|
|
87
|
+
self.observation_spec = Composite(
|
|
88
|
+
task=task.observation_spec(N),
|
|
89
|
+
proprio=Composite(
|
|
90
|
+
qpos=Unbounded(shape=(N, n), dtype=f32),
|
|
91
|
+
qvel=Unbounded(shape=(N, n), dtype=f32),
|
|
92
|
+
effort=Unbounded(shape=(N, n), dtype=f32),
|
|
93
|
+
**({"gripper": Unbounded(shape=(N, 1), dtype=f32)} if self._has_gripper else {}),
|
|
94
|
+
shape=(N,),
|
|
95
|
+
),
|
|
96
|
+
success=Binary(shape=(N, 1), dtype=torch.bool),
|
|
97
|
+
episode_step=Unbounded(shape=(N, 1), dtype=torch.int64),
|
|
98
|
+
shape=(N,),
|
|
99
|
+
)
|
|
100
|
+
self.action_spec = Bounded(low=-1.0, high=1.0, shape=(N, m), dtype=f32)
|
|
101
|
+
self.reward_spec = Unbounded(shape=(N, 1), dtype=f32)
|
|
102
|
+
self.done_spec = Composite(
|
|
103
|
+
done=Binary(shape=(N, 1), dtype=torch.bool),
|
|
104
|
+
terminated=Binary(shape=(N, 1), dtype=torch.bool),
|
|
105
|
+
truncated=Binary(shape=(N, 1), dtype=torch.bool),
|
|
106
|
+
shape=(N,),
|
|
107
|
+
)
|
|
108
|
+
|
|
109
|
+
@property
|
|
110
|
+
def q_target(self) -> np.ndarray:
|
|
111
|
+
"""Position targets, ``(N, n)``; meaningful in ``joint_pos_delta`` mode."""
|
|
112
|
+
return self._q_target.copy()
|
|
113
|
+
|
|
114
|
+
@property
|
|
115
|
+
def interface(self):
|
|
116
|
+
return self._iface
|
|
117
|
+
|
|
118
|
+
@property
|
|
119
|
+
def task(self):
|
|
120
|
+
"""The engine side of the task (``tasks.base.TaskImpl``)."""
|
|
121
|
+
return self._task
|
|
122
|
+
|
|
123
|
+
# --- sampling -------------------------------------------------------------------
|
|
124
|
+
|
|
125
|
+
def _rand(self, *shape, dtype=torch.float64) -> torch.Tensor:
|
|
126
|
+
return torch.rand(*shape, generator=self._gen, dtype=dtype)
|
|
127
|
+
|
|
128
|
+
def _task_rand(self, *shape, dtype=torch.float64) -> torch.Tensor:
|
|
129
|
+
return torch.rand(*shape, generator=self._task_gen, dtype=dtype)
|
|
130
|
+
|
|
131
|
+
# --- observations ---------------------------------------------------------------
|
|
132
|
+
|
|
133
|
+
def _observe(self) -> tuple[TensorDict, dict]:
|
|
134
|
+
s = self._sim
|
|
135
|
+
task_obs = self._task.observe(s)
|
|
136
|
+
out = TensorDict(
|
|
137
|
+
{
|
|
138
|
+
"task": TensorDict(dict(task_obs), batch_size=self.batch_size),
|
|
139
|
+
"proprio": TensorDict(
|
|
140
|
+
{
|
|
141
|
+
"qpos": torch.from_numpy(s.qpos).to(torch.float32),
|
|
142
|
+
"qvel": torch.from_numpy(s.qvel).to(torch.float32),
|
|
143
|
+
"effort": torch.from_numpy(s.actuator_force).to(torch.float32),
|
|
144
|
+
**(
|
|
145
|
+
{"gripper": torch.from_numpy(s.gripper_opening).to(torch.float32)}
|
|
146
|
+
if self._has_gripper
|
|
147
|
+
else {}
|
|
148
|
+
),
|
|
149
|
+
},
|
|
150
|
+
batch_size=self.batch_size,
|
|
151
|
+
),
|
|
152
|
+
"success": self._task.success(s, task_obs),
|
|
153
|
+
"episode_step": self._episode_step.clone(),
|
|
154
|
+
},
|
|
155
|
+
batch_size=self.batch_size,
|
|
156
|
+
)
|
|
157
|
+
return out, task_obs
|
|
158
|
+
|
|
159
|
+
# --- EnvBase interface ----------------------------------------------------------
|
|
160
|
+
|
|
161
|
+
def _reset(self, tensordict: TensorDictBase | None = None, **kwargs) -> TensorDictBase:
|
|
162
|
+
N, n = self.batch_size[0], self._n
|
|
163
|
+
mask = torch.ones(N, dtype=torch.bool)
|
|
164
|
+
if tensordict is not None and tensordict.get("_reset", None) is not None:
|
|
165
|
+
mask = tensordict.get("_reset").reshape(N).to(torch.bool)
|
|
166
|
+
|
|
167
|
+
def injected(key):
|
|
168
|
+
if tensordict is not None and tensordict.get(key, None) is not None:
|
|
169
|
+
return tensordict.get(key)
|
|
170
|
+
return None
|
|
171
|
+
|
|
172
|
+
# Random draws always cover the whole batch so that rng use does not depend on the mask.
|
|
173
|
+
noise = (self._rand(N, n) * 2.0 - 1.0) * self._reset_noise
|
|
174
|
+
qpos = torch.from_numpy(self._iface.home_qpos).expand(N, n) + noise
|
|
175
|
+
qpos = np.clip(qpos.numpy(), self._iface.joint_range[:, 0], self._iface.joint_range[:, 1])
|
|
176
|
+
qpos_in, qvel_in = injected("qpos"), injected("qvel")
|
|
177
|
+
if qpos_in is not None:
|
|
178
|
+
qpos = qpos_in.to(torch.float64).numpy()
|
|
179
|
+
qvel = None if qvel_in is None else qvel_in.to(torch.float64).numpy()
|
|
180
|
+
|
|
181
|
+
self._sim.reset(mask=mask.numpy(), qpos=qpos, qvel=qvel)
|
|
182
|
+
if self._mode == "joint_pos_delta":
|
|
183
|
+
self._q_target[mask.numpy()] = self._sim.qpos[mask.numpy()]
|
|
184
|
+
self._task.reset(self._sim, mask, self._task_rand, injected)
|
|
185
|
+
self._episode_step[mask] = 0
|
|
186
|
+
|
|
187
|
+
out, _ = self._observe()
|
|
188
|
+
for key in ("done", "terminated", "truncated"):
|
|
189
|
+
prev = injected(key)
|
|
190
|
+
flags = torch.zeros(N, 1, dtype=torch.bool)
|
|
191
|
+
if prev is not None:
|
|
192
|
+
flags = prev.to(torch.bool).clone()
|
|
193
|
+
flags[mask] = False
|
|
194
|
+
out.set(key, flags)
|
|
195
|
+
return out
|
|
196
|
+
|
|
197
|
+
def _step(self, tensordict: TensorDictBase) -> TensorDictBase:
|
|
198
|
+
action = tensordict.get("action").to(torch.float64).clamp(-1.0, 1.0).numpy()
|
|
199
|
+
n = self._n
|
|
200
|
+
arm_action = action[:, :n]
|
|
201
|
+
if self._mode == "joint_pos_delta":
|
|
202
|
+
self._q_target = np.clip(
|
|
203
|
+
self._q_target + arm_action * self._scale, self._range[:, 0], self._range[:, 1]
|
|
204
|
+
)
|
|
205
|
+
ctrl = self._q_target
|
|
206
|
+
else:
|
|
207
|
+
ctrl = arm_action * self._scale + self._offset
|
|
208
|
+
if self._has_gripper:
|
|
209
|
+
# The last action component is the opening, [-1, 1] -> [0, max_opening], in every control mode.
|
|
210
|
+
opening = (action[:, n:] + 1.0) / 2.0 * self._iface.gripper_max_opening
|
|
211
|
+
ctrl = np.concatenate([ctrl, opening], axis=1)
|
|
212
|
+
self._sim.set_ctrl(ctrl)
|
|
213
|
+
self._sim.step()
|
|
214
|
+
self._episode_step += 1
|
|
215
|
+
|
|
216
|
+
out, task_obs = self._observe()
|
|
217
|
+
success = out["success"]
|
|
218
|
+
truncated = self._episode_step >= self._max_episode_steps
|
|
219
|
+
terminated = success.clone() if self._terminate_on_success else torch.zeros_like(success)
|
|
220
|
+
out.set("reward", self._task.reward(self._sim, task_obs))
|
|
221
|
+
out.set("terminated", terminated)
|
|
222
|
+
out.set("truncated", truncated)
|
|
223
|
+
out.set("done", terminated | truncated)
|
|
224
|
+
return out
|
|
225
|
+
|
|
226
|
+
def _set_seed(self, seed: int | None):
|
|
227
|
+
if seed is not None:
|
|
228
|
+
self._gen.manual_seed(int(seed))
|
|
229
|
+
self._task_gen.manual_seed(task_seed(int(seed)))
|
|
230
|
+
return seed
|
|
231
|
+
|
|
232
|
+
# --- rendering (needs an OpenGL context: MUJOCO_GL=egl on headless machines) ----------------------------
|
|
233
|
+
|
|
234
|
+
def render(
|
|
235
|
+
self,
|
|
236
|
+
env_index: int = 0,
|
|
237
|
+
*,
|
|
238
|
+
camera=None,
|
|
239
|
+
size: tuple[int, int] = (240, 320),
|
|
240
|
+
overlays: bool = True,
|
|
241
|
+
) -> np.ndarray:
|
|
242
|
+
"""Render the live state of one env to ``uint8 (H, W, 3)`` (Gymnasium's ``rgb_array``).
|
|
243
|
+
|
|
244
|
+
Shows the whole scene plus the overlays the task draws (``TaskImpl.overlays``). ``camera`` is ``None`` (the
|
|
245
|
+
``"iso"`` view framed on the workspace), a view name (``iso``, ``front``, ``side``, ``top``), a
|
|
246
|
+
``robotenv.viz.CameraSpec``, a ``mujoco.MjvCamera``, or the name / id of a camera in the model.
|
|
247
|
+
Rendering never changes the simulation.
|
|
248
|
+
"""
|
|
249
|
+
from .render import get_renderer
|
|
250
|
+
|
|
251
|
+
return get_renderer(self).render(env_index, camera=camera, size=size, overlays=overlays)
|
|
252
|
+
|
|
253
|
+
def close(self, *, raise_if_closed: bool = True):
|
|
254
|
+
renderer, self._renderer = getattr(self, "_renderer", None), None
|
|
255
|
+
if renderer is not None:
|
|
256
|
+
renderer.close()
|
|
257
|
+
super().close(raise_if_closed=raise_if_closed)
|