129 lines
6.4 KiB
Python
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()
|