277 lines
16 KiB
Python
277 lines
16 KiB
Python
"""Build an L20 visual MJCF and provide coupled 21-landmark kinematics.
|
|
|
|
The supplied L20 URDFs have 21 revolute joints and five passive mimic joints.
|
|
The accompanying G20 calibration also has only 16 active motor channels; its
|
|
20-byte command slots 11--14 are unassigned. This module preserves the URDF
|
|
linear mimic model. Calibration tables are metadata, not a hardware command
|
|
conversion: their nonlinear passive curves differ from the URDF mimic ratios.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from collections import defaultdict
|
|
import json
|
|
import os
|
|
from pathlib import Path
|
|
import xml.etree.ElementTree as ET
|
|
|
|
import mujoco
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
|
|
REPO_ROOT = Path(__file__).resolve().parents[1]
|
|
FINGERS = ("thumb", "index", "middle", "ring", "pinky")
|
|
LANDMARK_NAMES = (
|
|
"wrist", "thumb_cmc", "thumb_mcp", "thumb_ip", "thumb_tip",
|
|
"index_mcp", "index_pip", "index_dip", "index_tip",
|
|
"middle_mcp", "middle_pip", "middle_dip", "middle_tip",
|
|
"ring_mcp", "ring_pip", "ring_dip", "ring_tip",
|
|
"pinky_mcp", "pinky_pip", "pinky_dip", "pinky_tip",
|
|
)
|
|
|
|
|
|
def source_urdf(side: str = "right") -> Path:
|
|
side = side.lower()
|
|
if side not in ("right", "left"):
|
|
raise ValueError(f"Expected right or left hand, got {side!r}")
|
|
return REPO_ROOT / "L20" / side.upper() / f"linkerhand_g20_{side}.urdf"
|
|
|
|
|
|
def _fmt(values) -> str:
|
|
return " ".join(f"{float(value):.12g}" for value in values)
|
|
|
|
|
|
def _origin(element: ET.Element | None) -> dict[str, str]:
|
|
if element is None:
|
|
return {"pos": "0 0 0", "quat": "1 0 0 0"}
|
|
rpy = np.fromstring(element.get("rpy", "0 0 0"), sep=" ")
|
|
quat = Rotation.from_euler("xyz", rpy).as_quat()
|
|
return {"pos": element.get("xyz", "0 0 0"), "quat": _fmt(quat[[3, 0, 1, 2]])}
|
|
|
|
|
|
def _mesh_tip(path: Path) -> np.ndarray:
|
|
"""Approximate distal surface landmark from the terminal 2 mm STL cap.
|
|
|
|
Distal link mesh coordinates have their fingertip in local +Z. Averaging
|
|
unique cap vertices avoids triangle tessellation multiplicity bias. This
|
|
is a geometric approximation, not a measured tactile contact center.
|
|
"""
|
|
dtype = np.dtype([("normal", "<f4", (3,)), ("vertices", "<f4", (3, 3)), ("attr", "<u2")])
|
|
with path.open("rb") as stream:
|
|
stream.seek(80)
|
|
count = int(np.fromfile(stream, dtype="<u4", count=1)[0])
|
|
triangles = np.fromfile(stream, dtype=dtype, count=count)
|
|
if len(triangles) != count:
|
|
raise ValueError(f"Invalid binary STL: {path}")
|
|
vertices = np.unique(triangles["vertices"].reshape(-1, 3), axis=0)
|
|
return vertices[vertices[:, 2] >= vertices[:, 2].max() - .002].mean(axis=0)
|
|
|
|
|
|
def build_model(side: str = "right", output_dir: str | Path | None = None) -> Path:
|
|
"""Create a fixed-wrist, visual-only MJCF without modifying source assets.
|
|
|
|
Mesh references are relative to the generated XML. The output therefore
|
|
remains relocatable with its enclosing repository, but is not an asset
|
|
bundle independent of the repository. All URDF joint limits and all five
|
|
mimic equalities are preserved. Contacts are disabled for kinematic replay.
|
|
"""
|
|
urdf_path = source_urdf(side)
|
|
side = side.lower()
|
|
output_dir = Path(output_dir or (REPO_ROOT / "retarget_l20" / "models")).resolve()
|
|
output_dir.mkdir(parents=True, exist_ok=True)
|
|
tree = ET.parse(urdf_path).getroot()
|
|
links = {link.get("name"): link for link in tree.findall("link")}
|
|
joints = tree.findall("joint")
|
|
children = defaultdict(list)
|
|
for joint in joints:
|
|
children[joint.find("parent").get("link")].append(joint)
|
|
|
|
root = ET.Element("mujoco", model=f"l20_{side}_retarget")
|
|
root.append(ET.Comment("Kinematic preview; meshes and joint limits from the original L20 URDF."))
|
|
ET.SubElement(root, "compiler", angle="radian", balanceinertia="true", autolimits="true", strippath="false")
|
|
ET.SubElement(root, "option", timestep="0.002", gravity="0 0 0")
|
|
ET.SubElement(root, "statistic", center="0 0 0.135", extent="0.32")
|
|
visual = ET.SubElement(root, "visual")
|
|
ET.SubElement(visual, "global", offwidth="1600", offheight="1200", azimuth="20", elevation="-8")
|
|
ET.SubElement(visual, "quality", shadowsize="2048", offsamples="4")
|
|
ET.SubElement(visual, "headlight", ambient="0.22 0.22 0.26", diffuse="0.35 0.35 0.35", specular="0.15 0.15 0.15")
|
|
ET.SubElement(visual, "rgba", haze="0.08 0.10 0.14 1")
|
|
default = ET.SubElement(root, "default")
|
|
ET.SubElement(default, "joint", damping="0.1", armature="0.00001")
|
|
ET.SubElement(default, "geom", contype="0", conaffinity="0", group="1")
|
|
ET.SubElement(default, "site", size="0.0018", rgba="0.1 0.9 0.8 0", group="3")
|
|
asset = ET.SubElement(root, "asset")
|
|
ET.SubElement(asset, "material", name="shell", rgba="0.76 0.82 0.90 1", specular="0.4", shininess="0.5")
|
|
ET.SubElement(asset, "material", name="joints", rgba="0.15 0.24 0.34 1", specular="0.35", shininess="0.4")
|
|
ET.SubElement(asset, "material", name="tips", rgba="0.10 0.68 0.72 1", specular="0.3", shininess="0.3")
|
|
ET.SubElement(asset, "material", name="palm", rgba="0.36 0.44 0.55 1", specular="0.45", shininess="0.5")
|
|
for name, link in links.items():
|
|
for index, visual_link in enumerate(link.findall("visual")):
|
|
mesh = visual_link.find("geometry/mesh")
|
|
if mesh is None:
|
|
raise ValueError(f"Expected visual mesh for {name}")
|
|
path = (urdf_path.parent / mesh.get("filename")).resolve()
|
|
attributes = {"name": f"{name}_mesh_{index}", "file": os.path.relpath(path, output_dir)}
|
|
if mesh.get("scale"):
|
|
attributes["scale"] = mesh.get("scale")
|
|
ET.SubElement(asset, "mesh", **attributes)
|
|
|
|
world = ET.SubElement(root, "worldbody")
|
|
ET.SubElement(world, "light", name="key", pos="0.4 0.25 0.5", dir="-0.7 -0.3 -0.65", diffuse="0.55 0.6 0.65", castshadow="true")
|
|
ET.SubElement(world, "light", name="fill", pos="-0.3 -0.25 0.3", dir="0.8 0.4 -0.3", diffuse="0.25 0.3 0.35", castshadow="false")
|
|
ET.SubElement(world, "camera", name="palm", pos="0.56 0 0.14", xyaxes="0 1 0 0 0 1", fovy="36")
|
|
ET.SubElement(world, "geom", name="floor", type="plane", pos="0 0 -0.003", size="0.7 0.7 0.01", rgba="0.07 0.09 0.13 1", group="0")
|
|
body_elements = {}
|
|
|
|
def add_link(parent: ET.Element, name: str, joint: ET.Element | None = None):
|
|
body = ET.SubElement(parent, "body", name=name, **_origin(None if joint is None else joint.find("origin")))
|
|
body_elements[name] = body
|
|
link = links[name]
|
|
inertial = link.find("inertial")
|
|
if inertial is not None:
|
|
inertia = inertial.find("inertia")
|
|
# MuJoCo fullinertia uses the body-aligned frame. All current L20
|
|
# inertial origins have zero orientation; rotate defensively.
|
|
matrix = np.array([[float(inertia.get("ixx")), float(inertia.get("ixy")), float(inertia.get("ixz"))],
|
|
[float(inertia.get("ixy")), float(inertia.get("iyy")), float(inertia.get("iyz"))],
|
|
[float(inertia.get("ixz")), float(inertia.get("iyz")), float(inertia.get("izz"))]])
|
|
attributes = _origin(inertial.find("origin"))
|
|
quat = np.fromstring(attributes.pop("quat"), sep=" ")
|
|
rotation = Rotation.from_quat(quat[[1, 2, 3, 0]]).as_matrix()
|
|
matrix = rotation @ matrix @ rotation.T
|
|
ET.SubElement(body, "inertial", **attributes, mass=inertial.find("mass").get("value"),
|
|
fullinertia=_fmt([matrix[0, 0], matrix[1, 1], matrix[2, 2], matrix[0, 1], matrix[0, 2], matrix[1, 2]]))
|
|
if joint is not None and joint.get("type") != "fixed":
|
|
if joint.get("type") != "revolute":
|
|
raise ValueError(f"Unsupported joint type {joint.get('type')}")
|
|
limit = joint.find("limit")
|
|
ET.SubElement(body, "joint", name=joint.get("name"), type="hinge", axis=joint.find("axis").get("xyz"),
|
|
range=f"{limit.get('lower')} {limit.get('upper')}")
|
|
material = "palm" if name == "hand_base_link" else "tips" if name.endswith("distal") else "joints" if "metacarpals" in name else "shell"
|
|
for index, link_visual in enumerate(link.findall("visual")):
|
|
ET.SubElement(body, "geom", name=f"{name}_visual_{index}", type="mesh", mesh=f"{name}_mesh_{index}", material=material, **_origin(link_visual.find("origin")))
|
|
for child_joint in children[name]:
|
|
add_link(body, child_joint.find("child").get("link"), child_joint)
|
|
|
|
add_link(world, "hand_base_link")
|
|
placements = [("hand_base_link", "0 0 0"), ("thumb_metacarpals", "0 0 0"),
|
|
("thumb_proximal", "0 0 0"), ("thumb_distal", "0 0 0"),
|
|
("thumb_distal", _fmt(_mesh_tip(urdf_path.parent / "meshes/thumb_distal.STL")))]
|
|
for finger in FINGERS[1:]:
|
|
placements.extend([(f"{finger}_proximal", "0 0 0"), (f"{finger}_middle", "0 0 0"),
|
|
(f"{finger}_distal", "0 0 0"), (f"{finger}_distal", _fmt(_mesh_tip(urdf_path.parent / f"meshes/{finger}_distal.STL")))])
|
|
for index, (body_name, pos) in enumerate(placements):
|
|
ET.SubElement(body_elements[body_name], "site", name=f"landmark_{index:02d}", pos=pos)
|
|
equality = ET.SubElement(root, "equality")
|
|
for joint in joints:
|
|
mimic = joint.find("mimic")
|
|
if mimic is not None:
|
|
ET.SubElement(equality, "joint", name=f"mimic_{joint.get('name')}", joint1=joint.get("name"), joint2=mimic.get("joint"),
|
|
polycoef=f"{mimic.get('offset', '0')} {mimic.get('multiplier', '1')} 0 0 0")
|
|
ET.indent(root, space=" ")
|
|
output_path = output_dir / f"l20_{side}.xml"
|
|
ET.ElementTree(root).write(output_path, encoding="utf-8", xml_declaration=True)
|
|
return output_path
|
|
|
|
|
|
class HandKinematics:
|
|
"""FK in native URDF coordinates; optimization variables are in radians.
|
|
|
|
+Z points from wrist to extended fingers. +X is palm-facing and positive
|
|
flexion brings fingers toward +X. Radial direction (pinky toward index) is
|
|
+Y for the right hand and -Y for the left. Wrist pose is fixed at identity.
|
|
|
|
`landmarks` and `jacobian` accept independent coordinates (16). `landmarks`
|
|
additionally accepts full URDF coordinates (21) for playback. `expand`
|
|
enforces exact linear mimic coupling. Returned arrays are fresh copies.
|
|
"""
|
|
|
|
def __init__(self, model_path: str | Path, side: str = "right"):
|
|
self.side = side.lower()
|
|
self.model_path = Path(model_path).resolve()
|
|
self.model = mujoco.MjModel.from_xml_path(str(self.model_path))
|
|
self.data = mujoco.MjData(self.model)
|
|
urdf = ET.parse(source_urdf(side)).getroot()
|
|
xml_joints = [j for j in urdf.findall("joint") if j.get("type") != "fixed"]
|
|
by_name = {joint.get("name"): joint for joint in xml_joints}
|
|
self.joint_names = [mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_JOINT, j) for j in range(self.model.njnt)]
|
|
self.independent_joint_names = [name for name in self.joint_names if by_name[name].find("mimic") is None]
|
|
self.independent_indices = np.array([self.joint_names.index(name) for name in self.independent_joint_names], dtype=int)
|
|
self.expansion = np.zeros((len(self.joint_names), len(self.independent_joint_names)))
|
|
self.offset = np.zeros(len(self.joint_names))
|
|
self.mimic = {}
|
|
for index, name in enumerate(self.joint_names):
|
|
mimic = by_name[name].find("mimic")
|
|
if mimic is None:
|
|
self.expansion[index, self.independent_joint_names.index(name)] = 1.
|
|
else:
|
|
parent = mimic.get("joint")
|
|
multiplier = float(mimic.get("multiplier", "1"))
|
|
offset = float(mimic.get("offset", "0"))
|
|
if parent not in self.independent_joint_names:
|
|
raise ValueError("Nested mimic coupling is not supported")
|
|
self.expansion[index, self.independent_joint_names.index(parent)] = multiplier
|
|
self.offset[index] = offset
|
|
self.mimic[name] = {"joint": parent, "multiplier": multiplier, "offset": offset}
|
|
self.full_lower = self.model.jnt_range[:, 0].copy()
|
|
self.full_upper = self.model.jnt_range[:, 1].copy()
|
|
self.lower = self.full_lower[self.independent_indices].copy()
|
|
self.upper = self.full_upper[self.independent_indices].copy()
|
|
# Shrink active limits as needed so passive limits are respected too.
|
|
for index, name in enumerate(self.joint_names):
|
|
if name in self.mimic:
|
|
column = np.flatnonzero(self.expansion[index])[0]
|
|
limits = (np.array([self.full_lower[index], self.full_upper[index]]) - self.offset[index]) / self.expansion[index, column]
|
|
self.lower[column] = max(self.lower[column], min(limits))
|
|
self.upper[column] = min(self.upper[column], max(limits))
|
|
self.site_ids = np.array([mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_SITE, f"landmark_{i:02d}") for i in range(21)])
|
|
if np.any(self.site_ids < 0):
|
|
raise ValueError("MJCF does not contain all 21 hand landmark sites")
|
|
self.calibration = json.loads((source_urdf(side).parent / f"g20_{self.side}_G20.json").read_text())
|
|
self.motor_indices = np.array([self.calibration["joints"][name]["motor_index"] for name in self.independent_joint_names], dtype=int)
|
|
self.palm_basis = np.array([[0., 0., 1.], [1. if self.side == "right" else -1., 0., 0.], [0., 1., 0.]])
|
|
self.neutral_landmarks = self.landmarks(np.zeros(len(self.independent_joint_names)))
|
|
|
|
def expand(self, q: np.ndarray) -> np.ndarray:
|
|
q = np.asarray(q, dtype=float)
|
|
if q.shape[-1] != len(self.independent_joint_names):
|
|
raise ValueError(f"Expected {len(self.independent_joint_names)} independent coordinates, got {q.shape}")
|
|
return q @ self.expansion.T + self.offset
|
|
|
|
def landmarks(self, q: np.ndarray) -> np.ndarray:
|
|
q = np.asarray(q, dtype=float)
|
|
if q.shape == (len(self.independent_joint_names),):
|
|
q = self.expand(q)
|
|
if q.shape != (self.model.nq,):
|
|
raise ValueError(f"Expected independent or full hand qpos, got {q.shape}")
|
|
self.data.qpos[:] = q
|
|
mujoco.mj_kinematics(self.model, self.data)
|
|
return self.data.site_xpos[self.site_ids].copy()
|
|
|
|
def jacobian(self, q: np.ndarray) -> np.ndarray:
|
|
self.landmarks(q)
|
|
mujoco.mj_comPos(self.model, self.data)
|
|
jacobian = np.empty((21, 3, len(self.independent_joint_names)))
|
|
jacp = np.zeros((3, self.model.nv))
|
|
for index, site_id in enumerate(self.site_ids):
|
|
mujoco.mj_jacSite(self.model, self.data, jacp, None, int(site_id))
|
|
jacobian[index] = jacp @ self.expansion
|
|
return jacobian
|
|
|
|
def metadata(self) -> dict:
|
|
return {
|
|
"source_urdf": str(source_urdf(self.side).relative_to(REPO_ROOT)),
|
|
"side": self.side,
|
|
"joint_names": self.joint_names,
|
|
"independent_joint_names": self.independent_joint_names,
|
|
"motor_indices": self.motor_indices.tolist(),
|
|
"unused_command_indices": [11, 12, 13, 14],
|
|
"independent_limits_rad": np.column_stack([self.lower, self.upper]).tolist(),
|
|
"mimic": self.mimic,
|
|
"landmark_names": list(LANDMARK_NAMES),
|
|
"landmark_definition": "Joint origins; wrist at base origin; thumb CMC at thumb_metacarpals origin; fingertips approximate distal STL terminal-cap centers.",
|
|
"model_note": "URDF kinematics and linear mimic coupling; calibration nonlinear passive tables differ. No hardware-equivalence claim or motor command conversion.",
|
|
"contact_note": "Collision geoms and contact disabled for kinematic visual replay; no object interaction or collision avoidance.",
|
|
}
|