94 lines
4.0 KiB
Python
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()
|