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

129 lines
6.4 KiB
Python

"""Saved-state, position-only IK diagnostic. Never connects to hardware.
The board top is an infinite conservative plane, NOT an observed table solid.
Solutions are diagnostic candidates, never executable motion plans.
"""
import json
import argparse
import tempfile
import xml.etree.ElementTree as ET
from pathlib import Path
from project_paths import configured_path
import numpy as np
import trimesh
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation as R
from g20_left_lookup import LeftLookup
from offline_table_guard import ROOT, origin
from search_cup_poses_offline import PoseGuard
def hand_geometry(raw):
lookup = LeftLookup()
values = {k: v['table_angle_rad'] for k, v in lookup.describe(raw).items()}
path = configured_path('CALIBRATED_LEFT_URDF', 'external/calibrated_left/linkerhand_g20_left.urdf', required=True)
robot = ET.parse(path).getroot()
transforms = {'hand_base_link': np.eye(4)}
pending = list(robot.findall('joint'))
while pending:
progressed = False
for joint in pending[:]:
parent = joint.find('parent').get('link')
if parent not in transforms:
continue
motion = np.eye(4)
if joint.get('type') != 'fixed':
if joint.get('type') != 'revolute':
raise ValueError('Unsupported hand joint')
axis = np.fromstring(joint.find('axis').get('xyz'), sep=' ')
motion[:3, :3] = R.from_rotvec(axis * values[joint.get('name')]).as_matrix()
transforms[joint.find('child').get('link')] = transforms[parent] @ origin(joint.find('origin')) @ motion
pending.remove(joint)
progressed = True
if not progressed:
raise ValueError('Unresolved hand tree')
points = []
for link in robot.findall('link'):
for geom in link.findall('collision'):
mesh = geom.find('geometry/mesh')
if mesh is None:
raise ValueError('Unsupported collision geometry')
vertices = np.asarray(trimesh.load_mesh(path.parent / mesh.get('filename'), process=False).vertices)
vertices = vertices * np.fromstring(mesh.get('scale', '1 1 1'), sep=' ')
transform = transforms[link.get('name')] @ origin(geom.find('origin'))
points.append(vertices @ transform[:3, :3].T + transform[:3, 3])
return transforms['index_tip'][:3, 3], np.concatenate(points), lookup.provenance()
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('--pointing', action='store_true', help='Offline index-extended, other fingers curled candidate')
args = parser.parse_args()
camera_path = ROOT / 'data/real_calib_20260927_9jkoqN/camera.json'
hand_path = ROOT / 'data/left_hand_open_q10r1mak/result.json'
board = np.array(json.loads(camera_path.read_text())['T_base_board'])
raw = json.loads(hand_path.read_text())['final_feedback']
source_raw = raw
if args.pointing:
from left_index_pointing_pose import pointing_target
raw = pointing_target(raw, LeftLookup())
tip, cloud, provenance = hand_geometry(raw)
normal = board[:3, 2]
if normal[2] < 0:
normal = -normal
guard = PoseGuard(normal, board[:3, 3])
mount = json.loads((ROOT / 'config/left_hand_mount.json').read_text())
guard.mount = np.array(mount['rotation_Arm_Tip_to_hand'])
guard.hand_center = np.array(mount['translation_L7_to_hand_m'])
guard.cloud = cloud
# Read-only arm_state sample from this session; NOT a live execution start.
start = np.array([.23342778667887387, -.1501840238040728, .11527534905012615,
1.3934699015793086, 1.180948958571756, .014769054703593909, -.08573723964293833])
target = board[:3, :3] @ [-.0135, -.0135, 0] + board[:3, 3] + [0, 0, .05]
tip_l7 = guard.hand_center + guard.mount @ tip
def residual(q):
transform = guard.fk(q)
return transform[:3, :3] @ tip_l7 + transform[:3, 3] - target
lower = np.array([j[3] for j in guard.chain])
upper = np.array([j[4] for j in guard.chain])
rng = np.random.default_rng(27)
candidates = []
for seed in [start] + list(rng.uniform(lower + .001, upper - .001, (31, 7))):
fit = least_squares(residual, seed, bounds=(lower, upper), max_nfev=240,
ftol=1e-10, xtol=1e-10, gtol=1e-10)
error = float(np.linalg.norm(residual(fit.x)))
if error > .002:
continue
gap, body = guard.clearance(fit.x)
candidates.append(dict(joints_rad=fit.x.tolist(), tip_error_m=error,
infinite_plane_gap_m=gap, limiting_body=body,
joint_distance_rad=float(np.linalg.norm(fit.x-start))))
candidates.sort(key=lambda c: (-c['infinite_plane_gap_m'], c['joint_distance_rad']))
report = dict(executable=False, hardware_connected=False, target_base_m=target.tolist(),
hand_pose_policy='index_extended_others_curled' if args.pointing else 'saved_open_feedback',
source_hand_raw=source_raw, planned_hand_raw=raw,
hand_transition_validated=False,
camera_source=str(camera_path), hand_source=str(hand_path), lookup=provenance,
saved_start_rad=start.tolist(), start_plane_gap=guard.clearance(start),
tip_hand_m=tip.tolist(), candidates=candidates,
limitations=['Position-only IK; hand orientation unconstrained.',
'Mesh-derived fingertip, not physically calibrated contact pad.',
'Saved non-synchronized states; no live start validation.',
'Infinite board plane is not measured finite table geometry.',
'No self/other-arm collision or controller trajectory validation.',
'No candidate authorizes motion.'])
output = Path(tempfile.mkdtemp(prefix='index_board_hover_', dir=ROOT/'data')) / 'report.json'
output.write_text(json.dumps(report, indent=2))
print('Target (m):', target.tolist())
print('Position-only IK candidates:', len(candidates))
print('Start conservative plane gap:', report['start_plane_gap'])
if candidates:
print('Best candidate plane gap:', candidates[0]['infinite_plane_gap_m'])
print('NOT executable:', output)
if __name__ == '__main__':
main()