Files
a7l20-hug-sim/scripts/audit_zero_elbow_hand_reference.py
T

86 lines
5.0 KiB
Python

"""Offline mounting-orientation audit at the user's ZERO/J4=90deg references.
Never sends motion. Selection follows the user reference, not hardware acceptance.
"""
import json
import xml.etree.ElementTree as ET
import numpy as np
from scipy.spatial.transform import Rotation as R
from search_cup_poses_offline import ROOT,PoseGuard
def audit():
configuration=json.loads((ROOT/'config/left_hand_mount.json').read_text())
selected_matrix=np.array(configuration['rotation_Arm_Tip_to_hand'])
hand=ET.parse(ROOT/'assets/l20_left_mj.urdf').getroot()
roots={}
for joint in hand.findall('joint'):
child=joint.find('child').get('link')
if child in ('index_metacarpals','pinky_metacarpals'):
roots[child]=np.fromstring(joint.find('origin').get('xyz'),sep=' ')
if not (roots['index_metacarpals'][1]<0<roots['pinky_metacarpals'][1]):
raise ValueError('LEFT hand root geometry does not agree with pinky=+Y')
scene=json.loads((ROOT/'scene/a7l20_scene.json').read_text())
baseline=R.from_quat(np.roll(scene['hands']['left']['mount_quat_wxyz'],-1)).as_matrix()
guard=PoseGuard([0,0,1],[0,0,-2])
references={'all_joints_zero':np.zeros(7),'only_J4_plus90':np.array([0,0,0,np.pi/2,0,0,0])}
rows=[]
for angle in (0,45,90,135):
mount=baseline@R.from_euler('z',angle,degrees=True).as_matrix()
row={'hand_local_z_twist_deg':angle,'mount_rotation':mount.tolist(),
'mount_axis_preserved':bool(np.allclose(mount[:,2],baseline[:,2])), 'references':{}}
for label,joints in references.items():
hand_R=guard.fk(joints)[:3,:3]@mount
normal=hand_R[:,0]
row['references'][label]={'arm_joints_rad':joints.tolist(),
'palm_normal_base':normal.tolist(),'finger_axis_base':hand_R[:,2].tolist(),
'pinky_side_base':hand_R[:,1].tolist(),
'thumb_side_base':(-hand_R[:,1]).tolist(),
'pinky_minus_index_root_height_m':float((hand_R@(roots['pinky_metacarpals']-roots['index_metacarpals']))[2]),
'palm_plane_angle_to_horizontal_deg':float(np.degrees(np.arccos(np.clip(abs(normal[2]),0,1))))}
rows.append(row)
chosen=next((row for row in rows if np.allclose(row['mount_rotation'],selected_matrix)),None)
if chosen is None or chosen['references']['only_J4_plus90']['pinky_side_base'][2]>=-.7:
raise ValueError('Selected mount is not the user-resolved LEFT pinky-low candidate')
report={'executable':False,'hardware_connected':False,
'selected_mount':{'source':'config/left_hand_mount.json',
'hand_local_z_twist_deg':chosen['hand_local_z_twist_deg'],
'rotation_Arm_Tip_to_hand':selected_matrix.tolist(),
'status':'user_reference_resolved_not_physical_motion_validation'},
'user_reference':'All 7 arm joints zero; then only elbow joint rotates 90 degrees.',
'model_assumption':'Elbow=J4, +90deg within downloaded model limits; no real joint was moved.',
'axes':{'palm_normal':'+X_hand','finger_and_mount_normal':'+Z_hand',
'left_pinky_side':'+Y_hand','left_thumb_side':'-Y_hand',
'base_X':'forward','base_Y':'left','base_Z':'up'},
'left_hand_finger_roots_m':{k:v.tolist() for k,v in roots.items()},
'candidates':rows,
'conclusion':'Hand-axis twist can produce palm-down 45deg tilt without tilting flange mating faces.',
'resolved_choice':'User specified pinky side lower: LEFT hand selects +45deg, not +135deg. LEFT +Y is pinky; right-hand sign assumptions are invalid here.',
'prohibited_inference':'Do not conjugate world-Z45 by arbitrary current flange pose to obtain a fixed mounting transform.'}
return report
def main():
report=audit()
# Pure geometry regression checks at exactly the user-specified references.
assert all(row['mount_axis_preserved'] for row in report['candidates'])
for angle in (45,135):
row=next(r for r in report['candidates'] if r['hand_local_z_twist_deg']==angle)
p=row['references']['only_J4_plus90']
assert abs(p['palm_plane_angle_to_horizontal_deg']-45)<1e-6
assert p['palm_normal_base'][2]<-.7
assert np.count_nonzero(p['arm_joints_rad'])==1
path=ROOT/'assets/hand_adapter/zero_elbow_reference_audit.json'
path.write_text(json.dumps(report,indent=2));print(path)
for row in report['candidates']:
print('local twist',row['hand_local_z_twist_deg'],'elbow90 palm',
np.round(row['references']['only_J4_plus90']['palm_normal_base'],4),
'tilt',round(row['references']['only_J4_plus90']['palm_plane_angle_to_horizontal_deg'],3))
assert report['selected_mount']['hand_local_z_twist_deg']==45
chosen=next(r for r in report['candidates'] if r['hand_local_z_twist_deg']==45)
assert chosen['references']['only_J4_plus90']['pinky_minus_index_root_height_m']<-.049
print('PASS: reference-only geometry tests. No hardware. LEFT pinky-low +45deg selected.')
if __name__=='__main__':main()