mujoco-robot 0.2.0__tar.gz → 0.3.0__tar.gz
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.
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/PKG-INFO +1 -1
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/ik.py +1 -1
- mujoco_robot-0.3.0/mujoco_robot/mujoco_object.py +407 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/mujoco_robot.py +248 -610
- mujoco_robot-0.3.0/mujoco_robot/types.py +142 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot.egg-info/PKG-INFO +1 -1
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot.egg-info/SOURCES.txt +2 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/pyproject.toml +1 -1
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_cosmetics.py +8 -8
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_getters.py +67 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_wrapping.py +26 -3
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/LICENSE +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/README.md +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/__init__.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/py.typed +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/utils/__init__.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/utils/model_builder.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/utils/robot_loader_utils.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot/viewer_utils.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot.egg-info/dependency_links.txt +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot.egg-info/requires.txt +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/mujoco_robot.egg-info/top_level.txt +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/setup.cfg +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_contacts.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_control.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_ik.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_lifecycle.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_object.py +0 -0
- {mujoco_robot-0.2.0 → mujoco_robot-0.3.0}/tests/test_smoke.py +0 -0
|
@@ -0,0 +1,407 @@
|
|
|
1
|
+
"""Model structure for mujoco_robot: name<->id maps and joint/link/dof bookkeeping."""
|
|
2
|
+
|
|
3
|
+
from typing import Dict, List, Mapping, Optional, Sequence
|
|
4
|
+
|
|
5
|
+
import numpy as np
|
|
6
|
+
import mujoco
|
|
7
|
+
|
|
8
|
+
from .types import Frame, MujocoJointInfo
|
|
9
|
+
|
|
10
|
+
# qpos / dof widths per joint type. free=7/6, ball=4/3, slide=1/1, hinge=1/1.
|
|
11
|
+
_QPOS_DIM: Dict[int, int] = {
|
|
12
|
+
int(mujoco.mjtJoint.mjJNT_FREE): 7,
|
|
13
|
+
int(mujoco.mjtJoint.mjJNT_BALL): 4,
|
|
14
|
+
int(mujoco.mjtJoint.mjJNT_SLIDE): 1,
|
|
15
|
+
int(mujoco.mjtJoint.mjJNT_HINGE): 1,
|
|
16
|
+
}
|
|
17
|
+
_DOF_DIM: Dict[int, int] = {
|
|
18
|
+
int(mujoco.mjtJoint.mjJNT_FREE): 6,
|
|
19
|
+
int(mujoco.mjtJoint.mjJNT_BALL): 3,
|
|
20
|
+
int(mujoco.mjtJoint.mjJNT_SLIDE): 1,
|
|
21
|
+
int(mujoco.mjtJoint.mjJNT_HINGE): 1,
|
|
22
|
+
}
|
|
23
|
+
|
|
24
|
+
|
|
25
|
+
def _id_name_maps(model: mujoco.MjModel, obj_type: int, n: int):
|
|
26
|
+
"""Build (names, name->id, id->name) for a MuJoCo element kind. Unnamed elements map to ''."""
|
|
27
|
+
names: List[str] = []
|
|
28
|
+
name_to_id: Dict[str, int] = {}
|
|
29
|
+
id_to_name: Dict[int, str] = {}
|
|
30
|
+
for i in range(n):
|
|
31
|
+
name = mujoco.mj_id2name(model, obj_type, i)
|
|
32
|
+
name = name if name is not None else ""
|
|
33
|
+
names.append(name)
|
|
34
|
+
id_to_name[i] = name
|
|
35
|
+
if name:
|
|
36
|
+
name_to_id[name] = i
|
|
37
|
+
return names, name_to_id, id_to_name
|
|
38
|
+
|
|
39
|
+
|
|
40
|
+
class MujocoObject:
|
|
41
|
+
"""Static information about a MuJoCo object: name<->id maps and joint/link/dof bookkeeping.
|
|
42
|
+
|
|
43
|
+
Built from an existing ``MjModel``. No physics is performed here; this only reads the model's
|
|
44
|
+
structure (equivalent role to reading a loaded body's joint/link tables).
|
|
45
|
+
|
|
46
|
+
By default the whole model is the object: every non-free joint is an actuated joint, every
|
|
47
|
+
body is a link, and the root is the first child of the world. Passing ``joint_names`` scopes
|
|
48
|
+
the object to one robot inside a larger world: only those joints are actuated, and the links
|
|
49
|
+
are the subtree under the root body. Everything else in the model is foreign.
|
|
50
|
+
"""
|
|
51
|
+
|
|
52
|
+
def __init__(
|
|
53
|
+
self,
|
|
54
|
+
model: mujoco.MjModel,
|
|
55
|
+
joint_names: Optional[Sequence[str]] = None,
|
|
56
|
+
root_body: Optional[str] = None,
|
|
57
|
+
):
|
|
58
|
+
"""Read all addressing/structure information from a compiled model.
|
|
59
|
+
|
|
60
|
+
Args:
|
|
61
|
+
model (mujoco.MjModel): A compiled MuJoCo model.
|
|
62
|
+
joint_names (Sequence[str], optional): Scope the object to exactly these 1-dof (hinge
|
|
63
|
+
or slide) joints, in this order. Defaults to None (the whole model).
|
|
64
|
+
root_body (str, optional): Name of the robot's root body. Base pose and velocity are
|
|
65
|
+
read from it, and the base is floating iff it carries a free joint. Defaults to
|
|
66
|
+
the first child of the world, or, when ``joint_names`` is given, the deepest body
|
|
67
|
+
that is an ancestor of every scoped joint.
|
|
68
|
+
"""
|
|
69
|
+
self.model = model
|
|
70
|
+
self.is_scoped: bool = joint_names is not None
|
|
71
|
+
"""Whether this object covers only the given ``joint_names`` (not the whole model)."""
|
|
72
|
+
|
|
73
|
+
obj = mujoco.mjtObj
|
|
74
|
+
self.body_names, self.body_name_to_id, self.body_id_to_name = _id_name_maps(
|
|
75
|
+
model, obj.mjOBJ_BODY, model.nbody
|
|
76
|
+
)
|
|
77
|
+
self.joint_names, self.joint_name_to_id, self.joint_id_to_name = _id_name_maps(
|
|
78
|
+
model, obj.mjOBJ_JOINT, model.njnt
|
|
79
|
+
)
|
|
80
|
+
self.actuator_names, self.actuator_name_to_id, self.actuator_id_to_name = _id_name_maps(
|
|
81
|
+
model, obj.mjOBJ_ACTUATOR, model.nu
|
|
82
|
+
)
|
|
83
|
+
self.site_names, self.site_name_to_id, self.site_id_to_name = _id_name_maps(
|
|
84
|
+
model, obj.mjOBJ_SITE, model.nsite
|
|
85
|
+
)
|
|
86
|
+
self.geom_names, self.geom_name_to_id, self.geom_id_to_name = _id_name_maps(
|
|
87
|
+
model, obj.mjOBJ_GEOM, model.ngeom
|
|
88
|
+
)
|
|
89
|
+
self.sensor_names, self.sensor_name_to_id, self.sensor_id_to_name = _id_name_maps(
|
|
90
|
+
model, obj.mjOBJ_SENSOR, model.nsensor
|
|
91
|
+
)
|
|
92
|
+
|
|
93
|
+
scoped_joint_ids = self._resolve_scoped_joints(joint_names) if self.is_scoped else None
|
|
94
|
+
|
|
95
|
+
# Base = root body: the given one, else the common ancestor of the scoped joints, else
|
|
96
|
+
# the first body whose parent is the world.
|
|
97
|
+
if root_body is not None:
|
|
98
|
+
if root_body not in self.body_name_to_id or self.body_name_to_id[root_body] == 0:
|
|
99
|
+
raise ValueError(f"'{root_body}' is not a body that can be a robot root.")
|
|
100
|
+
self.base_id: int = self.body_name_to_id[root_body]
|
|
101
|
+
elif self.is_scoped:
|
|
102
|
+
self.base_id = self._infer_root_body(scoped_joint_ids)
|
|
103
|
+
else:
|
|
104
|
+
self.base_id = next(
|
|
105
|
+
(i for i in range(1, model.nbody) if int(model.body_parentid[i]) == 0), 1
|
|
106
|
+
)
|
|
107
|
+
self.base_name: str = self.body_id_to_name.get(self.base_id, "")
|
|
108
|
+
self.name: str = self.base_name
|
|
109
|
+
"""Name of the robot (its base/root link name)."""
|
|
110
|
+
|
|
111
|
+
# Links are bodies, excluding the world body (id 0). Scoped: the subtree under the root.
|
|
112
|
+
if self.is_scoped:
|
|
113
|
+
self.link_ids: List[int] = self._subtree_body_ids(self.base_id)
|
|
114
|
+
else:
|
|
115
|
+
self.link_ids = list(range(1, model.nbody))
|
|
116
|
+
self.link_names: List[str] = [self.body_id_to_name[i] for i in self.link_ids]
|
|
117
|
+
self.link_name_to_id: Dict[str, int] = {
|
|
118
|
+
n: i for n, i in zip(self.link_names, self.link_ids) if n
|
|
119
|
+
}
|
|
120
|
+
self.link_id_to_name: Dict[int, str] = dict(zip(self.link_ids, self.link_names))
|
|
121
|
+
|
|
122
|
+
# Geoms belonging to the robot's links (for transparency / collision toggling).
|
|
123
|
+
link_id_set = set(self.link_ids)
|
|
124
|
+
self.robot_geom_ids: List[int] = [
|
|
125
|
+
g for g in range(model.ngeom) if int(model.geom_bodyid[g]) in link_id_set
|
|
126
|
+
]
|
|
127
|
+
|
|
128
|
+
# Map each joint-transmission actuator to the joint it drives (matched by transmission
|
|
129
|
+
# target, so actuator names need not match joint names).
|
|
130
|
+
joint_trn = (int(mujoco.mjtTrn.mjTRN_JOINT), int(mujoco.mjtTrn.mjTRN_JOINTINPARENT))
|
|
131
|
+
self.joint_id_to_actuator_id: Dict[int, int] = {}
|
|
132
|
+
joint_actuators: Dict[int, List[int]] = {}
|
|
133
|
+
for a in range(model.nu):
|
|
134
|
+
if int(model.actuator_trntype[a]) in joint_trn:
|
|
135
|
+
jid = int(model.actuator_trnid[a, 0])
|
|
136
|
+
self.joint_id_to_actuator_id[jid] = a
|
|
137
|
+
joint_actuators.setdefault(jid, []).append(a)
|
|
138
|
+
if self.is_scoped:
|
|
139
|
+
self.robot_actuator_ids: List[int] = sorted(
|
|
140
|
+
a for j in scoped_joint_ids for a in joint_actuators.get(j, [])
|
|
141
|
+
)
|
|
142
|
+
"""Ids of the actuators driving this robot's actuated joints."""
|
|
143
|
+
self.has_actuators: bool = len(self.robot_actuator_ids) > 0
|
|
144
|
+
else:
|
|
145
|
+
self.robot_actuator_ids = list(range(model.nu))
|
|
146
|
+
self.has_actuators = model.nu > 0
|
|
147
|
+
|
|
148
|
+
# Total mass. Per-body dynamics (mass/inertia/com) are cheap direct lookups on the
|
|
149
|
+
# model's ``body_*`` arrays, so there is no per-link info struct; see get_link_mass.
|
|
150
|
+
self.mass: float = float(sum(model.body_mass[i] for i in self.link_ids))
|
|
151
|
+
"""Total mass of this object."""
|
|
152
|
+
|
|
153
|
+
self._build_joint_info(scoped_joint_ids)
|
|
154
|
+
|
|
155
|
+
def _resolve_scoped_joints(self, joint_names: Sequence[str]) -> List[int]:
|
|
156
|
+
"""Validate the scoped joint names and return their ids, in the given order."""
|
|
157
|
+
one_dof = (int(mujoco.mjtJoint.mjJNT_HINGE), int(mujoco.mjtJoint.mjJNT_SLIDE))
|
|
158
|
+
joint_names = list(joint_names)
|
|
159
|
+
if not joint_names:
|
|
160
|
+
raise ValueError("joint_names must name at least one joint.")
|
|
161
|
+
if len(set(joint_names)) != len(joint_names):
|
|
162
|
+
raise ValueError(f"joint_names contains duplicates: {joint_names}")
|
|
163
|
+
ids = []
|
|
164
|
+
for name in joint_names:
|
|
165
|
+
if name not in self.joint_name_to_id:
|
|
166
|
+
raise ValueError(f"No joint named '{name}' in the model.")
|
|
167
|
+
jid = self.joint_name_to_id[name]
|
|
168
|
+
if int(self.model.jnt_type[jid]) not in one_dof:
|
|
169
|
+
raise ValueError(f"Joint '{name}' is not a hinge or slide joint.")
|
|
170
|
+
ids.append(jid)
|
|
171
|
+
return ids
|
|
172
|
+
|
|
173
|
+
def _ancestors(self, body_id: int) -> List[int]:
|
|
174
|
+
"""The body and its ancestors, nearest first, ending at the world body (id 0)."""
|
|
175
|
+
chain = [body_id]
|
|
176
|
+
while chain[-1] != 0:
|
|
177
|
+
chain.append(int(self.model.body_parentid[chain[-1]]))
|
|
178
|
+
return chain
|
|
179
|
+
|
|
180
|
+
def _common_ancestor(self, body_ids: List[int]) -> int:
|
|
181
|
+
"""Deepest body that is an ancestor of (or equal to) every body in ``body_ids``."""
|
|
182
|
+
common = self._ancestors(body_ids[0])
|
|
183
|
+
for b in body_ids[1:]:
|
|
184
|
+
ancestors = set(self._ancestors(b))
|
|
185
|
+
common = [c for c in common if c in ancestors]
|
|
186
|
+
return common[0]
|
|
187
|
+
|
|
188
|
+
def _infer_root_body(self, joint_ids: List[int]) -> int:
|
|
189
|
+
"""Root of a scoped robot: the deepest body that the scoped joints all move relative to.
|
|
190
|
+
|
|
191
|
+
That is the common ancestor of the joints' parent bodies (the arm base for a serial
|
|
192
|
+
chain, the hand for a pair of fingers). If that is the world, fall back to the common
|
|
193
|
+
ancestor of the joint bodies themselves.
|
|
194
|
+
"""
|
|
195
|
+
joint_bodies = [int(self.model.jnt_bodyid[j]) for j in joint_ids]
|
|
196
|
+
root = self._common_ancestor([int(self.model.body_parentid[b]) for b in joint_bodies])
|
|
197
|
+
if root == 0:
|
|
198
|
+
root = self._common_ancestor(joint_bodies)
|
|
199
|
+
if root == 0:
|
|
200
|
+
raise ValueError(
|
|
201
|
+
"The given joints share no common body other than the world; pass root_body."
|
|
202
|
+
)
|
|
203
|
+
return root
|
|
204
|
+
|
|
205
|
+
def _subtree_body_ids(self, root_id: int) -> List[int]:
|
|
206
|
+
"""Ids of ``root_id`` and all of its descendants (MuJoCo orders parents before children)."""
|
|
207
|
+
subtree = {root_id}
|
|
208
|
+
for b in range(root_id + 1, self.model.nbody):
|
|
209
|
+
if int(self.model.body_parentid[b]) in subtree:
|
|
210
|
+
subtree.add(b)
|
|
211
|
+
return sorted(subtree)
|
|
212
|
+
|
|
213
|
+
def _build_joint_info(self, scoped_joint_ids: Optional[List[int]] = None):
|
|
214
|
+
model = self.model
|
|
215
|
+
free_type = int(mujoco.mjtJoint.mjJNT_FREE)
|
|
216
|
+
hinge_type = int(mujoco.mjtJoint.mjJNT_HINGE)
|
|
217
|
+
|
|
218
|
+
self.joint_name_to_info: Mapping[str, MujocoJointInfo] = {}
|
|
219
|
+
# Keyed by id as well, since unnamed joints all share the name "".
|
|
220
|
+
self._joint_info_by_id: Dict[int, MujocoJointInfo] = {}
|
|
221
|
+
self.actuated_joint_names: List[str] = []
|
|
222
|
+
self.actuated_joint_ids: List[int] = []
|
|
223
|
+
self.actuated_joint_lower_limits: List[float] = []
|
|
224
|
+
self.actuated_joint_upper_limits: List[float] = []
|
|
225
|
+
self.actuated_joint_qpos_adrs: List[int] = []
|
|
226
|
+
self.actuated_joint_dof_adrs: List[int] = []
|
|
227
|
+
self.continuous_joint_names: List[str] = []
|
|
228
|
+
self.continuous_joint_ids: List[int] = []
|
|
229
|
+
self.free_joint_id: Optional[int] = None
|
|
230
|
+
|
|
231
|
+
for j in range(model.njnt):
|
|
232
|
+
jtype = int(model.jnt_type[j])
|
|
233
|
+
name = self.joint_id_to_name[j]
|
|
234
|
+
qadr = int(model.jnt_qposadr[j])
|
|
235
|
+
dadr = int(model.jnt_dofadr[j])
|
|
236
|
+
limited = bool(model.jnt_limited[j])
|
|
237
|
+
lower, upper = (float(x) for x in model.jnt_range[j])
|
|
238
|
+
dof_dim = _DOF_DIM[jtype]
|
|
239
|
+
|
|
240
|
+
info = MujocoJointInfo(
|
|
241
|
+
joint_id=j,
|
|
242
|
+
joint_name=name,
|
|
243
|
+
joint_type=jtype,
|
|
244
|
+
qpos_adr=qadr,
|
|
245
|
+
qpos_dim=_QPOS_DIM[jtype],
|
|
246
|
+
dof_adr=dadr,
|
|
247
|
+
dof_dim=dof_dim,
|
|
248
|
+
body_id=int(model.jnt_bodyid[j]),
|
|
249
|
+
body_name=self.body_id_to_name[int(model.jnt_bodyid[j])],
|
|
250
|
+
lower_limit=lower,
|
|
251
|
+
upper_limit=upper,
|
|
252
|
+
limited=limited,
|
|
253
|
+
axis=np.array(model.jnt_axis[j]),
|
|
254
|
+
damping=float(model.dof_damping[dadr]) if dof_dim == 1 else float("nan"),
|
|
255
|
+
actuator_id=self.joint_id_to_actuator_id.get(j),
|
|
256
|
+
)
|
|
257
|
+
self.joint_name_to_info[name] = info
|
|
258
|
+
self._joint_info_by_id[j] = info
|
|
259
|
+
|
|
260
|
+
# A free joint on the root body is the floating base. Free joints elsewhere belong to
|
|
261
|
+
# other bodies in the world. Neither is an actuated joint.
|
|
262
|
+
if jtype == free_type:
|
|
263
|
+
if int(model.jnt_bodyid[j]) == self.base_id:
|
|
264
|
+
self.free_joint_id = j
|
|
265
|
+
continue
|
|
266
|
+
|
|
267
|
+
if scoped_joint_ids is None:
|
|
268
|
+
scoped_joint_ids = [
|
|
269
|
+
j for j in range(model.njnt) if int(model.jnt_type[j]) != free_type
|
|
270
|
+
]
|
|
271
|
+
else:
|
|
272
|
+
subtree = set(self.link_ids)
|
|
273
|
+
outside = [
|
|
274
|
+
self.joint_id_to_name[j]
|
|
275
|
+
for j in scoped_joint_ids
|
|
276
|
+
if int(model.jnt_bodyid[j]) not in subtree
|
|
277
|
+
]
|
|
278
|
+
if outside:
|
|
279
|
+
raise ValueError(f"Joints {outside} are not under root body '{self.base_name}'.")
|
|
280
|
+
|
|
281
|
+
for j in scoped_joint_ids:
|
|
282
|
+
info = self._joint_info_by_id[j]
|
|
283
|
+
self.actuated_joint_ids.append(j)
|
|
284
|
+
self.actuated_joint_names.append(info.joint_name)
|
|
285
|
+
self.actuated_joint_lower_limits.append(info.lower_limit)
|
|
286
|
+
self.actuated_joint_upper_limits.append(info.upper_limit)
|
|
287
|
+
self.actuated_joint_qpos_adrs.append(info.qpos_adr)
|
|
288
|
+
self.actuated_joint_dof_adrs.append(info.dof_adr)
|
|
289
|
+
|
|
290
|
+
# A revolute joint with no limits is a continuous joint.
|
|
291
|
+
if info.joint_type == hinge_type and not info.limited:
|
|
292
|
+
self.continuous_joint_names.append(info.joint_name)
|
|
293
|
+
self.continuous_joint_ids.append(j)
|
|
294
|
+
|
|
295
|
+
self.num_actuated_joints: int = len(self.actuated_joint_ids)
|
|
296
|
+
self.has_floating_base: bool = self.free_joint_id is not None
|
|
297
|
+
self.has_continuous_joints: bool = len(self.continuous_joint_ids) > 0
|
|
298
|
+
|
|
299
|
+
# --- convenience loaders ------------------------------------------------
|
|
300
|
+
|
|
301
|
+
@classmethod
|
|
302
|
+
def from_mjcf_path(cls, mjcf_path: str) -> "MujocoObject":
|
|
303
|
+
"""Compile a model from an MJCF (or URDF) file and read its structure.
|
|
304
|
+
|
|
305
|
+
Args:
|
|
306
|
+
mjcf_path (str): Path to an MJCF/URDF file.
|
|
307
|
+
|
|
308
|
+
Returns:
|
|
309
|
+
MujocoObject: Structure information for the compiled model.
|
|
310
|
+
"""
|
|
311
|
+
return cls(mujoco.MjModel.from_xml_path(mjcf_path))
|
|
312
|
+
|
|
313
|
+
# --- name <-> id accessors ---------------------------------------------
|
|
314
|
+
|
|
315
|
+
def get_joint_id(self, joint_name: str) -> int:
|
|
316
|
+
"""Get the id of the specified joint."""
|
|
317
|
+
return self.joint_name_to_id[joint_name]
|
|
318
|
+
|
|
319
|
+
def get_joint_name(self, joint_id: int) -> str:
|
|
320
|
+
"""Get the name of the joint with the given id."""
|
|
321
|
+
return self.joint_id_to_name[joint_id]
|
|
322
|
+
|
|
323
|
+
def get_joint_ids(self, joint_names: List[str]) -> List[int]:
|
|
324
|
+
"""Get the ids of the specified joints, in the given order."""
|
|
325
|
+
return [self.joint_name_to_id[name] for name in joint_names]
|
|
326
|
+
|
|
327
|
+
def get_link_id(self, link_name: str) -> int:
|
|
328
|
+
"""Get the body id of the specified link."""
|
|
329
|
+
return self.link_name_to_id[link_name]
|
|
330
|
+
|
|
331
|
+
def get_link_name(self, link_id: int) -> str:
|
|
332
|
+
"""Get the name of the link (body) with the given id."""
|
|
333
|
+
return self.link_id_to_name[link_id]
|
|
334
|
+
|
|
335
|
+
def get_link_mass(self, link_name: int | str) -> float:
|
|
336
|
+
"""Get the mass of the specified link (body), given by name or id."""
|
|
337
|
+
return float(self.model.body_mass[self._resolve_link_id(link_name)])
|
|
338
|
+
|
|
339
|
+
def get_actuator_id(self, actuator_name: str) -> int:
|
|
340
|
+
"""Get the id of the specified actuator."""
|
|
341
|
+
return self.actuator_name_to_id[actuator_name]
|
|
342
|
+
|
|
343
|
+
def get_site_id(self, site_name: str) -> int:
|
|
344
|
+
"""Get the id of the specified site."""
|
|
345
|
+
return self.site_name_to_id[site_name]
|
|
346
|
+
|
|
347
|
+
# --- name-or-id resolution ----------------------------------------------
|
|
348
|
+
#
|
|
349
|
+
# Methods bound to one kind of element take a name or that kind's id. Numpy integers count as
|
|
350
|
+
# ids (ids read from model arrays are numpy types). Unknown names and invalid ids raise
|
|
351
|
+
# KeyError. Links resolve only to this object's own links, by name or by id. Names are looked
|
|
352
|
+
# up in the name->id maps, which leave out unnamed elements, so "" never resolves.
|
|
353
|
+
|
|
354
|
+
@staticmethod
|
|
355
|
+
def _is_id(x) -> bool:
|
|
356
|
+
return isinstance(x, (int, np.integer)) and not isinstance(x, bool)
|
|
357
|
+
|
|
358
|
+
@staticmethod
|
|
359
|
+
def _checked_id(x, n: int, kind: str) -> int:
|
|
360
|
+
i = int(x)
|
|
361
|
+
if not 0 <= i < n:
|
|
362
|
+
raise KeyError(f"No {kind} with id {i}.")
|
|
363
|
+
return i
|
|
364
|
+
|
|
365
|
+
def _resolve_link_id(self, link: int | str) -> int:
|
|
366
|
+
if not self._is_id(link):
|
|
367
|
+
return self.link_name_to_id[link]
|
|
368
|
+
if int(link) not in self.link_id_to_name:
|
|
369
|
+
raise KeyError(f"Body id {int(link)} is not a link of this robot.")
|
|
370
|
+
return int(link)
|
|
371
|
+
|
|
372
|
+
def _resolve_site_id(self, site: int | str) -> int:
|
|
373
|
+
if self._is_id(site):
|
|
374
|
+
return self._checked_id(site, self.model.nsite, "site")
|
|
375
|
+
return self.site_name_to_id[site]
|
|
376
|
+
|
|
377
|
+
def _resolve_geom_id(self, geom: int | str) -> int:
|
|
378
|
+
if self._is_id(geom):
|
|
379
|
+
return self._checked_id(geom, self.model.ngeom, "geom")
|
|
380
|
+
return self.geom_name_to_id[geom]
|
|
381
|
+
|
|
382
|
+
def _joint_info(self, joint: int | str) -> MujocoJointInfo:
|
|
383
|
+
if self._is_id(joint):
|
|
384
|
+
return self._joint_info_by_id[int(joint)]
|
|
385
|
+
return self._joint_info_by_id[self.joint_name_to_id[joint]]
|
|
386
|
+
|
|
387
|
+
def _resolve_joint_ids(self, joints) -> List[int]:
|
|
388
|
+
return [self._joint_info(j).joint_id for j in joints]
|
|
389
|
+
|
|
390
|
+
def frame(self, name: str) -> Frame:
|
|
391
|
+
"""Resolve a link or site name to a typed :class:`Frame` handle.
|
|
392
|
+
|
|
393
|
+
Use it to cache a frame for methods that accept any frame (e.g. ``get_jacobian``), such as
|
|
394
|
+
a tool frame that is a body in one description and a site in another. A link name takes
|
|
395
|
+
precedence over a site with the same name.
|
|
396
|
+
|
|
397
|
+
Args:
|
|
398
|
+
name (str): Link (body) or site name.
|
|
399
|
+
|
|
400
|
+
Returns:
|
|
401
|
+
Frame: The resolved frame.
|
|
402
|
+
"""
|
|
403
|
+
if name in self.link_name_to_id:
|
|
404
|
+
return Frame("link", self.link_name_to_id[name], name)
|
|
405
|
+
if name in self.site_name_to_id:
|
|
406
|
+
return Frame("site", self.site_name_to_id[name], name)
|
|
407
|
+
raise KeyError(f"No link or site named '{name}'.")
|