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

106
tests/mocks/mock_dynamixel.py
View File

@ -51,34 +51,34 @@ DXL_CRC_TABLE = [
# https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction # https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction
INSTRUCTION_TYPES = { INSTRUCTION_TYPES = {
"Ping": 0x01, # Checks whether the Packet has arrived at a device with the same ID as the specified packet ID "Ping": 0x01, # Checks whether the Packet has arrived at a device with the same ID as the specified packet ID
"Read": 0x02, # Read data from the Device "Read": 0x02, # Read data from the Device
"Write": 0x03, # Write data to the Device "Write": 0x03, # Write data to the Device
"Reg_Write": 0x04, # Register the Instruction Packet in standby status; Packet can later be executed using the Action command "Reg_Write": 0x04, # Register the Instruction Packet in standby status; Packet can later be executed using the Action command
"Action": 0x05, # Executes a Packet that was registered beforehand using Reg Write "Action": 0x05, # Executes a Packet that was registered beforehand using Reg Write
"Factory_Reset": 0x06, # Resets the Control Table to its initial factory default settings "Factory_Reset": 0x06, # Resets the Control Table to its initial factory default settings
"Reboot": 0x08, # Reboot the Device "Reboot": 0x08, # Reboot the Device
"Clear": 0x10, # Reset certain information stored in memory "Clear": 0x10, # Reset certain information stored in memory
"Control_Table_Backup": 0x20, # Store current Control Table status data to a Backup or to restore backup EEPROM data. "Control_Table_Backup": 0x20, # Store current Control Table status data to a Backup or to restore backup EEPROM data.
"Status": 0x55, # Return packet sent following the execution of an Instruction Packet "Status": 0x55, # Return packet sent following the execution of an Instruction Packet
"Sync_Read": 0x82, # Read data from multiple devices with the same Address with the same length at once "Sync_Read": 0x82, # Read data from multiple devices with the same Address with the same length at once
"Sync_Write": 0x83, # Write data to multiple devices with the same Address with the same length at once "Sync_Write": 0x83, # Write data to multiple devices with the same Address with the same length at once
"Fast_Sync_Read": 0x8A, # Read data from multiple devices with the same Address with the same length at once "Fast_Sync_Read": 0x8A, # Read data from multiple devices with the same Address with the same length at once
"Bulk_Read": 0x92, # Read data from multiple devices with different Addresses with different lengths at once "Bulk_Read": 0x92, # Read data from multiple devices with different Addresses with different lengths at once
"Bulk_Write": 0x93, # Write data to multiple devices with different Addresses with different lengths at once "Bulk_Write": 0x93, # Write data to multiple devices with different Addresses with different lengths at once
"Fast_Bulk_Read": 0x9A, # Read data from multiple devices with different Addresses with different lengths at once "Fast_Bulk_Read": 0x9A, # Read data from multiple devices with different Addresses with different lengths at once
} # fmt: skip } # fmt: skip
# https://emanual.robotis.com/docs/en/dxl/protocol2/#error # https://emanual.robotis.com/docs/en/dxl/protocol2/#error
STATUS_TYPE = { ERROR_TYPE = {
"Success": 0x00, # No error "Success": 0x00, # No error
"Result_Fail": 0x01, # Failed to process the sent Instruction Packet "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 "Instruction_Error": 0x02, # An undefined Instruction has been usedAction has been used without Reg Write
"CRC_Error": 0x03, # The CRC of the sent Packet does not match the expected value "CRC_Error": 0x03, # The CRC of the sent Packet does not match the expected value
"Data_Range_Error": 0x04, # Data to be written to the specified Address is outside the range of the minimum/maximum value "Data_Range_Error": 0x04, # Data to be written to the specified Address is outside the range of the minimum/maximum value
"Data_Length_Error": 0x05, # Attempted to write Data that is shorter than the required data length of the specified Address "Data_Length_Error": 0x05, # Attempted to write Data that is shorter than the required data length of the specified Address
# (ex: when you attempt to only use 2 bytes of a register that has been defined as 4 bytes) # (ex: when you attempt to only use 2 bytes of a register that has been defined as 4 bytes)
"Data_Limit_Error": 0x06, # Data to be written to the specified Address is outside of the configured Limit value "Data_Limit_Error": 0x06, # Data to be written to the specified Address is outside of the configured Limit value
"Access_Error": 0x07, # Attempted to write a value to an Address that is Read Only or has not been defined "Access_Error": 0x07, # Attempted to write a value to an Address that is Read Only or has not been defined
# Attempted to read a value from an Address that is Write Only or has not been defined # Attempted to read a value from an Address that is Write Only or has not been defined
# Attempted to write a value to an EEPROM register while Torque was Enabled. # Attempted to write a value to an EEPROM register while Torque was Enabled.
@ -153,17 +153,15 @@ class MockDynamixelPacketv2(abc.ABC):
class MockInstructionPacket(MockDynamixelPacketv2): 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 Protocol 2.0 Instruction Packet structure
(from https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction-packet) (from https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction-packet)
0xFF 0xFF 0xFD 0x00 # 4-byte header | Header | Packet ID | Length | Instruction | Params | CRC |
<servo_id> # typically 0x01 | ------------------- | --------- | ----------- | ----------- | ----------------- | ----------- |
<length L> <length H> # 2-byte length of (instruction+error+params+CRC) | 0xFF 0xFF 0xFD 0x00 | ID | Len_L Len_H | Instr | Param 1 … Param N | CRC_L CRC_H |
<instruction> # instruction type
<parameters...> # for a 4-byte read, we have 4 param bytes
<crc_low> <crc_high> # 16-bit CRC
""" """
@classmethod @classmethod
@ -182,8 +180,8 @@ class MockInstructionPacket(MockDynamixelPacketv2):
[ [
0xFF, 0xFF, 0xFD, 0x00, # header 0xFF, 0xFF, 0xFD, 0x00, # header
dxl_id, # servo id dxl_id, # servo id
packet_length & 0xFF, # length_l dxl.DXL_LOBYTE(packet_length), # length_l
(packet_length >> 8) & 0xFF, # length_h dxl.DXL_HIBYTE(packet_length), # length_h
instruct_value, # instruction type instruct_value, # instruction type
*params, # data bytes *params, # data bytes
0x00, 0x00 # placeholder for CRC 0x00, 0x00 # placeholder for CRC
@ -198,10 +196,8 @@ class MockInstructionPacket(MockDynamixelPacketv2):
data_length: int, data_length: int,
) -> bytes: ) -> bytes:
""" """
Helper method to build a "Sync Read" broadcast instruction. Helper method to build a Sync Read broadcast instruction.
(from https://emanual.robotis.com/docs/en/dxl/protocol2/#sync-read-0x82)
The official SDK might add some “stuffing” or check param_length,
but this is enough for basic compliance if you want a raw packet.
The parameters for Sync Read (Protocol 2.0) are: The parameters for Sync Read (Protocol 2.0) are:
param[0] = start_address L param[0] = start_address L
@ -210,46 +206,40 @@ class MockInstructionPacket(MockDynamixelPacketv2):
param[3] = data_length H param[3] = data_length H
param[4+] = motor IDs to read from param[4+] = motor IDs to read from
""" """
# Example param: [LowAddr, HighAddr, LowLen, HighLen, ID1, ID2, ...]
params = [ params = [
(start_address & 0xFF), dxl.DXL_LOBYTE(start_address),
((start_address >> 8) & 0xFF), dxl.DXL_HIBYTE(start_address),
(data_length & 0xFF), dxl.DXL_LOBYTE(data_length),
((data_length >> 8) & 0xFF), dxl.DXL_HIBYTE(data_length),
] + dxl_ids ] + dxl_ids
# broadcast ID: 0xFE return cls.build(dxl_id=dxl.BROADCAST_ID, instruct_type="Sync_Read", params=params)
return cls.build(dxl_id=0xFE, instruct_type="Sync_Read", params=params)
class MockStatusPacket(MockDynamixelPacketv2): 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 Protocol 2.0 Status Packet structure
(from https://emanual.robotis.com/docs/en/dxl/protocol2/#status-packet) (from https://emanual.robotis.com/docs/en/dxl/protocol2/#status-packet)
0xFF 0xFF 0xFD 0x00 # 4-byte header | Header | Packet ID | Length | Instruction | Error | Params | CRC |
<servo_id> # typically 0x01 | ------------------- | --------- | ----------- | ----------- | ----- | ----------------- | ----------- |
<length L> <length H> # 2-byte length of (instruction+error+params+CRC) | 0xFF 0xFF 0xFD 0x00 | ID | Len_L Len_H | 0x55 | Err | Param 1 … Param N | CRC_L CRC_H |
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
""" """
@classmethod @classmethod
def _build(cls, dxl_id: int, params: list[int], status: str = "Success") -> bytearray: def _build(cls, dxl_id: int, params: list[int], error: str = "Success") -> bytearray:
status_byte = STATUS_TYPE[status] err_byte = ERROR_TYPE[error]
return bytearray( return bytearray(
[ [
0xFF, 0xFF, 0xFD, 0x00, # header 0xFF, 0xFF, 0xFD, 0x00, # header
dxl_id, # servo id dxl_id, # servo id
0x08, 0x00, # length_l, length_h = 8 0x08, 0x00, # length_l, length_h = 8
0x55, # instruction = status 0x55, # instruction = 'status'
status_byte, # status err_byte, # error
*params, # data bytes *params, # data bytes
0x00, 0x00 # placeholder for CRC 0x00, 0x00 # placeholder for CRC
] ]
) # fmt: skip ) # fmt: skip