149 lines
8.6 KiB
Python
149 lines
8.6 KiB
Python
#!/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()
|