125 lines
5.7 KiB
Python
125 lines
5.7 KiB
Python
"""Named joint trajectories and a position-only damped least-squares solver."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import json
|
|
from collections.abc import Mapping
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
|
|
import mujoco
|
|
import numpy as np
|
|
|
|
from .model import JointControl, finite, object_id, vector
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Trajectory:
|
|
time_s: np.ndarray
|
|
q: np.ndarray
|
|
|
|
@classmethod
|
|
def from_document(cls, doc: Mapping | Path, joints: JointControl) -> Trajectory:
|
|
if isinstance(doc, Path):
|
|
doc = json.loads(doc.read_text("utf-8"))
|
|
if not isinstance(doc, Mapping) or doc.get("schema", "mjrl-trajectory/1") != (
|
|
"mjrl-trajectory/1"
|
|
):
|
|
raise ValueError("Expected an mjrl-trajectory/1 object")
|
|
names = doc.get("joint_order")
|
|
if (
|
|
not isinstance(names, list)
|
|
or len(names) != len(joints.names)
|
|
or any(names.count(joint) != 1 for joint in joints.names)
|
|
):
|
|
raise ValueError("trajectory.joint_order must list each controlled joint once")
|
|
times = doc.get("time_s")
|
|
if not isinstance(times, list) or len(times) < 2:
|
|
raise ValueError("trajectory.time_s must list at least two keyframe times")
|
|
t = vector(times, len(times), "trajectory.time_s")
|
|
if t[0] != 0 or np.any(np.diff(t) <= 0):
|
|
raise ValueError("Trajectory times must start at zero and strictly increase")
|
|
frames = doc.get("q")
|
|
if not isinstance(frames, list) or len(frames) != len(t):
|
|
raise ValueError("Trajectory needs one q row for each keyframe time")
|
|
q = np.stack([vector(row, len(names), "trajectory.q") for row in frames])
|
|
q = q[:, [names.index(name) for name in joints.names]]
|
|
if np.any(q < joints.lo) or np.any(q > joints.hi):
|
|
raise ValueError("Trajectory keyframe lies outside a joint's position limits")
|
|
return cls(t, q)
|
|
|
|
def at(self, t: float) -> np.ndarray:
|
|
"""Interpolate joint position at t seconds, holding endpoint poses."""
|
|
t = float(np.clip(t, self.time_s[0], self.time_s[-1]))
|
|
upper = int(np.clip(self.time_s.searchsorted(t, side="right"), 1, len(self.time_s) - 1))
|
|
lower = upper - 1
|
|
mix = (t - self.time_s[lower]) / (self.time_s[upper] - self.time_s[lower])
|
|
return self.q[lower] + mix * (self.q[upper] - self.q[lower])
|
|
|
|
|
|
class DampedLeastSquaresIK:
|
|
"""Solve a world-space site position with only the selected scalar joints."""
|
|
|
|
def __init__(self, model: mujoco.MjModel, joints: JointControl, config: Mapping):
|
|
self.model = model
|
|
self.joints = joints
|
|
self.site = object_id(model, mujoco.mjtObj.mjOBJ_SITE, config["site"])
|
|
body = object_id(model, mujoco.mjtObj.mjOBJ_BODY, config["mocap_body"])
|
|
self.mocap = model.body_mocapid[body]
|
|
if self.mocap < 0:
|
|
raise ValueError("IK target body must have mocap=true")
|
|
ancestors = set()
|
|
body = int(model.site_bodyid[self.site])
|
|
while body:
|
|
ancestors.add(body)
|
|
body = int(model.body_parentid[body])
|
|
selected = [object_id(model, mujoco.mjtObj.mjOBJ_JOINT, name) for name in joints.names]
|
|
if not all(model.jnt_bodyid[j] in ancestors for j in selected):
|
|
raise ValueError("Each IK joint must be an ancestor of the end-effector site")
|
|
self.damping = finite(config.get("damping", 0.02), "damping", positive=True)
|
|
self.max_step = finite(config.get("max_step", 0.2), "max_step", positive=True)
|
|
self.tolerance = finite(config.get("tolerance", 1e-5), "tolerance", positive=True)
|
|
self.iterations = config.get("iterations", 16)
|
|
if type(self.iterations) is not int or not 1 <= self.iterations <= 1000:
|
|
raise ValueError("iterations must be an integer between 1 and 1000")
|
|
self._work = mujoco.MjData(model)
|
|
self._jac = np.zeros((3, model.nv))
|
|
self._ridge = np.eye(3) * self.damping**2
|
|
|
|
def solve(self, data: mujoco.MjData, target: np.ndarray) -> np.ndarray:
|
|
"""Return a bounded position target without changing simulated qpos."""
|
|
if target.shape != (3,) or not np.all(np.isfinite(target)):
|
|
raise ValueError("IK target must be a finite world-space position")
|
|
model, work, joints = self.model, self._work, self.joints
|
|
work.qpos[:] = data.qpos
|
|
work.mocap_pos[:] = data.mocap_pos
|
|
work.mocap_quat[:] = data.mocap_quat
|
|
q = np.clip(data.qpos[joints.pos], joints.lo, joints.hi)
|
|
work.qpos[joints.pos] = q
|
|
mujoco.mj_kinematics(model, work)
|
|
|
|
for _ in range(self.iterations):
|
|
error = target - work.site_xpos[self.site]
|
|
distance = np.linalg.norm(error)
|
|
if distance <= self.tolerance:
|
|
break
|
|
mujoco.mj_comPos(model, work)
|
|
mujoco.mj_jacSite(model, work, self._jac, None, self.site)
|
|
jac = self._jac[:, joints.vel]
|
|
delta = jac.T @ np.linalg.solve(jac @ jac.T + self._ridge, error)
|
|
norm = np.linalg.norm(delta)
|
|
delta *= min(1.0, self.max_step / max(norm, self.max_step))
|
|
# Near a stop or singularity a clipped DLS step can increase the error.
|
|
# Backtrack and keep the best reachable pose for this tick.
|
|
for _ in range(8):
|
|
candidate = np.clip(q + delta, joints.lo, joints.hi)
|
|
work.qpos[joints.pos] = candidate
|
|
mujoco.mj_kinematics(model, work)
|
|
if np.linalg.norm(target - work.site_xpos[self.site]) < distance:
|
|
q = candidate
|
|
break
|
|
delta *= 0.5
|
|
else:
|
|
break
|
|
return q
|