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,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))
|