liyang
ae28d55f81
Source: RGB-D -> Dyn-HaMR -> L20 retargeting -> FoundationPose -> reference repair -> SPIDER, documented in docs/PIPELINE_LATEST.md and docs/SETUP_AND_WEIGHTS.md. Adds FoundationPose and nvdiffrast upstream snapshots, requirements/pipeline_venv.txt and the FoundationPose weight manifest/downloader. Assets (Git LFS): weights/ (WiLoR detector, HandFlow denoiser, UniDepth-L), FoundationPose checkpoints, HaMeR checkpoint, Dyn-HaMR HMP model and BMC constraints, L20 URDF/meshes, the 20260915_171525 D405 recording and the two box CADs. MANO models are not redistributed (third_party/hamer/_DATA/data/mano/README.txt). Environments, caches and run outputs excluded. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
106 lines
4.8 KiB
Python
106 lines
4.8 KiB
Python
"""L20 assets and JSON-calibrated passive coupling for dex-retargeting."""
|
|
from pathlib import Path
|
|
import json
|
|
import xml.etree.ElementTree as ET
|
|
import numpy as np
|
|
from dex_retargeting.kinematics_adaptor import MimicJointKinematicAdaptor
|
|
import l20_model_source as source
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
source.REPO_ROOT = ROOT / 'third_party/l20_assets'
|
|
ASSET = source.REPO_ROOT / 'L20/RIGHT'
|
|
FINGERS = ['thumb','index','middle','ring','pinky']
|
|
TIPS = np.array([4,8,12,16,20])
|
|
CHAINS = [(1,2,3,4),(5,6,7,8),(9,10,11,12),(13,14,15,16),(17,18,19,20)]
|
|
|
|
def palm_basis(p):
|
|
z=p[9]-p[0]; z=z/np.linalg.norm(z)
|
|
y=p[5]-p[17]; y=y-y.dot(z)*z; y=y/np.linalg.norm(y)
|
|
return np.column_stack([np.cross(y,z),y,z])
|
|
|
|
class Calibration:
|
|
def __init__(self):
|
|
self.raw=json.loads((ASSET/'g20_right_G20.json').read_text())
|
|
self.tables=self.raw['joints']
|
|
urdf=ET.parse(ASSET/'linkerhand_g20_right.urdf').getroot()
|
|
self.urdf_limits={j.get('name'):np.array([float(j.find('limit').get(k)) for k in ['lower','upper']]) for j in urdf.findall('joint')}
|
|
self.active=sorted([n for n,x in self.tables.items() if not x.get('passive',False)],key=lambda n:self.tables[n]['motor_index'])
|
|
self.passive=[n for n in self.tables if n not in self.active]
|
|
self.values={n:np.array(x['angle_rad'])+x.get('zero_angles',{}).get('urdf_zero_offset_rad',0) for n,x in self.tables.items()}
|
|
self.valid_commands={}
|
|
self.bounds={}
|
|
for n in self.active:
|
|
motor=self.tables[n]['motor_index']
|
|
valid=np.ones(256,dtype=bool)
|
|
for name,x in self.tables.items():
|
|
if x['motor_index']==motor:
|
|
low,high=self.urdf_limits[name]
|
|
valid &= (self.values[name]>=low)&(self.values[name]<=high)
|
|
commands=np.flatnonzero(valid)
|
|
assert len(commands)>1 and np.all(np.diff(commands)==1),n
|
|
self.valid_commands[n]=commands
|
|
self.bounds[n]=[self.values[n][commands].min(),self.values[n][commands].max()]
|
|
self.curves={}
|
|
for n in self.passive:
|
|
motor=self.tables[n]['motor_index']
|
|
active=next(a for a in self.active if self.tables[a]['motor_index']==motor)
|
|
order=np.argsort(self.values[active])
|
|
x=self.values[active][order]; y=self.values[n][order]
|
|
x,idx=np.unique(x,return_index=True)
|
|
self.curves[n]=(active,x,y[idx])
|
|
|
|
def passive_value(self,name,q):
|
|
_,x,y=self.curves[name]
|
|
i=np.clip(np.searchsorted(x,q,side='right')-1,0,len(x)-2)
|
|
slope=(y[i+1]-y[i])/(x[i+1]-x[i])
|
|
return float(np.interp(q,x,y)),float(slope)
|
|
|
|
def command(self,q_by_name):
|
|
cmd=np.array(self.raw['baseline_command_u8'],dtype=np.uint8)
|
|
for n in self.active:
|
|
allowed=self.valid_commands[n]
|
|
cmd[self.tables[n]['motor_index']]=allowed[np.argmin(np.abs(self.values[n][allowed]-q_by_name[n]))]
|
|
return cmd
|
|
|
|
def decode(self,commands,joint_names):
|
|
return np.array([self.values[n][commands[self.tables[n]['motor_index']]] for n in joint_names])
|
|
|
|
class CalibratedAdaptor(MimicJointKinematicAdaptor):
|
|
def __init__(self,robot,calibration):
|
|
c=calibration
|
|
parents=[c.curves[n][0] for n in c.passive]
|
|
super().__init__(robot,c.active,parents,c.passive,[1.]*len(parents),[0.]*len(parents))
|
|
self.calibration=c
|
|
|
|
def forward_qpos(self,pin_qpos):
|
|
for i,n in enumerate(self.calibration.passive):
|
|
value,slope=self.calibration.passive_value(n,pin_qpos[self.idx_pin2source[i]])
|
|
pin_qpos[self.idx_pin2mimic[i]]=value
|
|
self.multipliers[i]=slope
|
|
return pin_qpos
|
|
|
|
def build_assets(out, side='right'):
|
|
out.mkdir(parents=True,exist_ok=True)
|
|
asset=source.REPO_ROOT/'L20'/side.upper()
|
|
fixed=source.build_model(side,out)
|
|
mj=ET.parse(fixed)
|
|
# Preserve both the original URDF and JSON. Adapt only generated files.
|
|
eq=mj.getroot().find('equality')
|
|
if eq is not None: mj.getroot().remove(eq)
|
|
mj.write(fixed)
|
|
tree=ET.parse(asset/f'linkerhand_g20_{side}.urdf')
|
|
root=tree.getroot()
|
|
for mesh in root.iter('mesh'): mesh.set('filename',str(asset/mesh.get('filename')))
|
|
for j in root.findall('joint'):
|
|
mimic=j.find('mimic')
|
|
if mimic is not None:j.remove(mimic)
|
|
for body in mj.getroot().iter('body'):
|
|
for site in body.findall('site'):
|
|
name=site.get('name')
|
|
ET.SubElement(root,'link',name=name)
|
|
j=ET.SubElement(root,'joint',name=name+'_fixed',type='fixed')
|
|
ET.SubElement(j,'parent',link=body.get('name'));ET.SubElement(j,'child',link=name)
|
|
ET.SubElement(j,'origin',xyz=site.get('pos'),rpy='0 0 0')
|
|
path=out/'l20_dex.urdf';tree.write(path)
|
|
return fixed,path
|