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,130 @@
1
+ """Engine side of ``menagerie.describe``: open a model and read the numbers an ``ExternalRobot`` needs."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import warnings
6
+
7
+ import mujoco
8
+ import numpy as np
9
+
10
+ from ....config import EndEffectorRef, ExternalJoint, ExternalRobot, Pose
11
+ from ....errors import ConfigError
12
+ from ....simulators.mujoco.external import actuator_kind
13
+
14
+ O = mujoco.mjtObj
15
+ # Sites that conventional names mark as the end effector, in order of preference.
16
+ EE_SITES = ("attachment_site", "tcp_site", "pinch_site", "gripperframe", "pinch", "ee_gripper_link", "link_tcp")
17
+ _HINGE, _SLIDE, _FREE = (int(mujoco.mjtJoint.mjJNT_HINGE), int(mujoco.mjtJoint.mjJNT_SLIDE), int(mujoco.mjtJoint.mjJNT_FREE))
18
+ _JOINT_TRN = int(mujoco.mjtTrn.mjTRN_JOINT)
19
+
20
+
21
+ def _end_effector_from_card(card: dict) -> EndEffectorRef:
22
+ offset = card.get("offset")
23
+ return EndEffectorRef(
24
+ site=card.get("site"),
25
+ body=card.get("body"),
26
+ offset=Pose.identity() if offset is None else Pose(tuple(offset), (1.0, 0.0, 0.0, 0.0)),
27
+ )
28
+
29
+
30
+ def describe(provider, name, entry, controlled, end_effector, keyframe, effort_limit) -> ExternalRobot:
31
+ from . import DEFAULT, EXTRA, _entry_meta, _robot_meta, cards
32
+
33
+ meta = _robot_meta(name)
34
+ card = cards().get(name, {})
35
+ entry = entry or card.get("entry") or meta.default_model
36
+ if _entry_meta(meta, entry).kind == "scene":
37
+ raise ConfigError("entry", f"{entry!r} is a scene: the compile and the scene presets bring floor and lights; use a robot entry")
38
+ try:
39
+ model = meta.spec(entry).compile()
40
+ except Exception as e:
41
+ raise ConfigError("name", f"menagerie/{name} could not be opened ({type(e).__name__}: {e}); {EXTRA}") from e
42
+ jname = lambda j: mujoco.mj_id2name(model, O.mjOBJ_JOINT, j)
43
+
44
+ # --- the driven joints and the actuator of each ---
45
+ joint_act: dict[int, int] = {}
46
+ for a in range(model.nu):
47
+ if int(model.actuator_trntype[a]) == _JOINT_TRN:
48
+ joint_act.setdefault(int(model.actuator_trnid[a, 0]), a)
49
+ if controlled is None:
50
+ controlled = card.get("controlled")
51
+ if controlled is None:
52
+ driven = list(joint_act)
53
+ else:
54
+ driven = []
55
+ for n in controlled:
56
+ j = mujoco.mj_name2id(model, O.mjOBJ_JOINT, n)
57
+ if j < 0:
58
+ raise ConfigError("controlled", f"menagerie/{name} has no joint {n!r}")
59
+ if int(model.jnt_type[j]) == _FREE:
60
+ raise ConfigError("controlled", f"{n!r} is a free joint: a free root is never driven")
61
+ if j not in joint_act:
62
+ raise ConfigError("controlled", f"joint {n!r} of menagerie/{name} has no actuator with a joint transmission")
63
+ driven.append(j)
64
+ driven = [j for j in driven if int(model.jnt_type[j]) in (_HINGE, _SLIDE)]
65
+ if not driven:
66
+ raise ConfigError("name", f"menagerie/{name} has no joint with an actuator that could be driven (actuators: {model.nu})")
67
+
68
+ # --- ranges, effort limits ---
69
+ joints = []
70
+ for j in driven:
71
+ a = joint_act[j]
72
+ n = jname(j)
73
+ rng = tuple(float(x) for x in model.jnt_range[j]) if model.jnt_limited[j] else None
74
+ if model.actuator_forcelimited[a]:
75
+ effort = float(np.abs(model.actuator_forcerange[a]).max())
76
+ elif model.jnt_actfrclimited[j]:
77
+ effort = float(np.abs(model.jnt_actfrcrange[j]).max())
78
+ elif actuator_kind(model, a) == "torque" and model.actuator_ctrllimited[a]:
79
+ # a plain motor: force = gain * ctrl * gear, so its ctrlrange is its force limit
80
+ effort = float(np.abs(model.actuator_ctrlrange[a]).max() * model.actuator_gainprm[a, 0] * abs(model.actuator_gear[a, 0]))
81
+ elif isinstance(effort_limit, dict) and n in effort_limit:
82
+ effort = float(effort_limit[n])
83
+ elif effort_limit is not None and not isinstance(effort_limit, dict):
84
+ effort = float(effort_limit)
85
+ else:
86
+ raise ConfigError(
87
+ "effort_limit",
88
+ f"joint {n!r} of menagerie/{name} has no force limit in the model (actuator or joint): pass "
89
+ f"effort_limit={{{n!r}: <N·m or N>}} or one number for all joints",
90
+ )
91
+ joints.append(ExternalJoint(name=n, range=rng, effort_limit=effort))
92
+
93
+ # --- keyframe, home posture, root ---
94
+ if keyframe is DEFAULT:
95
+ keyframe = card["keyframe"] if "keyframe" in card else ("home" if mujoco.mj_name2id(model, O.mjOBJ_KEY, "home") >= 0 else None)
96
+ key_id = None
97
+ if keyframe is not None:
98
+ key_id = mujoco.mj_name2id(model, O.mjOBJ_KEY, keyframe)
99
+ if key_id < 0:
100
+ raise ConfigError("keyframe", f"menagerie/{name} has no keyframe {keyframe!r}")
101
+ state = model.qpos0 if key_id is None else model.key_qpos[key_id]
102
+ home = []
103
+ for j, spec in zip(driven, joints):
104
+ q = float(state[model.jnt_qposadr[j]])
105
+ if spec.range is not None and not spec.range[0] <= q <= spec.range[1]:
106
+ clipped = min(max(q, spec.range[0]), spec.range[1])
107
+ warnings.warn(f"menagerie/{name}: home value {q:.4f} of {spec.name!r} is outside its range {list(spec.range)}; clipped to {clipped:.4f}", stacklevel=3)
108
+ q = clipped
109
+ home.append(q)
110
+ free = [j for j in range(model.njnt) if int(model.jnt_type[j]) == _FREE]
111
+ home_root = None
112
+ if free:
113
+ a = model.jnt_qposadr[free[0]]
114
+ home_root = Pose(tuple(float(x) for x in state[a : a + 3]), tuple(float(x) for x in state[a + 3 : a + 7]))
115
+
116
+ # --- end effector ---
117
+ if end_effector is None and "end_effector" in card:
118
+ end_effector = _end_effector_from_card(card["end_effector"])
119
+ if end_effector is None:
120
+ for site in EE_SITES:
121
+ if mujoco.mj_name2id(model, O.mjOBJ_SITE, site) >= 0:
122
+ end_effector = EndEffectorRef(site=site, body=None, offset=Pose.identity())
123
+ break
124
+
125
+ kinds = {actuator_kind(model, joint_act[j]) for j in driven}
126
+ return ExternalRobot(
127
+ provider="menagerie", name=name, entry=entry, source=meta.oid, joints=tuple(joints), home_qpos=tuple(home),
128
+ home_root=home_root, keyframe=keyframe, end_effector=end_effector,
129
+ actuator_kind=next(iter(kinds)) if len(kinds) == 1 else "mixed",
130
+ )
@@ -0,0 +1,37 @@
1
+ {
2
+ "_comment": "What cannot be derived from a MuJoCo Menagerie model, per robot: 'entry' (the model file to use when it is not the library's default), 'controlled' (the driven joints, where a finger or gripper is a joint actuator), 'end_effector' (a site, or a body plus an offset in the body's frame, where the model has no suitable site) and 'keyframe' (a keyframe that is not called 'home', or null for none). A key that is absent is derived. Body offsets are approximations of the gripper mounting point or tool centre, not calibrated values.",
3
+ "agilex_piper": {"controlled": ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], "end_effector": {"body": "link6", "offset": [0.0, 0.0, 0.135]}},
4
+ "arx_l5": {"controlled": ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], "end_effector": {"body": "link6", "offset": [0.087, 0.0, 0.0]}},
5
+ "dynamixel_2r": {"end_effector": {"site": "end"}, "keyframe": null},
6
+ "fanuc_crx5ia": {},
7
+ "flexiv_rizon4": {"end_effector": {"body": "link7"}},
8
+ "flexiv_rizon4s": {"end_effector": {"body": "link7"}},
9
+ "franka_emika_panda": {"end_effector": {"body": "hand", "offset": [0.0, 0.0, 0.1034]}},
10
+ "franka_fr3": {},
11
+ "franka_fr3_v2": {"end_effector": {"body": "fr3v2_link8"}},
12
+ "i2rt_yam": {"controlled": ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]},
13
+ "kinova_gen3": {},
14
+ "kuka_iiwa_14": {},
15
+ "low_cost_robot_arm": {"controlled": ["base_rotation", "pitch", "elbow", "wrist_pitch", "wrist_roll"], "end_effector": {"body": "gripper_static_finger"}},
16
+ "rethink_robotics_sawyer": {},
17
+ "robotis_omy_3m": {},
18
+ "robotstudio_so101": {"controlled": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll"], "keyframe": null},
19
+ "seeed_rebot_devarm": {"controlled": ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], "end_effector": {"body": "gripper_end"}},
20
+ "trossen_vx300s": {"controlled": ["waist", "shoulder", "elbow", "forearm_roll", "wrist_angle", "wrist_rotate"]},
21
+ "trossen_wx250s": {"controlled": ["waist", "shoulder", "elbow", "forearm_roll", "wrist_angle", "wrist_rotate"], "end_effector": {"body": "wx250s/gripper_link", "offset": [0.066, 0.0, 0.0]}},
22
+ "trossen_wxai": {"entry": "wxai_follower", "controlled": ["joint_0", "joint_1", "joint_2", "joint_3", "joint_4", "joint_5"], "keyframe": null},
23
+ "trs_so_arm100": {"controlled": ["Rotation", "Pitch", "Elbow", "Wrist_Pitch", "Wrist_Roll"], "end_effector": {"body": "Fixed_Jaw"}},
24
+ "ufactory_lite6": {},
25
+ "ufactory_xarm7": {},
26
+ "unitree_z1": {"end_effector": {"body": "link06"}},
27
+ "universal_robots_ur10e": {},
28
+ "universal_robots_ur5e": {},
29
+ "anybotics_anymal_b": {"keyframe": null},
30
+ "anybotics_anymal_c": {"keyframe": null},
31
+ "boston_dynamics_spot": {},
32
+ "google_barkour_v0": {"keyframe": "standing"},
33
+ "google_barkour_vb": {"keyframe": null},
34
+ "unitree_a1": {},
35
+ "unitree_go1": {},
36
+ "unitree_go2": {}
37
+ }
@@ -0,0 +1,51 @@
1
+ """Simulator registry. Importing this module never imports mujoco, torch or torchrl."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import importlib
6
+ from typing import Any
7
+
8
+ from ..config import ConfigError
9
+ from .base import Simulator, Capabilities, Problem, check_capabilities
10
+
11
+ __all__ = [
12
+ "Simulator",
13
+ "Capabilities",
14
+ "Problem",
15
+ "check_capabilities",
16
+ "get_simulator",
17
+ "list_simulators",
18
+ "register_simulator",
19
+ ]
20
+
21
+ # name -> simulator object, or a lazy "module:attr" string resolved on first use
22
+ _REGISTRY: dict[str, Any] = {}
23
+
24
+
25
+ def register_simulator(name: str, target: Any) -> None:
26
+ """Register a simulator by name. ``target`` is a simulator instance or a lazy ``"module:attr"`` entry.
27
+
28
+ The attribute may be a simulator class (instantiated without arguments) or an instance.
29
+ """
30
+ _REGISTRY[name] = target
31
+
32
+
33
+ def list_simulators() -> tuple[str, ...]:
34
+ return tuple(sorted(_REGISTRY))
35
+
36
+
37
+ def get_simulator(name: str) -> Simulator:
38
+ if name not in _REGISTRY:
39
+ raise ConfigError("sim.simulator", f"unknown simulator {name!r}; registered: {list(list_simulators())}")
40
+ target = _REGISTRY[name]
41
+ if isinstance(target, str):
42
+ module, _, attr = target.partition(":")
43
+ target = getattr(importlib.import_module(module), attr)
44
+ if isinstance(target, type):
45
+ target = target()
46
+ _REGISTRY[name] = target
47
+ return target
48
+
49
+
50
+ register_simulator("mujoco", "robotenv.simulators.mujoco:MujocoSimulator")
51
+ register_simulator("warp", "robotenv.simulators.warp:WarpSimulator")
@@ -0,0 +1,70 @@
1
+ """Simulator protocol and capability checks. No heavy imports.
2
+
3
+ A simulator is a physics engine realized as code (``robotenv.simulators.<name>``). Which tasks exist is the business of task providers (``config.providers``); a provider
4
+ checks its own tasks against the simulator (``TaskProvider.validate``), so a simulator never lists task ids.
5
+ """
6
+
7
+ from __future__ import annotations
8
+
9
+ from dataclasses import dataclass
10
+ from typing import Any, Protocol, runtime_checkable
11
+
12
+ from ..errors import Problem
13
+
14
+
15
+ _TASK_STREAM = 0x5DEECE66D # offset of the task's random stream from ``sim.seed``, so the two streams differ
16
+
17
+
18
+ def task_seed(seed: int) -> int:
19
+ """Seed of the task's random stream, shared by every simulator so the same seed gives the same task draws."""
20
+ return (int(seed) + _TASK_STREAM) % (2**63)
21
+
22
+
23
+ @dataclass(frozen=True)
24
+ class Capabilities:
25
+ control_modes: frozenset[str]
26
+ devices: frozenset[str] # {"cpu"}; "cuda" matches any "cuda:N"
27
+ robot_kinds: frozenset[str] # {"parametric"}
28
+ integrators: frozenset[str]
29
+
30
+
31
+ @runtime_checkable
32
+ class Simulator(Protocol):
33
+ name: str
34
+
35
+ def capabilities(self) -> Capabilities: ...
36
+
37
+ def validate(self, cfg: Any) -> list[Problem]:
38
+ """Engine-specific checks beyond the capabilities."""
39
+ ...
40
+
41
+ def make_env(self, cfg: Any) -> Any:
42
+ """Return a torchrl ``EnvBase``."""
43
+ ...
44
+
45
+
46
+ def _robot_kind(robot: Any) -> str:
47
+ kinds = {"ParametricArm": "parametric", "ExternalRobot": "external"}
48
+ return kinds.get(type(robot).__name__, type(robot).__name__)
49
+
50
+
51
+ def check_capabilities(cfg: Any, caps: Capabilities, simulator: str = "") -> list[Problem]:
52
+ """Every way ``cfg`` falls outside ``caps``, as problems naming the config path."""
53
+ who = f"simulator {simulator!r}" if simulator else "this simulator"
54
+ problems: list[Problem] = []
55
+
56
+ def check(path: str, value: str, allowed: frozenset[str], what: str) -> None:
57
+ if value not in allowed:
58
+ problems.append(Problem(path, f"{who} does not support {what} {value!r}; supported: {sorted(allowed)}"))
59
+
60
+ check("control.mode", cfg.control.mode, caps.control_modes, "control mode")
61
+ check("sim.integrator", cfg.sim.integrator, caps.integrators, "integrator")
62
+ check("robot", _robot_kind(cfg.robot), caps.robot_kinds, "robot kind")
63
+
64
+ device = cfg.sim.device
65
+ family = "cuda" if device.startswith("cuda") else device
66
+ if family not in caps.devices:
67
+ problems.append(
68
+ Problem("sim.device", f"{who} does not support device {device!r}; supported: {sorted(caps.devices)}")
69
+ )
70
+ return problems
@@ -0,0 +1,38 @@
1
+ """CPU MuJoCo simulator: the reference and debugging simulator."""
2
+
3
+ from __future__ import annotations
4
+
5
+ from typing import Any
6
+
7
+ from ..base import Capabilities, Problem
8
+
9
+
10
+ class MujocoSimulator:
11
+ name = "mujoco"
12
+
13
+ def capabilities(self) -> Capabilities:
14
+ return Capabilities(
15
+ control_modes=frozenset({"joint_pos_abs", "joint_pos_delta", "joint_torque", "joint_vel"}),
16
+ devices=frozenset({"cpu"}),
17
+ robot_kinds=frozenset({"parametric", "external"}),
18
+ integrators=frozenset({"euler", "implicit", "implicitfast", "rk4"}),
19
+ )
20
+
21
+ def validate(self, cfg: Any) -> list[Problem]:
22
+ from .scenes import SCENES
23
+
24
+ problems = []
25
+ if cfg.sim.scene not in SCENES:
26
+ problems.append(Problem("sim.scene", f"unknown scene {cfg.sim.scene!r}; available: {sorted(SCENES)}"))
27
+ if type(cfg.robot).__name__ == "ExternalRobot":
28
+ from .external import problems_of
29
+
30
+ problems += problems_of(cfg)
31
+ return problems
32
+
33
+ def make_env(self, cfg: Any):
34
+ from ...config.providers import get_task_provider
35
+ from .env import MujocoEnv
36
+
37
+ provider = get_task_provider(cfg.task.params.provider)
38
+ return MujocoEnv(cfg, provider.build(cfg, self.name))
@@ -0,0 +1,137 @@
1
+ """The kinematic chain of a ``ParametricArm`` as MuJoCo spec bodies, shared by every MuJoCo-based simulator."""
2
+
3
+ from __future__ import annotations
4
+
5
+ import mujoco
6
+ import numpy as np
7
+
8
+ from ...robot import ParametricArm, TwoFingerGripper
9
+
10
+ _LIMITED = mujoco.mjtLimited.mjLIMITED_TRUE
11
+ _UNLIMITED = mujoco.mjtLimited.mjLIMITED_FALSE
12
+
13
+ # Collision filtering: robot geoms have contype 1; scene geoms have conaffinity 1 (see scenes.py).
14
+ # Giving robot geoms conaffinity 1 as well turns on self-collision.
15
+ ROBOT_CONTYPE = 1
16
+
17
+
18
+ def add_arm(parent, arm: ParametricArm, *, conaffinity: int):
19
+ """Add ``link_i`` bodies, ``joint_i`` hinges and ``geom_i`` capsules under ``parent``.
20
+
21
+ ``parent`` is the base body: the base pose (the task's mount) is the caller's business. Angles in the spec are radians
22
+ (the caller must set ``spec.compiler.degree = False``). Returns ``(last_body, tip)`` where ``tip`` is the last
23
+ link's ``tip_offset`` in its own frame, from which the caller places the end effector.
24
+ """
25
+ prev_tip = np.zeros(3)
26
+ for i, seg in enumerate(arm.segments):
27
+ j, link = seg.joint, seg.link
28
+ # Pose(previous tip_offset, identity) composed with joint_frame: the rotation is joint_frame's.
29
+ body = parent.add_body(
30
+ name=f"link_{i}",
31
+ pos=list(prev_tip + np.asarray(link.joint_frame.pos)),
32
+ quat=list(link.joint_frame.quat),
33
+ )
34
+ lo_hi = j.range
35
+ body.add_joint(
36
+ name=f"joint_{i}",
37
+ type=mujoco.mjtJoint.mjJNT_HINGE,
38
+ axis=list(j.axis),
39
+ limited=_UNLIMITED if lo_hi is None else _LIMITED,
40
+ range=[0.0, 0.0] if lo_hi is None else list(lo_hi),
41
+ damping=j.damping,
42
+ armature=j.armature,
43
+ frictionloss=j.frictionloss,
44
+ )
45
+ geom_kwargs = dict(
46
+ name=f"geom_{i}",
47
+ type=mujoco.mjtGeom.mjGEOM_CAPSULE,
48
+ fromto=[0.0, 0.0, 0.0, *link.tip_offset],
49
+ size=[link.geometry.radius, 0.0, 0.0],
50
+ contype=ROBOT_CONTYPE,
51
+ conaffinity=conaffinity,
52
+ )
53
+ if link.inertia is None:
54
+ # Mass and inertia derived from the capsule at the configured mass.
55
+ body.add_geom(mass=link.mass, **geom_kwargs)
56
+ else:
57
+ # Explicit principal moments about the centre of mass (capsule midpoint), axes of the link frame.
58
+ body.add_geom(mass=0.0, **geom_kwargs)
59
+ body.explicitinertial = True
60
+ body.mass = link.mass
61
+ body.ipos = list(np.asarray(link.tip_offset) / 2.0)
62
+ body.iquat = [1.0, 0.0, 0.0, 0.0]
63
+ body.inertia = list(link.inertia)
64
+ prev_tip = np.asarray(link.tip_offset, dtype=float)
65
+ parent = body
66
+ return parent, prev_tip
67
+
68
+
69
+ def _quat_mul(a, b) -> list[float]:
70
+ aw, ax, ay, az = a
71
+ bw, bx, by, bz = b
72
+ return [
73
+ aw * bw - ax * bx - ay * by - az * bz,
74
+ aw * bx + ax * bw + ay * bz - az * by,
75
+ aw * by - ax * bz + ay * bw + az * bx,
76
+ aw * bz + ax * by - ay * bx + az * bw,
77
+ ]
78
+
79
+
80
+ def _rotate(quat, v) -> np.ndarray:
81
+ w, x, y, z = quat
82
+ R = np.array(
83
+ [
84
+ [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
85
+ [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
86
+ [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
87
+ ]
88
+ )
89
+ return R @ np.asarray(v, dtype=float)
90
+
91
+
92
+ def add_gripper(parent, arm: ParametricArm, tip: np.ndarray, *, conaffinity: int) -> None:
93
+ """Add the arm's ``TwoFingerGripper`` under the last link ``parent`` (``tip`` is that link's tip, as returned by
94
+ :func:`add_arm`).
95
+
96
+ Names: body ``gripper``, bodies ``finger_0`` / ``finger_1`` with slide joints ``finger_joint_0`` / ``finger_joint_1``
97
+ (coupled by an equality constraint), tendon ``gripper_width`` (the sum of both joints), actuator ``act_gripper`` and
98
+ site ``tcp``. The gripper frame is ``end_effector`` composed with ``gripper.mount``.
99
+ """
100
+ g: TwoFingerGripper = arm.gripper
101
+ ee = arm.end_effector
102
+ pos = tip + np.asarray(ee.pos) + _rotate(ee.quat, g.mount.pos)
103
+ base = parent.add_body(name="gripper", pos=list(pos), quat=_quat_mul(ee.quat, g.mount.quat))
104
+ r = g.finger_radius
105
+ # Palm: a box from the frame to ``palm_length`` along +x, wide enough to carry both fingers fully open.
106
+ base.add_geom(
107
+ name="palm",
108
+ type=mujoco.mjtGeom.mjGEOM_BOX,
109
+ pos=[g.palm_length / 2.0, 0.0, 0.0],
110
+ size=[g.palm_length / 2.0, g.stroke + 2.0 * r, r],
111
+ mass=g.palm_mass,
112
+ contype=ROBOT_CONTYPE,
113
+ conaffinity=conaffinity,
114
+ )
115
+ for i, side in enumerate((1.0, -1.0)):
116
+ finger = base.add_body(name=f"finger_{i}", pos=[g.palm_length, side * r, 0.0])
117
+ finger.add_joint(
118
+ name=f"finger_joint_{i}",
119
+ type=mujoco.mjtJoint.mjJNT_SLIDE,
120
+ axis=[0.0, side, 0.0],
121
+ limited=_LIMITED,
122
+ range=[0.0, g.stroke],
123
+ damping=0.0,
124
+ )
125
+ finger.add_geom(
126
+ name=f"finger_geom_{i}",
127
+ type=mujoco.mjtGeom.mjGEOM_CAPSULE,
128
+ fromto=[0.0, 0.0, 0.0, g.finger_length, 0.0, 0.0],
129
+ size=[r, 0.0, 0.0],
130
+ mass=g.finger_mass,
131
+ contype=ROBOT_CONTYPE,
132
+ # Fingers always collide with the scene and with each other's object; the palm and fingers never collide
133
+ # (siblings in the same chain are filtered by MuJoCo's parent-child rule only for direct parents).
134
+ conaffinity=conaffinity,
135
+ friction=[1.5, 0.005, 0.0001],
136
+ )
137
+ base.add_site(name="tcp", pos=list(g.tcp_offset))