File size: 2,652 Bytes
ef6a683 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 |
"""
Utility functions for working with Mujoco models.
copied from https://github.com/kevinzakka/mink/blob/main/mink/utils.py
"""
from typing import List
import mujoco
def get_body_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
"""Get immediate children bodies belonging to a given body.
Args:
model: Mujoco model.
body_id: ID of body.
Returns:
A List containing all child body ids.
"""
return [
i
for i in range(model.nbody)
if model.body_parentid[i] == body_id and body_id != i # Exclude the body itself.
]
def get_subtree_body_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
"""Get all bodies belonging to subtree starting at a given body.
Args:
model: Mujoco model.
body_id: ID of body where subtree starts.
Returns:
A List containing all subtree body ids.
"""
body_ids: List[int] = []
stack = [body_id]
while stack:
body_id = stack.pop()
body_ids.append(body_id)
stack += get_body_body_ids(model, body_id)
return body_ids
def get_subtree_body_names(model: mujoco.MjModel, body_id: int) -> List[str]:
"""Get all bodies belonging to subtree starting at a given body.
Args:
model: Mujoco model.
body_id: ID of body where subtree starts.
Returns:
A List containing all subtree body names.
"""
return [model.body(i).name for i in get_subtree_body_ids(model, body_id)]
def get_body_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
"""Get immediate geoms belonging to a given body.
Here, immediate geoms are those directly attached to the body and not its
descendants.
Args:
model: Mujoco model.
body_id: ID of body.
Returns:
A list containing all body geom ids.
"""
geom_start = model.body_geomadr[body_id]
geom_end = geom_start + model.body_geomnum[body_id]
return list(range(geom_start, geom_end))
def get_subtree_geom_ids(model: mujoco.MjModel, body_id: int) -> List[int]:
"""Get all geoms belonging to subtree starting at a given body.
Here, a subtree is defined as the kinematic tree starting at the body and including
all its descendants.
Args:
model: Mujoco model.
body_id: ID of body where subtree starts.
Returns:
A list containing all subtree geom ids.
"""
geom_ids: List[int] = []
stack = [body_id]
while stack:
body_id = stack.pop()
geom_ids.extend(get_body_geom_ids(model, body_id))
stack += get_body_body_ids(model, body_id)
return geom_ids
|