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