Files
a7l20-hug-sim/scripts/table_escape_geometry.py

258 lines
12 KiB
Python

"""Fixed-hand geometry and analytic Jacobians for OFFLINE table escape checks.
No controller, ROS, CAN, network, enable or motion calls exist in this module.
The caller must supply an explicit hand posture. A nominal/open posture is
not a measured posture and cannot certify escape for an unknown real hand.
Each returned point cloud is a convex support hull, not a visual-only mesh.
"""
from collections.abc import Mapping
import hashlib
import json
from pathlib import Path
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial import ConvexHull
from scipy.spatial.transform import Rotation
import trimesh
from offline_table_guard import ROOT, TableGuard
from search_cup_poses_offline import hand_cloud
HAND_URDF = ROOT / "assets/l20_left_mj.urdf"
ARM_URDF = ROOT / "assets/controller_model/lingzuv2_description.urdf"
ADAPTER_MESH = ROOT / "assets/hand_adapter/adapter_m_binary.stl"
def _digest(path):
with Path(path).open("rb") as stream:
return hashlib.file_digest(stream, "sha256").hexdigest()
def _finite_scalar(value, name):
if isinstance(value, (bool, str, bytes)) or np.asarray(value).shape != ():
raise ValueError(f"Hand joint {name}: one finite numeric value required")
try:
result = float(value)
except (ValueError, TypeError, OverflowError) as exc:
raise ValueError(f"Hand joint {name}: finite numeric value required") from exc
if not np.isfinite(result):
raise ValueError(f"Hand joint {name}: non-finite value")
return result
def _validated_hand_values(values):
"""Require all 16 active joints; compute and validate all 5 mimic joints."""
if not isinstance(values, Mapping):
raise ValueError("An explicit hand joint-name/value mapping is required")
joints = {j.get("name"): j for j in ET.parse(HAND_URDF).getroot().findall("joint")}
if any(j.get("type") != "revolute" for j in joints.values()):
raise ValueError("Unexpected hand joint type; fixed-hand validator needs updating")
active = tuple(name for name, j in joints.items() if j.find("mimic") is None)
if len(active) != 16 or len(joints) != 21:
raise ValueError("Expected the verified 16-active / 5-mimic LEFT hand")
missing, unknown = set(active) - set(values), set(values) - set(joints)
if missing or unknown:
raise ValueError(f"Invalid hand joint names: missing={sorted(missing)}, unknown={sorted(unknown)}")
supplied = {name: _finite_scalar(value, name) for name, value in values.items()}
expanded = {name: supplied[name] for name in active}
pending = {name for name in joints if name not in active}
while pending:
progressed = False
for name in sorted(pending):
mimic = joints[name].find("mimic")
parent = mimic.get("joint")
if parent not in expanded:
continue
value = (expanded[parent] * float(mimic.get("multiplier", "1"))
+ float(mimic.get("offset", "0")))
if name in supplied and not np.isclose(supplied[name], value, rtol=0, atol=1e-9):
raise ValueError(f"Hand mimic {name} disagrees with {parent}")
expanded[name] = value
pending.remove(name)
progressed = True
if not progressed:
raise ValueError("Unresolved hand mimic dependency")
for name, value in expanded.items():
limit = joints[name].find("limit")
if limit is None:
raise ValueError(f"Missing hand limit: {name}")
lo, hi = float(limit.get("lower")), float(limit.get("upper"))
if not np.isfinite([lo, hi, value]).all() or not lo <= value <= hi:
raise ValueError(f"Hand joint limit violation: {name}={value}, range [{lo}, {hi}]")
return active, expanded
def _hull(points):
points = np.asarray(points, dtype=float)
if points.ndim != 2 or points.shape[1] != 3 or len(points) < 4 or not np.isfinite(points).all():
raise ValueError("Expected a finite three-dimensional mesh")
return np.ascontiguousarray(points[ConvexHull(points).vertices])
class EscapeGeometry:
"""Geometry of the 7-link LEFT arm, actual adapter and one fixed hand pose.
``sample(q)[body]`` contains ``points`` (N, 3) and ``jacobian`` (N, 3, 7),
both in controller BASE coordinates. Point identity/order is stable
across calls. ``reach`` bounds distance from any ancestor axis to any
geometry point, for conservative joint-linear interpolation bounds.
The model covers table avoidance only, not robot self/other-arm collisions.
"""
body_names = [*(f"L{i}_Link" for i in range(1, 8)), "adapter", "hand"]
def __init__(self, hand_values, mount_path=ROOT / "config/left_hand_mount.json"):
self.active_hand_joint_names, self.hand_values = _validated_hand_values(hand_values)
self.active_hand_values = {name: self.hand_values[name] for name in self.active_hand_joint_names}
mount_path = Path(mount_path).resolve()
mount = json.loads(mount_path.read_text())
self.mount = np.asarray(mount["rotation_Arm_Tip_to_hand"], dtype=float)
self.hand_center = np.asarray(mount["translation_L7_to_hand_m"], dtype=float)
tip_translation = np.asarray(mount["translation_Arm_Tip_to_hand_m"], dtype=float)
if (self.mount.shape != (3, 3) or self.hand_center.shape != (3,)
or tip_translation.shape != (3,)
or not np.isfinite(self.mount).all()
or not np.isfinite(self.hand_center).all()
or not np.isfinite(tip_translation).all()
or not np.allclose(self.mount.T @ self.mount, np.eye(3), atol=1e-9, rtol=0)
or not np.isclose(np.linalg.det(self.mount), 1, atol=1e-9, rtol=0)):
raise ValueError("Invalid hand mount rigid transform")
self.adapter_origin = np.array([-.028, 0., -.0525])
if (not np.allclose(self.mount[:, 2], [0, 0, -1], atol=1e-9, rtol=0)
or not np.allclose(self.adapter_origin + self.mount @ [0, 0, .036],
self.hand_center, atol=1e-9, rtol=0)
or not np.allclose(np.array([-.02325, 0, -.052]) + tip_translation,
self.hand_center, atol=1e-9, rtol=0)):
raise ValueError("Mount no longer agrees with the verified 36 mm face-touch assembly")
# Reuse its verified URDF chain and conservative all-posture reach,
# but NEVER use its large, orientation-independent hand sphere here.
template = TableGuard([0, 0, 1], [0, 0, 0])
self.chain = template.chain
self.lo = np.array([joint[3] for joint in self.chain])
self.hi = np.array([joint[4] for joint in self.chain])
self.reach = float(template.reach)
self._local_points = {f"L{i}_Link": joint[2].copy()
for i, joint in enumerate(self.chain, 1)}
adapter = _hull(trimesh.load_mesh(ADAPTER_MESH, process=False).vertices)
self._local_points["adapter"] = adapter @ self.mount.T + self.adapter_origin
hand = _hull(hand_cloud(self.hand_values))
self._local_points["hand"] = hand @ self.mount.T + self.hand_center
# Defensive enlargement if a future explicit posture/mesh extends
# farther than the template's independently derived all-posture bound.
lengths = sum(np.linalg.norm(joint[0][:3, 3]) for joint in self.chain)
actual_bound = lengths + max(np.linalg.norm(v, axis=1).max()
for v in self._local_points.values())
self.reach = max(self.reach, float(actual_bound))
if not np.isfinite(self.reach) or self.reach <= 0:
raise ValueError("Invalid conservative reach")
for points in self._local_points.values():
points.flags.writeable = False
mesh_sources = set()
for root, base, use_basename in (
(ET.parse(ARM_URDF).getroot(), ARM_URDF.parent / "meshes", True),
(ET.parse(HAND_URDF).getroot(), None, False)):
if base is None:
base = Path(root.find("mujoco/compiler").get("meshdir"))
for mesh in root.findall(".//geometry/mesh"):
file = Path(mesh.get("filename"))
if not file.is_absolute():
file = base / (file.name if use_basename else file)
mesh_sources.add(file.resolve())
files = {ARM_URDF, HAND_URDF, mount_path, ADAPTER_MESH, *mesh_sources}
self.sources = {
"sha256": {str(path): _digest(path) for path in sorted(files)},
"mount": mount,
"hand_active_values_rad": self.active_hand_values.copy(),
"hand_all_values_rad": self.hand_values.copy(),
"hand_posture_provenance": "caller_supplied_not_assumed_measured",
"body_vertex_counts": {name: len(self._local_points[name]) for name in self.body_names},
"geometry_scope": "left-arm collision meshes, CAD adapter hull, fixed whole-hand hull",
"reach_m": self.reach,
}
def sample(self, q):
q = np.asarray(q, dtype=float)
if q.shape != (7,) or not np.isfinite(q).all():
raise ValueError("Seven finite LEFT-arm joint angles are required")
if np.any(q < self.lo) or np.any(q > self.hi):
raise ValueError("LEFT-arm joint limit violation")
transform = np.eye(4)
axes, origins = [], []
result = {}
def body_sample(name, count):
points = self._local_points[name] @ transform[:3, :3].T + transform[:3, 3]
jacobian = np.zeros((len(points), 3, 7))
offsets = points[:, None, :] - np.asarray(origins)[None, :count, :]
jacobian[:, :, :count] = np.cross(np.asarray(axes)[None, :count, :],
offsets).transpose(0, 2, 1)
result[name] = {"points": points, "jacobian": jacobian}
for index, (angle, (origin, axis, _vertices, _lo, _hi)) in enumerate(zip(q, self.chain), 1):
transform = transform @ origin
origins.append(transform[:3, 3].copy())
axes.append(transform[:3, :3] @ axis)
rotation = np.eye(4)
rotation[:3, :3] = Rotation.from_rotvec(axis * angle).as_matrix()
transform = transform @ rotation
body_sample(f"L{index}_Link", index)
body_sample("adapter", 7)
body_sample("hand", 7)
return result
def _self_test():
"""Purely synthetic/model-zero posture; never labels it measured hardware."""
root = ET.parse(HAND_URDF).getroot()
nominal = {j.get("name"): 0. for j in root.findall("joint") if j.find("mimic") is None}
geometry = EscapeGeometry(nominal)
q = .37 * geometry.lo + .63 * geometry.hi
sample = geometry.sample(q)
assert list(sample) == geometry.body_names
maximum = 0.
eps = 1e-6
for joint in range(7):
dq = np.zeros(7)
dq[joint] = eps
plus, minus = geometry.sample(q + dq), geometry.sample(q - dq)
for name, row in sample.items():
finite_difference = (plus[name]["points"] - minus[name]["points"]) / (2 * eps)
maximum = max(maximum, float(np.max(np.abs(finite_difference - row["jacobian"][:, :, joint]))))
assert maximum < 2e-8, maximum
for bad_q in (np.zeros(6), np.full(7, np.nan), geometry.hi + .1):
try:
geometry.sample(bad_q)
except ValueError:
pass
else:
raise AssertionError("Bad arm input was accepted")
bad_hands = [dict(nominal, index_mcp_pitch=np.nan), dict(nominal, index_mcp_pitch=2),
dict(nominal, nonexistent=0), dict(nominal, thumb_ip=.2),
{name: value for name, value in nominal.items() if name != "thumb_mcp"}]
for bad_hand in bad_hands:
try:
_validated_hand_values(bad_hand)
except ValueError:
pass
else:
raise AssertionError("Bad hand input was accepted")
print(json.dumps({"scope": "offline synthetic/model-zero hand tests, not a real posture",
"jacobian_max_error_m_per_rad": maximum,
"reach_m": geometry.reach,
"body_vertex_counts": geometry.sources["body_vertex_counts"],
"input_rejection_checks": 8}, indent=2))
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--self-test", action="store_true")
args = parser.parse_args()
if not args.self_test:
parser.error("Library module; pass --self-test for offline synthetic checks")
_self_test()