116 lines
6.1 KiB
Python
116 lines
6.1 KiB
Python
"""Check left/right hand meshes throughout a named-joint closing trajectory."""
|
|
from pathlib import Path
|
|
import xml.etree.ElementTree as ET
|
|
import numpy as np
|
|
import mujoco
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
|
|
|
|
def check_closure(side, position, rotation, start, end, table_z, margin=0.005,
|
|
start_position=None, samples=101):
|
|
if side not in ('left', 'right'):
|
|
raise ValueError('Invalid hand side')
|
|
position = np.asarray(position, float)
|
|
rotation = np.asarray(rotation, float)
|
|
start_position = position if start_position is None else np.asarray(start_position, float)
|
|
if position.shape != (3,) or start_position.shape != (3,) or rotation.shape != (3,3):
|
|
raise ValueError('Invalid hand pose shape')
|
|
if not all(np.isfinite(x).all() for x in (position, start_position, rotation, [table_z, margin])):
|
|
raise ValueError('Nonfinite pose/table')
|
|
if margin < 0 or samples < 2 or not np.allclose(rotation.T @ rotation, np.eye(3), atol=1e-5) or np.linalg.det(rotation) < 0:
|
|
raise ValueError('Invalid rotation or sampling')
|
|
path = ROOT / f'assets/l20_{side}_mj.urdf'
|
|
model = mujoco.MjModel.from_xml_path(str(path))
|
|
data = mujoco.MjData(model)
|
|
mimic = {}
|
|
for joint in ET.parse(path).getroot().findall('joint'):
|
|
m = joint.find('mimic')
|
|
if m is not None:
|
|
mimic[joint.attrib['name']] = (m.attrib['joint'], float(m.get('multiplier', 1)), float(m.get('offset', 0)))
|
|
active = {model.joint(i).name for i in range(model.njnt)} - set(mimic)
|
|
if set(start) != active or set(end) != active:
|
|
raise ValueError('Requires all named active joints')
|
|
for pose in (start, end):
|
|
for name, value in pose.items():
|
|
joint = model.joint(name)
|
|
if not np.isfinite(value) or (joint.limited[0] and not joint.range[0]-1e-6 <= value <= joint.range[1]+1e-6):
|
|
raise ValueError(f'Joint out of range: {name}={value}')
|
|
worst = (float('inf'), '', 0.0)
|
|
for fraction in np.linspace(0, 1, samples):
|
|
current_position = (1-fraction)*start_position + fraction*position
|
|
values = {name: (1-fraction)*start[name] + fraction*end[name] for name in start}
|
|
for name, (parent, scale, offset) in mimic.items():
|
|
values[name] = scale * values[parent] + offset
|
|
for name, value in values.items():
|
|
joint = model.joint(name)
|
|
data.qpos[joint.qposadr[0]] = value
|
|
mujoco.mj_forward(model, data)
|
|
for i in range(model.ngeom):
|
|
if model.geom_type[i] != mujoco.mjtGeom.mjGEOM_MESH:
|
|
raise RuntimeError('Unsupported hand geometry; refusing incomplete collision check')
|
|
mesh = model.geom_dataid[i]
|
|
first = model.mesh_vertadr[mesh]; count = model.mesh_vertnum[mesh]
|
|
vertices = model.mesh_vert[first:first+count]
|
|
local = vertices @ data.geom_xmat[i].reshape(3,3).T + data.geom_xpos[i]
|
|
world = local @ rotation.T + current_position
|
|
height = float(world[:,2].min() - table_z)
|
|
if height < worst[0]:
|
|
worst = (height, model.body(model.geom_bodyid[i]).name, float(fraction))
|
|
if worst[0] < margin:
|
|
raise RuntimeError(f'手部路径碰桌/间距不足: {worst[1]}, 阶段={worst[2]:.2f}, 最小间距={worst[0]*1000:.1f}mm, 要求{margin*1000:.1f}mm')
|
|
return worst[0]
|
|
|
|
|
|
def check_candidate(grasp, side, table_z, clearance=0.10, margin=0.005):
|
|
from scipy.spatial.transform import Rotation
|
|
if not np.isfinite(clearance) or clearance <= 0:
|
|
raise ValueError('Invalid pregrasp clearance')
|
|
quat = np.asarray(grasp['wrist_quat_wxyz'], float)
|
|
if quat.shape != (4,) or not np.isfinite(quat).all() or not np.isclose(np.linalg.norm(quat), 1, atol=1e-4):
|
|
raise ValueError('Invalid hand quaternion')
|
|
rotation = Rotation.from_quat(np.roll(quat, -1)).as_matrix()
|
|
position = np.asarray(grasp['wrist_pos'], float)
|
|
approach = np.asarray(grasp['approach'], float)
|
|
if approach.shape != (3,) or not np.isfinite(approach).all() or np.linalg.norm(approach) < 1e-8:
|
|
raise ValueError('Invalid approach vector')
|
|
pre_position = position - clearance * approach/np.linalg.norm(approach)
|
|
pre, closed = grasp['hand_pre'], grasp['hand_qpos']
|
|
approach_gap = check_closure(side, position, rotation, pre, pre, table_z, margin,
|
|
start_position=pre_position, samples=51)
|
|
closure_gap = check_closure(side, position, rotation, pre, closed, table_z, margin)
|
|
return {'approach_gap_m': approach_gap, 'closure_gap_m': closure_gap,
|
|
'pregrasp_position': pre_position.tolist(), 'table_z_m': table_z}
|
|
|
|
|
|
def filter_candidates(paths, side, table_z, clearance=0.10, margin=0.005):
|
|
import json
|
|
valid, rejected = [], []
|
|
for path in paths:
|
|
try:
|
|
result = check_candidate(json.loads(Path(path).read_text()), side, table_z, clearance, margin)
|
|
valid.append(Path(path))
|
|
print(f'[hand-check] PASS {Path(path).name}: {result}')
|
|
except (ValueError, RuntimeError, KeyError) as exc:
|
|
rejected.append({'candidate': str(path), 'reason': str(exc)})
|
|
print(f'[hand-check] REJECT {Path(path).name}: {exc}')
|
|
return valid, rejected
|
|
|
|
|
|
if __name__ == '__main__':
|
|
import argparse
|
|
import json
|
|
ap = argparse.ArgumentParser(description='Read-only hand/table check; no hardware motion')
|
|
ap.add_argument('--capture', type=Path, required=True)
|
|
ap.add_argument('--side', choices=['left','right'], required=True)
|
|
ap.add_argument('--table-top', type=float, required=True)
|
|
ap.add_argument('--clearance', type=float, default=.10)
|
|
args = ap.parse_args()
|
|
valid, rejected = filter_candidates(sorted(args.capture.glob('grasp_l20_*.json')),
|
|
args.side, args.table_top, args.clearance)
|
|
(args.capture / 'hand_collision_report.json').write_text(json.dumps(
|
|
{'valid': [str(p) for p in valid], 'rejected': rejected, 'executed': False,
|
|
'scope': 'sampled hand meshes vs horizontal table; excludes arm transit, object and other obstacles'},
|
|
indent=2, ensure_ascii=False))
|
|
raise SystemExit(0 if valid else 2)
|