#!/usr/bin/env python3 """Fit one bounded zero/travel model to O30 right fingertip-pad contacts. No ROS publishing or hardware connection. Units are metres and radians. Use --fit to fit/export; --check checks the actual launcher-generated MJCF. """ from __future__ import annotations import argparse import hashlib import json import os from pathlib import Path import sys import xml.etree.ElementTree as ET ROOT = Path(__file__).resolve().parents[2] PKG = ROOT / 'src/linkerhand-sim/linker_hand_mujoco_ros2' sys.path.insert(0, str(PKG)) if '--render' in sys.argv: os.environ.setdefault('MUJOCO_GL', 'egl') import mujoco import numpy as np from scipy.optimize import least_squares from scipy.spatial import ConvexHull from scipy.spatial.transform import Rotation from linker_hand_mujoco_ros2.utils.o30_right_joints import ( O30_RIGHT_SDK_TO_URDF as NAMES, O30_RIGHT_OPEN_CMD, ) MODEL_DIR = PKG / 'linker_hand_mujoco_ros2/urdf/O30/linker_hand_o30_right' STOCK_XML = MODEL_DIR / 'linker_hand_o30_right_stock.xml' STOCK_URDF = MODEL_DIR / 'linkerhand_o30_right_stock.urdf' LIVE_XML = MODEL_DIR / 'linker_hand_o30_right.xml' CAL_URDF = MODEL_DIR / 'linkerhand_o30_right_pinch_calibrated.urdf' CAL_JSON = MODEL_DIR / 'o30_right_pinch_calibration.json' SAMPLES = Path(__file__).with_name('samples.json') REPORT_DIR = ROOT / 'outputs/reports/O30/o30_right_pinch_calibration' FINGERS = ('thumb', 'index', 'middle', 'ring', 'pinky') def sha256(path): return hashlib.sha256(Path(path).read_bytes()).hexdigest() class PadSurface: """Smooth exterior of the same convex STL hull used for MuJoCo contacts. Fix the near-tip region, but allow the contact to slide across the pad. This avoids forcing unmeasured pad centres to touch at every pinch. """ terms = [(i, j) for i in range(6) for j in range(6-i)] def __init__(self, finger): self.thumb = finger == 'thumb' path = MODEL_DIR / 'meshes' / f'{finger}_distal.STL' raw = path.read_bytes() count = int.from_bytes(raw[80:84], 'little') dtype = np.dtype([('normal', ' 1e-6] lengths = np.linspace(.028, .043, 50) if self.thumb else np.linspace(.009, .020, 50) length, lateral = np.meshgrid(lengths, np.linspace(-.004, .004, 25)) length, lateral = length.ravel(), lateral.ravel() surface = np.min(-(length[:, None]*eq[:, length_axis] + lateral[:, None]*eq[:, lateral_axis] + eq[:, 3]) / eq[:, normal_axis], axis=1) self.center = .036 if self.thumb else .016 u, v = (length-self.center)/.01, lateral/.01 basis = np.array([u**i*v**j for i, j in self.terms]).T self.coef = np.linalg.lstsq(basis, surface, rcond=None)[0] self.approximation_error_mm = float(np.max(abs(basis @ self.coef-surface))*1000) def point_normal(self, length, lateral): u, v = (length-self.center)/.01, lateral/.01 height = sum(c*u**i*v**j for c, (i, j) in zip(self.coef, self.terms)) dl = sum(c*i*u**(i-1)*v**j for c, (i, j) in zip(self.coef, self.terms) if i)/.01 ds = sum(c*j*u**i*v**(j-1) for c, (i, j) in zip(self.coef, self.terms) if j)/.01 if self.thumb: point, normal = np.array([lateral, length, height]), np.array([-ds, -dl, 1.]) else: point, normal = np.array([height, lateral, length]), np.array([1., -ds, -dl]) return point, normal/np.linalg.norm(normal) class Calibration: def __init__(self): self.samples = json.loads(SAMPLES.read_text())['samples'] self.commands = np.array([s['command'] for s in self.samples], dtype=float) if self.commands.shape != (len(self.samples), 20) or not np.all( np.isfinite(self.commands) & (self.commands >= 0) & (self.commands <= 255)): raise ValueError('Samples must contain 20 finite SDK values in [0, 255]') if any(s['finger'] not in FINGERS[1:] for s in self.samples): raise ValueError('Unknown opposing finger') self.n = len(self.samples) self.model = mujoco.MjModel.from_xml_path(str(STOCK_XML)) self.data = mujoco.MjData(self.model) self.ids = np.array([self.model.joint(n).id for n in NAMES]) self.adr = self.model.jnt_qposadr[self.ids] self.limits = self.model.jnt_range[self.ids].copy() self.lo = self.limits[:, 0] self.span = self.limits[:, 1]-self.lo self.pads = {f: PadSurface(f) for f in FINGERS} self.bodies = {f: self.model.body(f+'_distal').id for f in FINGERS} self.geoms = {f: next(g for g in range(self.model.ngeom) if self.model.geom_bodyid[g] == self.bodies[f] and self.model.geom_group[g] == 3) for f in FINGERS} categories = ('hand', *FINGERS) self.geom_categories = [next((i for i, f in enumerate(categories) if self.model.body(self.model.geom_bodyid[g]).name.startswith(f)), 0) for g in range(self.model.ngeom)] self.pairs = [(i, j) for i in range(6) for j in range(i+1, 6)] self.pair_index = {p: i for i, p in enumerate(self.pairs)} self.evaluations = 0 def initial(self): return np.r_[np.zeros(40), np.full(self.n, .036), np.full(self.n, .016), np.zeros(2*self.n)] def bounds(self): return (np.r_[np.full(20, -.4), np.full(20, -.5), np.full(self.n, .028), np.full(self.n, .009), np.full(2*self.n, -.004)], np.r_[np.full(20, .4), np.full(20, .5), np.full(self.n, .043), np.full(self.n, .020), np.full(2*self.n, .004)]) def positions(self, x, command): return self.lo+x[:20]+self.span*(1+x[20:40])*np.asarray(command)/255 def set_pose(self, x, command): self.data.qpos[self.adr] = self.positions(x, command) mujoco.mj_forward(self.model, self.data) def world_pad(self, x, i, finger): thumb = finger == 'thumb' length = x[40+i+(0 if thumb else self.n)] lateral = x[40+2*self.n+i+(0 if thumb else self.n)] point, normal = self.pads[finger].point_normal(length, lateral) b = self.bodies[finger] rotation = self.data.xmat[b].reshape(3, 3) return self.data.xpos[b]+rotation@point, rotation@normal def inter_finger_penetrations(self): depths = np.zeros(len(self.pairs)) for contact in self.data.contact: a, b = sorted([self.geom_categories[contact.geom1], self.geom_categories[contact.geom2]]) if a != b: k = self.pair_index[a, b] depths[k] = max(depths[k], -contact.dist) return depths def residual(self, x): residual = [] for i, sample in enumerate(self.samples): self.set_pose(x, sample['command']) thumb, tn = self.world_pad(x, i, 'thumb') finger, fn = self.world_pad(x, i, sample['finger']) residual.extend((thumb-finger)/.0005) residual.extend((tn+fn)/.08) residual.extend(np.maximum(self.inter_finger_penetrations()-.00005, 0)/.00015) self.set_pose(x, O30_RIGHT_OPEN_CMD) residual.extend(np.maximum(self.inter_finger_penetrations()-.00005, 0)/.00015) # Regularization selects small changes; it is not additional measured data. residual.extend(x[:20]/.175*.6) residual.extend(x[20:40]/.25*.6) residual.extend((x[40:40+self.n]-.036)/.008*.3) residual.extend((x[40+self.n:40+2*self.n]-.016)/.006*.3) residual.extend(x[40+2*self.n:]/.004*.3) self.evaluations += 1 return np.array(residual) def metrics(self, x, pose_x=None): rows = [] for i, sample in enumerate(self.samples): self.set_pose(x if pose_x is None else pose_x, sample['command']) thumb, tn = self.world_pad(x, i, 'thumb') finger, fn = self.world_pad(x, i, sample['finger']) ft = np.zeros(6) gap = mujoco.mj_geomDistance(self.model, self.data, self.geoms['thumb'], self.geoms[sample['finger']], .2, ft) rows.append(dict(finger=sample['finger'], label=sample['label'], mesh_gap_mm=float(gap*1000), pad_point_error_mm=float(np.linalg.norm(thumb-finger)*1000), pad_opposition_error_deg=float(np.degrees(np.arccos(np.clip(-tn@fn, -1, 1)))), max_penetration_mm=float(max([0.] + [-c.dist*1000 for c in self.data.contact])))) return rows def fit(self, starts, max_nfev, warm_start=None): rng = np.random.default_rng(20260921) best = None for i in range(starts): x = self.initial() if warm_start is None else warm_start.copy() if i: x[:40] += rng.normal(0, .02, 40) low, high = self.bounds() result = least_squares(self.residual, np.clip(x, low+1e-10, high-1e-10), bounds=(low, high), max_nfev=max_nfev, ftol=1e-6, diff_step=1e-4, x_scale='jac') print(f'fit {i+1}/{starts}: cost={result.cost:.6f}, nfev={result.nfev}, ' f'evaluations={self.evaluations}', flush=True) if best is None or result.cost < best.cost: best = result return best def check_metrics(rows): for row in rows: if (abs(row['mesh_gap_mm']) > .2 or row['pad_point_error_mm'] > .5 or row['pad_opposition_error_deg'] > 5 or row['max_penetration_mm'] > .2): raise RuntimeError(f'Pinch validation failed: {row}') def export_urdf(cal, x): tree = ET.parse(STOCK_URDF) joints = {j.get('name'): j for j in tree.getroot().findall('joint')} for i, name in enumerate(NAMES): joint = joints[name] origin = joint.find('origin') rpy = np.fromstring(origin.get('rpy', '0 0 0'), sep=' ') axis = np.fromstring(joint.find('axis').get('xyz'), sep=' ') axis /= np.linalg.norm(axis) rotation = Rotation.from_euler('xyz', rpy)*Rotation.from_rotvec(axis*x[i]) origin.set('rpy', ' '.join(f'{v:.12g}' for v in rotation.as_euler('xyz'))) joint.find('limit').set('upper', f'{cal.lo[i]+cal.span[i]*(1+x[20+i]):.12g}') tree.getroot().insert(0, ET.Comment( ' Four-pinch fitted candidate, 2026-09-21. Joint origin = stock origin * ' 'axis_rotation(zero_offset); SDK 0..255 maps to limit lower..upper. ' 'Offsets are already baked in; do not apply twice. Not a measured full-range calibration. ')) ET.indent(tree, space=' ') tree.write(CAL_URDF, encoding='utf-8', xml_declaration=True) def save_solution(cal, x, fit_info): rows = cal.metrics(x) check_metrics(rows) export_urdf(cal, x) info = dict(schema_version=1, status='four-pinch fitted candidate; hardware validation pending', stock_urdf_sha256=sha256(STOCK_URDF), stock_xml_sha256=sha256(STOCK_XML), samples_sha256=sha256(SAMPLES), samples=cal.samples, method='Bounded regularized nonlinear least squares using MuJoCo forward kinematics/contact', limitations='Four contacts do not uniquely identify 20 joint zero offsets and 20 travels. ' 'Pad coordinates are fitted nuisance parameters, not measured. ' 'No full-travel, unseen-pose, hardware, or PD dynamics validation.', parameter_vector=x.tolist(), optimizer=fit_info, pad_surface_approximation_error_mm={f: p.approximation_error_mm for f, p in cal.pads.items()}, joints={name: dict(zero_offset_rad=float(x[i]), travel_scale=float(1+x[20+i]), sdk0_angle_in_calibrated_urdf_rad=float(cal.lo[i]), sdk255_angle_in_calibrated_urdf_rad=float(cal.lo[i]+cal.span[i]*(1+x[20+i]))) for i, name in enumerate(NAMES)}, baseline=cal.metrics(x, np.zeros_like(x)), fitted=rows) CAL_JSON.write_text(json.dumps(info, indent=2, ensure_ascii=False)+'\n') return info def load_solution(): info = json.loads(CAL_JSON.read_text()) for key, path in [('stock_urdf_sha256', STOCK_URDF), ('stock_xml_sha256', STOCK_XML), ('samples_sha256', SAMPLES)]: if info[key] != sha256(path): raise ValueError(f'Calibration input changed: {path}; refit before checking') return info, np.array(info['parameter_vector']) def validate_live(cal, x): """Check actual generated model and its SDK mapping, including random commands.""" from linker_hand_mujoco_ros2.utils.o30_right_joints import ( configure_o30_right_limits, sdk_range_to_full_o30_right_urdf_positions, ) live = mujoco.MjModel.from_xml_path(str(LIVE_XML)) data = mujoco.MjData(live) configure_o30_right_limits(LIVE_XML) rng = np.random.default_rng(30) commands = [*cal.commands, O30_RIGHT_OPEN_CMD, np.zeros(20), np.full(20, 255), *rng.uniform(0, 255, (30, 20))] max_error = 0. for command in commands: cal.set_pose(x, command) mapping = sdk_range_to_full_o30_right_urdf_positions(list(command)) for name, value in mapping.items(): jid = live.joint(name).id if not live.jnt_range[jid, 0]-1e-12 <= value <= live.jnt_range[jid, 1]+1e-12: raise AssertionError(f'Mapping exceeds joint range: {name}') data.qpos[live.jnt_qposadr[jid]] = value mujoco.mj_forward(live, data) for bid in range(1, cal.model.nbody): name = cal.model.body(bid).name other = live.body(name).id error = np.linalg.norm(data.xpos[other]-cal.data.xpos[bid]) max_error = max(max_error, float(error)) if error > 2e-7 or np.max(abs(data.xmat[other]-cal.data.xmat[bid])) > 2e-6: raise AssertionError(f'Export/mapping mismatch for {name}: {error} m') # Evaluate actual compiled MJCF, rather than only trusting the optimized source. rows = [] for sample in cal.samples: mapping = sdk_range_to_full_o30_right_urdf_positions(sample['command']) for name, value in mapping.items(): data.qpos[live.jnt_qposadr[live.joint(name).id]] = value mujoco.mj_forward(live, data) def geom(f): return next(g for g in range(live.ngeom) if live.geom_group[g] == 3 and live.geom_bodyid[g] == live.body(f+'_distal').id) gap = mujoco.mj_geomDistance(live, data, geom('thumb'), geom(sample['finger']), .2, np.zeros(6)) penetration = max([0.] + [-c.dist for c in data.contact]) if abs(gap) > .0002 or penetration > .0002: raise AssertionError(f'Actual model collision check failed: {sample["finger"]}') rows.append(dict(finger=sample['finger'], mesh_gap_mm=gap*1000, max_penetration_mm=penetration*1000)) check_metrics(cal.metrics(x)) return dict(tested_commands=len(commands), max_export_position_error_m=max_error, actual_mjcf=rows) def render(cal, x): from PIL import Image, ImageDraw, ImageFont REPORT_DIR.mkdir(parents=True, exist_ok=True) cal.model.vis.global_.offwidth = 600 cal.model.vis.global_.offheight = 600 renderer = mujoco.Renderer(cal.model, 600, 600) sheet = Image.new('RGB', (600*cal.n, 1280), '#f1f5f9') draw = ImageDraw.Draw(sheet) font = ImageFont.truetype('/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf', 20) for row in range(2): for i, sample in enumerate(cal.samples): cal.set_pose(np.zeros_like(x) if row == 0 else x, sample['command']) camera = mujoco.MjvCamera() camera.lookat[:] = [.045, 0, .10] camera.distance, camera.azimuth, camera.elevation = .29, 215, -15 renderer.update_scene(cal.data, camera=camera) sheet.paste(Image.fromarray(renderer.render()), (600*i, 640*row+40)) draw.text((600*i+16, 640*row+10), ('Before' if row == 0 else 'After')+' / '+sample['finger'], fill='#172033', font=font) renderer.close() path = REPORT_DIR / 'before_after.png' sheet.save(path) print(path) def preview(finger, stock=False): import time import mujoco.viewer from linker_hand_mujoco_ros2.utils.o30_right_joints import configure_o30_right_limits, sdk_range_to_full_o30_right_urdf_positions path = STOCK_XML if stock else LIVE_XML configure_o30_right_limits(path) model = mujoco.MjModel.from_xml_path(str(path)) data = mujoco.MjData(model) command = next(s['command'] for s in json.loads(SAMPLES.read_text())['samples'] if s['finger'] == finger) for name, value in sdk_range_to_full_o30_right_urdf_positions(command).items(): data.qpos[model.jnt_qposadr[model.joint(name).id]] = value mujoco.mj_forward(model, data) with mujoco.viewer.launch_passive(model, data) as viewer: viewer.cam.lookat[:] = [.045, 0, .10] viewer.cam.distance, viewer.cam.azimuth, viewer.cam.elevation = .29, 215, -15 while viewer.is_running(): viewer.sync() time.sleep(.02) def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('--fit', action='store_true') parser.add_argument('--check', action='store_true') parser.add_argument('--render', action='store_true') parser.add_argument('--starts', type=int, default=3) parser.add_argument('--max-nfev', type=int, default=300) parser.add_argument('--preview', choices=FINGERS[1:]) parser.add_argument('--stock', action='store_true', help='Use original model for --preview') args = parser.parse_args() if args.preview: preview(args.preview, args.stock) return if not (args.fit or args.check or args.render): parser.error('Choose --fit, --check, --render, or --preview') if args.starts < 1 or args.max_nfev < 1: parser.error('--starts and --max-nfev must be positive') cal = Calibration() if args.fit: warm = None if CAL_JSON.exists(): try: _, warm = load_solution() except ValueError: pass result = cal.fit(args.starts, args.max_nfev, warm) x = result.x info = save_solution(cal, x, dict(cost=float(result.cost), success=bool(result.success), message=result.message, nfev=result.nfev, evaluations=cal.evaluations, seed=20260921, starts=args.starts, max_nfev=args.max_nfev)) print(f'Exported {CAL_URDF}\nRegenerate MJCF with O30_RIGHT_PREPARE_ONLY=1 ./hands/O30/scripts/run_o30_right_mujoco_plan.sh') else: info, x = load_solution() if args.check: info['live_validation'] = validate_live(cal, x) REPORT_DIR.mkdir(parents=True, exist_ok=True) (REPORT_DIR/'validation.json').write_text(json.dumps(info, indent=2, ensure_ascii=False)+'\n') print(json.dumps(info['live_validation'], indent=2)) if args.render: render(cal, x) if __name__ == '__main__': main()