92 lines
4.7 KiB
Python
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'}
|