1341 lines
47 KiB
Python
1341 lines
47 KiB
Python
from __future__ import annotations
|
|
|
|
import warnings
|
|
from dataclasses import dataclass, field
|
|
from pathlib import Path
|
|
from typing import TYPE_CHECKING, Callable, Sequence
|
|
|
|
import mujoco
|
|
import mujoco_warp as mjwarp
|
|
import numpy as np
|
|
import torch
|
|
|
|
from mjlab import actuator
|
|
from mjlab.actuator import BuiltinActuatorGroup, FusedActuatorGroup
|
|
from mjlab.actuator.actuator import TransmissionType
|
|
from mjlab.actuator.xml_actuator import XmlActuator
|
|
from mjlab.entity.data import EntityData
|
|
from mjlab.utils import spec_config as spec_cfg
|
|
from mjlab.utils.lab_api.string import resolve_matching_names
|
|
from mjlab.utils.mujoco import dof_width, qpos_width
|
|
from mjlab.utils.spec import auto_wrap_fixed_base_mocap
|
|
from mjlab.utils.string import resolve_expr
|
|
from mjlab.utils.xml import fix_spec_xml, strip_buffer_textures
|
|
|
|
if TYPE_CHECKING:
|
|
from mjlab.entity.variants import VariantMetadata
|
|
|
|
|
|
@dataclass(frozen=False)
|
|
class EntityIndexing:
|
|
"""Maps entity elements to global indices and addresses in the simulation."""
|
|
|
|
# Elements.
|
|
bodies: tuple[mujoco.MjsBody, ...]
|
|
joints: tuple[mujoco.MjsJoint, ...]
|
|
geoms: tuple[mujoco.MjsGeom, ...]
|
|
sites: tuple[mujoco.MjsSite, ...]
|
|
tendons: tuple[mujoco.MjsTendon, ...]
|
|
cameras: tuple[mujoco.MjsCamera, ...]
|
|
lights: tuple[mujoco.MjsLight, ...]
|
|
materials: tuple[mujoco.MjsMaterial, ...]
|
|
textures: tuple[mujoco.MjsTexture, ...]
|
|
pairs: tuple[mujoco.MjsPair, ...]
|
|
actuators: tuple[mujoco.MjsActuator, ...] | None
|
|
|
|
# Indices.
|
|
body_ids: torch.Tensor
|
|
geom_ids: torch.Tensor
|
|
site_ids: torch.Tensor
|
|
tendon_ids: torch.Tensor
|
|
cam_ids: torch.Tensor
|
|
light_ids: torch.Tensor
|
|
mat_ids: torch.Tensor
|
|
tex_ids: torch.Tensor
|
|
pair_ids: torch.Tensor
|
|
ctrl_ids: torch.Tensor
|
|
joint_ids: torch.Tensor
|
|
mocap_id: int | None
|
|
|
|
# Addresses.
|
|
joint_q_adr: torch.Tensor
|
|
joint_v_adr: torch.Tensor
|
|
free_joint_q_adr: torch.Tensor
|
|
free_joint_v_adr: torch.Tensor
|
|
|
|
@property
|
|
def root_body_id(self) -> int:
|
|
return self.bodies[0].id
|
|
|
|
|
|
def _outer_index(
|
|
env_ids: torch.Tensor | slice, ids: torch.Tensor | slice
|
|
) -> tuple[torch.Tensor | slice, torch.Tensor | slice]:
|
|
"""Make [env_ids, ids] select the outer product when both are tensors.
|
|
|
|
Plain [tensor, tensor] indexing pairs the ids elementwise, which silently
|
|
writes a diagonal whenever the shapes happen to broadcast.
|
|
"""
|
|
if isinstance(env_ids, torch.Tensor) and isinstance(ids, torch.Tensor):
|
|
return env_ids[:, None], ids
|
|
return env_ids, ids
|
|
|
|
|
|
@dataclass
|
|
class EntityCfg:
|
|
@dataclass
|
|
class InitialStateCfg:
|
|
# Root position and orientation.
|
|
pos: tuple[float, float, float] = (0.0, 0.0, 0.0)
|
|
rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)
|
|
# Root linear and angular velocity (only for floating base entities).
|
|
lin_vel: tuple[float, float, float] = (0.0, 0.0, 0.0)
|
|
ang_vel: tuple[float, float, float] = (0.0, 0.0, 0.0)
|
|
# Articulation (only for articulated entities).
|
|
# Set to None to use the model's existing keyframe (errors if none exists).
|
|
joint_pos: dict[str, float] | None = field(default_factory=lambda: {".*": 0.0})
|
|
joint_vel: dict[str, float] = field(default_factory=lambda: {".*": 0.0})
|
|
|
|
init_state: InitialStateCfg = field(default_factory=InitialStateCfg)
|
|
spec_fn: Callable[[], mujoco.MjSpec] = field(
|
|
default_factory=lambda: lambda: mujoco.MjSpec()
|
|
)
|
|
articulation: EntityArticulationInfoCfg | None = None
|
|
sort_actuators: bool = False
|
|
"""When True, reorder actuators so that ``model.ctrl`` follows joint/tendon/site
|
|
definition order rather than the order actuators appear in the config. XML actuators
|
|
are excluded from sorting and always retain their declaration order.
|
|
"""
|
|
|
|
# Editors.
|
|
lights: tuple[spec_cfg.LightCfg, ...] = field(default_factory=tuple)
|
|
cameras: tuple[spec_cfg.CameraCfg, ...] = field(default_factory=tuple)
|
|
textures: tuple[spec_cfg.TextureCfg, ...] = field(default_factory=tuple)
|
|
materials: tuple[spec_cfg.MaterialCfg, ...] = field(default_factory=tuple)
|
|
meshes: tuple[spec_cfg.MeshCfg, ...] = field(default_factory=tuple)
|
|
geoms: tuple[spec_cfg.GeomCfg, ...] = field(default_factory=tuple)
|
|
collisions: tuple[spec_cfg.CollisionCfg, ...] = field(default_factory=tuple)
|
|
|
|
def build(self) -> Entity:
|
|
"""Build entity instance from this config.
|
|
|
|
Override in subclasses to return custom Entity types.
|
|
"""
|
|
return Entity(self)
|
|
|
|
|
|
@dataclass
|
|
class EntityArticulationInfoCfg:
|
|
actuators: tuple[actuator.ActuatorCfg, ...] = field(default_factory=tuple)
|
|
soft_joint_pos_limit_factor: float = 1.0
|
|
|
|
|
|
class Entity:
|
|
"""An entity represents a physical object in the simulation.
|
|
|
|
Entity Type Matrix
|
|
==================
|
|
MuJoCo entities can be categorized along two dimensions:
|
|
|
|
1. Base Type:
|
|
- Fixed Base: Entity is welded to the world (no freejoint)
|
|
- Floating Base: Entity has 6 DOF movement (has freejoint)
|
|
|
|
2. Articulation:
|
|
- Non-articulated: No joints other than freejoint
|
|
- Articulated: Has joints in kinematic tree (may or may not be actuated)
|
|
|
|
Fixed non-articulated entities can optionally be mocap bodies, whereby their
|
|
position and orientation can be set directly each timestep rather than being
|
|
determined by physics. This property can be useful for creating props with
|
|
adjustable position and orientation.
|
|
|
|
Supported Combinations:
|
|
----------------------
|
|
| Type | Example | is_fixed_base | is_articulated | is_actuated |
|
|
|---------------------------|---------------------|---------------|----------------|-------------|
|
|
| Fixed Non-articulated | Table, wall | True | False | False |
|
|
| Fixed Articulated | Robot arm, door | True | True | True/False |
|
|
| Floating Non-articulated | Box, ball, mug | False | False | False |
|
|
| Floating Articulated | Humanoid, quadruped | False | True | True/False |
|
|
"""
|
|
|
|
def __init__(self, cfg: EntityCfg) -> None:
|
|
self.cfg = cfg
|
|
self._actuators: list[actuator.Actuator] = []
|
|
self._variant_metadata: VariantMetadata | None = None
|
|
self._build_spec()
|
|
self._identify_joints()
|
|
self._apply_spec_editors()
|
|
self._add_actuators()
|
|
self._add_initial_state_keyframe()
|
|
|
|
def _build_spec(self) -> None:
|
|
from mjlab.entity.variants import VariantEntityCfg, build_merged_variant_spec
|
|
|
|
if isinstance(self.cfg, VariantEntityCfg):
|
|
self._spec, self._variant_metadata = build_merged_variant_spec(self.cfg)
|
|
else:
|
|
self._spec = auto_wrap_fixed_base_mocap(self.cfg.spec_fn)()
|
|
|
|
@property
|
|
def variant_metadata(self) -> VariantMetadata | None:
|
|
return self._variant_metadata
|
|
|
|
def _identify_joints(self) -> None:
|
|
self._all_joints = self._spec.joints
|
|
self._free_joint = None
|
|
self._non_free_joints = tuple(self._all_joints)
|
|
|
|
free_joints = [j for j in self._all_joints if j.type == mujoco.mjtJoint.mjJNT_FREE]
|
|
if len(free_joints) > 1:
|
|
raise ValueError(
|
|
f"Entity spec has {len(free_joints)} freejoints. An Entity models a "
|
|
"single rigid- or articulated-body system with at most one freejoint, "
|
|
"which serves as its root. Model each detached floating body as its own "
|
|
"entry in SceneCfg.entities instead."
|
|
)
|
|
|
|
if self._all_joints and self._all_joints[0].type == mujoco.mjtJoint.mjJNT_FREE:
|
|
self._free_joint = self._all_joints[0]
|
|
if not self._free_joint.name:
|
|
self._free_joint.name = "floating_base_joint"
|
|
self._non_free_joints = tuple(self._all_joints[1:])
|
|
|
|
def _apply_spec_editors(self) -> None:
|
|
spec_cfg.warn_overlapping_geom_edits(
|
|
self.cfg.geoms, self.cfg.collisions, self._spec
|
|
)
|
|
for cfg_list in [
|
|
self.cfg.lights,
|
|
self.cfg.cameras,
|
|
self.cfg.textures,
|
|
self.cfg.materials,
|
|
self.cfg.meshes,
|
|
self.cfg.geoms,
|
|
self.cfg.collisions,
|
|
]:
|
|
for cfg in cfg_list:
|
|
cfg.edit_spec(self._spec)
|
|
|
|
def _add_actuators(self) -> None:
|
|
if self.cfg.articulation is None:
|
|
return
|
|
|
|
# Collect actuator instances and their targets.
|
|
pending: list[tuple[actuator.ActuatorCfg, actuator.Actuator, list[str]]] = []
|
|
for actuator_cfg in self.cfg.articulation.actuators:
|
|
# Find targets based on transmission type. resolve_matching_names raises
|
|
# ValueError when no regex matches; we catch that to produce a better error with
|
|
# namespace hints below.
|
|
target_ids: list[int] = []
|
|
target_names: list[str] = []
|
|
target_spec_names: list[str] = []
|
|
try:
|
|
if actuator_cfg.transmission_type == TransmissionType.JOINT:
|
|
target_ids, target_names = self.find_joints(actuator_cfg.target_names_expr)
|
|
target_spec_names = [self._non_free_joints[i].name for i in target_ids]
|
|
elif actuator_cfg.transmission_type == TransmissionType.TENDON:
|
|
target_ids, target_names = self.find_tendons(actuator_cfg.target_names_expr)
|
|
target_spec_names = [self._spec.tendons[i].name for i in target_ids]
|
|
elif actuator_cfg.transmission_type == TransmissionType.SITE:
|
|
target_ids, target_names = self.find_sites(actuator_cfg.target_names_expr)
|
|
target_spec_names = [self.spec.sites[i].name for i in target_ids]
|
|
else:
|
|
raise TypeError(
|
|
f"Invalid transmission_type: {actuator_cfg.transmission_type}. "
|
|
f"Must be TransmissionType.JOINT, TransmissionType.TENDON, "
|
|
f"or TransmissionType.SITE."
|
|
)
|
|
except ValueError:
|
|
pass # target_names stays empty, fall through to hint logic
|
|
|
|
# Check other namespaces for matches. If we found nothing, this produces a
|
|
# helpful error. If we did find targets, it warns about unactuated matches in
|
|
# other namespaces.
|
|
current = actuator_cfg.transmission_type
|
|
other_matches: dict[TransmissionType, tuple[str, list[str]]] = {}
|
|
other_namespaces = {
|
|
TransmissionType.JOINT: ("joint", self.joint_names),
|
|
TransmissionType.TENDON: ("tendon", self.tendon_names),
|
|
TransmissionType.SITE: ("site", self.site_names),
|
|
}
|
|
for tt, (label, names) in other_namespaces.items():
|
|
if tt == current or not names:
|
|
continue
|
|
try:
|
|
_, matched = resolve_matching_names(actuator_cfg.target_names_expr, names)
|
|
other_matches[tt] = (label, matched)
|
|
except ValueError:
|
|
pass
|
|
|
|
if len(target_names) == 0:
|
|
msg = (
|
|
f"No {current.value}s matched expressions: {actuator_cfg.target_names_expr}"
|
|
)
|
|
if other_matches:
|
|
hints = [
|
|
f"{label}s ({', '.join(matched)})"
|
|
for label, matched in other_matches.values()
|
|
]
|
|
msg += (
|
|
f". Matches were found in: {'; '.join(hints)}. "
|
|
f"Check that transmission_type is correct."
|
|
)
|
|
raise ValueError(msg)
|
|
|
|
for tt, (label, matched) in other_matches.items():
|
|
warnings.warn(
|
|
f"Actuator config matched {len(target_names)} {current.value}(s) "
|
|
f"but the same expressions also match {len(matched)} {label}(s): "
|
|
f"{', '.join(matched)}. Add a separate config with "
|
|
f"transmission_type=TransmissionType.{tt.name} if those should "
|
|
f"be actuated too.",
|
|
stacklevel=2,
|
|
)
|
|
|
|
actuator_instance = actuator_cfg.build(self, target_ids, target_names)
|
|
self._actuators.append(actuator_instance)
|
|
pending.append((actuator_cfg, actuator_instance, target_spec_names))
|
|
|
|
if not self.cfg.sort_actuators:
|
|
for _, inst, names in pending:
|
|
inst.edit_spec(self._spec, names)
|
|
return
|
|
|
|
# Sort actuators so ctrl order matches joint/tendon/site definition order.
|
|
# XmlActuators are added first (they wrap pre-existing XML actuators),
|
|
# then remaining actuators sorted by transmission type and target order.
|
|
order_maps = {
|
|
TransmissionType.JOINT: {name: i for i, name in enumerate(self.joint_names)},
|
|
TransmissionType.TENDON: {name: i for i, name in enumerate(self.tendon_names)},
|
|
TransmissionType.SITE: {name: i for i, name in enumerate(self.site_names)},
|
|
}
|
|
# Group by transmission type (ordering is conventional, not physics-motivated).
|
|
# Within each group, actuators are sorted by their target's definition order in the
|
|
# spec.
|
|
type_priority = {
|
|
TransmissionType.JOINT: 0,
|
|
TransmissionType.TENDON: 1,
|
|
TransmissionType.SITE: 2,
|
|
}
|
|
|
|
# XmlActuators go first in declaration order (they reference actuators already
|
|
# present in the spec).
|
|
for _, inst, names in pending:
|
|
if isinstance(inst, XmlActuator):
|
|
inst.edit_spec(self._spec, names)
|
|
|
|
# Flatten remaining actuators to (instance, single_target) pairs and sort.
|
|
flat: list[tuple[actuator.ActuatorCfg, actuator.Actuator, str]] = []
|
|
for cfg, inst, names in pending:
|
|
if not isinstance(inst, XmlActuator):
|
|
for name in names:
|
|
flat.append((cfg, inst, name))
|
|
|
|
flat.sort(
|
|
key=lambda item: (
|
|
type_priority[item[0].transmission_type],
|
|
order_maps[item[0].transmission_type].get(item[2], float("inf")),
|
|
)
|
|
)
|
|
for _, inst, name in flat:
|
|
inst.edit_spec(self._spec, [name])
|
|
|
|
def _add_initial_state_keyframe(self) -> None:
|
|
# If joint_pos is None, use existing keyframe from the model.
|
|
if self.cfg.init_state.joint_pos is None:
|
|
if not self._spec.keys:
|
|
raise ValueError(
|
|
"joint_pos=None requires the model to have a keyframe, but none exists."
|
|
)
|
|
# Keep the existing keyframe, just rename it.
|
|
self._spec.keys[0].name = "init_state"
|
|
if self.is_fixed_base:
|
|
self.root_body.pos[:] = self.cfg.init_state.pos
|
|
self.root_body.quat[:] = self.cfg.init_state.rot
|
|
return
|
|
|
|
qpos_components = []
|
|
|
|
if self._free_joint is not None:
|
|
qpos_components.extend([self.cfg.init_state.pos, self.cfg.init_state.rot])
|
|
|
|
joint_pos = None
|
|
if self._non_free_joints:
|
|
joint_pos = resolve_expr(self.cfg.init_state.joint_pos, self.joint_names, 0.0)
|
|
qpos_components.append(joint_pos)
|
|
|
|
key_qpos = np.hstack(qpos_components) if qpos_components else np.array([])
|
|
key = self._spec.add_key(name="init_state", qpos=key_qpos.tolist())
|
|
|
|
if self.is_actuated and joint_pos is not None:
|
|
name_to_pos = {name: joint_pos[i] for i, name in enumerate(self.joint_names)}
|
|
ctrl = []
|
|
for act in self._spec.actuators:
|
|
joint_name = act.target
|
|
ctrl.append(name_to_pos.get(joint_name, 0.0))
|
|
key.ctrl = np.array(ctrl)
|
|
|
|
if self.is_fixed_base:
|
|
self.root_body.pos[:] = self.cfg.init_state.pos
|
|
self.root_body.quat[:] = self.cfg.init_state.rot
|
|
|
|
# Attributes.
|
|
|
|
@property
|
|
def is_fixed_base(self) -> bool:
|
|
"""Entity is welded to the world."""
|
|
return self._free_joint is None
|
|
|
|
@property
|
|
def is_articulated(self) -> bool:
|
|
"""Entity is articulated (has fixed or actuated joints)."""
|
|
return len(self._non_free_joints) > 0
|
|
|
|
@property
|
|
def is_actuated(self) -> bool:
|
|
"""Entity has actuated joints."""
|
|
return len(self._actuators) > 0
|
|
|
|
@property
|
|
def has_tendon_actuators(self) -> bool:
|
|
"""Entity has actuators using tendon transmission."""
|
|
if self.cfg.articulation is None:
|
|
return False
|
|
return any(
|
|
act.transmission_type == TransmissionType.TENDON
|
|
for act in self.cfg.articulation.actuators
|
|
)
|
|
|
|
@property
|
|
def has_site_actuators(self) -> bool:
|
|
"""Entity has actuators using site transmission."""
|
|
if self.cfg.articulation is None:
|
|
return False
|
|
return any(
|
|
act.transmission_type == TransmissionType.SITE
|
|
for act in self.cfg.articulation.actuators
|
|
)
|
|
|
|
@property
|
|
def is_mocap(self) -> bool:
|
|
"""Entity root body is a mocap body (only for fixed-base entities)."""
|
|
return bool(self.root_body.mocap) if self.is_fixed_base else False
|
|
|
|
@property
|
|
def spec(self) -> mujoco.MjSpec:
|
|
return self._spec
|
|
|
|
@property
|
|
def data(self) -> EntityData:
|
|
return self._data
|
|
|
|
@property
|
|
def actuators(self) -> list[actuator.Actuator]:
|
|
return self._actuators
|
|
|
|
# Names.
|
|
|
|
@property
|
|
def body_names(self) -> tuple[str, ...]:
|
|
return tuple(b.name.split("/")[-1] for b in self.spec.bodies[1:])
|
|
|
|
@property
|
|
def all_joint_names(self) -> tuple[str, ...]:
|
|
return tuple(j.name.split("/")[-1] for j in self._all_joints)
|
|
|
|
@property
|
|
def joint_names(self) -> tuple[str, ...]:
|
|
return tuple(j.name.split("/")[-1] for j in self._non_free_joints)
|
|
|
|
@property
|
|
def geom_names(self) -> tuple[str, ...]:
|
|
return tuple(g.name.split("/")[-1] for g in self.spec.geoms)
|
|
|
|
@property
|
|
def site_names(self) -> tuple[str, ...]:
|
|
return tuple(s.name.split("/")[-1] for s in self.spec.sites)
|
|
|
|
@property
|
|
def tendon_names(self) -> tuple[str, ...]:
|
|
return tuple(t.name.split("/")[-1] for t in self._spec.tendons)
|
|
|
|
@property
|
|
def camera_names(self) -> tuple[str, ...]:
|
|
return tuple(c.name.split("/")[-1] for c in self.spec.cameras)
|
|
|
|
@property
|
|
def light_names(self) -> tuple[str, ...]:
|
|
return tuple(lt.name.split("/")[-1] for lt in self.spec.lights)
|
|
|
|
@property
|
|
def material_names(self) -> tuple[str, ...]:
|
|
return tuple(m.name.split("/")[-1] for m in self.spec.materials)
|
|
|
|
@property
|
|
def texture_names(self) -> tuple[str, ...]:
|
|
return tuple(t.name.split("/")[-1] for t in self.spec.textures)
|
|
|
|
@property
|
|
def pair_names(self) -> tuple[str, ...]:
|
|
return tuple(p.name.split("/")[-1] for p in self.spec.pairs)
|
|
|
|
@property
|
|
def actuator_names(self) -> tuple[str, ...]:
|
|
return tuple(a.name.split("/")[-1] for a in self.spec.actuators)
|
|
|
|
# Counts.
|
|
|
|
@property
|
|
def num_bodies(self) -> int:
|
|
return len(self.body_names)
|
|
|
|
@property
|
|
def num_joints(self) -> int:
|
|
return len(self.joint_names)
|
|
|
|
@property
|
|
def num_geoms(self) -> int:
|
|
return len(self.geom_names)
|
|
|
|
@property
|
|
def num_sites(self) -> int:
|
|
return len(self.site_names)
|
|
|
|
@property
|
|
def num_tendons(self) -> int:
|
|
return len(self.tendon_names)
|
|
|
|
@property
|
|
def num_cameras(self) -> int:
|
|
return len(self.camera_names)
|
|
|
|
@property
|
|
def num_lights(self) -> int:
|
|
return len(self.light_names)
|
|
|
|
@property
|
|
def num_materials(self) -> int:
|
|
return len(self.material_names)
|
|
|
|
@property
|
|
def num_textures(self) -> int:
|
|
return len(self.texture_names)
|
|
|
|
@property
|
|
def num_pairs(self) -> int:
|
|
return len(self.pair_names)
|
|
|
|
@property
|
|
def num_actuators(self) -> int:
|
|
return len(self.actuator_names)
|
|
|
|
@property
|
|
def root_body(self) -> mujoco.MjsBody:
|
|
return self.spec.bodies[1]
|
|
|
|
# Find methods.
|
|
|
|
def find_bodies(
|
|
self, name_keys: str | Sequence[str], preserve_order: bool = False
|
|
) -> tuple[list[int], list[str]]:
|
|
return resolve_matching_names(name_keys, self.body_names, preserve_order)
|
|
|
|
def find_joints(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
joint_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if joint_subset is None:
|
|
joint_subset = self.joint_names
|
|
return resolve_matching_names(name_keys, joint_subset, preserve_order)
|
|
|
|
def find_joints_by_actuator_names(
|
|
self,
|
|
actuator_name_keys: str | Sequence[str],
|
|
) -> tuple[list[int], list[str]]:
|
|
# Collect all actuated joint names.
|
|
actuated_joint_names_set = set()
|
|
for act in self._actuators:
|
|
actuated_joint_names_set.update(act.target_names)
|
|
|
|
# Filter self.joint_names to only actuated joints, preserving natural order.
|
|
actuated_in_natural_order = [
|
|
name for name in self.joint_names if name in actuated_joint_names_set
|
|
]
|
|
|
|
# Find joints matching the pattern within actuated joints.
|
|
_, matched_joint_names = self.find_joints(
|
|
actuator_name_keys, joint_subset=actuated_in_natural_order, preserve_order=False
|
|
)
|
|
|
|
# Map joint names back to entity-local indices (indices into self.joint_names).
|
|
name_to_entity_idx = {name: i for i, name in enumerate(self.joint_names)}
|
|
joint_ids = [name_to_entity_idx[name] for name in matched_joint_names]
|
|
return joint_ids, matched_joint_names
|
|
|
|
def find_geoms(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
geom_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if geom_subset is None:
|
|
geom_subset = self.geom_names
|
|
return resolve_matching_names(name_keys, geom_subset, preserve_order)
|
|
|
|
def find_sites(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
site_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if site_subset is None:
|
|
site_subset = self.site_names
|
|
return resolve_matching_names(name_keys, site_subset, preserve_order)
|
|
|
|
def find_tendons(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
tendon_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if tendon_subset is None:
|
|
tendon_subset = self.tendon_names
|
|
return resolve_matching_names(name_keys, tendon_subset, preserve_order)
|
|
|
|
def find_cameras(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
camera_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if camera_subset is None:
|
|
camera_subset = self.camera_names
|
|
return resolve_matching_names(name_keys, camera_subset, preserve_order)
|
|
|
|
def find_lights(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
light_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if light_subset is None:
|
|
light_subset = self.light_names
|
|
return resolve_matching_names(name_keys, light_subset, preserve_order)
|
|
|
|
def find_materials(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
material_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if material_subset is None:
|
|
material_subset = self.material_names
|
|
return resolve_matching_names(name_keys, material_subset, preserve_order)
|
|
|
|
def find_textures(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
texture_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if texture_subset is None:
|
|
texture_subset = self.texture_names
|
|
return resolve_matching_names(name_keys, texture_subset, preserve_order)
|
|
|
|
def find_pairs(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
pair_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if pair_subset is None:
|
|
pair_subset = self.pair_names
|
|
return resolve_matching_names(name_keys, pair_subset, preserve_order)
|
|
|
|
def find_actuators(
|
|
self,
|
|
name_keys: str | Sequence[str],
|
|
actuator_subset: Sequence[str] | None = None,
|
|
preserve_order: bool = False,
|
|
) -> tuple[list[int], list[str]]:
|
|
if actuator_subset is None:
|
|
actuator_subset = self.actuator_names
|
|
return resolve_matching_names(name_keys, actuator_subset, preserve_order)
|
|
|
|
def compile(self) -> mujoco.MjModel:
|
|
"""Compile the underlying MjSpec into an MjModel."""
|
|
return self.spec.compile()
|
|
|
|
def write_xml(self, xml_path: Path) -> None:
|
|
"""Write the MjSpec to disk.
|
|
|
|
Operates on a copy of the spec to avoid mutating the original.
|
|
"""
|
|
tmp = self.spec.copy()
|
|
strip_buffer_textures(tmp)
|
|
xml_path.write_text(fix_spec_xml(tmp.to_xml()))
|
|
|
|
def to_zip(self, path: Path) -> None:
|
|
"""Write the MjSpec to a zip file."""
|
|
with path.open("wb") as f:
|
|
mujoco.MjSpec.to_zip(self.spec, f)
|
|
|
|
def initialize(
|
|
self,
|
|
mj_model: mujoco.MjModel,
|
|
model: mjwarp.Model,
|
|
data: mjwarp.Data,
|
|
device: str,
|
|
) -> None:
|
|
"""Prepare the entity for simulation after the spec has been compiled.
|
|
|
|
Computes global index mappings, initializes actuators, and allocates all nworld
|
|
state and target tensors in ``EntityData``. Called once by the scene during
|
|
environment construction.
|
|
"""
|
|
indexing = self._compute_indexing(mj_model, device)
|
|
self.indexing = indexing
|
|
nworld = data.nworld
|
|
|
|
for act in self._actuators:
|
|
act.initialize(mj_model, model, data, device)
|
|
|
|
# Vectorize built-in actuators, then fuse ideal PD actuators; we'll loop
|
|
# through whatever custom actuators remain.
|
|
builtin_group, custom_actuators = BuiltinActuatorGroup.process(self._actuators)
|
|
builtin_group.initialize(nworld, device)
|
|
self._builtin_group = builtin_group
|
|
fused_actuator_group, custom_actuators = FusedActuatorGroup.process(
|
|
custom_actuators
|
|
)
|
|
fused_actuator_group.initialize(nworld, device)
|
|
self._fused_actuator_group = fused_actuator_group
|
|
self._custom_actuators = custom_actuators
|
|
|
|
# Root state.
|
|
default_root_state = self._build_default_root_state(nworld, device)
|
|
|
|
# Joint state.
|
|
if self.is_articulated:
|
|
if self.cfg.init_state.joint_pos is None:
|
|
# Use keyframe joint positions. The scene keyframe holds every entity's qpos.
|
|
key_qpos = torch.tensor(
|
|
mj_model.key("init_state").qpos, dtype=torch.float, device=device
|
|
)
|
|
default_joint_pos = key_qpos[indexing.joint_q_adr][None].repeat(nworld, 1)
|
|
else:
|
|
default_joint_pos = torch.tensor(
|
|
resolve_expr(self.cfg.init_state.joint_pos, self.joint_names, 0.0),
|
|
device=device,
|
|
)[None].repeat(nworld, 1)
|
|
default_joint_vel = torch.tensor(
|
|
resolve_expr(self.cfg.init_state.joint_vel, self.joint_names, 0.0),
|
|
device=device,
|
|
)[None].repeat(nworld, 1)
|
|
|
|
# Joint limits.
|
|
joint_ids_list = [j.id for j in self._non_free_joints]
|
|
dof_limits = model.jnt_range[:, joint_ids_list]
|
|
default_joint_pos_limits = dof_limits.clone()
|
|
joint_pos_limits = default_joint_pos_limits.clone()
|
|
|
|
joint_pos_mean = (joint_pos_limits[..., 0] + joint_pos_limits[..., 1]) / 2
|
|
joint_pos_range = joint_pos_limits[..., 1] - joint_pos_limits[..., 0]
|
|
|
|
# Soft limits.
|
|
soft_limit_factor = (
|
|
self.cfg.articulation.soft_joint_pos_limit_factor
|
|
if self.cfg.articulation
|
|
else 1.0
|
|
)
|
|
soft_joint_pos_limits = torch.stack(
|
|
[
|
|
joint_pos_mean - 0.5 * joint_pos_range * soft_limit_factor,
|
|
joint_pos_mean + 0.5 * joint_pos_range * soft_limit_factor,
|
|
],
|
|
dim=-1,
|
|
)
|
|
|
|
# Unlimited joints have jnt_range=[0,0] in MuJoCo, which makes all
|
|
# the computed limits [0,0]. Override to [-inf, inf] so downstream
|
|
# clamping becomes a no-op. (Can't do this before soft-limit math
|
|
# because inf - inf = NaN.)
|
|
unlimited = ~torch.tensor(
|
|
mj_model.jnt_limited[joint_ids_list], device=device, dtype=torch.bool
|
|
)
|
|
for limits in (joint_pos_limits, default_joint_pos_limits, soft_joint_pos_limits):
|
|
limits[:, unlimited, 0] = float("-inf")
|
|
limits[:, unlimited, 1] = float("inf")
|
|
else:
|
|
empty_shape = (nworld, 0)
|
|
default_joint_pos = torch.empty(*empty_shape, dtype=torch.float, device=device)
|
|
default_joint_vel = torch.empty(*empty_shape, dtype=torch.float, device=device)
|
|
default_joint_pos_limits = torch.empty(
|
|
*empty_shape, 2, dtype=torch.float, device=device
|
|
)
|
|
joint_pos_limits = torch.empty(*empty_shape, 2, dtype=torch.float, device=device)
|
|
soft_joint_pos_limits = torch.empty(
|
|
*empty_shape, 2, dtype=torch.float, device=device
|
|
)
|
|
|
|
if self.is_actuated:
|
|
joint_pos_target = torch.zeros(
|
|
(nworld, self.num_joints), dtype=torch.float, device=device
|
|
)
|
|
joint_vel_target = torch.zeros(
|
|
(nworld, self.num_joints), dtype=torch.float, device=device
|
|
)
|
|
joint_effort_target = torch.zeros(
|
|
(nworld, self.num_joints), dtype=torch.float, device=device
|
|
)
|
|
else:
|
|
joint_pos_target = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
joint_vel_target = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
joint_effort_target = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
|
|
# Only allocate tendon targets if there are actuators using tendon transmission.
|
|
if self.has_tendon_actuators:
|
|
num_tendons = len(self.tendon_names)
|
|
tendon_len_target = torch.zeros(
|
|
(nworld, num_tendons), dtype=torch.float, device=device
|
|
)
|
|
tendon_vel_target = torch.zeros(
|
|
(nworld, num_tendons), dtype=torch.float, device=device
|
|
)
|
|
tendon_effort_target = torch.zeros(
|
|
(nworld, num_tendons), dtype=torch.float, device=device
|
|
)
|
|
else:
|
|
tendon_len_target = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
tendon_vel_target = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
tendon_effort_target = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
|
|
# Only allocate site targets if there are actuators using site transmission.
|
|
if self.has_site_actuators:
|
|
num_sites = len(self.site_names)
|
|
site_effort_target = torch.zeros(
|
|
(nworld, num_sites), dtype=torch.float, device=device
|
|
)
|
|
else:
|
|
site_effort_target = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
|
|
# Encoder bias for simulating encoder calibration errors.
|
|
# Shape: (num_envs, num_joints). Defaults to zero (no bias).
|
|
if self.is_articulated:
|
|
encoder_bias = torch.zeros(
|
|
(nworld, self.num_joints), dtype=torch.float, device=device
|
|
)
|
|
else:
|
|
encoder_bias = torch.empty(nworld, 0, dtype=torch.float, device=device)
|
|
|
|
self._data = EntityData(
|
|
indexing=indexing,
|
|
data=data,
|
|
model=model,
|
|
device=device,
|
|
default_root_state=default_root_state,
|
|
default_joint_pos=default_joint_pos,
|
|
default_joint_vel=default_joint_vel,
|
|
default_joint_pos_limits=default_joint_pos_limits,
|
|
joint_pos_limits=joint_pos_limits,
|
|
soft_joint_pos_limits=soft_joint_pos_limits,
|
|
gravity_vec_w=torch.tensor([0.0, 0.0, -1.0], device=device).repeat(nworld, 1),
|
|
forward_vec_b=torch.tensor([1.0, 0.0, 0.0], device=device).repeat(nworld, 1),
|
|
is_fixed_base=self.is_fixed_base,
|
|
is_articulated=self.is_articulated,
|
|
is_actuated=self.is_actuated,
|
|
joint_pos_target=joint_pos_target,
|
|
joint_vel_target=joint_vel_target,
|
|
joint_effort_target=joint_effort_target,
|
|
tendon_len_target=tendon_len_target,
|
|
tendon_vel_target=tendon_vel_target,
|
|
tendon_effort_target=tendon_effort_target,
|
|
site_effort_target=site_effort_target,
|
|
encoder_bias=encoder_bias,
|
|
)
|
|
|
|
def update(self, dt: float) -> None:
|
|
"""Advance actuator internal state by one physics substep.
|
|
|
|
Called after each ``sim.step()`` within the decimation loop.
|
|
"""
|
|
for act in self._actuators:
|
|
act.update(dt)
|
|
|
|
def reset(self, env_ids: torch.Tensor | slice | None = None) -> None:
|
|
"""Zero actuator targets and reset actuator internal state.
|
|
|
|
Called by the scene when environments are reset at episode boundaries,
|
|
and by commands that teleport the robot to a new pose mid-episode.
|
|
"""
|
|
self._data.clear_state(env_ids)
|
|
|
|
for act in self._actuators:
|
|
act.reset(env_ids)
|
|
|
|
def clear_state(self, env_ids: torch.Tensor | slice | None = None) -> None:
|
|
"""Deprecated. Use ``reset`` instead."""
|
|
warnings.warn(
|
|
"Entity.clear_state() is deprecated. Use Entity.reset().",
|
|
DeprecationWarning,
|
|
stacklevel=2,
|
|
)
|
|
self.reset(env_ids)
|
|
|
|
def write_data_to_sim(self) -> None:
|
|
"""Convert actuator targets into low-level controls and write them to the sim.
|
|
|
|
Called before each ``sim.step()`` within the decimation loop. Builtin actuators are
|
|
applied in a single batched operation; custom actuators are applied individually.
|
|
"""
|
|
self._apply_actuator_controls()
|
|
|
|
def write_ctrl_to_sim(
|
|
self,
|
|
ctrl: torch.Tensor,
|
|
ctrl_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Write control inputs to the simulation.
|
|
|
|
Args:
|
|
ctrl: A tensor of control inputs.
|
|
ctrl_ids: A tensor of control indices.
|
|
env_ids: Optional tensor or slice specifying which environments to set.
|
|
If None, all environments are set.
|
|
"""
|
|
self._data.write_ctrl(ctrl, ctrl_ids, env_ids)
|
|
|
|
def write_root_state_to_sim(
|
|
self, root_state: torch.Tensor, env_ids: torch.Tensor | slice | None = None
|
|
) -> None:
|
|
"""Set the root state into the simulation.
|
|
|
|
The root state consists of position (3), orientation as a (w, x, y, z)
|
|
quaternion (4), linear velocity (3), and angular velocity (3), for a total
|
|
of 13 values. All of the quantities are in the world frame.
|
|
|
|
Args:
|
|
root_state: Tensor of shape (N, 13) where N is the number of environments.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_root_state(root_state, env_ids)
|
|
|
|
def write_root_link_pose_to_sim(
|
|
self,
|
|
root_pose: torch.Tensor,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
):
|
|
"""Set the root pose into the simulation. Like `write_root_state_to_sim()`
|
|
but only sets position and orientation.
|
|
|
|
Args:
|
|
root_pose: Tensor of shape (N, 7) where N is the number of environments.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_root_pose(root_pose, env_ids)
|
|
|
|
def write_root_link_velocity_to_sim(
|
|
self,
|
|
root_velocity: torch.Tensor,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
):
|
|
"""Set the root link (body origin) velocity into the simulation. Like
|
|
`write_root_state_to_sim()` but only sets linear and angular velocity.
|
|
|
|
Args:
|
|
root_velocity: Tensor of shape (N, 6) where N is the number of environments.
|
|
Contains linear velocity (3) at body origin and angular velocity (3),
|
|
both in world frame.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_root_velocity(root_velocity, env_ids)
|
|
|
|
def write_root_link_velocity_b_to_sim(
|
|
self,
|
|
root_velocity_b: torch.Tensor,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
):
|
|
"""Like `write_root_link_velocity_to_sim()` but the velocity is expressed
|
|
in the root link's body frame. Reads the orientation from qpos, so it is
|
|
safe to call during a reset before forward() runs.
|
|
|
|
Args:
|
|
root_velocity_b: Tensor of shape (N, 6) where N is the number of
|
|
environments. Contains linear velocity (3) at body origin and angular
|
|
velocity (3), both in the root link's body frame.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_root_velocity_b(root_velocity_b, env_ids)
|
|
|
|
def write_root_com_velocity_to_sim(
|
|
self,
|
|
root_velocity: torch.Tensor,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
):
|
|
"""Set the root COM velocity into the simulation.
|
|
|
|
Args:
|
|
root_velocity: Tensor of shape (N, 6) where N is the number of environments.
|
|
Contains linear velocity (3) at COM and angular velocity (3),
|
|
both in world frame.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_root_com_velocity(root_velocity, env_ids)
|
|
|
|
def write_joint_state_to_sim(
|
|
self,
|
|
position: torch.Tensor,
|
|
velocity: torch.Tensor,
|
|
joint_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
):
|
|
"""Set the joint state into the simulation.
|
|
|
|
The joint state consists of joint positions and velocities. It does not include
|
|
the root state.
|
|
|
|
Args:
|
|
position: Tensor of shape (N, num_joints) where N is the number of environments.
|
|
velocity: Tensor of shape (N, num_joints) where N is the number of environments.
|
|
joint_ids: Optional tensor or slice specifying which joints to set. If None,
|
|
all joints are set.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_joint_state(position, velocity, joint_ids, env_ids)
|
|
|
|
def write_joint_position_to_sim(
|
|
self,
|
|
position: torch.Tensor,
|
|
joint_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
):
|
|
"""Set the joint positions into the simulation. Like `write_joint_state_to_sim()`
|
|
but only sets joint positions.
|
|
|
|
Args:
|
|
position: Tensor of shape (N, num_joints) where N is the number of environments.
|
|
joint_ids: Optional tensor or slice specifying which joints to set. If None,
|
|
all joints are set.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_joint_position(position, joint_ids, env_ids)
|
|
|
|
def write_joint_velocity_to_sim(
|
|
self,
|
|
velocity: torch.Tensor,
|
|
joint_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
):
|
|
"""Set the joint velocities into the simulation. Like `write_joint_state_to_sim()`
|
|
but only sets joint velocities.
|
|
|
|
Args:
|
|
velocity: Tensor of shape (N, num_joints) where N is the number of environments.
|
|
joint_ids: Optional tensor or slice specifying which joints to set. If None,
|
|
all joints are set.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_joint_velocity(velocity, joint_ids, env_ids)
|
|
|
|
def set_joint_position_target(
|
|
self,
|
|
position: torch.Tensor,
|
|
joint_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set joint position targets.
|
|
|
|
Args:
|
|
position: Target joint poisitions with shape (N, num_joints).
|
|
joint_ids: Optional joint indices to set. If None, set all joints.
|
|
env_ids: Optional environment indices. If None, set all environments.
|
|
"""
|
|
if env_ids is None:
|
|
env_ids = slice(None)
|
|
if joint_ids is None:
|
|
joint_ids = slice(None)
|
|
env_ids, joint_ids = _outer_index(env_ids, joint_ids)
|
|
self._data.joint_pos_target[env_ids, joint_ids] = position
|
|
|
|
def set_joint_velocity_target(
|
|
self,
|
|
velocity: torch.Tensor,
|
|
joint_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set joint velocity targets.
|
|
|
|
Args:
|
|
velocity: Target joint velocities with shape (N, num_joints).
|
|
joint_ids: Optional joint indices to set. If None, set all joints.
|
|
env_ids: Optional environment indices. If None, set all environments.
|
|
"""
|
|
if env_ids is None:
|
|
env_ids = slice(None)
|
|
if joint_ids is None:
|
|
joint_ids = slice(None)
|
|
env_ids, joint_ids = _outer_index(env_ids, joint_ids)
|
|
self._data.joint_vel_target[env_ids, joint_ids] = velocity
|
|
|
|
def set_joint_effort_target(
|
|
self,
|
|
effort: torch.Tensor,
|
|
joint_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set joint effort targets.
|
|
|
|
Args:
|
|
effort: Target joint efforts with shape (N, num_joints).
|
|
joint_ids: Optional joint indices to set. If None, set all joints.
|
|
env_ids: Optional environment indices. If None, set all environments.
|
|
"""
|
|
if env_ids is None:
|
|
env_ids = slice(None)
|
|
if joint_ids is None:
|
|
joint_ids = slice(None)
|
|
env_ids, joint_ids = _outer_index(env_ids, joint_ids)
|
|
self._data.joint_effort_target[env_ids, joint_ids] = effort
|
|
|
|
def set_tendon_len_target(
|
|
self,
|
|
length: torch.Tensor,
|
|
tendon_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set tendon length targets.
|
|
|
|
Args:
|
|
length: Target tendon lengths with shape (N, num_tendons).
|
|
tendon_ids: Optional tendon indices to set. If None, set all tendons.
|
|
env_ids: Optional environment indices. If None, set all environments.
|
|
"""
|
|
if env_ids is None:
|
|
env_ids = slice(None)
|
|
if tendon_ids is None:
|
|
tendon_ids = slice(None)
|
|
env_ids, tendon_ids = _outer_index(env_ids, tendon_ids)
|
|
self._data.tendon_len_target[env_ids, tendon_ids] = length
|
|
|
|
def set_tendon_vel_target(
|
|
self,
|
|
velocity: torch.Tensor,
|
|
tendon_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set tendon velocity targets.
|
|
|
|
Args:
|
|
velocity: Target tendon velocities with shape (N, num_tendons).
|
|
tendon_ids: Optional tendon indices to set. If None, set all tendons.
|
|
env_ids: Optional environment indices. If None, set all environments.
|
|
"""
|
|
if env_ids is None:
|
|
env_ids = slice(None)
|
|
if tendon_ids is None:
|
|
tendon_ids = slice(None)
|
|
env_ids, tendon_ids = _outer_index(env_ids, tendon_ids)
|
|
self._data.tendon_vel_target[env_ids, tendon_ids] = velocity
|
|
|
|
def set_tendon_effort_target(
|
|
self,
|
|
effort: torch.Tensor,
|
|
tendon_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set tendon effort targets.
|
|
|
|
Args:
|
|
effort: Target tendon efforts with shape (N, num_tendons).
|
|
tendon_ids: Optional tendon indices to set. If None, set all tendons.
|
|
env_ids: Optional environment indices. If None, set all environments.
|
|
"""
|
|
if env_ids is None:
|
|
env_ids = slice(None)
|
|
if tendon_ids is None:
|
|
tendon_ids = slice(None)
|
|
env_ids, tendon_ids = _outer_index(env_ids, tendon_ids)
|
|
self._data.tendon_effort_target[env_ids, tendon_ids] = effort
|
|
|
|
def set_site_effort_target(
|
|
self,
|
|
effort: torch.Tensor,
|
|
site_ids: torch.Tensor | slice | None = None,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set site effort targets.
|
|
|
|
Args:
|
|
effort: Target site efforts with shape (N, num_sites).
|
|
site_ids: Optional site indices to set. If None, set all sites.
|
|
env_ids: Optional environment indices. If None, set all environments.
|
|
"""
|
|
if env_ids is None:
|
|
env_ids = slice(None)
|
|
if site_ids is None:
|
|
site_ids = slice(None)
|
|
env_ids, site_ids = _outer_index(env_ids, site_ids)
|
|
self._data.site_effort_target[env_ids, site_ids] = effort
|
|
|
|
def write_external_wrench_to_sim(
|
|
self,
|
|
forces: torch.Tensor,
|
|
torques: torch.Tensor,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
body_ids: Sequence[int] | slice | None = None,
|
|
) -> None:
|
|
"""Apply external wrenches to bodies in the simulation.
|
|
|
|
Underneath the hood, this sets the `xfrc_applied` field in the MuJoCo data
|
|
structure. The wrenches are specified in the world frame and persist until
|
|
the next call to this function or until the simulation is reset.
|
|
|
|
Args:
|
|
forces: Tensor of shape (N, num_bodies, 3) where N is the number of
|
|
environments.
|
|
torques: Tensor of shape (N, num_bodies, 3) where N is the number of
|
|
environments.
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
body_ids: Optional list of body indices or slice specifying which bodies to
|
|
apply the wrenches to. If None, wrenches are applied to all bodies.
|
|
"""
|
|
self._data.write_external_wrench(forces, torques, body_ids, env_ids)
|
|
|
|
def write_mocap_pose_to_sim(
|
|
self,
|
|
mocap_pose: torch.Tensor,
|
|
env_ids: torch.Tensor | slice | None = None,
|
|
) -> None:
|
|
"""Set the pose of a mocap body into the simulation.
|
|
|
|
Args:
|
|
mocap_pose: Tensor of shape (N, 7) where N is the number of environments.
|
|
Format: [pos_x, pos_y, pos_z, quat_w, quat_x, quat_y, quat_z]
|
|
env_ids: Optional tensor or slice specifying which environments to set. If
|
|
None, all environments are set.
|
|
"""
|
|
self._data.write_mocap_pose(mocap_pose, env_ids)
|
|
|
|
##
|
|
# Private methods.
|
|
##
|
|
|
|
def _build_default_root_state(self, nworld: int, device: str) -> torch.Tensor:
|
|
"""Build default root state tensor, uniform across all worlds."""
|
|
base = self.cfg.init_state
|
|
components: list[tuple[float, ...]] = [base.pos, base.rot]
|
|
if not self.is_fixed_base:
|
|
components.extend([base.lin_vel, base.ang_vel])
|
|
return torch.tensor(
|
|
sum((tuple(c) for c in components), ()),
|
|
dtype=torch.float,
|
|
device=device,
|
|
).repeat(nworld, 1)
|
|
|
|
def _compute_indexing(self, model: mujoco.MjModel, device: str) -> EntityIndexing:
|
|
bodies = tuple([b for b in self.spec.bodies[1:]])
|
|
joints = self._non_free_joints
|
|
geoms = tuple(self.spec.geoms)
|
|
sites = tuple(self.spec.sites)
|
|
tendons = tuple(self.spec.tendons)
|
|
cameras = tuple(self.spec.cameras)
|
|
lights = tuple(self.spec.lights)
|
|
materials = tuple(self.spec.materials)
|
|
textures = tuple(self.spec.textures)
|
|
pairs = tuple(self.spec.pairs)
|
|
|
|
body_ids = torch.tensor([b.id for b in bodies], dtype=torch.int, device=device)
|
|
geom_ids = torch.tensor([g.id for g in geoms], dtype=torch.int, device=device)
|
|
site_ids = torch.tensor([s.id for s in sites], dtype=torch.int, device=device)
|
|
tendon_ids = torch.tensor([t.id for t in tendons], dtype=torch.int, device=device)
|
|
cam_ids = torch.tensor([c.id for c in cameras], dtype=torch.int, device=device)
|
|
light_ids = torch.tensor([lt.id for lt in lights], dtype=torch.int, device=device)
|
|
mat_ids = torch.tensor([m.id for m in materials], dtype=torch.int, device=device)
|
|
tex_ids = torch.tensor([t.id for t in textures], dtype=torch.int, device=device)
|
|
pair_ids = torch.tensor([p.id for p in pairs], dtype=torch.int, device=device)
|
|
joint_ids = torch.tensor([j.id for j in joints], dtype=torch.int, device=device)
|
|
|
|
if self.is_actuated:
|
|
actuators = tuple(self.spec.actuators)
|
|
ctrl_ids = torch.tensor([a.id for a in actuators], dtype=torch.int, device=device)
|
|
else:
|
|
actuators = None
|
|
ctrl_ids = torch.empty(0, dtype=torch.int, device=device)
|
|
|
|
joint_q_adr = []
|
|
joint_v_adr = []
|
|
free_joint_q_adr = []
|
|
free_joint_v_adr = []
|
|
for joint in self.spec.joints:
|
|
jnt = model.joint(joint.name)
|
|
jnt_type = jnt.type[0]
|
|
vadr = jnt.dofadr[0]
|
|
qadr = jnt.qposadr[0]
|
|
if jnt_type == mujoco.mjtJoint.mjJNT_FREE:
|
|
free_joint_v_adr.extend(range(vadr, vadr + 6))
|
|
free_joint_q_adr.extend(range(qadr, qadr + 7))
|
|
else:
|
|
joint_v_adr.extend(range(vadr, vadr + dof_width(jnt_type)))
|
|
joint_q_adr.extend(range(qadr, qadr + qpos_width(jnt_type)))
|
|
joint_q_adr = torch.tensor(joint_q_adr, dtype=torch.int, device=device)
|
|
joint_v_adr = torch.tensor(joint_v_adr, dtype=torch.int, device=device)
|
|
free_joint_v_adr = torch.tensor(free_joint_v_adr, dtype=torch.int, device=device)
|
|
free_joint_q_adr = torch.tensor(free_joint_q_adr, dtype=torch.int, device=device)
|
|
|
|
if self.is_fixed_base and self.is_mocap:
|
|
mocap_id = int(model.body_mocapid[self.root_body.id])
|
|
else:
|
|
mocap_id = None
|
|
|
|
return EntityIndexing(
|
|
bodies=bodies,
|
|
joints=joints,
|
|
geoms=geoms,
|
|
sites=sites,
|
|
tendons=tendons,
|
|
cameras=cameras,
|
|
lights=lights,
|
|
materials=materials,
|
|
textures=textures,
|
|
pairs=pairs,
|
|
actuators=actuators,
|
|
body_ids=body_ids,
|
|
geom_ids=geom_ids,
|
|
site_ids=site_ids,
|
|
tendon_ids=tendon_ids,
|
|
cam_ids=cam_ids,
|
|
light_ids=light_ids,
|
|
mat_ids=mat_ids,
|
|
tex_ids=tex_ids,
|
|
pair_ids=pair_ids,
|
|
ctrl_ids=ctrl_ids,
|
|
joint_ids=joint_ids,
|
|
mocap_id=mocap_id,
|
|
joint_q_adr=joint_q_adr,
|
|
joint_v_adr=joint_v_adr,
|
|
free_joint_q_adr=free_joint_q_adr,
|
|
free_joint_v_adr=free_joint_v_adr,
|
|
)
|
|
|
|
def _apply_actuator_controls(self) -> None:
|
|
self._builtin_group.apply_controls(self._data)
|
|
self._fused_actuator_group.apply_controls(self._data)
|
|
for act in self._custom_actuators:
|
|
command = act.get_command(self._data)
|
|
command = act.apply_delay(command)
|
|
self._data.write_ctrl(act.compute(command), act.ctrl_ids)
|