Files
a7l20-hug-sim/scripts/g20_left_lookup.py
T

92 lines
4.7 KiB
Python

"""G20 LEFT schema-v4 u8 lookup; no SDK initialization or hardware access.
angle_rad is used exactly as stored. zero_angles offsets are provenance, not
silently applied again. Passive angles are table estimates, not sensor readings.
"""
import hashlib
import json
import math
from pathlib import Path
from project_paths import configured_path
DEFAULT_TABLE = configured_path('G20_LEFT_TABLE', 'assets/L20/LEFT/g20_left_G20.json')
# SDK20 motor_index -> native G20 (finger index, payload slot).
MOTOR_SLOT = {0:(0,2), 5:(0,1), 10:(0,0), 15:(0,5)}
for _i in range(1,5):
MOTOR_SLOT.update({_i:(_i,2), 5+_i:(_i,0), 15+_i:(_i,5)})
EXPECTED = {'thumb_cmc_pitch':0, 'thumb_cmc_roll':5, 'thumb_cmc_yaw':10, 'thumb_mcp':15}
for _i,_finger in enumerate(('index','middle','ring','pinky'),1):
EXPECTED.update({f'{_finger}_mcp_pitch':_i, f'{_finger}_mcp_roll':5+_i, f'{_finger}_pip':15+_i})
PASSIVE = {'thumb_ip':15, 'index_dip':16, 'middle_dip':17, 'ring_dip':18, 'pinky_dip':19}
class LeftLookup:
def __init__(self, path=DEFAULT_TABLE):
self.path = Path(path).resolve()
self.raw = self.path.read_bytes()
self.sha256 = hashlib.sha256(self.raw).hexdigest()
self.data = json.loads(self.raw)
d = self.data
if (d.get('schema_version') != 4 or d.get('side') != 'left' or
d.get('model') != 'G20' or d.get('angle_unit') != 'rad' or
d.get('command_range') != [0,255]):
raise ValueError('Expected LEFT G20 schema-v4 rad table with commands 0..255')
self.joints = d['joints']
if set(self.joints) != set(EXPECTED) | set(PASSIVE):
raise ValueError('Unexpected joint names in calibration table')
for name,j in self.joints.items():
passive = name in PASSIVE
if j.get('motor_index') != (PASSIVE if passive else EXPECTED)[name] or bool(j.get('passive',False)) != passive:
raise ValueError(f'Wrong motor_index/passive flag: {name}')
a = j.get('angle_rad',[])
if len(a) != 256 or not all(type(x) in (int,float) and math.isfinite(x) for x in a):
raise ValueError(f'Invalid 256-point table: {name}')
if max(a)-min(a) < 1e-6:
raise ValueError(f'Degenerate table: {name}')
def angle(self, name, raw):
if type(raw) is not int or not 0 <= raw <= 255:
raise ValueError('u8 must be an integer in 0..255')
return self.joints[name]['angle_rad'][raw]
def command(self, name, angle, near=None):
if name not in EXPECTED:
raise ValueError('Passive joints cannot be commanded independently')
curve = self.joints[name]['angle_rad']
if not math.isfinite(angle) or not min(curve) <= angle <= max(curve):
raise ValueError(f'Angle outside calibrated range: {name}: {angle}')
# Nearest measured point, robust to slight nonmonotonic calibration noise.
return min(range(256), key=lambda u:(abs(curve[u]-angle), abs(u-near) if near is not None else u))
def describe(self, rows):
result = {}
for name,j in self.joints.items():
finger,slot = MOTOR_SLOT[j['motor_index']]
raw = rows[finger][slot]
result[name] = {'motor_index':j['motor_index'], 'can_property':0x41+finger,
'slot':slot, 'raw_u8':raw, 'table_angle_rad':self.angle(name,raw),
'passive_estimate':bool(j.get('passive',False)),
'source_joint':j.get('source_joint')}
return result
def differences(self, actual, command):
observed, requested = self.describe(actual), self.describe(command)
result = []
for name in EXPECTED:
a,t = observed[name],requested[name]
delta = a['table_angle_rad']-t['table_angle_rad']
result.append({'joint':name, 'motor_index':a['motor_index'],
'can_property':a['can_property'], 'slot':a['slot'],
'actual_u8':a['raw_u8'], 'command_u8':t['raw_u8'],
'error_u8':a['raw_u8']-t['raw_u8'],
'actual_table_rad':a['table_angle_rad'], 'command_table_rad':t['table_angle_rad'],
'error_rad':delta, 'error_deg':math.degrees(delta)})
return result
def provenance(self):
return {'path':str(self.path), 'sha256':self.sha256,
'declared_serial':self.data.get('serial_number'),
'physical_serial_verified':False, 'schema_version':4, 'side':'left',
'angle_convention':'angle_rad as stored; zero_angles offsets not added',
'passive_values':'estimates from driver motor, not measured joint angles'}