wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improve formats & docstrings

This commit is contained in:
Simon Alibert 2025-03-20 09:36:17 +01:00
parent 64ce2669ca
commit 2d56f35071
1 changed files with 48 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
tests/mocks/mock_dynamixel.py
View File

@ -70,7 +70,7 @@ INSTRUCTION_TYPES = {
} # fmt: skip
# https://emanual.robotis.com/docs/en/dxl/protocol2/#error
STATUS_TYPE = {
ERROR_TYPE = {
"Success": 0x00, # No error
"Result_Fail": 0x01, # Failed to process the sent Instruction Packet
"Instruction_Error": 0x02, # An undefined Instruction has been usedAction has been used without Reg Write
@ -153,17 +153,15 @@ class MockDynamixelPacketv2(abc.ABC):
class MockInstructionPacket(MockDynamixelPacketv2):
"""
Helper class to build valid Dynamixel Protocol 2.0 Instruction Packets with correct CRC.
Helper class to build valid Dynamixel Protocol 2.0 Instruction Packets.
Protocol 2.0 Instruction Packet structure
(from https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction-packet)
0xFF 0xFF 0xFD 0x00 # 4-byte header
<servo_id> # typically 0x01
<length L> <length H> # 2-byte length of (instruction+error+params+CRC)
<instruction> # instruction type
<parameters...> # for a 4-byte read, we have 4 param bytes
<crc_low> <crc_high> # 16-bit CRC
| Header | Packet ID | Length | Instruction | Params | CRC |
| ------------------- | --------- | ----------- | ----------- | ----------------- | ----------- |
| 0xFF 0xFF 0xFD 0x00 | ID | Len_L Len_H | Instr | Param 1 … Param N | CRC_L CRC_H |
"""
@classmethod
@ -182,8 +180,8 @@ class MockInstructionPacket(MockDynamixelPacketv2):
[
0xFF, 0xFF, 0xFD, 0x00, # header
dxl_id, # servo id
packet_length & 0xFF, # length_l
(packet_length >> 8) & 0xFF, # length_h
dxl.DXL_LOBYTE(packet_length), # length_l
dxl.DXL_HIBYTE(packet_length), # length_h
instruct_value, # instruction type
*params, # data bytes
0x00, 0x00 # placeholder for CRC
@ -198,10 +196,8 @@ class MockInstructionPacket(MockDynamixelPacketv2):
data_length: int,
) -> bytes:
"""
Helper method to build a "Sync Read" broadcast instruction.
The official SDK might add some “stuffing” or check param_length,
but this is enough for basic compliance if you want a raw packet.
Helper method to build a Sync Read broadcast instruction.
(from https://emanual.robotis.com/docs/en/dxl/protocol2/#sync-read-0x82)
The parameters for Sync Read (Protocol 2.0) are:
param[0] = start_address L
@ -210,44 +206,38 @@ class MockInstructionPacket(MockDynamixelPacketv2):
param[3] = data_length H
param[4+] = motor IDs to read from
"""
# Example param: [LowAddr, HighAddr, LowLen, HighLen, ID1, ID2, ...]
params = [
(start_address & 0xFF),
((start_address >> 8) & 0xFF),
(data_length & 0xFF),
((data_length >> 8) & 0xFF),
dxl.DXL_LOBYTE(start_address),
dxl.DXL_HIBYTE(start_address),
dxl.DXL_LOBYTE(data_length),
dxl.DXL_HIBYTE(data_length),
] + dxl_ids
# broadcast ID: 0xFE
return cls.build(dxl_id=0xFE, instruct_type="Sync_Read", params=params)
return cls.build(dxl_id=dxl.BROADCAST_ID, instruct_type="Sync_Read", params=params)
class MockStatusPacket(MockDynamixelPacketv2):
"""
Helper class to build valid Dynamixel Protocol 2.0 Status Packets with correct CRC.
Helper class to build valid Dynamixel Protocol 2.0 Status Packets.
Protocol 2.0 Status Packet structure
(from https://emanual.robotis.com/docs/en/dxl/protocol2/#status-packet)
0xFF 0xFF 0xFD 0x00 # 4-byte header
<servo_id> # typically 0x01
<length L> <length H> # 2-byte length of (instruction+error+params+CRC)
0x55 # instruction = 0x55 means "status packet"
<error> # 0 if no error
<parameters...> # for a 4-byte read, we have 4 param bytes
<crc_low> <crc_high> # 16-bit CRC
| Header | Packet ID | Length | Instruction | Error | Params | CRC |
| ------------------- | --------- | ----------- | ----------- | ----- | ----------------- | ----------- |
| 0xFF 0xFF 0xFD 0x00 | ID | Len_L Len_H | 0x55 | Err | Param 1 … Param N | CRC_L CRC_H |
"""
@classmethod
def _build(cls, dxl_id: int, params: list[int], status: str = "Success") -> bytearray:
status_byte = STATUS_TYPE[status]
def _build(cls, dxl_id: int, params: list[int], error: str = "Success") -> bytearray:
err_byte = ERROR_TYPE[error]
return bytearray(
[
0xFF, 0xFF, 0xFD, 0x00, # header
dxl_id, # servo id
0x08, 0x00, # length_l, length_h = 8
0x55, # instruction = status
status_byte, # status
0x55, # instruction = 'status'
err_byte, # error
*params, # data bytes
0x00, 0x00 # placeholder for CRC
]