#!/usr/bin/env python3 """V9: fit corrected middle pinch (SDK middle_pip=216); only middle_pip upper changes.""" import argparse,json,os import xml.etree.ElementTree as ET from pathlib import Path import numpy as np from scipy.optimize import brentq import fit_pinky_root as v8 v7=v8.v7 base=v7.base OUTPUT=base.MODEL_DIR/'linkerhand_o30_right_middle_calibrated.urdf' PARAMETERS=base.MODEL_DIR/'o30_right_middle_calibration.json' SAMPLES=Path(__file__).with_name('samples_v9.json') REPORT=base.ROOT/'outputs/reports/O30/o30_right_middle_calibration' JOINT='middle_pip' def setup(): fit,reference=v8.setup();_,q=v8.load(reference) fit.cal.samples=json.loads(SAMPLES.read_text())['samples'] fit.cal.commands=np.array([s['command'] for s in fit.cal.samples],dtype=float) return fit,q def fit_root(fit,old): c=fit.cal;cmd=c.commands[1];positions=c.positions(old,cmd);start=positions[12];evaluations=0 def distance(angle): nonlocal evaluations evaluations+=1;p=positions.copy();p[12]=angle;c.data.qpos[c.adr]=p base.mujoco.mj_forward(c.model,c.data) return base.mujoco.mj_geomDistance(c.model,c.data,c.geoms['thumb'],c.geoms['middle'],.2,np.zeros(6)) # Keep 0.02 mm separation: within the existing 0.2 mm contact tolerance, avoiding penetration. target=.00002 grid=start+np.deg2rad(np.linspace(-15,15,121));values=[distance(x)-target for x in grid] roots=[] for a,b,fa,fb in zip(grid[:-1],grid[1:],values[:-1],values[1:]): if fa*fb<0:roots.append(brentq(lambda x:distance(x)-target,a,b,xtol=1e-12)) for angle in sorted(roots,key=lambda x:abs(x-start)): q=old.copy();q[32]=(angle-c.lo[12]-q[12])/(c.span[12]*cmd[12]/255)-1 row=fit.contacts(q)[1];p=row['local_contact_m']['middle'];t=row['local_contact_m']['thumb'] if (abs(row['mesh_gap_mm'])<=.2 and row['penetration_mm']<=.2 and p[0]>=-.004 and .014<=p[2]<=.025 and abs(p[1])<=.006 and .027<=t[1]<=.046 and t[2]>=-.001): return q,dict(evaluations=evaluations,target_gap_mm=target*1000, before_pinch_angle_deg=float(np.rad2deg(start)),after_pinch_angle_deg=float(np.rad2deg(angle))) raise RuntimeError('No near-tip contact found by moving this joint alone') def save(fit,old,q,search): tree=ET.parse(v8.OUTPUT);root=tree.getroot() upper=fit.cal.lo[12]+fit.cal.span[12]*(1+q[32]) root.find(f"joint[@name='{JOINT}']/limit").set('upper',format(upper,'.16g')) ET.indent(tree,space=' ');tree.write(OUTPUT,encoding='utf-8',xml_declaration=True) info=dict(status='Middle pinch restored at corrected command; only one URDF limit upper changed',parameter_vector=q.tolist(), search=search,command=fit.cal.commands[1].tolist(),baseline_contacts=fit.contacts(old),contacts=fit.contacts(q), previous_endpoint_deg=float(np.rad2deg(fit.cal.positions(old,np.full(20,255))[12])), endpoint_deg=float(np.rad2deg(fit.cal.positions(q,np.full(20,255))[12])), inputs={str(p.relative_to(base.ROOT)):base.sha256(p) for p in [v8.OUTPUT,v8.PARAMETERS,SAMPLES]}, output_sha256=base.sha256(OUTPUT), limitations='Simulation contact fit at the original command; hardware/full-stroke correspondence is not newly measured.') PARAMETERS.write_text(json.dumps(info,indent=2,ensure_ascii=False)+'\n');return info def load(old): info=json.loads(PARAMETERS.read_text()) for p,h in info['inputs'].items(): if base.sha256(base.ROOT/p)!=h:raise ValueError('Reference changed: '+p) if base.sha256(OUTPUT)!=info['output_sha256']:raise ValueError('Output changed') q=np.array(info['parameter_vector']);mask=np.ones(56,dtype=bool);mask[32]=False if not np.array_equal(q[mask],old[mask]):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('Nonlinear mapping present') original=ET.parse(v8.OUTPUT).getroot();urdf=ET.parse(OUTPUT).getroot() comparison=ET.fromstring(ET.tostring(urdf)) comparison.find(f"joint[@name='{JOINT}']/limit").set('upper',original.find(f"joint[@name='{JOINT}']/limit").get('upper')) def canonical(e):return e.tag,sorted(e.attrib.items()),(e.text or '').strip(),[canonical(x) for x in e] if canonical(original)!=canonical(comparison):raise AssertionError('Change outside single allowed limit') c=fit.cal for cmd in [c.commands[0],c.commands[2],c.commands[3],c.zero_command,*v7.review.COMMANDS]: if not np.array_equal(c.positions(old,cmd),c.positions(q,cmd)):raise AssertionError('Protected pose 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) commands=[*c.commands,c.zero_command,*v7.review.COMMANDS,np.zeros(20),np.full(20,255), *np.random.default_rng(308).uniform(0,255,(30,20))] maximum=0.;rows=[] for k,cmd in enumerate(commands): mapped=mapping.sdk_range_to_full_o30_right_urdf_positions(list(cmd)) for i,name in enumerate(base.NAMES): limit=urdf.find(f"joint[@name='{name}']/limit");lo,hi=float(limit.get('lower')),float(limit.get('upper')) angle=lo+(hi-lo)*float(np.clip(cmd[i],0,255))/255 if abs(angle-mapped[name])>1e-10:raise AssertionError('Not standard linear mapping') 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): other=live.body(native.body(bid).name).id err=float(np.linalg.norm(nd.xpos[bid]-ld.xpos[other]));maximum=max(maximum,err) if err>2e-8 or np.max(abs(nd.xmat[bid]-ld.xmat[other]))>2e-8:raise AssertionError('Native URDF mismatch') if k<4: finger=c.samples[k]['finger'] def geom(n):return next(g for g in range(live.ngeom) if live.geom_group[g]==3 and live.geom_bodyid[g]==live.body(n+'_distal').id) gap=base.mujoco.mj_geomDistance(live,ld,geom('thumb'),geom(finger),.2,np.zeros(6))*1000 penetration=max([0.]+[-ct.dist*1000 for ct in ld.contact]) if abs(gap)>.2 or penetration>.2:raise AssertionError('Pinch contact failure') rows.append(dict(finger=finger,gap_mm=float(gap),penetration_mm=float(penetration))) return dict(tested_commands=len(commands),only_middle_pip_upper_changed=True,other_19_joints_unchanged=True, zero_other_three_pinches_and_two_latest_tests_unchanged=True,no_piecewise_mapping=True, native_urdf_error_m=maximum,actual_contacts=rows) def render(fit,old,q): from PIL import Image,ImageDraw,ImageFont REPORT.mkdir(parents=True,exist_ok=True);c=fit.cal;m=c.model m.vis.global_.offwidth=600;m.vis.global_.offheight=720 for g in range(m.ngeom): if m.geom_group[g]==2:m.geom_rgba[g]=[.48,.50,.52,1] if m.body(m.geom_bodyid[g]).name.endswith('_distal') else [.68,.70,.72,1] font=ImageFont.truetype('/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf',20) sheet=Image.new('RGB',(1200,1520),'#eef2f6');draw=ImageDraw.Draw(sheet) with base.mujoco.Renderer(m,720,600) as r: for row,parameters in enumerate([old,q]): for col,azimuth in enumerate([90,175]): c.set_pose(parameters,c.commands[1]);camera=base.mujoco.MjvCamera() camera.lookat[:]=[.045,0,.10];camera.distance=.32;camera.azimuth=azimuth;camera.elevation=-5 r.update_scene(c.data,camera=camera) sheet.paste(Image.fromarray(r.render()),(600*col,760*row+40)) draw.text((600*col+10,760*row+10),('V8 / middle SDK 216' if row==0 else 'V9 / middle SDK 216')+(' / left' if col==0 else ' / front'),font=font,fill='#172033') sheet.save(REPORT/'comparison.png') def main(): p=argparse.ArgumentParser(description=__doc__) for flag in ['fit','check','render']:p.add_argument('--'+flag,action='store_true') a=p.parse_args() if not any([a.fit,a.check,a.render]):p.error('Choose an action') fit,old=setup() if a.fit:q,search=fit_root(fit,old);info=save(fit,old,q,search);print(json.dumps(search,indent=2)) else:info,q=load(old) if a.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 a.render:render(fit,old,q) if __name__=='__main__':main()