ceca51b0d8
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
262 lines
11 KiB
Python
262 lines
11 KiB
Python
"""Resolve MuJoCo joints and their controls by name."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from collections.abc import Mapping
|
|
from dataclasses import dataclass
|
|
from numbers import Real
|
|
|
|
import mujoco
|
|
import numpy as np
|
|
|
|
_SCALAR = (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE)
|
|
_JOINT = mujoco.mjtTrn.mjTRN_JOINT
|
|
|
|
|
|
def object_id(model: mujoco.MjModel, kind: mujoco.mjtObj, name: str) -> int:
|
|
index = mujoco.mj_name2id(model, kind, name)
|
|
if index < 0:
|
|
raise ValueError(f"Model has no {kind.name.removeprefix('mjOBJ_').lower()} {name!r}")
|
|
return index
|
|
|
|
|
|
def joint_ids(model: mujoco.MjModel, names: list[str]) -> np.ndarray:
|
|
if (
|
|
not isinstance(names, list)
|
|
or not names
|
|
or not all(isinstance(name, str) and name for name in names)
|
|
or len(set(names)) != len(names)
|
|
):
|
|
raise ValueError("joints must be a nonempty list of unique names")
|
|
ids = np.array([object_id(model, mujoco.mjtObj.mjOBJ_JOINT, name) for name in names])
|
|
if any(model.jnt_type[j] not in _SCALAR for j in ids):
|
|
raise ValueError("Controlled joints must be hinges or slides")
|
|
return ids
|
|
|
|
|
|
def joint_actuators(model: mujoco.MjModel, ids: np.ndarray) -> list[np.ndarray]:
|
|
# Both joint transmissions have the same length law for hinges and slides.
|
|
direct = np.isin(model.actuator_trntype, [_JOINT, mujoco.mjtTrn.mjTRN_JOINTINPARENT])
|
|
return [
|
|
np.flatnonzero(direct & (model.actuator_trnid[:, 0] == joint))
|
|
for joint in ids
|
|
]
|
|
|
|
|
|
def finite(value, field: str, *, positive: bool = False) -> float:
|
|
if not isinstance(value, Real) or isinstance(value, bool) or not np.isfinite(value):
|
|
raise ValueError(f"{field} must be a finite number")
|
|
if positive and value <= 0:
|
|
raise ValueError(f"{field} must be positive")
|
|
return float(value)
|
|
|
|
|
|
def vector(value, size: int, field: str) -> np.ndarray:
|
|
if not isinstance(value, (list, tuple, np.ndarray)) or len(value) != size:
|
|
raise ValueError(f"{field} must contain {size} numbers")
|
|
return np.array([finite(v, field) for v in value], dtype=np.float64)
|
|
|
|
|
|
def joint_values(value, names, field: str, *, positive=False, nonnegative=False) -> np.ndarray:
|
|
"""Expand a scalar or exact named map in controller joint order."""
|
|
if not isinstance(names, (list, tuple)) or not names or not all(
|
|
isinstance(name, str) and name for name in names
|
|
) or len(set(names)) != len(names):
|
|
raise ValueError("joints must be a nonempty list of unique names")
|
|
if isinstance(value, Mapping):
|
|
if set(value) != set(names):
|
|
raise ValueError(f"{field} must cover controlled joints exactly")
|
|
values = [finite(value[name], f"{field}.{name}", positive=positive) for name in names]
|
|
else:
|
|
values = [finite(value, field, positive=positive)] * len(names)
|
|
values = np.asarray(values, dtype=np.float64)
|
|
if nonnegative and np.any(values < 0):
|
|
raise ValueError(f"{field} must be nonnegative")
|
|
return values
|
|
|
|
|
|
def joint_effort_bounds(model, ids, effort) -> np.ndarray:
|
|
"""Intersect requested joint effort with authored actuator and joint bounds.
|
|
|
|
MuJoCo actuator force is before gear transmission; normalize it to joint
|
|
effort before replacing an actuator with a unit-gear servo.
|
|
"""
|
|
bounds = np.column_stack((-effort, effort))
|
|
for i, (joint, actuators) in enumerate(zip(ids, joint_actuators(model, ids), strict=True)):
|
|
if len(actuators) > 1:
|
|
raise ValueError("A controlled joint has multiple actuators")
|
|
if len(actuators):
|
|
aid = int(actuators[0])
|
|
gear = model.actuator_gear[aid]
|
|
if not np.isfinite(gear).all() or gear[0] == 0 or np.any(gear[1:] != 0):
|
|
raise ValueError("Cannot normalize a non-scalar or zero actuator gear")
|
|
if model.actuator_forcelimited[aid]:
|
|
low, high = np.sort(model.actuator_forcerange[aid] * gear[0])
|
|
bounds[i] = [max(bounds[i, 0], low), min(bounds[i, 1], high)]
|
|
# A motor's input is effort, whereas a position/velocity servo's
|
|
# ctrlrange is a target range and must never be treated as force.
|
|
if (model.actuator_ctrllimited[aid]
|
|
and model.actuator_dyntype[aid] == mujoco.mjtDyn.mjDYN_NONE
|
|
and model.actuator_gaintype[aid] == mujoco.mjtGain.mjGAIN_FIXED
|
|
and model.actuator_biastype[aid] == mujoco.mjtBias.mjBIAS_NONE):
|
|
low, high = np.sort(
|
|
model.actuator_ctrlrange[aid] * model.actuator_gainprm[aid, 0] * gear[0]
|
|
)
|
|
bounds[i] = [max(bounds[i, 0], low), min(bounds[i, 1], high)]
|
|
if model.jnt_actfrclimited[joint]:
|
|
low, high = model.jnt_actfrcrange[joint]
|
|
bounds[i] = [max(bounds[i, 0], low), min(bounds[i, 1], high)]
|
|
if not bounds[i, 0] < 0 < bounds[i, 1]:
|
|
raise ValueError("Controlled joint effort must permit both directions")
|
|
return bounds
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class JointControl:
|
|
names: tuple[str, ...]
|
|
ctrl: np.ndarray
|
|
pos: np.ndarray
|
|
vel: np.ndarray
|
|
lo: np.ndarray
|
|
hi: np.ndarray
|
|
|
|
@classmethod
|
|
def bind(cls, model: mujoco.MjModel, names: list[str], kind: str) -> JointControl:
|
|
ids = joint_ids(model, names)
|
|
actuators = joint_actuators(model, ids)
|
|
if any(len(group) != 1 for group in actuators):
|
|
raise ValueError("Each controlled joint must have one joint actuator. Run setup first")
|
|
ctrl = np.array([group[0] for group in actuators])
|
|
if not np.allclose(model.actuator_gear[ctrl], [1, 0, 0, 0, 0, 0], rtol=0, atol=1e-12):
|
|
raise ValueError("Controller needs unit actuator gear. Run setup first")
|
|
if np.any(model.actuator_dyntype[ctrl] != mujoco.mjtDyn.mjDYN_NONE) or np.any(
|
|
model.actuator_delay[ctrl] != 0
|
|
):
|
|
raise ValueError("Controller needs stateless actuators. Run setup first")
|
|
if np.any(model.actuator_gaintype[ctrl] != mujoco.mjtGain.mjGAIN_FIXED):
|
|
raise ValueError("Controller needs fixed-gain actuators. Run setup first")
|
|
gain = model.actuator_gainprm[ctrl, 0]
|
|
bias = model.actuator_biasprm[ctrl, :3]
|
|
bias_type = model.actuator_biastype[ctrl]
|
|
if kind == "motor":
|
|
valid = (bias_type == mujoco.mjtBias.mjBIAS_NONE) & np.isclose(gain, 1)
|
|
elif kind == "velocity":
|
|
valid = (
|
|
(bias_type == mujoco.mjtBias.mjBIAS_AFFINE)
|
|
& (gain > 0)
|
|
& np.isclose(bias[:, 0], 0)
|
|
& np.isclose(bias[:, 1], 0)
|
|
& np.isclose(bias[:, 2], -gain)
|
|
)
|
|
else:
|
|
valid = (
|
|
(bias_type == mujoco.mjtBias.mjBIAS_AFFINE)
|
|
& (gain > 0)
|
|
& np.isclose(bias[:, 0], 0)
|
|
& np.isclose(bias[:, 1], -gain)
|
|
& (bias[:, 2] <= 0)
|
|
)
|
|
if not np.all(valid):
|
|
raise ValueError(f"Controller needs {kind} actuators. Run setup first")
|
|
ranges = model.jnt_range[ids]
|
|
limited = model.jnt_limited[ids]
|
|
lo = np.where(limited, ranges[:, 0], -np.inf)
|
|
hi = np.where(limited, ranges[:, 1], np.inf)
|
|
if kind == "position":
|
|
has_ctrl_range = model.actuator_ctrllimited[ctrl]
|
|
ctrl_range = model.actuator_ctrlrange[ctrl]
|
|
lo = np.maximum(lo, np.where(has_ctrl_range, ctrl_range[:, 0], -np.inf))
|
|
hi = np.minimum(hi, np.where(has_ctrl_range, ctrl_range[:, 1], np.inf))
|
|
if np.any(lo >= hi):
|
|
raise ValueError("Actuator and joint position ranges do not overlap")
|
|
return cls(tuple(names), ctrl, model.jnt_qposadr[ids], model.jnt_dofadr[ids], lo, hi)
|
|
|
|
|
|
def _position_limits(model, joint, actuators):
|
|
"""Return unit-gear joint targets, respecting the source servo's length units."""
|
|
limits = model.jnt_range[joint] if model.jnt_limited[joint] else None
|
|
if not len(actuators):
|
|
return limits
|
|
aid = actuators[0]
|
|
if not (model.actuator_ctrllimited[aid] or model.actuator_actlimited[aid]):
|
|
return limits
|
|
|
|
bias_type = model.actuator_biastype[aid]
|
|
bias = model.actuator_biasprm[aid]
|
|
# Motors and velocity servos do not take position targets.
|
|
if bias_type == mujoco.mjtBias.mjBIAS_NONE or (
|
|
bias_type == mujoco.mjtBias.mjBIAS_AFFINE and bias[1] == 0
|
|
):
|
|
return limits
|
|
|
|
name = model.joint(joint).name
|
|
gain = model.actuator_gainprm[aid, 0]
|
|
if not (
|
|
model.actuator_gaintype[aid] == mujoco.mjtGain.mjGAIN_FIXED
|
|
and bias_type == mujoco.mjtBias.mjBIAS_AFFINE
|
|
and gain > 0 and bias[0] == 0 and bias[1] == -gain and bias[2] <= 0
|
|
and model.actuator_dyntype[aid] in (
|
|
mujoco.mjtDyn.mjDYN_NONE, mujoco.mjtDyn.mjDYN_FILTER, mujoco.mjtDyn.mjDYN_FILTEREXACT,
|
|
)
|
|
and not model.actuator_actlimited[aid]
|
|
):
|
|
raise ValueError(
|
|
f"Cannot infer position target bounds for joint {name!r} "
|
|
"from its actuator law or activation limits"
|
|
)
|
|
|
|
# For a scalar joint the actuator length is gear * qpos. Sorting handles
|
|
# a reversed transmission, and works for both hinge and slide coordinates.
|
|
source = np.sort(model.actuator_ctrlrange[aid] / model.actuator_gear[aid, 0])
|
|
if not np.isfinite(source).all():
|
|
raise ValueError(f"Nonfinite position target bounds for joint {name!r}")
|
|
if limits is not None:
|
|
source = [max(limits[0], source[0]), min(limits[1], source[1])]
|
|
if source[0] >= source[1]:
|
|
raise ValueError(f"Actuator and joint position ranges do not overlap for {name!r}")
|
|
return source
|
|
|
|
|
|
def normalize_actuators(
|
|
spec: mujoco.MjSpec,
|
|
model: mujoco.MjModel,
|
|
names: list[str],
|
|
kind: str,
|
|
*,
|
|
kp: float | Mapping[str, float],
|
|
kd: float | Mapping[str, float],
|
|
effort: float | Mapping[str, float],
|
|
speed: float,
|
|
) -> None:
|
|
ids = joint_ids(model, names)
|
|
gains = joint_values(kp, names, "kp", positive=kind == "position", nonnegative=True)
|
|
damping = joint_values(kd, names, "kd", positive=kind == "velocity", nonnegative=True)
|
|
efforts = joint_values(effort, names, "effort_limit", positive=True)
|
|
bounds = joint_effort_bounds(model, ids, efforts)
|
|
for index, (joint, name, existing) in enumerate(
|
|
zip(ids, names, joint_actuators(model, ids), strict=True)
|
|
):
|
|
if len(existing) > 1:
|
|
raise ValueError(f"Joint {name!r} has multiple actuators")
|
|
if len(existing):
|
|
actuator = spec.actuators[int(existing[0])]
|
|
if actuator.plugin.active:
|
|
raise ValueError(f"Joint {name!r} uses an actuator plugin")
|
|
else:
|
|
actuator = spec.add_actuator(name=f"mjrl_{name}", target=name, trntype=_JOINT)
|
|
actuator.gear = [1, 0, 0, 0, 0, 0]
|
|
actuator.delay = 0
|
|
if kind == "motor":
|
|
actuator.set_to_motor()
|
|
limits = bounds[index]
|
|
elif kind == "velocity":
|
|
actuator.set_to_velocity(damping[index])
|
|
limits = [-speed, speed]
|
|
else:
|
|
actuator.set_to_position(gains[index], kv=damping[index], timeconst=0)
|
|
limits = _position_limits(model, joint, existing)
|
|
actuator.ctrllimited = int(limits is not None)
|
|
actuator.ctrlrange = limits if limits is not None else [0, 0]
|
|
actuator.forcelimited = 1
|
|
actuator.forcerange = bounds[index]
|