cpp对齐官方
This commit is contained in:
7
tests/cpp/package_consumer/CMakeLists.txt
Normal file
7
tests/cpp/package_consumer/CMakeLists.txt
Normal file
@@ -0,0 +1,7 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
project(unitree_legged_sdk_package_consumer LANGUAGES CXX)
|
||||
|
||||
find_package(unitree_legged_sdk 3.8 CONFIG REQUIRED)
|
||||
|
||||
add_executable(package_consumer main.cpp)
|
||||
target_link_libraries(package_consumer PRIVATE unitree_legged_sdk::unitree_legged_sdk)
|
||||
9
tests/cpp/package_consumer/main.cpp
Normal file
9
tests/cpp/package_consumer/main.cpp
Normal file
@@ -0,0 +1,9 @@
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
|
||||
#include <string>
|
||||
|
||||
int main() {
|
||||
UNITREE_LEGGED_SDK::LowCmd command{};
|
||||
command.motorCmd[UNITREE_LEGGED_SDK::FR_0].q = 0.0f;
|
||||
return UNITREE_LEGGED_SDK::VersionSDK().empty() ? 1 : 0;
|
||||
}
|
||||
127
tests/cpp/test_compat.cpp
Normal file
127
tests/cpp/test_compat.cpp
Normal file
@@ -0,0 +1,127 @@
|
||||
#include "pro_codec.h"
|
||||
#include "unitree_legged_sdk/a1_const.h"
|
||||
#include "unitree_legged_sdk/aliengo_const.h"
|
||||
#include "unitree_legged_sdk/b1_const.h"
|
||||
#include "unitree_legged_sdk/go1_const.h"
|
||||
#include "unitree_legged_sdk/unitree_legged_sdk.h"
|
||||
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <fstream>
|
||||
#include <iterator>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
|
||||
using namespace UNITREE_LEGGED_SDK;
|
||||
|
||||
namespace {
|
||||
|
||||
#define CHECK(expression) do { if (!(expression)) return __LINE__; } while (false)
|
||||
|
||||
uint64_t Fnv1a(const uint8_t* data, std::size_t size) {
|
||||
uint64_t result = 1469598103934665603ull;
|
||||
for (std::size_t i = 0; i < size; ++i) {
|
||||
result ^= data[i];
|
||||
result *= 1099511628211ull;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> ReadFile(const std::string& path) {
|
||||
std::ifstream stream(path, std::ios::binary);
|
||||
return std::vector<uint8_t>(
|
||||
(std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main() {
|
||||
static_assert(std::is_same<decltype(LowCmd::motorCmd), std::array<MotorCmd, 20>>::value,
|
||||
"official LowCmd field shape changed");
|
||||
static_assert(sizeof(IMU) == 53, "IMU must match official packed declaration");
|
||||
static_assert(sizeof(MotorCmd) == 33, "MotorCmd must match official packed declaration");
|
||||
static_assert(sizeof(MotorState) == 38, "MotorState must match official packed declaration");
|
||||
static_assert(sizeof(BmsState) == 34, "BmsState must match official packed declaration");
|
||||
static_assert(sizeof(LowCmd) == 734, "LowCmd must match official packed declaration");
|
||||
static_assert(sizeof(LowState) == 937, "LowState must match official packed declaration");
|
||||
static_assert(sizeof(HighCmd) == 129, "HighCmd must match official packed declaration");
|
||||
static_assert(sizeof(HighState) == 1087, "HighState must match official packed declaration");
|
||||
static_assert(go1_Hip_max > 1.0 && a1_Hip_max < 1.0 &&
|
||||
aliengo_Hip_max > 1.0 && b1_Hip_max < 1.0,
|
||||
"official robot constant headers must be available");
|
||||
|
||||
go1_pro_internal::ProCodec codec(go1_pro_internal::ProCodec::FindStateFile());
|
||||
LowCmd initialized{};
|
||||
go1_pro_internal::InitLowCmd(initialized);
|
||||
CHECK(initialized.head[0] == 0xfe && initialized.head[1] == 0xef);
|
||||
CHECK(initialized.levelFlag == LOWLEVEL && initialized.bandWidth == 0x3ac0);
|
||||
for (const auto& motor : initialized.motorCmd) {
|
||||
CHECK(motor.mode == 0x0a);
|
||||
CHECK(motor.q == static_cast<float>(PosStopF));
|
||||
CHECK(motor.dq == static_cast<float>(VelStopF));
|
||||
CHECK(motor.Kp == 0.0f && motor.Kd == 0.0f && motor.tau == 0.0f);
|
||||
}
|
||||
std::array<uint8_t, 8> zero{};
|
||||
std::array<uint8_t, 8> encrypted{};
|
||||
codec.Encrypt(zero.data(), encrypted.data(), encrypted.size());
|
||||
const std::array<uint8_t, 8> known = {{0x12, 0xaa, 0x03, 0x54, 0x23, 0x6b, 0x66, 0xe3}};
|
||||
CHECK(encrypted == known);
|
||||
|
||||
LowCmd cmd{};
|
||||
cmd.head = {{0xfe, 0xef}};
|
||||
cmd.levelFlag = LOWLEVEL;
|
||||
cmd.bandWidth = 0x3ac0;
|
||||
const auto packet = codec.EncodeLowCmd(cmd);
|
||||
CHECK(Fnv1a(packet.data(), packet.size()) == 0x63c5ef1a61977e18ull);
|
||||
|
||||
#if GO1_PRO_HAVE_CAPTURE_FIXTURES
|
||||
const auto real_lowcmd = ReadFile(GO1_PRO_TEST_LOWCMD);
|
||||
const auto real_lowcmd_plain = ReadFile(GO1_PRO_TEST_LOWCMD_PLAIN);
|
||||
CHECK(real_lowcmd.size() == go1_pro_internal::kLowCmdWireSize);
|
||||
CHECK(real_lowcmd_plain.size() == go1_pro_internal::kLowCmdWireSize);
|
||||
std::vector<uint8_t> decrypted_lowcmd(real_lowcmd.size());
|
||||
codec.Decrypt(real_lowcmd.data(), decrypted_lowcmd.data(), real_lowcmd.size());
|
||||
CHECK(decrypted_lowcmd == real_lowcmd_plain);
|
||||
std::vector<uint8_t> encrypted_lowcmd(real_lowcmd_plain.size());
|
||||
codec.Encrypt(real_lowcmd_plain.data(), encrypted_lowcmd.data(), real_lowcmd_plain.size());
|
||||
CHECK(encrypted_lowcmd == real_lowcmd);
|
||||
|
||||
const auto state_packet = ReadFile(GO1_PRO_TEST_CAPTURE);
|
||||
CHECK(state_packet.size() == go1_pro_internal::kLowStateDatagramSize);
|
||||
LowState state{};
|
||||
CHECK(codec.DecodeLowState(state_packet.data(), state_packet.size(), state));
|
||||
CHECK(state.head[0] == 0xfe && state.head[1] == 0xef);
|
||||
CHECK(state.levelFlag == LOWLEVEL);
|
||||
CHECK(state.motorState[FR_1].temperature == 50);
|
||||
CHECK(std::isfinite(state.motorState[FR_1].q));
|
||||
CHECK(state.wirelessRemote[0] == 0x55 && state.wirelessRemote[1] == 0x51);
|
||||
|
||||
const auto old_state_packet = ReadFile(GO1_PRO_TEST_CAPTURE_OLD);
|
||||
CHECK(old_state_packet.size() == go1_pro_internal::kLowStateDatagramSize);
|
||||
LowState old_state{};
|
||||
CHECK(codec.DecodeLowState(old_state_packet.data(), old_state_packet.size(), old_state));
|
||||
CHECK(old_state.bms.SOC == 76);
|
||||
CHECK(old_state.motorState[FR_0].temperature == 52);
|
||||
#endif
|
||||
|
||||
Safety safety(LeggedType::Go1);
|
||||
cmd.motorCmd[FR_0].q = 4.0f;
|
||||
safety.PositionLimit(cmd);
|
||||
CHECK(cmd.motorCmd[FR_0].q == 0.78f);
|
||||
cmd.motorCmd[FR_1].q = static_cast<float>(PosStopF);
|
||||
safety.PositionLimit(cmd);
|
||||
CHECK(cmd.motorCmd[FR_1].q == static_cast<float>(PosStopF));
|
||||
|
||||
std::atomic<int> callbacks{0};
|
||||
LoopFunc loop("compat-test", 0.001f, Callback([&callbacks] { ++callbacks; }));
|
||||
loop.start();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
loop.shutdown();
|
||||
CHECK(callbacks.load() > 0);
|
||||
return 0;
|
||||
}
|
||||
108
tests/test_capture_fixtures.py
Normal file
108
tests/test_capture_fixtures.py
Normal file
@@ -0,0 +1,108 @@
|
||||
import socket
|
||||
import struct
|
||||
import tarfile
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from go1_pro_sdk import Blowfish, parse_low_state
|
||||
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
CAPTURES = ROOT / "data/captures"
|
||||
STATE_FILE = ROOT / "go1_pro_sdk/_data/blowfish_state.bin"
|
||||
|
||||
if not CAPTURES.is_dir():
|
||||
pytest.skip("private data/captures fixtures are not available", allow_module_level=True)
|
||||
|
||||
|
||||
@pytest.fixture(scope="module")
|
||||
def blowfish():
|
||||
return Blowfish.from_state_file(str(STATE_FILE))
|
||||
|
||||
|
||||
def _pcap_udp_packets(path):
|
||||
packets = []
|
||||
with path.open("rb") as stream:
|
||||
header = stream.read(24)
|
||||
assert len(header) == 24
|
||||
magic = header[:4]
|
||||
assert magic in (b"\xd4\xc3\xb2\xa1", b"\xa1\xb2\xc3\xd4")
|
||||
endian = "<" if magic == b"\xd4\xc3\xb2\xa1" else ">"
|
||||
linktype = struct.unpack(endian + "I", header[20:24])[0]
|
||||
assert linktype == 113 # Linux cooked capture v1
|
||||
|
||||
while True:
|
||||
packet_header = stream.read(16)
|
||||
if not packet_header:
|
||||
break
|
||||
assert len(packet_header) == 16
|
||||
_, _, captured_length, _ = struct.unpack(endian + "IIII", packet_header)
|
||||
frame = stream.read(captured_length)
|
||||
assert len(frame) == captured_length
|
||||
assert struct.unpack(">H", frame[14:16])[0] == 0x0800
|
||||
ip_offset = 16
|
||||
ip_header_length = (frame[ip_offset] & 0x0F) * 4
|
||||
assert frame[ip_offset + 9] == 17
|
||||
udp_offset = ip_offset + ip_header_length
|
||||
source_port, target_port, udp_length, _ = struct.unpack(
|
||||
">HHHH", frame[udp_offset:udp_offset + 8]
|
||||
)
|
||||
payload = frame[udp_offset + 8:udp_offset + udp_length]
|
||||
packets.append((
|
||||
socket.inet_ntoa(frame[ip_offset + 12:ip_offset + 16]),
|
||||
socket.inet_ntoa(frame[ip_offset + 16:ip_offset + 20]),
|
||||
source_port,
|
||||
target_port,
|
||||
payload,
|
||||
))
|
||||
return packets
|
||||
|
||||
|
||||
def test_real_lowcmd_full_cipher_plain_pair(blowfish):
|
||||
cipher = (CAPTURES / "real_lowcmd.bin").read_bytes()
|
||||
plain = (CAPTURES / "real_lowcmd_decrypted.bin").read_bytes()
|
||||
assert len(cipher) == len(plain) == 616
|
||||
assert blowfish.decrypt_ecb(cipher) == plain
|
||||
assert blowfish.encrypt_ecb(plain) == cipher
|
||||
assert plain[:22] == bytes.fromhex(
|
||||
"feefff00000000000000000000000000000000003ac0"
|
||||
)
|
||||
assert plain[22] == 0x0A # This fixture contains active servo commands.
|
||||
|
||||
|
||||
def test_real_lowcmd_pcap_matches_extracted_first_frame():
|
||||
packets = _pcap_udp_packets(CAPTURES / "real_lowcmd.pcap")
|
||||
assert len(packets) == 1406
|
||||
assert {(src, dst, sport, dport) for src, dst, sport, dport, _ in packets} == {
|
||||
("192.168.123.161", "192.168.123.10", 8008, 8007)
|
||||
}
|
||||
assert {len(payload) for *_, payload in packets} == {616}
|
||||
assert packets[0][-1] == (CAPTURES / "real_lowcmd.bin").read_bytes()
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"filename,soc,temperatures",
|
||||
[
|
||||
("mcu_response.bin", 76, [52, 42, 40]),
|
||||
("mcu_response_new.bin", 19, [79, 50, 51]),
|
||||
],
|
||||
)
|
||||
def test_real_lowstate_samples(blowfish, filename, soc, temperatures):
|
||||
cipher = (CAPTURES / filename).read_bytes()
|
||||
assert len(cipher) == 858
|
||||
plain = blowfish.decrypt_ecb(cipher[:856])
|
||||
assert plain[:4] == b"\xfe\xef\xff\x00"
|
||||
state = parse_low_state(plain)
|
||||
assert state.bms.SOC == soc
|
||||
assert [motor.temperature for motor in state.motorState[:3]] == temperatures
|
||||
assert state.wirelessRemote[:2] == b"\x55\x51"
|
||||
|
||||
|
||||
def test_memory_dump_reproduces_packaged_blowfish_state():
|
||||
with tarfile.open(CAPTURES / "blowfish_dump.tar.gz", "r:gz") as archive:
|
||||
member = archive.extractfile("./rw_7f86a1f000.bin")
|
||||
assert member is not None
|
||||
memory = member.read()
|
||||
extracted = memory[0x232190:0x232190 + 4168]
|
||||
assert extracted == STATE_FILE.read_bytes()
|
||||
@@ -61,8 +61,8 @@ def test_plain_crc_at_612():
|
||||
assert plain[612:616] == expected_crc
|
||||
|
||||
|
||||
def test_encrypted_damping_matches_real(bf):
|
||||
"""全 damping 加密后, 前 16B 应跟真实 Legged_sport 抓包一致."""
|
||||
def test_encrypted_default_header_matches_real_capture(bf):
|
||||
"""默认命令与活动实机命令的公共头部加密结果必须一致."""
|
||||
cmd = LowCmd()
|
||||
enc = build_low_cmd_encrypted(cmd, bf)
|
||||
assert len(enc) == 616
|
||||
|
||||
97
tests/test_mcu_client.py
Normal file
97
tests/test_mcu_client.py
Normal file
@@ -0,0 +1,97 @@
|
||||
from pathlib import Path
|
||||
from hashlib import sha256
|
||||
|
||||
import pytest
|
||||
|
||||
from go1_pro_sdk import LowCmd, MCUClient
|
||||
from go1_pro_sdk.connection import mcu_client as mcu_module
|
||||
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
CAPTURE_FILE = ROOT / "data/captures/mcu_response_new.bin"
|
||||
if not CAPTURE_FILE.is_file():
|
||||
pytest.skip("private LowState capture is not available", allow_module_level=True)
|
||||
CAPTURE = CAPTURE_FILE.read_bytes()
|
||||
DAMPING_SHA256 = "ca911b04a09d8e8b069c96ac31e410b683fcf1bff625dd203f9efde215888400"
|
||||
|
||||
|
||||
class FakeSocket:
|
||||
def __init__(self):
|
||||
self.sent = []
|
||||
self.recv_batches = []
|
||||
self.closed = False
|
||||
self.bound = None
|
||||
self.blocking = None
|
||||
|
||||
def setblocking(self, value):
|
||||
self.blocking = value
|
||||
|
||||
def setsockopt(self, *args):
|
||||
pass
|
||||
|
||||
def bind(self, address):
|
||||
self.bound = address
|
||||
|
||||
def getsockname(self):
|
||||
return ("0.0.0.0", 45678)
|
||||
|
||||
def sendto(self, data, address):
|
||||
self.sent.append((bytes(data), address))
|
||||
return len(data)
|
||||
|
||||
def recvfrom(self, size):
|
||||
if not self.recv_batches:
|
||||
raise BlockingIOError
|
||||
batch = self.recv_batches[0]
|
||||
if not batch:
|
||||
self.recv_batches.pop(0)
|
||||
raise BlockingIOError
|
||||
return batch.pop(0)[:size], ("192.168.123.10", 8007)
|
||||
|
||||
def close(self):
|
||||
self.closed = True
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def fake_socket(monkeypatch):
|
||||
sock = FakeSocket()
|
||||
monkeypatch.setattr(mcu_module.socket, "socket", lambda *args, **kwargs: sock)
|
||||
monkeypatch.setattr(mcu_module.time, "sleep", lambda duration: None)
|
||||
return sock
|
||||
|
||||
|
||||
def test_native_client_send_receive_and_close(fake_socket):
|
||||
with MCUClient() as client:
|
||||
assert client.local_port == 45678
|
||||
assert client.send(LowCmd().all_damping()) == 616
|
||||
packet, address = fake_socket.sent[-1]
|
||||
assert address == ("192.168.123.10", 8007)
|
||||
assert sha256(packet).hexdigest() == DAMPING_SHA256
|
||||
|
||||
fake_socket.recv_batches = [[CAPTURE]]
|
||||
state = client.recv_latest()
|
||||
assert state is client.last_state
|
||||
assert [motor.temperature for motor in state.motorState[:3]] == [79, 50, 51]
|
||||
assert state.wirelessRemote[:2] == b"\x55\x51"
|
||||
|
||||
assert fake_socket.closed
|
||||
assert client.sock is None
|
||||
|
||||
|
||||
def test_native_client_ignores_invalid_state(fake_socket):
|
||||
client = MCUClient()
|
||||
fake_socket.recv_batches = [[b"\x00" * 858]]
|
||||
assert client.recv_latest() is None
|
||||
assert client.last_state is None
|
||||
|
||||
|
||||
def test_native_wake_and_safe_stop(fake_socket):
|
||||
client = MCUClient()
|
||||
fake_socket.recv_batches = [[CAPTURE], [CAPTURE], [CAPTURE]]
|
||||
assert client.wake_mcu(n_frames=3, dt=0) == 3
|
||||
assert len(fake_socket.sent) == 3
|
||||
assert all(sha256(packet).hexdigest() == DAMPING_SHA256 for packet, _ in fake_socket.sent)
|
||||
|
||||
client.safe_stop(n_frames=2, dt=0)
|
||||
assert len(fake_socket.sent) == 5
|
||||
assert all(sha256(packet).hexdigest() == DAMPING_SHA256 for packet, _ in fake_socket.sent)
|
||||
@@ -1,11 +1,23 @@
|
||||
"""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:
|
||||
@@ -15,8 +27,8 @@ class _FakeMCUClient:
|
||||
self.mcu_ip = mcu_ip
|
||||
self.mcu_port = mcu_port
|
||||
self.local_port = local_port or 49152
|
||||
self.next_state = sdk.LowState()
|
||||
self.next_state.motorState[sdk.FR_1].q = 1.23
|
||||
self.next_state = NativeLowState()
|
||||
self.next_state.motorState[FR_1].q = 1.23
|
||||
self.sent = []
|
||||
self.closed = False
|
||||
self.__class__.instances.append(self)
|
||||
@@ -46,60 +58,117 @@ def fake_client(monkeypatch):
|
||||
|
||||
|
||||
def test_official_names_and_data_types_are_available():
|
||||
assert sdk.LOWLEVEL == 0xff
|
||||
assert sdk.LeggedType.Go1 == 2
|
||||
assert sdk.LeggedType.Go1 != 2
|
||||
assert int(sdk.LeggedType.Go1) == 2
|
||||
assert sdk.Go1 is sdk.LeggedType.Go1
|
||||
assert sdk.FR_0 == 0
|
||||
assert sdk.RL_2 == 11
|
||||
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(sdk.LOWLEVEL, 0, "192.168.123.10", 8007)
|
||||
udp = sdk.UDP(LOWLEVEL, 0, "192.168.123.10", 8007)
|
||||
cmd = sdk.LowCmd()
|
||||
|
||||
assert udp.InitCmdData(cmd) is None
|
||||
assert cmd.levelFlag == sdk.LOWLEVEL
|
||||
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 == sdk.PosStopF for m in cmd.motorCmd)
|
||||
assert all(m.dq == sdk.VelStopF 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(sdk.LOWLEVEL, 0, "192.168.123.10", 8007)
|
||||
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[sdk.FR_1].q == pytest.approx(1.23)
|
||||
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[sdk.FR_1].q = 1.2
|
||||
cmd.motorCmd[sdk.FR_1].dq = 0.0
|
||||
cmd.motorCmd[sdk.FR_1].Kp = 5.0
|
||||
cmd.motorCmd[sdk.FR_1].Kd = 1.0
|
||||
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[sdk.FR_1].q == pytest.approx(1.2)
|
||||
assert udp.udpState.SendCount == 1
|
||||
assert udp.udpState.RecvCount == 1
|
||||
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(sdk.LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd)
|
||||
cmd.motorCmd[sdk.FR_1].q = 99.0
|
||||
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[sdk.FR_0].q == sdk.PosStopF
|
||||
assert cmd.motorCmd[sdk.FR_1].q == pytest.approx(3.5)
|
||||
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():
|
||||
@@ -131,10 +200,133 @@ def test_fixed_size_official_arrays_are_validated():
|
||||
|
||||
|
||||
def test_highlevel_udp_reports_the_unsupported_boundary():
|
||||
udp = sdk.UDP(sdk.HIGHLEVEL, 8080, "192.168.123.161", 8082)
|
||||
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)"
|
||||
|
||||
Reference in New Issue
Block a user