#!/usr/bin/env python3 """Refine the accepted five-photo O30 calibration with eight side views. One joint calibration is shared by all poses/views. Image landmarks are manual estimates, and camera poses are nuisance parameters, not measured extrinsics. """ from __future__ import annotations import argparse import json import os from pathlib import Path import sys import xml.etree.ElementTree as ET if '--render' in sys.argv: os.environ.setdefault('MUJOCO_GL', 'egl') import numpy as np from scipy.optimize import least_squares from scipy.spatial.transform import Rotation import fit_photos as photo base = photo.base SIDE = Path(__file__).with_name('side_landmarks.json') PREVIOUS = base.MODEL_DIR / 'o30_right_photo_calibration_v2.json' OUTPUT_JSON = base.MODEL_DIR / 'o30_right_multiview_calibration.json' OUTPUT_URDF = base.MODEL_DIR / 'linkerhand_o30_right_multiview_calibrated.urdf' REPORT = base.ROOT / 'outputs/reports/O30/o30_right_multiview_calibration' JOINT_PAD_COUNT = 56 class MultiviewCalibration(photo.PhotoCalibration): def __init__(self): super().__init__() self.previous = json.loads(PREVIOUS.read_text()) self.v2 = np.array(self.previous['parameter_vector']) self.side_annotations = json.loads(SIDE.read_text()) if self.side_annotations['image_size'] != [self.width, self.height]: raise ValueError('Side annotations must use the same pixel coordinate size') sides = self.side_annotations['photos'] expected = [f'{view}_{finger}' for view in ['left', 'right'] for finger in base.FINGERS[1:]] if [p['name'] for p in sides] != expected: raise ValueError('Incorrect side-photo order') landmarks = {name: (int(self.model.jnt_bodyid[self.model.joint(name).id]), np.zeros(3)) for name in base.NAMES} # Reuse the exact visible tip definition used by the original photos. for old in self.photos: for k, point in enumerate(old['points']): if point[0].endswith('_tip'): i = self.photos.index(old) landmarks[point[0]] = (self.photo_data[i][0][k], self.photo_data[i][1][k]) for side in sides: self.photo_commands.append(next(s['command'] for s in self.samples if s['finger'] == side['pose'])) points = side['points'] body = np.array([landmarks[p[0]][0] for p in points]) local = np.array([landmarks[p[0]][1] for p in points]) observed = np.array([p[1:3] for p in points], dtype=float) sigma = np.array([p[3] for p in points], dtype=float) if not np.all(np.isfinite(observed)) or np.any(sigma <= 0): raise ValueError('Invalid side annotations') self.photo_data.append((body, local, observed, sigma)) self.photos.extend(sides) self.view_count = len(self.photos) def camera_bounds(self): return (np.r_[np.tile([-6, -6, -6, -.3, -.2, .2], self.view_count), np.log(500)], np.r_[np.tile([6, 6, 6, .3, .3, 1.5], self.view_count), np.log(2000)]) def camera_initial(self, q): focal = self.previous['photo_fit']['focal_px'] cameras = list(np.array(self.previous['camera_parameters'][:30]).reshape(5, 6)) for i, side in enumerate(self.photos[5:], 5): rotation = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]]) if side['view'] == 'left' else np.array( [[-1, 0, 0], [0, 0, -1], [0, -1, 0]]) world = self.world_points(i, q) _, _, observed, sigma = self.photo_data[i] best = None for yaw in [-.25, 0, .25]: rv = (Rotation.from_matrix(rotation)*Rotation.from_euler('z', yaw)).as_rotvec() result = least_squares( lambda v: ((self.project(world, v, focal)-observed)/sigma[:, None]).ravel(), np.r_[rv, 0, .08, .4], max_nfev=200, bounds=([-6, -6, -6, -.3, -.2, .2], [6, 6, 6, .3, .3, 1.5])) if best is None or result.cost < best.cost: best = result cameras.append(best.x) return np.r_[np.array(cameras).ravel(), np.log(focal)] def photo_residual(self, q, cameras): poses, focal = cameras[:-1].reshape(self.view_count, 6), np.exp(cameras[-1]) residual = [] for i in range(self.view_count): _, _, observed, sigma = self.photo_data[i] predicted = self.project(self.world_points(i, q), poses[i], focal) residual.extend(self.robust_photo_residual((predicted-observed)/sigma[:, None]).ravel()) residual.append((cameras[-1]-np.log(self.previous['photo_fit']['focal_px']))/.35) return residual def joint_residual(self, vector): q = vector[:JOINT_PAD_COUNT] residual = list(base.Calibration.residual(self, q)) residual.extend(self.photo_residual(q, vector[JOINT_PAD_COUNT:])) # Stay near the accepted V2, rather than refitting from a different hand. residual.extend((q[:20]-self.v2[:20])/.087*.6) residual.extend((q[20:40]-self.v2[20:40])/.2*.6) for i in range(4): self.set_pose(q, self.commands[i]) _, tn = self.world_pad(q, i, 'thumb') _, fn = self.world_pad(q, i, self.samples[i]['finger']) residual.extend((tn+fn)/.065) sigma = np.full(20, 3.) sigma[[0, 1, 6, 15]] = 5. residual.extend(self.positions(q, self.zero_command)/np.deg2rad(sigma)) return np.array(residual) def metrics_photos(self, q, cameras): poses, focal = cameras[:-1].reshape(self.view_count, 6), np.exp(cameras[-1]) groups = {k: [] for k in ['all', 'front', 'side', 'side_motors', 'side_pip_dip_ip']} rows = [] for i, image in enumerate(self.photos): _, _, observed, sigma = self.photo_data[i] predicted = self.project(self.world_points(i, q), poses[i], focal) error = np.linalg.norm(predicted-observed, axis=1) groups['all'].extend(error) groups['front' if i < 5 else 'side'].extend(error) details = [] for point, obs, pred, s, e in zip(image['points'], observed, predicted, sigma, error): name = point[0] if i >= 5: target = image['pose'] if name in [target+'_mcp_pitch', target+'_pip', target+'_dip', 'thumb_mcp', 'thumb_ip']: groups['side_motors'].append(e) if name in [target+'_pip', target+'_dip', 'thumb_ip']: groups['side_pip_dip_ip'].append(e) details.append(dict(name=name, observed_px=obs.tolist(), predicted_px=pred.tolist(), sigma_px=float(s), error_px=float(e))) rows.append(dict(photo=image['name'], points=len(error), rms_px=float(np.sqrt(np.mean(error**2))), points_detail=details)) return dict(focal_px=float(focal), photos=rows, **{key+'_rms_px': float(np.sqrt(np.mean(np.array(values)**2))) for key, values in groups.items()}) def accept(self, q, camera, reference): base.check_metrics(self.metrics(q)) self.set_pose(q, self.zero_command) if max([0.] + [-c.dist for c in self.data.contact]) > .0002: raise RuntimeError('Zero pose collision regression') score = self.metrics_photos(q, camera) if score['front_rms_px'] > reference['front_rms_px']+2.5: raise RuntimeError('Original front-view fit regressed beyond tolerance') if score['photos'][4]['rms_px'] > reference['photos'][4]['rms_px']+1.5: raise RuntimeError('Zero photo regressed beyond tolerance') if score['side_pip_dip_ip_rms_px'] >= reference['side_pip_dip_ip_rms_px']: raise RuntimeError('Visible side-view intermediate hinges did not improve') max_change = max(float(np.max(abs(self.positions(q, command)-self.positions(self.v2, command)))) for command in self.photo_commands[:5]) if max_change > np.deg2rad(18): raise RuntimeError('Too large a joint-pose change from the accepted V2') return score def search(self, initial, starts, max_nfev): initial_camera = self.camera_initial(self.v2) reference_camera = self.fit_cameras(self.v2, initial_camera) reference = self.metrics_photos(self.v2, reference_camera) if initial is None: initial = np.r_[self.v2, reference_camera] low, high = self.bounds() # Explicit trust region as well as soft regularization around V2. low[:20] = np.maximum(low[:20], self.v2[:20]-np.deg2rad(8)) high[:20] = np.minimum(high[:20], self.v2[:20]+np.deg2rad(8)) low[20:40] = np.maximum(low[20:40], self.v2[20:40]-.3) high[20:40] = np.minimum(high[20:40], self.v2[20:40]+.3) clow, chigh = self.camera_bounds() bounds = np.r_[low, clow], np.r_[high, chigh] rng, best, history = np.random.default_rng(20260922), None, [] for attempt in range(starts): start = initial.copy() if best is None else best.x.copy() if attempt: start[:40] += rng.normal(0, .018, 40) result = least_squares(self.joint_residual, np.clip(start, bounds[0]+1e-10, bounds[1]-1e-10), bounds=bounds, max_nfev=max_nfev, diff_step=1e-4, x_scale='jac', ftol=1e-7, xtol=1e-9) reason = '' try: score = self.accept(result.x[:JOINT_PAD_COUNT], result.x[JOINT_PAD_COUNT:], reference) accepted = True except RuntimeError as exc: accepted, reason, score = False, str(exc), None history.append(dict(start=attempt+1, cost=float(result.cost), nfev=result.nfev, success=bool(result.success), accepted=accepted, reason=reason, message=result.message, score=None if score is None else { k: v for k, v in score.items() if k.endswith('_rms_px')})) print(json.dumps(history[-1]), flush=True) if accepted and (best is None or result.cost < best.cost): best = result if best is None: raise RuntimeError('No refinement passed acceptance; V2 remains available') return best.x, history, reference_camera, reference def provenance(): return {'stock_urdf_sha256': base.STOCK_URDF, 'stock_xml_sha256': base.STOCK_XML, 'samples_sha256': base.SAMPLES, 'annotations_sha256': photo.ANNOTATIONS, 'side_annotations_sha256': SIDE, 'v2_sha256': PREVIOUS} def save(cal, vector, history, reference_camera, reference): q, camera = vector[:56], vector[56:] score = cal.accept(q, camera, reference) base.CAL_URDF, base.CAL_JSON = OUTPUT_URDF, OUTPUT_JSON info = base.save_solution(cal, q, dict(history=history, objective_evaluations=cal.evaluations, seed=20260922, method='Bounded multistart photogeometric refinement')) tree = ET.parse(OUTPUT_URDF) for link in tree.getroot().findall('link'): color = link.find('visual/material/color') if color is not None: color.set('rgba', '0.48 0.50 0.52 1' if link.get('name', '').endswith('_distal') else '0.68 0.70 0.72 1') tree.getroot().insert(0, ET.Comment(' Multiview refinement of V2; offsets in joint origins, travels in limits. See o30_right_multiview_calibration.json. ')) ET.indent(tree, space=' ') tree.write(OUTPUT_URDF, encoding='utf-8', xml_declaration=True) info.update({key: base.sha256(path) for key, path in provenance().items()}) info.update(status='13-photo refinement; hardware validation pending', method='MuJoCo contact and manual image landmark fitting, with shared joint calibration and per-view cameras', limitations='Manually estimated 2-D landmarks, unknown cameras and lens distortion. ' 'Image residual is training fit, not measured 3-D accuracy. ' 'Full stroke, unseen poses and hardware dynamics remain unvalidated.', calibrated_urdf_sha256=base.sha256(OUTPUT_URDF), zero_command=cal.zero_command.tolist(), camera_parameters=camera.tolist(), baseline_camera_parameters=reference_camera.tolist(), previous_v2=reference, photo_fit=score, pose_changes_deg={cal.photos[i]['name']: dict(zip(base.NAMES, np.rad2deg( cal.positions(q, cmd)-cal.positions(cal.v2, cmd)).tolist())) for i, cmd in enumerate(cal.photo_commands[:5])}) OUTPUT_JSON.write_text(json.dumps(info, indent=2, ensure_ascii=False)+'\n') return info def load(cal): info = json.loads(OUTPUT_JSON.read_text()) for key, path in {**provenance(), 'calibrated_urdf_sha256': OUTPUT_URDF}.items(): if info[key] != base.sha256(path): raise ValueError(f'Calibration input/output changed: {path}') vector = np.r_[info['parameter_vector'], info['camera_parameters']] if vector.shape != (56+6*cal.view_count+1,) or not np.all(np.isfinite(vector)): raise ValueError('Invalid calibration vector') if info['zero_command'] != cal.zero_command.tolist(): raise ValueError('Zero command changed') cal.accept(vector[:56], vector[56:], info['previous_v2']) return info, vector def render(cal, vector): from PIL import Image, ImageDraw, ImageFont REPORT.mkdir(parents=True, exist_ok=True) model, data = cal.model, cal.data model.vis.global_.offwidth, model.vis.global_.offheight = cal.width, cal.height for g in range(model.ngeom): if model.geom_group[g] == 2: model.geom_rgba[g] = [.48, .50, .52, 1] if model.body(model.geom_bodyid[g]).name.endswith('_distal') else [.68, .70, .72, 1] cameras, focal = vector[56:-1].reshape(cal.view_count, 6), np.exp(vector[-1]) font = ImageFont.truetype('/usr/share/fonts/truetype/dejavu/DejaVuSans.ttf', 19) row_height = cal.height+40 with base.mujoco.Renderer(model, cal.height, cal.width) as renderer: for group, indices in [('front_zero', range(5)), ('left', range(5, 9)), ('right', range(9, 13))]: sheet = Image.new('RGB', (cal.width*len(indices), row_height*2), '#f1f5f9') header = ImageDraw.Draw(sheet) for row, q in enumerate([cal.v2, vector[:56]]): for col, i in enumerate(indices): cal.set_pose(q, cal.photo_commands[i]) renderer.update_scene(data) rotation = Rotation.from_rotvec(cameras[i, :3]).as_matrix() eye = -rotation.T@cameras[i, 3:] forward, up = rotation.T@np.array([0, 0, 1]), rotation.T@np.array([0, -1, 0]) for camera in renderer.scene.camera: camera.pos[:], camera.forward[:], camera.up[:] = eye, forward, up camera.frustum_near, camera.frustum_far = .01, 5. camera.frustum_top = .01*cal.height/2/focal camera.frustum_bottom = -camera.frustum_top camera.frustum_center = 0 camera.frustum_width = .01*cal.width/2/focal for light in renderer.scene.lights[:renderer.scene.nlight]: light.pos[:], light.dir[:] = eye, forward light.ambient[:], light.diffuse[:] = [.25]*3, [.65]*3 image = Image.fromarray(renderer.render()) sheet.paste(image, (col*cal.width, row*row_height+40)) header.text((col*cal.width+12, row*row_height+10), f'V{row+2} / {cal.photos[i]["name"]}', fill='#172033', font=font) if row == 1: image.save(REPORT/f'{cal.photos[i]["name"]}_pose.png') draw = ImageDraw.Draw(image) predicted = cal.project(cal.world_points(i, q), cameras[i], focal) for observed, point in zip(cal.photo_data[i][2], predicted): draw.line([tuple(observed), tuple(point)], fill='yellow', width=2) draw.ellipse([observed[0]-4, observed[1]-4, observed[0]+4, observed[1]+4], outline='cyan', width=2) draw.ellipse([point[0]-2, point[1]-2, point[0]+2, point[1]+2], fill='red') draw.text((10, 8), 'Cyan: manual photo points / red: model', fill='white', font=font) image.save(REPORT/f'{cal.photos[i]["name"]}_landmarks.png') sheet.save(REPORT/f'{group}_comparison.png') print(REPORT/f'{group}_comparison.png') 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=4) parser.add_argument('--max-nfev', type=int, default=180) parser.add_argument('--preview', choices=['index', 'middle', 'ring', 'pinky', 'zero']) args = parser.parse_args() if not any([args.fit, args.check, args.render, args.preview]): parser.error('Choose --fit, --check, --render or --preview') if args.starts < 1 or args.max_nfev < 1: parser.error('Optimization budgets must be positive') cal = MultiviewCalibration() if args.preview: photo.preview(cal, args.preview) return if args.fit: initial = load(cal)[1] if OUTPUT_JSON.exists() else None vector, history, reference_camera, reference = cal.search(initial, args.starts, args.max_nfev) info = save(cal, vector, history, reference_camera, reference) print('Exported URDF; regenerate with O30_RIGHT_PREPARE_ONLY=1 ./hands/O30/scripts/run_o30_right_mujoco_plan.sh before --check') else: info, vector = load(cal) if args.check: info['live_validation'] = photo.validate(cal, vector) 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['live_validation'], indent=2)) if args.render: render(cal, vector) if __name__ == '__main__': main()