333 lines
11 KiB
Python
333 lines
11 KiB
Python
"""Contract tests for the official ``robot_interface`` compatibility API."""
|
|
|
|
import ast
|
|
from copy import deepcopy
|
|
import inspect
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
|
|
import robot_interface as sdk
|
|
from go1_pro_sdk import LowState as NativeLowState
|
|
from go1_pro_sdk import build_low_cmd_plain
|
|
from examples.example_official_compatible_position import initialize_transport
|
|
|
|
|
|
HIGHLEVEL = 0xee
|
|
LOWLEVEL = 0xff
|
|
POS_STOP_F = 2.146e9
|
|
FR_0 = 0
|
|
FR_1 = 1
|
|
|
|
|
|
class _FakeMCUClient:
|
|
instances = []
|
|
|
|
def __init__(self, mcu_ip, mcu_port, local_port):
|
|
self.mcu_ip = mcu_ip
|
|
self.mcu_port = mcu_port
|
|
self.local_port = local_port or 49152
|
|
self.next_state = NativeLowState()
|
|
self.next_state.motorState[FR_1].q = 1.23
|
|
self.sent = []
|
|
self.closed = False
|
|
self.__class__.instances.append(self)
|
|
|
|
def send(self, cmd):
|
|
self.sent.append(deepcopy(cmd))
|
|
return 616
|
|
|
|
def recv_latest(self):
|
|
return deepcopy(self.next_state)
|
|
|
|
def recv_state(self, timeout):
|
|
return deepcopy(self.next_state)
|
|
|
|
def close(self):
|
|
self.closed = True
|
|
|
|
|
|
@pytest.fixture
|
|
def fake_client(monkeypatch):
|
|
_FakeMCUClient.instances.clear()
|
|
monkeypatch.setattr(
|
|
"go1_pro_sdk.compat.robot_interface.MCUClient",
|
|
_FakeMCUClient,
|
|
)
|
|
return _FakeMCUClient
|
|
|
|
|
|
def test_official_names_and_data_types_are_available():
|
|
assert sdk.LeggedType.Go1 != 2
|
|
assert int(sdk.LeggedType.Go1) == 2
|
|
assert sdk.Go1 is sdk.LeggedType.Go1
|
|
assert sdk.BmsState().SOC == 0
|
|
assert sdk.HighCmd().velocity == [0.0, 0.0]
|
|
assert not hasattr(sdk, "LOWLEVEL")
|
|
assert not hasattr(sdk, "FR_0")
|
|
assert not hasattr(sdk, "__all__")
|
|
assert {name for name in dir(sdk) if not name.startswith("_")} == {
|
|
"LeggedType", "Aliengo", "A1", "Go1", "B1",
|
|
"RecvEnum", "nonBlock", "block", "blockTimeout",
|
|
"UDP", "Safety", "BmsCmd", "BmsState", "Cartesian", "IMU",
|
|
"LED", "MotorState", "MotorCmd", "LowState", "LowCmd",
|
|
"HighState", "HighCmd", "UDPState",
|
|
}
|
|
with pytest.raises(TypeError):
|
|
sdk.Safety(2)
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("factory", "kwargs"),
|
|
[
|
|
(sdk.MotorCmd, {"q": 1.0}),
|
|
(sdk.LowCmd, {"levelFlag": LOWLEVEL}),
|
|
(sdk.Cartesian, {"x": 1.0}),
|
|
(sdk.HighCmd, {"mode": 2}),
|
|
],
|
|
)
|
|
def test_official_struct_constructors_are_no_arg_only(factory, kwargs):
|
|
with pytest.raises(TypeError):
|
|
factory(**kwargs)
|
|
|
|
|
|
def test_init_cmd_data_uses_official_stop_sentinels():
|
|
udp = sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007)
|
|
cmd = sdk.LowCmd()
|
|
|
|
assert udp.InitCmdData(cmd) is None
|
|
assert cmd.levelFlag == LOWLEVEL
|
|
assert len(cmd.motorCmd) == 20
|
|
assert cmd.head == [0xfe, 0xef]
|
|
assert cmd.SN == [0, 0]
|
|
assert cmd.version == [0, 0]
|
|
assert isinstance(cmd.motorCmd[0], sdk.MotorCmd)
|
|
assert isinstance(cmd.motorCmd[0].mode, int)
|
|
assert isinstance(cmd.bms, sdk.BmsCmd)
|
|
assert isinstance(cmd.wirelessRemote, list)
|
|
assert isinstance(cmd.reserve, int)
|
|
assert isinstance(cmd.crc, int)
|
|
assert not hasattr(cmd, "set_motor")
|
|
assert not hasattr(cmd, "all_damping")
|
|
assert not hasattr(cmd.motorCmd[0], "to_bytes")
|
|
assert all(m.mode == 0x0A for m in cmd.motorCmd)
|
|
assert all(m.q == POS_STOP_F for m in cmd.motorCmd)
|
|
assert all(m.dq == 16000.0 for m in cmd.motorCmd)
|
|
plain = build_low_cmd_plain(cmd)
|
|
assert len(plain) == 616
|
|
assert plain[:4] == b"\xfe\xef\xff\x00"
|
|
|
|
|
|
def test_official_lowlevel_send_receive_sequence(fake_client):
|
|
udp = sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007)
|
|
cmd = sdk.LowCmd()
|
|
state = sdk.LowState()
|
|
udp.InitCmdData(cmd)
|
|
|
|
assert udp.Recv() == 858
|
|
assert udp.GetRecv(state) is None
|
|
assert state.motorState[FR_1].q == pytest.approx(1.23)
|
|
assert isinstance(state.head, list)
|
|
assert isinstance(state.SN, list)
|
|
assert isinstance(state.imu.rpy, list)
|
|
assert isinstance(state.imu, sdk.IMU)
|
|
assert isinstance(state.motorState[0], sdk.MotorState)
|
|
assert isinstance(state.bms, sdk.BmsState)
|
|
assert not hasattr(state, "remote")
|
|
assert not hasattr(state.motorState[0], "estimated_current")
|
|
assert not hasattr(state.bms, "voltage_v")
|
|
assert isinstance(state.reserve, int)
|
|
assert isinstance(state.crc, int)
|
|
|
|
cmd.motorCmd[FR_1].q = 1.2
|
|
cmd.motorCmd[FR_1].dq = 0.0
|
|
cmd.motorCmd[FR_1].Kp = 5.0
|
|
cmd.motorCmd[FR_1].Kd = 1.0
|
|
assert udp.SetSend(cmd) == 0
|
|
assert udp.Send() == 616
|
|
|
|
client = fake_client.instances[0]
|
|
assert client.sent[0].motorCmd[FR_1].q == pytest.approx(1.2)
|
|
assert not hasattr(udp, "udpState")
|
|
assert not hasattr(udp, "accessible")
|
|
assert not hasattr(udp, "close")
|
|
|
|
|
|
def test_position_limit_skips_stop_sentinel():
|
|
cmd = sdk.LowCmd()
|
|
sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd)
|
|
cmd.motorCmd[FR_1].q = 99.0
|
|
|
|
safe = sdk.Safety(sdk.LeggedType.Go1)
|
|
assert safe.PositionLimit(cmd) is None
|
|
|
|
assert cmd.motorCmd[FR_0].q == POS_STOP_F
|
|
assert cmd.motorCmd[FR_1].q == pytest.approx(3.5)
|
|
|
|
|
|
def test_position_protect_requires_official_explicit_limit():
|
|
safe = sdk.Safety(sdk.LeggedType.Go1)
|
|
with pytest.raises(TypeError):
|
|
safe.PositionProtect(sdk.LowCmd(), sdk.LowState())
|
|
|
|
|
|
def test_bms_command_is_serialized():
|
|
cmd = sdk.LowCmd()
|
|
cmd.bms.off = 0xA5
|
|
cmd.bms.reserve = [1, 2, 3]
|
|
|
|
plain = build_low_cmd_plain(cmd)
|
|
assert plain[562:566] == b"\xa5\x01\x02\x03"
|
|
|
|
|
|
def test_official_reserved_field_shapes_are_serialized():
|
|
cmd = sdk.LowCmd()
|
|
cmd.SN = [0x04030201, 0x08070605]
|
|
cmd.version = [0x01020304, 0x05060708]
|
|
cmd.reserve = 0x44332211
|
|
|
|
plain = build_low_cmd_plain(cmd)
|
|
assert plain[4:12] == bytes.fromhex("0102030405060708")
|
|
assert plain[12:20] == bytes.fromhex("0403020108070605")
|
|
assert plain[606:610] == bytes.fromhex("11223344")
|
|
|
|
|
|
def test_fixed_size_official_arrays_are_validated():
|
|
cmd = sdk.LowCmd()
|
|
cmd.wirelessRemote = [0] * 39
|
|
with pytest.raises(ValueError, match="wirelessRemote"):
|
|
build_low_cmd_plain(cmd)
|
|
|
|
|
|
def test_highlevel_udp_reports_the_unsupported_boundary():
|
|
udp = sdk.UDP(HIGHLEVEL, 8080, "192.168.123.161", 8082)
|
|
cmd = sdk.HighCmd()
|
|
udp.InitCmdData(cmd)
|
|
udp.SetSend(cmd)
|
|
|
|
with pytest.raises(NotImplementedError, match="HighCmd/HighState UDP"):
|
|
udp.Send()
|
|
|
|
|
|
def test_portable_initialization_sends_only_stop_sentinels_before_control():
|
|
class RecordingUDP:
|
|
def __init__(self):
|
|
self.cmd = None
|
|
self.sent_q = []
|
|
self.recv_count = 0
|
|
|
|
def SetSend(self, cmd):
|
|
self.cmd = cmd
|
|
return 0
|
|
|
|
def Send(self):
|
|
self.sent_q.append([motor.q for motor in self.cmd.motorCmd])
|
|
return 616
|
|
|
|
def Recv(self):
|
|
self.recv_count += 1
|
|
return 858 if self.recv_count >= 2 else 0
|
|
|
|
def GetRecv(self, state):
|
|
return None
|
|
|
|
udp = RecordingUDP()
|
|
cmd = sdk.LowCmd()
|
|
state = sdk.LowState()
|
|
sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd)
|
|
|
|
initialize_transport(udp, cmd, state, frames=2, dt=0)
|
|
|
|
assert len(udp.sent_q) == 2
|
|
assert all(all(q == POS_STOP_F for q in frame) for frame in udp.sent_q)
|
|
|
|
|
|
def test_portable_example_only_uses_official_module_exports():
|
|
source_path = (
|
|
Path(__file__).parents[1]
|
|
/ "examples"
|
|
/ "example_official_compatible_position.py"
|
|
)
|
|
tree = ast.parse(source_path.read_text())
|
|
sdk_attributes = {
|
|
node.attr
|
|
for node in ast.walk(tree)
|
|
if isinstance(node, ast.Attribute)
|
|
and isinstance(node.value, ast.Name)
|
|
and node.value.id == "sdk"
|
|
}
|
|
assert sdk_attributes <= {
|
|
"UDP", "Safety", "LeggedType", "LowCmd", "LowState",
|
|
}
|
|
called_attributes = {
|
|
node.func.attr
|
|
for node in ast.walk(tree)
|
|
if isinstance(node, ast.Call)
|
|
and isinstance(node.func, ast.Attribute)
|
|
}
|
|
assert called_attributes.isdisjoint({
|
|
"set_motor", "all_damping", "wake_mcu", "safe_stop", "close",
|
|
})
|
|
|
|
|
|
def test_official_struct_field_snapshot():
|
|
expected = {
|
|
"BmsCmd": {"off", "reserve"},
|
|
"BmsState": {
|
|
"version_h", "version_l", "bms_status", "SOC", "current",
|
|
"cycle", "BQ_NTC", "MCU_NTC", "cell_vol",
|
|
},
|
|
"Cartesian": {"x", "y", "z"},
|
|
"IMU": {"quaternion", "gyroscope", "accelerometer", "rpy", "temperature"},
|
|
"LED": {"r", "g", "b"},
|
|
"MotorState": {
|
|
"mode", "q", "dq", "ddq", "tauEst", "q_raw", "dq_raw",
|
|
"ddq_raw", "temperature", "reserve",
|
|
},
|
|
"MotorCmd": {"mode", "q", "dq", "tau", "Kp", "Kd", "reserve"},
|
|
"LowState": {
|
|
"head", "levelFlag", "frameReserve", "SN", "version",
|
|
"bandWidth", "imu", "motorState", "bms", "footForce",
|
|
"footForceEst", "tick", "wirelessRemote", "reserve", "crc",
|
|
},
|
|
"LowCmd": {
|
|
"head", "levelFlag", "frameReserve", "SN", "version",
|
|
"bandWidth", "motorCmd", "bms", "wirelessRemote", "reserve", "crc",
|
|
},
|
|
"HighState": {
|
|
"head", "levelFlag", "frameReserve", "SN", "version",
|
|
"bandWidth", "imu", "motorState", "bms", "footForce",
|
|
"footForceEst", "mode", "progress", "gaitType",
|
|
"footRaiseHeight", "position", "bodyHeight", "velocity",
|
|
"yawSpeed", "rangeObstacle", "footPosition2Body",
|
|
"footSpeed2Body", "wirelessRemote", "reserve", "crc",
|
|
},
|
|
"HighCmd": {
|
|
"head", "levelFlag", "frameReserve", "SN", "version",
|
|
"bandWidth", "mode", "gaitType", "speedLevel",
|
|
"footRaiseHeight", "bodyHeight", "position", "euler",
|
|
"velocity", "yawSpeed", "bms", "led", "wirelessRemote",
|
|
"reserve", "crc",
|
|
},
|
|
"UDPState": {
|
|
"TotalCount", "SendCount", "RecvCount", "SendError",
|
|
"FlagError", "RecvCRCError", "RecvLoseError",
|
|
},
|
|
}
|
|
for class_name, field_names in expected.items():
|
|
factory = getattr(sdk, class_name)
|
|
assert str(inspect.signature(factory)) == "()"
|
|
instance = factory()
|
|
assert not (field_names - set(dir(instance)))
|
|
|
|
|
|
def test_official_method_snapshot():
|
|
assert {
|
|
"SetIpPort", "SetRecvTimeout", "SetDisconnectTime",
|
|
"SetAccessibleTime", "Send", "Recv", "InitCmdData", "SetSend",
|
|
"GetRecv",
|
|
} <= set(dir(sdk.UDP))
|
|
assert str(inspect.signature(sdk.Safety.PositionLimit)) == "(self, lowcmd)"
|
|
assert str(inspect.signature(sdk.Safety.PowerProtect)) == "(self, lowcmd, lowstate, factor)"
|
|
assert str(inspect.signature(sdk.Safety.PositionProtect)) == "(self, lowcmd, lowstate, limit)"
|