86 lines
5.0 KiB
Python
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()
|