258 lines
12 KiB
Python
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()
|