99 lines
4.9 KiB
Python
99 lines
4.9 KiB
Python
"""Mirror calibrated O30 V10; retain existing V9 filenames for compatibility."""
|
|
from pathlib import Path
|
|
import hashlib
|
|
import json
|
|
import shutil
|
|
import struct
|
|
import xml.etree.ElementTree as ET
|
|
|
|
import numpy as np
|
|
|
|
ROOT = Path(__file__).resolve().parents[2]
|
|
MODEL = ROOT / 'src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O30'
|
|
SOURCE = MODEL / 'linker_hand_o30_right/linkerhand_o30_right_zero_v10_calibrated.urdf'
|
|
OUTPUT = ROOT / 'assets/exports/O30_V10_left_right'
|
|
RIGHT = OUTPUT / 'right/linkerhand_o30_right_v10.urdf'
|
|
LEFT = OUTPUT / 'left/linkerhand_o30_left_v10.urdf'
|
|
|
|
|
|
def mirror_stl(source, target):
|
|
raw = source.read_bytes()
|
|
count = struct.unpack_from('<I', raw, 80)[0]
|
|
if len(raw) != 84 + 50 * count:
|
|
raise ValueError(f'Expected binary STL: {source}')
|
|
dtype = np.dtype([('normal', '<f4', (3,)), ('vertices', '<f4', (3, 3)), ('attr', '<u2')])
|
|
triangles = np.frombuffer(raw, dtype=dtype, offset=84).copy()
|
|
triangles['normal'][:, 1] *= -1
|
|
triangles['vertices'][:, :, 1] *= -1
|
|
# Reflection reverses handedness: swap vertex winding to retain outward normals.
|
|
triangles['vertices'] = triangles['vertices'][:, [0, 2, 1], :]
|
|
target.write_bytes(raw[:84] + triangles.tobytes())
|
|
|
|
|
|
def build():
|
|
for side in ['right', 'left']:
|
|
(OUTPUT / side / 'meshes').mkdir(parents=True, exist_ok=True)
|
|
shutil.copy2(SOURCE, RIGHT)
|
|
tree = ET.parse(SOURCE)
|
|
root = tree.getroot()
|
|
root.set('name', 'linkerhand_o30_left_v10_mirrored')
|
|
def reflect_attr(element, attr, signs):
|
|
values = np.fromstring(element.get(attr, '0 0 0'), sep=' ')
|
|
element.set(attr, ' '.join(format(float(v), '.16g') for v in values * signs))
|
|
for origin in root.findall('.//origin'):
|
|
reflect_attr(origin, 'xyz', [1, -1, 1])
|
|
# S R(r,p,y) S = R(-r,p,-y), S=diag(1,-1,1).
|
|
reflect_attr(origin, 'rpy', [-1, 1, -1])
|
|
for axis in root.findall('.//axis'):
|
|
# Angular vectors are axial vectors, transformed by det(S)*S.
|
|
reflect_attr(axis, 'xyz', [-1, 1, -1])
|
|
for inertia in root.findall('.//inertia'):
|
|
for name in ['ixy', 'iyz']:
|
|
inertia.set(name, format(-float(inertia.get(name)), '.16g'))
|
|
for mesh in root.findall('.//mesh'):
|
|
filename = mesh.get('filename')
|
|
if not filename.startswith('meshes/'):
|
|
raise ValueError(f'Expected relative mesh path: {filename}')
|
|
for filename in sorted({m.get('filename') for m in root.findall('.//mesh')}):
|
|
shutil.copy2(SOURCE.parent / filename, RIGHT.parent / filename)
|
|
mirror_stl(SOURCE.parent / filename, LEFT.parent / filename)
|
|
ET.indent(tree, space=' ')
|
|
tree.write(LEFT, encoding='utf-8', xml_declaration=True)
|
|
samples = json.loads((ROOT / 'tools/o30_right_calibration/samples_v9.json').read_text())
|
|
shutil.copy2(ROOT / 'archive/exports/O30_right_V10_linear/joint_order.txt', OUTPUT / 'sdk_joint_order.txt')
|
|
# External commands use SDK order; radians below are internal reference targets.
|
|
sdk_order = ['thumb_cmc_roll','thumb_cmc_yaw','index_mcp_roll','middle_mcp_roll',
|
|
'ring_mcp_roll','pinky_mcp_roll','thumb_mcp','index_mcp_pitch',
|
|
'middle_mcp_pitch','ring_mcp_pitch','pinky_mcp_pitch','index_pip',
|
|
'middle_pip','ring_pip','pinky_pip','thumb_ip','index_dip',
|
|
'middle_dip','ring_dip','pinky_dip']
|
|
order = [j.get('name') for j in root.findall('joint') if j.get('type') == 'revolute']
|
|
commands = {}
|
|
for sample in samples['samples']:
|
|
angles = {}
|
|
for name, command in zip(sdk_order, sample['command']):
|
|
limit = root.find(f"joint[@name='{name}']/limit")
|
|
lo, hi = float(limit.get('lower')), float(limit.get('upper'))
|
|
angles[name] = lo + (hi - lo) * command / 255
|
|
commands[sample['finger']] = dict(sdk_command=sample['command'], left_ros_radians=[angles[n] for n in order])
|
|
zero = json.loads((ROOT / 'archive/exports/O30_right_V10_linear/zero_command.json').read_text())
|
|
zero_angles = {}
|
|
for name, value in zip(sdk_order, zero):
|
|
limit = root.find(f"joint[@name='{name}']/limit")
|
|
lo, hi = float(limit.get('lower')), float(limit.get('upper'))
|
|
zero_angles[name] = lo + (hi - lo) * value / 255
|
|
(OUTPUT / 'commands.json').write_text(json.dumps(dict(
|
|
asset_version='V10', command_topic='/cb_left_hand_control_cmd',
|
|
command_units='SDK 0..255; send sdk_command, not left_ros_radians',
|
|
sdk_joint_order=sdk_order, left_ros_order=order, pinches=commands,
|
|
zero=dict(sdk_command=zero, left_ros_radians=[zero_angles[n] for n in order])), indent=2)+'\n')
|
|
(OUTPUT / 'version.json').write_text(json.dumps(dict(
|
|
asset_version='V10', source=str(SOURCE.relative_to(ROOT)),
|
|
source_sha256=hashlib.sha256(SOURCE.read_bytes()).hexdigest(),
|
|
note='V9 directory and filenames retained; both assets now contain V10 calibration'), indent=2)+'\n')
|
|
print(OUTPUT)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
build()
|