"""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()