Files
chenlin c32df70326
Checks / Layout, translation and Colab checks (push) Has been cancelled
Checks / Ruff (push) Has been cancelled
Checks / Framework tests (native backend, CPU) (push) Has been cancelled
Checks / Portable Rust controller (push) Has been cancelled
release: v0.1 generic robot training and application platform
2026-11-05 11:05:01 +08:00

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