Source code for wbc_mjlab.rl.wm.kinematics

"""Differentiable forward kinematics of a floating-base robot (torch).

Mirrors MuJoCo's ``mj_kinematics`` for a tree of one free joint (the root) and at most one
hinge per body, plus body-origin velocities (mjlab ``body_link_*_vel_w``). Bodies at the same
tree depth are computed together, so one call is a few kernels per depth level, not per body.
"""

from __future__ import annotations

from dataclasses import dataclass

import mujoco
import numpy as np
import torch
from mjlab.utils.lab_api.math import quat_apply, quat_from_angle_axis, quat_mul


@dataclass
class RobotState:
  """Floating-base state in the env frame (mjlab conventions, quaternions wxyz)."""

  root_pos: torch.Tensor  # (N, 3)
  root_quat: torch.Tensor  # (N, 4)
  root_lin_vel: torch.Tensor  # (N, 3) world frame
  root_ang_vel: torch.Tensor  # (N, 3) world frame
  joint_pos: torch.Tensor  # (N, J)
  joint_vel: torch.Tensor  # (N, J)

  @staticmethod
  def from_vector(x: torch.Tensor, num_joints: int) -> RobotState:
    j = num_joints
    return RobotState(
      x[:, 0:3], x[:, 3:7], x[:, 7:10], x[:, 10:13], x[:, 13 : 13 + j], x[:, 13 + j :]
    )

  def vector(self) -> torch.Tensor:
    return torch.cat(
      [
        self.root_pos,
        self.root_quat,
        self.root_lin_vel,
        self.root_ang_vel,
        self.joint_pos,
        self.joint_vel,
      ],
      dim=-1,
    )


@dataclass
class BodyKinematics:
  """Poses and origin velocities of every robot body, world (env) frame."""

  pos: torch.Tensor  # (N, B, 3)
  quat: torch.Tensor  # (N, B, 4)
  lin_vel: torch.Tensor  # (N, B, 3)
  ang_vel: torch.Tensor  # (N, B, 3)


