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.
Files changed (63) hide show
  1. robotenv/__init__.py +21 -0
  2. robotenv/_canon.py +136 -0
  3. robotenv/config/__init__.py +93 -0
  4. robotenv/config/base_env.py +557 -0
  5. robotenv/config/control.py +161 -0
  6. robotenv/config/defaults.py +29 -0
  7. robotenv/config/env.py +107 -0
  8. robotenv/config/kinematics.py +121 -0
  9. robotenv/config/mount.py +172 -0
  10. robotenv/config/providers.py +171 -0
  11. robotenv/config/robot_providers.py +140 -0
  12. robotenv/config/sim.py +152 -0
  13. robotenv/config/specs.py +111 -0
  14. robotenv/config/sweep.py +100 -0
  15. robotenv/config/task.py +183 -0
  16. robotenv/envs/__init__.py +34 -0
  17. robotenv/envs/spaces.py +38 -0
  18. robotenv/errors.py +35 -0
  19. robotenv/robot/__init__.py +30 -0
  20. robotenv/robot/arm.py +319 -0
  21. robotenv/robot/external.py +234 -0
  22. robotenv/robot/generators.py +138 -0
  23. robotenv/robot/gripper.py +82 -0
  24. robotenv/robot/pose.py +49 -0
  25. robotenv/robot/providers/__init__.py +5 -0
  26. robotenv/robot/providers/menagerie/__init__.py +127 -0
  27. robotenv/robot/providers/menagerie/_describe.py +130 -0
  28. robotenv/robot/providers/menagerie/cards.json +37 -0
  29. robotenv/simulators/__init__.py +51 -0
  30. robotenv/simulators/base.py +70 -0
  31. robotenv/simulators/mujoco/__init__.py +38 -0
  32. robotenv/simulators/mujoco/arm_spec.py +137 -0
  33. robotenv/simulators/mujoco/compile.py +242 -0
  34. robotenv/simulators/mujoco/env.py +257 -0
  35. robotenv/simulators/mujoco/external.py +443 -0
  36. robotenv/simulators/mujoco/mount.py +65 -0
  37. robotenv/simulators/mujoco/render.py +158 -0
  38. robotenv/simulators/mujoco/scenes.py +35 -0
  39. robotenv/simulators/mujoco/sim.py +174 -0
  40. robotenv/simulators/warp/__init__.py +100 -0
  41. robotenv/simulators/warp/env.py +235 -0
  42. robotenv/simulators/warp/sim.py +332 -0
  43. robotenv/tasks/__init__.py +10 -0
  44. robotenv/tasks/base.py +147 -0
  45. robotenv/tasks/empty/__init__.py +42 -0
  46. robotenv/tasks/empty/params.py +33 -0
  47. robotenv/tasks/empty/task.py +27 -0
  48. robotenv/tasks/robosuite/__init__.py +133 -0
  49. robotenv/tasks/robosuite/_compat.py +165 -0
  50. robotenv/tasks/robosuite/_introspect.py +65 -0
  51. robotenv/tasks/robosuite/params.py +152 -0
  52. robotenv/tasks/robosuite/scene.py +140 -0
  53. robotenv/tasks/robosuite/task.py +145 -0
  54. robotenv/viz/__init__.py +22 -0
  55. robotenv/viz/_deps.py +16 -0
  56. robotenv/viz/camera.py +98 -0
  57. robotenv/viz/clip.py +180 -0
  58. robotenv/viz/frames.py +93 -0
  59. robotenv/viz/record.py +141 -0
  60. robotenv-0.2.1.dist-info/METADATA +136 -0
  61. robotenv-0.2.1.dist-info/RECORD +63 -0
  62. robotenv-0.2.1.dist-info/WHEEL +4 -0
  63. 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)