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,443 @@
1
+ """Compile an ``ExternalRobot``: the provider's model is attached to the scene and driven by its own actuators.
2
+
3
+ The placement follows ``docs/simulators.md``: the compile adds an empty, welded body ``base`` at the mount (so that
4
+ ``SimView`` frames work as for a ``ParametricArm``) and attaches the provider's ``MjSpec`` into a frame of the world body
5
+ at the same pose, with the prefix ``ext_``. The model's world frame is therefore the base frame. A free root joint stays
6
+ a direct child of the world, which MuJoCo requires.
7
+ """
8
+
9
+ from __future__ import annotations
10
+
11
+ import contextlib
12
+ import warnings
13
+
14
+ import mujoco
15
+ import numpy as np
16
+
17
+ from ...config import (
18
+ EnvConfig,
19
+ ExternalRobot,
20
+ FixedGains,
21
+ InertiaScaledGains,
22
+ Pose,
23
+ get_robot_provider,
24
+ )
25
+ from ...config.kinematics import quat_to_matrix
26
+ from ...errors import ConfigError, Problem
27
+ from .compile import (
28
+ _INTEGRATORS,
29
+ _LIMITED,
30
+ _UNLIMITED,
31
+ SUPPORTED_MODES,
32
+ CompiledRobot,
33
+ RobotInterface,
34
+ _per_joint,
35
+ inertia_scaled_gains,
36
+ )
37
+ from .mount import resolve_world_mount
38
+ from .scenes import apply_scene
39
+
40
+ PREFIX = "ext_"
41
+ _KINDS = ("position", "velocity", "torque")
42
+ _OBJ = mujoco.mjtObj
43
+ _HINGE, _SLIDE = int(mujoco.mjtJoint.mjJNT_HINGE), int(mujoco.mjtJoint.mjJNT_SLIDE)
44
+ _FREE, _BALL = int(mujoco.mjtJoint.mjJNT_FREE), int(mujoco.mjtJoint.mjJNT_BALL)
45
+ _JOINT_QPOS = {_FREE: 7, _BALL: 4}
46
+
47
+
48
+ def compose(a: Pose, b: Pose) -> Pose:
49
+ """``a`` followed by ``b``: the pose of a frame given in the frame ``a`` as a pose in the frame ``a`` is in."""
50
+ pos = quat_to_matrix(a.quat) @ np.asarray(b.pos) + np.asarray(a.pos)
51
+ aw, ax, ay, az = a.quat
52
+ bw, bx, by, bz = b.quat
53
+ quat = (
54
+ aw * bw - ax * bx - ay * by - az * bz,
55
+ aw * bx + ax * bw + ay * bz - az * by,
56
+ aw * by - ax * bz + ay * bw + az * bx,
57
+ aw * bz + ax * by - ay * bx + az * bw,
58
+ )
59
+ return Pose(tuple(float(x) for x in pos), tuple(float(x) for x in quat))
60
+
61
+
62
+ # The solver and contact options of a model's own ``<option>`` (never ``timestep`` or ``integrator``, which ``SimConfig`` owns).
63
+ OPTION_FIELDS = (
64
+ "cone", "impratio", "solver", "iterations", "ls_iterations", "noslip_iterations", "noslip_tolerance", "tolerance",
65
+ "ls_tolerance",
66
+ )
67
+ OVERRIDE_FIELDS = ("o_margin", "o_solref", "o_solimp", "o_friction")
68
+ CARRY_MODEL_OPTIONS = False # the policy of ``_model_options``; see docs/simulators.md
69
+
70
+
71
+ def _model_options(child: mujoco.MjModel, scene: mujoco.MjSpec) -> None:
72
+ """The one place where the contact and solver options of the model are, or are not, carried over to the scene.
73
+ ``MjSpec.attach`` keeps the scene's value where the two differ; with ``CARRY_MODEL_OPTIONS`` the model's are copied
74
+ onto the scene first (``SimConfig`` still owns ``timestep`` and ``integrator``)."""
75
+ if not CARRY_MODEL_OPTIONS:
76
+ return
77
+ for name in OPTION_FIELDS:
78
+ setattr(scene.option, name, getattr(child.opt, name))
79
+ if int(child.opt.enableflags) & int(mujoco.mjtEnableBit.mjENBL_OVERRIDE):
80
+ scene.option.enableflags |= int(mujoco.mjtEnableBit.mjENBL_OVERRIDE)
81
+ for name in OVERRIDE_FIELDS:
82
+ setattr(scene.option, name, getattr(child.opt, name))
83
+
84
+
85
+ def _id(model, kind, name) -> int:
86
+ return mujoco.mj_name2id(model, kind, name)
87
+
88
+
89
+ def actuator_kind(model: mujoco.MjModel, a: int) -> str:
90
+ """``position``, ``velocity`` or ``torque`` from the gain / bias of actuator ``a``; ``mixed`` if it is none of them."""
91
+ if int(model.actuator_biastype[a]) == int(mujoco.mjtBias.mjBIAS_NONE):
92
+ return "torque"
93
+ if int(model.actuator_biastype[a]) == int(mujoco.mjtBias.mjBIAS_AFFINE):
94
+ b = model.actuator_biasprm[a]
95
+ if b[1] < 0.0 and b[2] <= 0.0:
96
+ return "position"
97
+ if b[1] == 0.0 and b[2] < 0.0:
98
+ return "velocity"
99
+ return "mixed"
100
+
101
+
102
+ class Facts:
103
+ """What the model of an external robot looks like, read from a standalone compile of the provider's spec."""
104
+
105
+ def __init__(self, robot: ExternalRobot, model: mujoco.MjModel, own_actuators: bool = False):
106
+ """``own_actuators``: the compile replaces the actuators of the driven joints with its own (``FixedGains`` or
107
+ ``InertiaScaledGains``), so the model's need not exist, nor match ``actuator_kind``."""
108
+ self.model = model
109
+ self.joint_ids: list[int | None] = []
110
+ self.actuator_ids: list[int | None] = []
111
+ self.replaced: set[int] = set() # the actuators on driven joints: the compile deletes them when it brings its own
112
+ self.problems: list[Problem] = []
113
+ m = model
114
+ if m.nu == 0 and not own_actuators:
115
+ self.problems.append(Problem("robot", f"the model of {robot.provider}/{robot.name} has no actuators"))
116
+ for i, j in enumerate(robot.joints):
117
+ where = f"robot.joints[{i}]"
118
+ jid = _id(m, _OBJ.mjOBJ_JOINT, j.name)
119
+ self.joint_ids.append(None if jid < 0 else jid)
120
+ self.actuator_ids.append(None)
121
+ if jid < 0:
122
+ self.problems.append(Problem(f"{where}.name", f"the model has no joint {j.name!r}"))
123
+ continue
124
+ if int(m.jnt_type[jid]) not in (_HINGE, _SLIDE):
125
+ self.problems.append(Problem(f"{where}.name", f"joint {j.name!r} is not a hinge or a slide"))
126
+ continue
127
+ acts = [
128
+ a for a in range(m.nu) if int(m.actuator_trntype[a]) == int(mujoco.mjtTrn.mjTRN_JOINT) and int(m.actuator_trnid[a, 0]) == jid
129
+ ]
130
+ self.replaced.update(acts)
131
+ if own_actuators:
132
+ continue
133
+ if len(acts) != 1:
134
+ self.problems.append(Problem(where, f"joint {j.name!r} has {len(acts)} actuators with a joint transmission, expected 1"))
135
+ continue
136
+ self.actuator_ids[-1] = acts[0]
137
+ driven = [a for a in self.actuator_ids if a is not None]
138
+ kinds = {actuator_kind(m, a) for a in driven}
139
+ found = next(iter(kinds)) if len(kinds) == 1 else "mixed" if kinds else None
140
+ if found is not None and found != robot.actuator_kind and not own_actuators:
141
+ self.problems.append(
142
+ Problem("robot.actuator_kind", f"the driven actuators of the model are {found!r}, the config says {robot.actuator_kind!r}")
143
+ )
144
+ self.driven = driven
145
+ self.key = None
146
+ if robot.keyframe is not None:
147
+ key = _id(m, _OBJ.mjOBJ_KEY, robot.keyframe)
148
+ if key < 0:
149
+ self.problems.append(Problem("robot.keyframe", f"the model has no keyframe {robot.keyframe!r}"))
150
+ else:
151
+ self.key = key
152
+ free = [j for j in range(m.njnt) if int(m.jnt_type[j]) == _FREE]
153
+ self.free = free[0] if len(free) == 1 else None
154
+ if len(free) > 1:
155
+ self.problems.append(Problem("robot", f"the model has {len(free)} free joints, expected at most 1 (the root)"))
156
+ elif free and robot.home_root is None:
157
+ self.problems.append(Problem("robot.home_root", "the model has a free root joint: home_root is required"))
158
+ elif not free and robot.home_root is not None:
159
+ self.problems.append(Problem("robot.home_root", "the model has no free root joint: home_root must be None"))
160
+ ee = robot.end_effector
161
+ if ee is not None:
162
+ if ee.site is not None and _id(m, _OBJ.mjOBJ_SITE, ee.site) < 0:
163
+ self.problems.append(Problem("robot.end_effector.site", f"the model has no site {ee.site!r}"))
164
+ if ee.body is not None and _id(m, _OBJ.mjOBJ_BODY, ee.body) < 0:
165
+ self.problems.append(Problem("robot.end_effector.body", f"the model has no body {ee.body!r}"))
166
+
167
+
168
+ def _open(robot: ExternalRobot, own_actuators: bool = False):
169
+ """``(spec, facts)`` of the provider's model; ``ConfigError`` from the provider (a stale ``source``) is passed on."""
170
+ provider = get_robot_provider(robot.provider)
171
+ try:
172
+ model = provider.spec(robot).compile()
173
+ except ConfigError:
174
+ raise
175
+ except Exception as e: # a model that does not open or compile
176
+ raise ConfigError("robot", f"the model of {robot.provider}/{robot.name} could not be opened: {type(e).__name__}: {e}") from e
177
+ return Facts(robot, model, own_actuators)
178
+
179
+
180
+ def problems_of(cfg: EnvConfig) -> list[Problem]:
181
+ """Every reason the model cannot be compiled and driven as configured; nothing is built."""
182
+ robot = cfg.robot
183
+ provider = get_robot_provider(robot.provider)
184
+ problems = list(provider.validate(robot, "mujoco"))
185
+ try:
186
+ facts = _open(robot, isinstance(cfg.control.gains, (FixedGains, InertiaScaledGains)))
187
+ except ConfigError as e:
188
+ found = Problem(e.path or "robot", e.reason)
189
+ return problems if found in problems else problems + [found] # the provider's own check may have said it already
190
+ problems += facts.problems
191
+ own = facts.model.opt.timestep
192
+ if cfg.sim.dt > 2.0 * own:
193
+ warnings.warn(
194
+ f"sim.dt = {cfg.sim.dt} is more than twice the timestep of the model of {robot.provider}/{robot.name} ({own}); "
195
+ "its own actuators and contacts may be unstable at that step",
196
+ stacklevel=2,
197
+ )
198
+ return problems
199
+
200
+
201
+ def _add_own_actuators(spec: mujoco.MjSpec, robot: ExternalRobot, control, kp: list[float], kd: list[float]) -> None:
202
+ """The actuators a ``ParametricArm`` gets (``act_i``: gains, ``forcerange`` from the effort limit, ``ctrlrange`` from the
203
+ joint range), on the driven joints of the attached model."""
204
+ for i, joint in enumerate(robot.joints):
205
+ effort = joint.effort_limit
206
+ act = spec.add_actuator(name=f"act_{i}", target=PREFIX + joint.name, trntype=mujoco.mjtTrn.mjTRN_JOINT)
207
+ act.forcelimited = _LIMITED
208
+ act.forcerange = [-effort, effort]
209
+ act.ctrllimited = _LIMITED
210
+ rng = joint.range
211
+ if control.mode in ("joint_pos_abs", "joint_pos_delta"):
212
+ act.gainprm[0] = kp[i]
213
+ act.biasprm[:3] = [0.0, -kp[i], -kd[i]]
214
+ act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
215
+ if rng is not None:
216
+ act.ctrlrange = list(rng)
217
+ elif control.mode == "joint_pos_abs":
218
+ act.ctrlrange = [-np.pi, np.pi]
219
+ else: # a delta target on an unlimited joint is not bounded
220
+ act.ctrllimited = _UNLIMITED
221
+ elif control.mode == "joint_vel":
222
+ act.gainprm[0] = kd[i] # force = kd * (ctrl - qvel)
223
+ act.biasprm[:3] = [0.0, 0.0, -kd[i]]
224
+ act.biastype = mujoco.mjtBias.mjBIAS_AFFINE
225
+ act.ctrllimited = _UNLIMITED
226
+ else: # joint_torque
227
+ act.ctrlrange = [-effort, effort]
228
+
229
+
230
+ def _build_external_spec(
231
+ cfg: EnvConfig, facts: Facts, scene_hook, own: tuple[list[float], list[float]] | None, mount: Pose
232
+ ) -> mujoco.MjSpec:
233
+ """The scene with the model attached. ``own = (kp, kd)`` replaces the model's actuators on the driven joints."""
234
+ robot: ExternalRobot = cfg.robot
235
+ spec = mujoco.MjSpec()
236
+ spec.compiler.degree = False # all angles here are radians
237
+ spec.option.timestep = cfg.sim.dt
238
+ spec.option.integrator = _INTEGRATORS[cfg.sim.integrator]
239
+ apply_scene(spec, cfg.sim.scene)
240
+ if scene_hook is not None:
241
+ scene_hook(spec)
242
+ spec.worldbody.add_body(name="base", pos=list(mount.pos), quat=list(mount.quat))
243
+ frame = spec.worldbody.add_frame(pos=list(mount.pos), quat=list(mount.quat))
244
+ child = get_robot_provider(robot.provider).spec(robot)
245
+ _model_options(facts.model, spec)
246
+ if own is not None:
247
+ # Removing an actuator breaks the keyframes' ctrl size; the keyframe state is read from the standalone compile.
248
+ for key in list(child.keys):
249
+ child.delete(key)
250
+ replaced = [a for k, a in enumerate(child.actuators) if k in facts.replaced]
251
+ for a in replaced:
252
+ child.delete(a)
253
+ spec.attach(child, prefix=PREFIX, frame=frame)
254
+ ee = robot.end_effector
255
+ if ee is not None and ee.body is not None:
256
+ body = spec.body(PREFIX + ee.body)
257
+ body.add_site(name="ee", pos=list(ee.offset.pos), quat=list(ee.offset.quat))
258
+ if own is not None:
259
+ _add_own_actuators(spec, robot, cfg.control, *own)
260
+ return spec
261
+
262
+
263
+ @contextlib.contextmanager
264
+ def _scene_options_win():
265
+ """MuJoCo warns when the model's own option differs from the scene's and the scene's value is kept (as a Python warning
266
+ and on stderr). That is the policy (``CARRY_MODEL_OPTIONS``, docs/simulators.md), not news on every build; other
267
+ MuJoCo warnings pass."""
268
+ previous = mujoco.get_mju_user_warning()
269
+
270
+ def handler(message):
271
+ text = message.decode() if isinstance(message, bytes) else str(message)
272
+ if "Attach conflict" in text:
273
+ return
274
+ if previous is not None:
275
+ previous(message)
276
+ else:
277
+ warnings.warn(text, stacklevel=2)
278
+
279
+ mujoco.set_mju_user_warning(handler)
280
+ try:
281
+ with warnings.catch_warnings():
282
+ warnings.filterwarnings("ignore", message="Attach conflict")
283
+ yield
284
+ finally:
285
+ mujoco.set_mju_user_warning(previous)
286
+
287
+
288
+ def compile_external(cfg: EnvConfig, scene_hook=None, mount_site: str | None = None) -> CompiledRobot:
289
+ with _scene_options_win():
290
+ return _compile_external(cfg, scene_hook, mount_site)
291
+
292
+
293
+ def _compile_external(cfg: EnvConfig, scene_hook=None, mount_site: str | None = None) -> CompiledRobot:
294
+ robot: ExternalRobot = cfg.robot
295
+ mount = resolve_world_mount(cfg, scene_hook, mount_site)
296
+ control = cfg.control
297
+ own_actuators = isinstance(control.gains, (FixedGains, InertiaScaledGains))
298
+ if own_actuators and control.mode not in SUPPORTED_MODES:
299
+ raise NotImplementedError(f"control mode {control.mode!r} is not compiled")
300
+ facts = _open(robot, own_actuators)
301
+ if facts.problems:
302
+ raise ConfigError(facts.problems[0].path, "; ".join(f"{p.path}: {p.message}" for p in facts.problems))
303
+ c = facts.model
304
+ n = robot.n
305
+ # With our own actuators the model's remaining ones come first among the attached ones, ours are the last n.
306
+ keep = [a for a in range(c.nu) if a not in facts.replaced] if own_actuators else list(range(c.nu))
307
+
308
+ own = None
309
+ if own_actuators:
310
+ gains = control.gains
311
+ if isinstance(gains, FixedGains):
312
+ own = (_per_joint(gains.kp, n), _per_joint(gains.kd, n))
313
+ else:
314
+ # Two passes: compile with placeholder gains, read M at home and the model's joint damping, build with the final gains.
315
+ first = _build_external_spec(cfg, facts, scene_hook, ([0.0] * n, [0.0] * n), mount).compile()
316
+ ids = [first.njnt - c.njnt + jc for jc in facts.joint_ids]
317
+ qpos_adr, qvel_adr = first.jnt_qposadr[ids], first.jnt_dofadr[ids]
318
+ kp_arr, kd_arr = inertia_scaled_gains(
319
+ first, np.array(robot.home_qpos), qpos_adr, qvel_adr, first.dof_damping[qvel_adr], gains
320
+ )
321
+ own = (list(kp_arr), list(kd_arr))
322
+ spec = _build_external_spec(cfg, facts, scene_hook, own, mount)
323
+ model = spec.compile()
324
+ ee = robot.end_effector
325
+
326
+ # The attached model's joints and actuators come last, in their own order: ids shift by what the scene has before them.
327
+ joff = model.njnt - c.njnt
328
+ aoff = model.nu - (n if own_actuators else 0) - len(keep)
329
+ joint_ids = [joff + jc for jc in facts.joint_ids]
330
+ for j, jid in zip(robot.joints, joint_ids):
331
+ if _id(model, _OBJ.mjOBJ_JOINT, PREFIX + j.name) != jid:
332
+ raise ConfigError("robot.joints", f"joint {j.name!r} did not keep its place when the model was attached")
333
+ act_ids = (
334
+ np.arange(model.nu - n, model.nu) if own_actuators else np.array([aoff + a for a in facts.actuator_ids], dtype=int)
335
+ )
336
+
337
+ # The state at reset: the keyframe mapped onto this model, joint by joint, and the ctrl of the actuators not driven.
338
+ reset_qpos = None
339
+ if facts.key is not None:
340
+ reset_qpos = model.qpos0.copy()
341
+ for jc in range(c.njnt):
342
+ jp = joff + jc
343
+ size = _JOINT_QPOS.get(int(c.jnt_type[jc]), 1)
344
+ reset_qpos[model.jnt_qposadr[jp] : model.jnt_qposadr[jp] + size] = c.key_qpos[facts.key][c.jnt_qposadr[jc] : c.jnt_qposadr[jc] + size]
345
+ held = np.zeros(model.nu)
346
+ if facts.key is not None:
347
+ for k, a in enumerate(keep):
348
+ held[aoff + k] = c.key_ctrl[facts.key][a]
349
+ held[act_ids] = 0.0
350
+
351
+ root_adr = home_root = None
352
+ if facts.free is not None:
353
+ root_adr = int(model.jnt_qposadr[joff + facts.free])
354
+ home_root = compose(mount, robot.home_root)
355
+
356
+ # The range [-1, 1] maps onto: the actuator's ctrlrange; a position servo without one takes the joint's range.
357
+ ctrl_range = np.where(
358
+ model.actuator_ctrllimited[act_ids, None].astype(bool),
359
+ model.actuator_ctrlrange[act_ids],
360
+ np.array([-np.inf, np.inf]),
361
+ )
362
+ if own_actuators or robot.actuator_kind == "position":
363
+ unlimited = ~model.actuator_ctrllimited[act_ids].astype(bool)
364
+ ctrl_range[unlimited] = np.array([(-np.inf, np.inf) if j.range is None else j.range for j in robot.joints])[unlimited]
365
+ if own_actuators:
366
+ kp = None if control.mode == "joint_torque" else np.array(own[0])
367
+ kd = None if control.mode == "joint_torque" else np.array(own[1])
368
+ else:
369
+ servo = control.mode in ("joint_pos_abs", "joint_pos_delta")
370
+ kp = model.actuator_gainprm[act_ids, 0].copy() if servo else None
371
+ kd = -model.actuator_biasprm[act_ids, 2].copy() if servo else None
372
+
373
+ ee_site = None
374
+ if ee is not None:
375
+ ee_site = _id(model, _OBJ.mjOBJ_SITE, "ee" if ee.body is not None else PREFIX + ee.site)
376
+ joint_range = np.array([(-np.inf, np.inf) if j.range is None else j.range for j in robot.joints], dtype=float)
377
+ interface = RobotInterface(
378
+ n=n,
379
+ joint_names=tuple(PREFIX + j.name for j in robot.joints),
380
+ qpos_adr=model.jnt_qposadr[joint_ids].astype(int),
381
+ qvel_adr=model.jnt_dofadr[joint_ids].astype(int),
382
+ actuator_ids=act_ids,
383
+ ee_site_id=ee_site,
384
+ joint_range=joint_range,
385
+ effort_limit=np.array([j.effort_limit for j in robot.joints]),
386
+ home_qpos=np.array(robot.home_qpos),
387
+ mount=mount,
388
+ kp=kp,
389
+ kd=kd,
390
+ ctrl_range=ctrl_range,
391
+ root_qpos_adr=root_adr,
392
+ home_root=home_root,
393
+ reset_qpos=reset_qpos,
394
+ held_ctrl=held,
395
+ )
396
+ return CompiledRobot(model=model, spec=spec, interface=interface)
397
+
398
+
399
+ class MujocoKinematics:
400
+ """The host-side kinematics oracle of a MuJoCo-based robot provider (``RobotProvider.kinematics``): the model is
401
+ compiled alone, its world frame is the robot's base frame, and ``mj_kinematics`` gives the end effector."""
402
+
403
+ def __init__(self, robot: ExternalRobot, spec: mujoco.MjSpec):
404
+ if robot.end_effector is None:
405
+ raise ConfigError("robot.end_effector", "the robot has no end effector: it has no kinematics to ask for")
406
+ self.robot = robot
407
+ self.model = spec.compile()
408
+ self.data = mujoco.MjData(self.model)
409
+ m = self.model
410
+ self._adr = np.array([m.jnt_qposadr[_id(m, _OBJ.mjOBJ_JOINT, j.name)] for j in robot.joints])
411
+ ee = robot.end_effector
412
+ self._site = _id(m, _OBJ.mjOBJ_SITE, ee.site) if ee.site is not None else None
413
+ self._body = _id(m, _OBJ.mjOBJ_BODY, ee.body) if ee.body is not None else None
414
+ self._offset = np.asarray(ee.offset.pos)
415
+ self._rest = m.qpos0.copy()
416
+ if robot.keyframe is not None:
417
+ self._rest = m.key_qpos[_id(m, _OBJ.mjOBJ_KEY, robot.keyframe)].copy()
418
+ for j in range(m.njnt):
419
+ if int(m.jnt_type[j]) == _FREE and robot.home_root is not None:
420
+ a = m.jnt_qposadr[j]
421
+ self._rest[a : a + 7] = [*robot.home_root.pos, *robot.home_root.quat]
422
+ self._lo = np.array([-np.pi if j.range is None else j.range[0] for j in robot.joints])
423
+ self._hi = np.array([np.pi if j.range is None else j.range[1] for j in robot.joints])
424
+
425
+ def forward_ee_batch(self, q, base: Pose | None = None) -> np.ndarray:
426
+ q = np.asarray(q, dtype=float)
427
+ if q.ndim != 2 or q.shape[1] != self.robot.n:
428
+ raise ValueError(f"q must have shape (B, {self.robot.n}), got {q.shape}")
429
+ out = np.empty((len(q), 3))
430
+ for i, qi in enumerate(q):
431
+ self.data.qpos[:] = self._rest
432
+ self.data.qpos[self._adr] = qi
433
+ mujoco.mj_kinematics(self.model, self.data)
434
+ if self._site is not None:
435
+ out[i] = self.data.site_xpos[self._site]
436
+ else:
437
+ out[i] = self.data.xpos[self._body] + self.data.xmat[self._body].reshape(3, 3) @ self._offset
438
+ if base is not None:
439
+ out = out @ quat_to_matrix(base.quat).T + np.asarray(base.pos)
440
+ return out
441
+
442
+ def sample_joint_positions(self, num: int, rng: np.random.Generator) -> np.ndarray:
443
+ return rng.uniform(self._lo, self._hi, size=(num, self.robot.n))
@@ -0,0 +1,65 @@
1
+ """The robot base in the world, read from the compiled scene.
2
+
3
+ The scene owns the mount frame. A task that has one adds a *marker*, a site in a static place, in ``attach(spec)`` and names
4
+ it with ``TaskImpl.mount_site()``. The compile reads the marker's world pose from a compile of the scene alone (static: the
5
+ pose does not depend on how the marker is nested or prefixed) and creates the ``base`` body at ``marker @ mount``. The pose
6
+ the provider's ``mount_frame(params)`` gave (used by ``viz`` and the kinematics oracle) must agree with the marker: when it
7
+ does not, the compile raises and names both poses. A task without a marker (``mount_site()`` returns ``None``: the simulator
8
+ presets, ``empty/idle``) has the provider's frame, which is the world origin for them.
9
+ """
10
+
11
+ from __future__ import annotations
12
+
13
+ import mujoco
14
+ import numpy as np
15
+
16
+ from ...config import EnvConfig, Pose, scene_frame
17
+ from ...config.mount import mount_in
18
+ from ...errors import ConfigError
19
+ from .scenes import apply_scene
20
+
21
+ __all__ = ["POSE_ATOL", "marker_pose", "resolve_world_mount"]
22
+
23
+ POSE_ATOL = 1e-6 # m for the position, and for each component of the quaternion (sign aside)
24
+
25
+
26
+ def marker_pose(cfg: EnvConfig, scene_hook, site: str) -> Pose:
27
+ """The world pose of the site ``site`` in the scene of ``cfg`` without the robot."""
28
+ spec = mujoco.MjSpec()
29
+ spec.compiler.degree = False # all angles here are radians
30
+ apply_scene(spec, cfg.sim.scene)
31
+ if scene_hook is not None:
32
+ scene_hook(spec)
33
+ model = spec.compile()
34
+ sid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, site)
35
+ if sid < 0:
36
+ raise ConfigError("task", f"the task names the mount marker {site!r} but its scene has no site of that name")
37
+ data = mujoco.MjData(model)
38
+ mujoco.mj_forward(model, data)
39
+ quat = np.zeros(4)
40
+ mujoco.mju_mat2Quat(quat, data.site_xmat[sid])
41
+ return Pose(tuple(float(x) for x in data.site_xpos[sid]), tuple(float(x) for x in quat))
42
+
43
+
44
+ def _agree(a: Pose, b: Pose) -> bool:
45
+ qa, qb = np.asarray(a.quat), np.asarray(b.quat)
46
+ return bool(
47
+ np.allclose(a.pos, b.pos, atol=POSE_ATOL)
48
+ and (np.allclose(qa, qb, atol=POSE_ATOL) or np.allclose(qa, -qb, atol=POSE_ATOL))
49
+ )
50
+
51
+
52
+ def resolve_world_mount(cfg: EnvConfig, scene_hook=None, mount_site: str | None = None) -> Pose:
53
+ """``frame @ mount``, where ``frame`` is the marker's world pose when the task has a marker (checked against the
54
+ provider's ``mount_frame``) and the provider's frame otherwise."""
55
+ frame = scene_frame(cfg)
56
+ if mount_site is not None:
57
+ found = marker_pose(cfg, scene_hook, mount_site)
58
+ if not _agree(found, frame):
59
+ raise ConfigError(
60
+ "task",
61
+ f"the scene's mount marker {mount_site!r} is at {found.to_dict()} but the provider's mount_frame says "
62
+ f"{frame.to_dict()}: viz and the kinematics oracle use the second, the simulator the first",
63
+ )
64
+ frame = found
65
+ return mount_in(frame, cfg.task.mount)
@@ -0,0 +1,158 @@
1
+ """Rendering for the MuJoCo-based environments (CPU and Warp). Nothing but their ``render`` imports this module.
2
+
3
+ Needs an OpenGL context. On headless machines set ``MUJOCO_GL=egl`` before the first import of mujoco.
4
+
5
+ A frame shows everything in the MuJoCo model (floor, robot, any scene or object geometry) plus the overlays the task
6
+ draws for state that is not part of the model (``TaskImpl.overlays``, using ``add_geom`` and ``add_line`` below).
7
+ Rendering works on a scratch ``MjData`` that is synchronised from the live one, so it never changes the
8
+ simulation (no ``mj_forward`` on live data, no shared warm start).
9
+ """
10
+
11
+ from __future__ import annotations
12
+
13
+ import mujoco
14
+ import numpy as np
15
+
16
+ from ...viz.camera import VIEWS, CameraSpec, auto_camera
17
+
18
+ _GL_HINT = (
19
+ "could not create an offscreen OpenGL context ({err}). On headless machines set MUJOCO_GL=egl "
20
+ "before the first import of mujoco."
21
+ )
22
+
23
+
24
+ def add_geom(scene: mujoco.MjvScene, gtype, size, pos, mat, rgba) -> None:
25
+ if scene.ngeom >= scene.maxgeom:
26
+ return
27
+ mujoco.mjv_initGeom(
28
+ scene.geoms[scene.ngeom],
29
+ gtype,
30
+ np.asarray(size, dtype=float),
31
+ np.asarray(pos, dtype=float),
32
+ np.asarray(mat, dtype=float).ravel(),
33
+ np.asarray(rgba, dtype=np.float32),
34
+ )
35
+ scene.ngeom += 1
36
+
37
+
38
+ def add_line(scene: mujoco.MjvScene, a, b, rgba, width_px: float = 2.0) -> None:
39
+ """A GL line from ``a`` to ``b``, ``width_px`` wide whatever the zoom. Lines cast no shadow, unlike solid geoms."""
40
+ if scene.ngeom >= scene.maxgeom:
41
+ return
42
+ g = scene.geoms[scene.ngeom]
43
+ gtype = mujoco.mjtGeom.mjGEOM_LINE
44
+ mujoco.mjv_initGeom(
45
+ g, gtype, np.array([width_px, 0.0, 0.0]), np.zeros(3), np.eye(3).ravel(), np.asarray(rgba, dtype=np.float32)
46
+ )
47
+ mujoco.mjv_connector(g, gtype, width_px, np.asarray(a, dtype=float), np.asarray(b, dtype=float))
48
+ scene.ngeom += 1
49
+
50
+
51
+ class EnvRenderer:
52
+ """One persistent offscreen renderer for a ``MujocoEnv``, recreated only when the frame size changes."""
53
+
54
+ def __init__(self, env):
55
+ self._env = env
56
+ self._model = env._sim.model
57
+ self._data = mujoco.MjData(self._model) # scratch state, never stepped
58
+ self._renderer: mujoco.Renderer | None = None
59
+ self._size: tuple[int, int] | None = None
60
+ self._cameras: dict[tuple[str, int, int], mujoco.MjvCamera] = {}
61
+
62
+ # --- setup --------------------------------------------------------------------------------------
63
+
64
+ def _ensure(self, height: int, width: int) -> mujoco.Renderer:
65
+ if self._renderer is not None and self._size == (height, width):
66
+ return self._renderer
67
+ self.close()
68
+ g = self._model.vis.global_ # the offscreen buffer is 640x480 unless the model says otherwise
69
+ g.offwidth, g.offheight = max(int(g.offwidth), width), max(int(g.offheight), height)
70
+ try:
71
+ self._renderer = mujoco.Renderer(self._model, height, width)
72
+ except Exception as err:
73
+ raise RuntimeError(_GL_HINT.format(err=f"{type(err).__name__}: {err}")) from err
74
+ self._size = (height, width)
75
+ return self._renderer
76
+
77
+ def _camera(self, camera, height: int, width: int):
78
+ if camera is None:
79
+ camera = "iso"
80
+ if isinstance(camera, CameraSpec):
81
+ return camera.to_mjv()
82
+ if isinstance(camera, str) and camera in VIEWS:
83
+ key = (camera, height, width)
84
+ if key not in self._cameras:
85
+ spec = auto_camera(
86
+ self._env.cfg, view=camera, fovy=float(self._model.vis.global_.fovy), aspect=width / height
87
+ )
88
+ self._cameras[key] = spec.to_mjv()
89
+ return self._cameras[key]
90
+ return camera # a mujoco.MjvCamera, or the name / id of a camera in the model
91
+
92
+ # --- state --------------------------------------------------------------------------------------
93
+
94
+ def _sync(self, live: mujoco.MjData) -> None:
95
+ d = self._data
96
+ d.qpos[:] = live.qpos
97
+ d.qvel[:] = live.qvel
98
+ d.act[:] = live.act
99
+ d.ctrl[:] = live.ctrl
100
+ d.mocap_pos[:] = live.mocap_pos
101
+ d.mocap_quat[:] = live.mocap_quat
102
+ d.time = live.time
103
+ mujoco.mj_forward(self._model, d)
104
+
105
+ def _overlays(self, scene: mujoco.MjvScene, env_index: int) -> None:
106
+ env = self._env
107
+ env._task.overlays(scene, env._sim, self._data, env_index)
108
+
109
+ # --- public -------------------------------------------------------------------------------------
110
+
111
+ def render(self, env_index: int = 0, *, camera=None, size=(240, 320), overlays: bool = True) -> np.ndarray:
112
+ n = self._env._sim.num_envs
113
+ if not 0 <= env_index < n:
114
+ raise IndexError(f"env_index {env_index} out of range for {n} envs")
115
+ height, width = int(size[0]), int(size[1])
116
+ if height < 1 or width < 1:
117
+ raise ValueError(f"size must be positive (height, width), got {size}")
118
+ renderer = self._ensure(height, width)
119
+ self._sync(self._env._sim.host_data(env_index))
120
+ renderer.update_scene(self._data, camera=self._camera(camera, height, width))
121
+ if overlays:
122
+ self._overlays(renderer.scene, env_index)
123
+ return renderer.render().copy()
124
+
125
+ def close(self) -> None:
126
+ if self._renderer is not None:
127
+ self._renderer.close()
128
+ self._renderer, self._size = None, None
129
+
130
+
131
+ def get_renderer(env) -> EnvRenderer:
132
+ renderer = getattr(env, "_renderer", None)
133
+ if renderer is None:
134
+ renderer = env._renderer = EnvRenderer(env)
135
+ return renderer
136
+
137
+
138
+ def render_rgb(env, env_index: int = 0, camera=None, size: tuple[int, int] = (240, 320)) -> np.ndarray:
139
+ """Render one env to a ``uint8`` array ``(H, W, 3)``; same as ``env.render(env_index, ...)``.
140
+
141
+ ``camera`` is ``None`` (the ``"iso"`` view framed on the workspace), a view name, a ``CameraSpec``, or the
142
+ name / id of a camera in the model.
143
+ """
144
+ return env.render(env_index, camera=camera, size=size)
145
+
146
+
147
+ def launch_viewer(env, env_index: int = 0) -> None:
148
+ """Open a passive viewer on one env's data. Needs a display."""
149
+ import mujoco.viewer
150
+
151
+ sim = env._sim
152
+ if not hasattr(sim, "data"):
153
+ raise NotImplementedError("the live viewer needs the live state of a CPU simulation")
154
+ if not 0 <= env_index < sim.num_envs:
155
+ raise IndexError(f"env_index {env_index} out of range for {sim.num_envs} envs")
156
+ with mujoco.viewer.launch_passive(sim.model, sim.data[env_index]) as viewer:
157
+ while viewer.is_running():
158
+ viewer.sync()