[docs] class RobotKinematics: """Forward kinematics for one robot entity of a compiled MuJoCo model. Args: mj_model: The scene model. body_ids: Global body ids of the robot, in entity order (root first). joint_qpos_adr: ``qpos`` address of each robot hinge joint, in entity joint order. device: Torch device. """ def __init__( self, mj_model: mujoco.MjModel, body_ids: list[int], joint_qpos_adr: list[int], device: torch.device | str, ): body_ids = [int(b) for b in body_ids] local = {b: i for i, b in enumerate(body_ids)} root = body_ids[0] if mj_model.body_jntnum[root] != 1 or mj_model.jnt_type[ mj_model.body_jntadr[root] ] != (mujoco.mjtJoint.mjJNT_FREE): raise ValueError("the robot root body must have exactly one free joint") joint_of_qpos = {int(a): k for k, a in enumerate(joint_qpos_adr)} depth = [0] * len(body_ids) parent = [-1] * len(body_ids) joint = [-1] * len( body_ids ) # entity joint index of the body's hinge (-1: fixed body) axis = np.zeros((len(body_ids), 3)) anchor = np.zeros((len(body_ids), 3)) for i, b in enumerate(body_ids[1:], start=1): p = int(mj_model.body_parentid[b]) if p not in local: raise ValueError(f"body {mj_model.body(b).name!r} is not in the robot subtree") parent[i], depth[i] = local[p], depth[local[p]] + 1 n = int(mj_model.body_jntnum[b]) if n > 1: raise ValueError( f"body {mj_model.body(b).name!r} has {n} joints (at most 1 hinge)" ) if n == 1: j = int(mj_model.body_jntadr[b]) if mj_model.jnt_type[j] != mujoco.mjtJoint.mjJNT_HINGE: raise ValueError(f"joint {mj_model.joint(j).name!r} is not a hinge") joint[i] = joint_of_qpos[int(mj_model.jnt_qposadr[j])] axis[i], anchor[i] = mj_model.jnt_axis[j], mj_model.jnt_pos[j] def t(x, dtype=torch.float32): return torch.as_tensor(np.asarray(x), dtype=dtype, device=device) self.num_bodies = len(body_ids) self.num_joints = len(joint_qpos_adr) self._body_pos = t(mj_model.body_pos[body_ids]) self._body_quat = t(mj_model.body_quat[body_ids]) self._axis, self._anchor = t(axis), t(anchor) # Fixed bodies read a zero column appended after the joints. self._joint = t([j if j >= 0 else self.num_joints for j in joint], torch.long) self._levels = [ t([i for i in range(len(body_ids)) if depth[i] == d], torch.long) for d in range(1, max(depth) + 1) ] # Bodies are computed level by level into arrays in level order: column of each body # there, its parent's column, and the permutation back to entity order. level_order = [0] + [i for lv in self._levels for i in lv.tolist()] column = {b: c for c, b in enumerate(level_order)} self._parent_col = [ t([column[parent[i]] for i in lv.tolist()], torch.long) for lv in self._levels ] self._entity_order = t([column[i] for i in range(len(body_ids))], torch.long) self._mj_model = mj_model self._local = local self._device = device
[docs] def site_frame(self, site_id: int) -> tuple[int, torch.Tensor, torch.Tensor]: """(entity body index, local pos, local quat) of a site on the robot.""" m = self._mj_model b = self._local.get(int(m.site_bodyid[site_id])) if b is None: raise ValueError(f"site {m.site(site_id).name!r} is not on the robot") pos = torch.as_tensor(m.site_pos[site_id], dtype=torch.float32, device=self._device) quat = torch.as_tensor( m.site_quat[site_id], dtype=torch.float32, device=self._device ) return b, pos, quat
[docs] def bodies(self, s: RobotState) -> BodyKinematics: """Poses and origin velocities of every body (entity order).""" n = s.root_pos.shape[0] q = torch.cat([s.joint_pos, s.joint_pos.new_zeros(n, 1)], dim=-1) qd = torch.cat([s.joint_vel, s.joint_vel.new_zeros(n, 1)], dim=-1) pos, quat = s.root_pos.unsqueeze(1), s.root_quat.unsqueeze(1) lin, ang = s.root_lin_vel.unsqueeze(1), s.root_ang_vel.unsqueeze(1) for level, pc in zip(self._levels, self._parent_col, strict=True): k = level.numel() p_pos, p_quat, p_lin, p_ang = pos[:, pc], quat[:, pc], lin[:, pc], ang[:, pc] body_axis = self._axis[level].expand(n, k, 3) body_anchor = self._anchor[level].expand(n, k, 3) # Body frame before its joint, then the hinge about its anchor (mj_kinematics). b_quat = quat_mul(p_quat, self._body_quat[level].expand(n, k, 4)) b_pos = p_pos + quat_apply(p_quat, self._body_pos[level].expand(n, k, 3)) x_anchor = b_pos + quat_apply(b_quat, body_anchor) x_axis = quat_apply(b_quat, body_axis) ji = self._joint[level] b_quat = quat_mul(b_quat, quat_from_angle_axis(q[:, ji], body_axis)) b_pos = x_anchor - quat_apply(b_quat, body_anchor) # Origin velocities: the parent's motion carried to the body, plus the hinge's. w_joint = x_axis * qd[:, ji].unsqueeze(-1) b_ang = p_ang + w_joint b_lin = ( p_lin + torch.cross(p_ang, b_pos - p_pos, dim=-1) + torch.cross(w_joint, b_pos - x_anchor, dim=-1) ) pos, quat = torch.cat([pos, b_pos], 1), torch.cat([quat, b_quat], 1) lin, ang = torch.cat([lin, b_lin], 1), torch.cat([ang, b_ang], 1) o = self._entity_order return BodyKinematics(pos[:, o], quat[:, o], lin[:, o], ang[:, o])