401 lines
19 KiB
Python
401 lines
19 KiB
Python
#!/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', '<f4', 3), ('v', '<f4', (3, 3)), ('attr', '<u2')])
|
|
triangles = np.frombuffer(raw[84:], dtype=dtype, count=count)
|
|
vertices = np.unique(triangles['v'].reshape(-1, 3), axis=0)
|
|
eq = ConvexHull(vertices).equations
|
|
normal_axis, length_axis, lateral_axis = (2, 1, 0) if self.thumb else (0, 2, 1)
|
|
eq = eq[eq[:, normal_axis] > 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()
|