Files
a7l20-cup-grasp/runtime/hug/tools/read_left_hand.py
T

94 lines
4.0 KiB
Python

"""Read-only native left L20 position/identity queries; never initializes SDK."""
import portable_paths
import importlib
import importlib.util
import json
from pathlib import Path
import sys
import time
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT/'scripts'))
from g20_left_lookup import LeftLookup, EXPECTED
from hand_identity import validate_hand_identity
class LeftHandReader:
def __init__(self, channel, expected_serial=None):
if importlib.util.find_spec('can') is None:
sites = list(((portable_paths.VENV / "lib")).glob('python*/site-packages'))
if len(sites) != 1:
raise RuntimeError('Cannot locate existing python-can')
sys.path.append(str(sites[0]))
self.can = importlib.import_module('can')
self.bus = self.can.Bus(channel=channel, interface='socketcan', receive_own_messages=False)
try:
chunks = {r[0]: r[1:] for r in self.query(0xC0, True)}
if sorted(chunks) != list(range(max(chunks)+1)):
raise ValueError('Incomplete serial')
self.serial = bytes(sum([chunks[k] for k in sorted(chunks)], [])).rstrip(b'\0').decode('ascii')
validate_hand_identity(self.serial, 'left', expected_serial)
self.lookup = LeftLookup()
self.faults()
except BaseException:
self.close()
raise
def query(self, prop, multi=False):
if prop not in {0xC0, *range(0x41, 0x46), *range(0x59, 0x5E), *range(0xB0, 0xB6)}:
raise ValueError('Query is not in the read-only allowlist')
while self.bus.recv(0) is not None:
pass
self.bus.send(self.can.Message(arbitration_id=0x28, is_extended_id=False, data=[prop]),timeout=.1)
end = time.monotonic()+(.5 if multi else .15)
retry_at=end-.1;retried=False
rows = []
while time.monotonic() < end:
# Only a read-only one-byte query may be repeated. Keep the same
# overall 150ms freshness budget; never retry a position command.
if not multi and not retried and time.monotonic()>=retry_at:
self.bus.send(self.can.Message(arbitration_id=0x28,is_extended_id=False,data=[prop]),timeout=.1)
self.query_retries=getattr(self,'query_retries',0)+1;retried=True
message = self.bus.recv(.01)
if message is None:
continue
if message.is_extended_id:
raise ValueError('Unexpected arm traffic on hand CAN')
if message.is_error_frame:
raise ValueError('CAN error frame')
if message.arbitration_id == 0x28 and len(message.data) > 1 and message.data[0] == prop:
if not multi:
return list(message.data[1:])
rows.append(list(message.data[1:]))
if rows:
return rows
raise TimeoutError(f'No fresh left hand response {prop:#x}')
def faults(self):
rows = [self.query(0x59+i) for i in range(5)]
if any(len(r) != 6 or any(r) for r in rows):
raise ValueError('Left hand fault: '+str(rows))
def state(self):
start = time.monotonic()
self.faults()
rows = [self.query(0x41+i) for i in range(5)]
if any(len(r) != 6 for r in rows) or time.monotonic()-start > .3:
raise ValueError('Invalid/slow native position read')
described = self.lookup.describe(rows)
return rows, {n: described[n]['table_angle_rad'] for n in EXPECTED}
def tactile(self):
"""Type-2 short-query counts, not calibrated forces in newtons."""
if self.query(0xB0) != [2]:
raise ValueError('Expected type-2 left tactile sensors')
packets = [self.query(0xB1+i) for i in range(5)]
if any(len(p) != 2 or p[0] != 0 for p in packets):
raise ValueError('Malformed type-2 short tactile response: '+str(packets))
return [p[1] for p in packets]
def close(self):
self.bus.shutdown()