Initial snapshot: hand motion pipeline (Dyn-HaMR + dex-retargeting + SPIDER)

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
2026-09-15 14:13:49 +08:00
commit 7e4ef6f98b
7002 changed files with 1452249 additions and 0 deletions
@@ -0,0 +1,104 @@
+"""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):
+    out.mkdir(parents=True,exist_ok=True)
+    fixed=source.build_model('right',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/'linkerhand_g20_right.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