#!/usr/bin/env python3 """V7: only middle MCP pitch and thumb IP; ordinary linear URDF limits.""" import argparse import json import os import xml.etree.ElementTree as ET from pathlib import Path import numpy as np import two_joint_review as review linear=review.linear base=linear.base OUTPUT=base.MODEL_DIR/'linkerhand_o30_right_two_joint_calibrated.urdf' PARAMETERS=base.MODEL_DIR/'o30_right_two_joint_calibration.json' REPORT=base.ROOT/'outputs/reports/O30/o30_right_two_joint_calibration' SELECTED=['middle_mcp_pitch','thumb_ip'] def build(): fit=linear.LinearCalibration();_,old=fit.load();c=fit.cal;q=old.copy() target=np.deg2rad(90.) anchor=c.positions(old,c.commands[1])[8];fraction=78/255 zero=(anchor-fraction*target)/(1-fraction) q[8]=zero-c.lo[8];q[28]=(target-zero)/c.span[8]-1 q[35]=(target-c.lo[15]-q[15])/c.span[15]-1 return fit,old,q def save(fit,old,q): review.export(q,old,SELECTED,OUTPUT) info=dict(status='Two selected joint correction; thumb endpoint prioritized over old pinky pinch by user', method='Analytical linear fit: middle command 78 retained with new 255 endpoint; thumb zero retained with new 255 endpoint', limitations='90-degree targets are visual estimates from supplied photos, not measured angles. ' 'Old pinky pinch is allowed to change; no other joint parameters may change.', inputs={str(p.relative_to(base.ROOT)):base.sha256(p) for p in [linear.OUTPUT,linear.PARAMETERS,base.STOCK_URDF,base.SAMPLES]}, output_sha256=base.sha256(OUTPUT),parameter_vector=q.tolist(),confirmed_commands=review.COMMANDS, changed_joints=SELECTED,baseline_contacts=fit.contacts(old),contacts=fit.contacts(q), endpoints_deg={name:dict(before=float(np.rad2deg(fit.cal.positions(old,np.full(20,255))[base.NAMES.index(name)])), after=float(np.rad2deg(fit.cal.positions(q,np.full(20,255))[base.NAMES.index(name)]))) for name in SELECTED}, middle_zero_deg=float(np.rad2deg(fit.cal.positions(q,np.zeros(20))[8])), previous_middle_zero_deg=float(np.rad2deg(fit.cal.positions(old,np.zeros(20))[8]))) PARAMETERS.write_text(json.dumps(info,indent=2,ensure_ascii=False)+'\n') return info def load(fit,old): info=json.loads(PARAMETERS.read_text()) for p,digest in info['inputs'].items(): if base.sha256(base.ROOT/p)!=digest:raise ValueError('Reference changed: '+p) if base.sha256(OUTPUT)!=info['output_sha256']:raise ValueError('V7 URDF edited after calibration') q=np.array(info['parameter_vector']);unchanged=np.ones(56,dtype=bool);unchanged[[8,28,35]]=False if not np.array_equal(q[unchanged],old[unchanged]):raise AssertionError('Unexpected parameter change') return info,q def validate(fit,old,q): from linker_hand_mujoco_ros2.utils import o30_right_joints as mapping mapping.configure_o30_right_limits(base.LIVE_XML) if mapping.O30_RIGHT_SDK_KNOTS:raise AssertionError('Piecewise mapping must not be used') urdf=ET.parse(OUTPUT).getroot();original=ET.parse(linear.OUTPUT).getroot() def structure(e):return e.tag,sorted(e.attrib.items()),(e.text or '').strip(),[structure(child) for child in e] adjusted=ET.fromstring(ET.tostring(urdf)) for name in SELECTED: for tag in ['origin','limit']: a=adjusted.find(f"joint[@name='{name}']/{tag}");b=original.find(f"joint[@name='{name}']/{tag}") a.attrib.clear();a.attrib.update(b.attrib) if structure(adjusted)!=structure(original):raise AssertionError('Other URDF parameters changed') live=base.mujoco.MjModel.from_xml_path(str(base.LIVE_XML));ld=base.mujoco.MjData(live) native=base.mujoco.MjModel.from_xml_path(str(OUTPUT));nd=base.mujoco.MjData(native) c=fit.cal commands=[*review.COMMANDS,*c.commands,c.zero_command,np.zeros(20),np.full(20,255), *np.random.default_rng(307).uniform(0,255,(30,20))] max_native=max_export=0.;test_penetrations=[] for k,cmd in enumerate(commands): mapped=mapping.sdk_range_to_full_o30_right_urdf_positions(list(cmd)) c.set_pose(q,cmd) for i,name in enumerate(base.NAMES): limit=urdf.find(f"joint[@name='{name}']/limit");low,high=float(limit.get('lower')),float(limit.get('upper')) angle=low+(high-low)*float(np.clip(cmd[i],0,255))/255 if abs(angle-mapped[name])>1e-10:raise AssertionError('Runtime is not standard linear') for model,data in [(live,ld),(native,nd)]:data.qpos[model.jnt_qposadr[model.joint(name).id]]=angle base.mujoco.mj_forward(live,ld);base.mujoco.mj_forward(native,nd) for bid in range(1,native.nbody): name=native.body(bid).name;liveid=live.body(name).id;stockid=c.model.body(name).id en=float(np.linalg.norm(nd.xpos[bid]-ld.xpos[liveid]));ee=float(np.linalg.norm(nd.xpos[bid]-c.data.xpos[stockid])) max_native=max(max_native,en);max_export=max(max_export,ee) if en>2e-8 or ee>2e-7 or np.max(abs(nd.xmat[bid]-ld.xmat[liveid]))>2e-8: raise AssertionError('Native/compiled URDF pose mismatch') penetration=max([0.]+[-ct.dist*1000 for ct in ld.contact]) if k<2 or k==6: c.set_pose(old,cmd) baseline=max([0.]+[-ct.dist*1000 for ct in c.data.contact]) test_penetrations.append(dict(test=k,previous_penetration_mm=float(baseline),penetration_mm=float(penetration))) # Exact supplied commands already have a V6 palm/thumb-proximal collision. # Those geometries and joints are frozen; reject new/worsened penetration. if penetration>baseline+.0001:raise AssertionError('New test pose or zero collision regression') # Former middle pinch is retained exactly at its measured command 78. if abs(c.positions(q,c.commands[1])[8]-c.positions(old,c.commands[1])[8])>1e-12: raise AssertionError('Middle 78 anchor changed') contacts=fit.contacts(q) for row in contacts[:3]: if abs(row['mesh_gap_mm'])>.2 or row['penetration_mm']>.2:raise AssertionError('Unexpected first-three pinch regression') # Pinky gap is explicitly accepted and reported, not silently treated as contact. return dict(tested_commands=len(commands),other_18_joint_parameters_unchanged=True,no_piecewise_mapping=True, native_urdf_error_m=max_native,export_error_m=max_export,test_pose_penetrations=test_penetrations, middle_78_anchor_preserved=True,contacts=contacts) def render(): # Review rendering loads the same native pure URDF and same two supplied commands. review.main() from PIL import Image,ImageDraw,ImageFont REPORT.mkdir(parents=True,exist_ok=True) source=review.REPORT/'comparison.png';image=Image.open(source) draw=ImageDraw.Draw(image);font=ImageFont.truetype('/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf',20) draw.rectangle((0,760,1199,799),fill='#eef2f6') draw.text((10,770),'V7 / middle root 255',font=font,fill='#172033') draw.text((610,770),'V7 / thumb tip 255',font=font,fill='#172033') image.save(REPORT/'comparison.png') def main(): parser=argparse.ArgumentParser(description=__doc__) for flag in ['fit','check','render']:parser.add_argument('--'+flag,action='store_true') args=parser.parse_args() if not any([args.fit,args.check,args.render]):parser.error('Choose an action') fit,old,computed=build() if args.fit:info=save(fit,old,computed);q=computed else:info,q=load(fit,old) if args.check: info['validation']=validate(fit,old,q);REPORT.mkdir(parents=True,exist_ok=True) (REPORT/'validation.json').write_text(json.dumps(info,indent=2,ensure_ascii=False)+'\n') print(json.dumps(info['validation'],indent=2)) if args.render:render() if __name__=='__main__':main()