Skip to content

Commit 3521dd9

Browse files
pkooijpkooij
committed
add tests and debug
1 parent 6288439 commit 3521dd9

File tree

4 files changed

+577
-73
lines changed

4 files changed

+577
-73
lines changed

‎debug_can_communication.py‎

Lines changed: 166 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,166 @@
1+
#!/usr/bin/env python3
2+
"""
3+
Debug script for Damiao motor CAN communication on macOS.
4+
This replaces candump for macOS SLCAN debugging.
5+
"""
6+
7+
import can
8+
import time
9+
import sys
10+
11+
def test_can_communication(port="/dev/cu.usbmodem2101"):
12+
"""Test basic CAN communication with a Damiao motor."""
13+
14+
print("=" * 60)
15+
print("Damiao Motor CAN Communication Debug Tool")
16+
print("=" * 60)
17+
print(f"\nPort: {port}")
18+
print()
19+
20+
try:
21+
# Connect to SLCAN
22+
print("Step 1: Connecting to SLCAN...")
23+
bus = can.interface.Bus(
24+
channel=port,
25+
interface='slcan',
26+
bitrate=1000000
27+
)
28+
print("✓ Connected to SLCAN")
29+
30+
# Test 1: Send enable command and listen for ANY response
31+
print("\n" + "=" * 60)
32+
print("Test 1: Enable Motor (ID 0x01)")
33+
print("=" * 60)
34+
print("Sending enable command to 0x01...")
35+
36+
enable_msg = can.Message(
37+
arbitration_id=0x01,
38+
data=[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC],
39+
is_extended_id=False
40+
)
41+
bus.send(enable_msg)
42+
print("✓ Enable command sent")
43+
44+
print("\nListening for responses (2 seconds)...")
45+
print("Expected: Response from 0x11 (master ID)")
46+
print()
47+
48+
responses = []
49+
start_time = time.time()
50+
timeout = 2.0
51+
52+
while time.time() - start_time < timeout:
53+
msg = bus.recv(timeout=0.1)
54+
if msg:
55+
responses.append(msg)
56+
print(f" → Response from 0x{msg.arbitration_id:02X}: {msg.data.hex()}")
57+
58+
if not responses:
59+
print("✗ NO RESPONSES RECEIVED")
60+
print("\nPossible issues:")
61+
print(" 1. Motor not powered (check 24V supply)")
62+
print(" 2. CAN wiring incorrect (CANH, CANL, GND)")
63+
print(" 3. Motor master ID not set to 0x11")
64+
print(" 4. SLCAN adapter not working properly")
65+
print(" 5. Wrong CAN port specified")
66+
else:
67+
print(f"\n✓ Received {len(responses)} response(s)")
68+
69+
# Check if we got response from expected ID
70+
recv_ids = [msg.arbitration_id for msg in responses]
71+
if 0x11 in recv_ids:
72+
print("✓ Motor 0x11 is responding!")
73+
else:
74+
print(f"⚠ Responses from unexpected IDs: {[hex(id) for id in recv_ids]}")
75+
76+
# Test 2: Send refresh command
77+
print("\n" + "=" * 60)
78+
print("Test 2: Refresh Motor State (ID 0x01)")
79+
print("=" * 60)
80+
print("Sending refresh command...")
81+
82+
refresh_msg = can.Message(
83+
arbitration_id=0x7FF, # Parameter ID
84+
data=[0x01, 0x00, 0xCC, 0x00, 0x00, 0x00, 0x00, 0x00],
85+
is_extended_id=False
86+
)
87+
bus.send(refresh_msg)
88+
print("✓ Refresh command sent")
89+
90+
print("\nListening for responses (2 seconds)...")
91+
responses = []
92+
start_time = time.time()
93+
94+
while time.time() - start_time < timeout:
95+
msg = bus.recv(timeout=0.1)
96+
if msg:
97+
responses.append(msg)
98+
print(f" → Response from 0x{msg.arbitration_id:02X}: {msg.data.hex()}")
99+
100+
if not responses:
101+
print("✗ NO RESPONSES RECEIVED")
102+
else:
103+
print(f"\n✓ Received {len(responses)} response(s)")
104+
105+
# Test 3: Listen for any spontaneous traffic
106+
print("\n" + "=" * 60)
107+
print("Test 3: Listen for Any CAN Traffic")
108+
print("=" * 60)
109+
print("Listening for 5 seconds...")
110+
print("(This will catch any background CAN traffic)")
111+
print()
112+
113+
start_time = time.time()
114+
traffic_count = 0
115+
116+
while time.time() - start_time < 5.0:
117+
msg = bus.recv(timeout=0.1)
118+
if msg:
119+
traffic_count += 1
120+
print(f" [{time.time() - start_time:.2f}s] ID=0x{msg.arbitration_id:03X}: {msg.data.hex()}")
121+
122+
if traffic_count == 0:
123+
print("✗ No CAN traffic detected at all")
124+
print("\nThis suggests:")
125+
print(" - SLCAN adapter may not be working")
126+
print(" - No devices on the CAN bus are active")
127+
print(" - Wrong port specified")
128+
else:
129+
print(f"\n✓ Detected {traffic_count} CAN messages")
130+
131+
# Cleanup
132+
print("\n" + "=" * 60)
133+
print("Cleanup")
134+
print("=" * 60)
135+
print("Sending disable command...")
136+
disable_msg = can.Message(
137+
arbitration_id=0x01,
138+
data=[0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD],
139+
is_extended_id=False
140+
)
141+
bus.send(disable_msg)
142+
time.sleep(0.1)
143+
144+
bus.shutdown()
145+
print("✓ Disconnected from CAN bus")
146+
147+
except Exception as e:
148+
print(f"\n✗ Error: {e}")
149+
import traceback
150+
traceback.print_exc()
151+
return False
152+
153+
print("\n" + "=" * 60)
154+
print("Debug Complete")
155+
print("=" * 60)
156+
return True
157+
158+
159+
if __name__ == "__main__":
160+
if len(sys.argv) > 1:
161+
port = sys.argv[1]
162+
else:
163+
port = "/dev/cu.usbmodem2101"
164+
165+
test_can_communication(port)
166+

‎src/lerobot/motors/damiao/damiao.py‎

Lines changed: 66 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ def __init__(
7373
port: str,
7474
motors: dict[str, Motor],
7575
calibration: dict[str, MotorCalibration] | None = None,
76-
can_interface: str = "socketcan",
76+
can_interface: str = "auto",
7777
):
7878
"""
7979
Initialize the Damiao motors bus.
@@ -82,7 +82,7 @@ def __init__(
8282
port: CAN interface name (e.g., "can0" for Linux, "/dev/cu.usbmodem*" for macOS)
8383
motors: Dictionary mapping motor names to Motor objects
8484
calibration: Optional calibration data
85-
can_interface: CAN interface type - "socketcan" (Linux) or "slcan" (macOS/serial)
85+
can_interface: CAN interface type - "auto" (default), "socketcan" (Linux), or "slcan" (macOS/serial)
8686
"""
8787
super().__init__(port, motors, calibration)
8888
self.port = port
@@ -94,14 +94,18 @@ def __init__(
9494
self._motor_can_ids = {}
9595
self._recv_id_to_motor = {}
9696

97-
# Store motor types
97+
# Store motor types and recv IDs
9898
self._motor_types = {}
9999
for name, motor in self.motors.items():
100100
if hasattr(motor, "motor_type"):
101101
self._motor_types[name] = motor.motor_type
102102
else:
103103
# Default to DM4310 if not specified
104104
self._motor_types[name] = MotorType.DM4310
105+
106+
# Map recv_id to motor name for filtering responses
107+
if hasattr(motor, "recv_id"):
108+
self._recv_id_to_motor[motor.recv_id] = name
105109

106110
@property
107111
def is_connected(self) -> bool:
@@ -206,18 +210,20 @@ def configure_motors(self) -> None:
206210
def _enable_motor(self, motor: NameOrID) -> None:
207211
"""Enable a single motor."""
208212
motor_id = self._get_motor_id(motor)
213+
recv_id = self._get_motor_recv_id(motor)
209214
data = [0xFF] * 7 + [CAN_CMD_ENABLE]
210215
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False)
211216
self.canbus.send(msg)
212-
self._recv_motor_response()
217+
self._recv_motor_response(expected_recv_id=recv_id)
213218

214219
def _disable_motor(self, motor: NameOrID) -> None:
215220
"""Disable a single motor."""
216221
motor_id = self._get_motor_id(motor)
222+
recv_id = self._get_motor_recv_id(motor)
217223
data = [0xFF] * 7 + [CAN_CMD_DISABLE]
218224
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False)
219225
self.canbus.send(msg)
220-
self._recv_motor_response()
226+
self._recv_motor_response(expected_recv_id=recv_id)
221227

222228
def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
223229
"""Enable torque on selected motors."""
@@ -250,26 +256,54 @@ def set_zero_position(self, motors: str | list[str] | None = None) -> None:
250256
motors = self._get_motors_list(motors)
251257
for motor in motors:
252258
motor_id = self._get_motor_id(motor)
259+
recv_id = self._get_motor_recv_id(motor)
253260
data = [0xFF] * 7 + [CAN_CMD_SET_ZERO]
254261
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False)
255262
self.canbus.send(msg)
256-
self._recv_motor_response()
263+
self._recv_motor_response(expected_recv_id=recv_id)
257264
time.sleep(0.01)
258265

259-
def _refresh_motor(self, motor: NameOrID) -> None:
260-
"""Refresh motor status."""
266+
def _refresh_motor(self, motor: NameOrID) -> Optional[can.Message]:
267+
"""Refresh motor status and return the response."""
261268
motor_id = self._get_motor_id(motor)
269+
recv_id = self._get_motor_recv_id(motor)
262270
data = [motor_id & 0xFF, (motor_id >> 8) & 0xFF, CAN_CMD_REFRESH, 0, 0, 0, 0, 0]
263271
msg = can.Message(arbitration_id=CAN_PARAM_ID, data=data, is_extended_id=False)
264272
self.canbus.send(msg)
265-
self._recv_motor_response()
273+
return self._recv_motor_response(expected_recv_id=recv_id)
266274

267-
def _recv_motor_response(self, timeout: float = 0.1) -> Optional[can.Message]:
268-
"""Receive a response from a motor."""
275+
def _recv_motor_response(self, expected_recv_id: Optional[int] = None, timeout: float = 0.5) -> Optional[can.Message]:
276+
"""
277+
Receive a response from a motor.
278+
279+
Args:
280+
expected_recv_id: If provided, only return messages from this CAN ID
281+
timeout: Timeout in seconds
282+
283+
Returns:
284+
CAN message if received, None otherwise
285+
"""
269286
try:
270-
msg = self.canbus.recv(timeout=timeout)
271-
if msg:
272-
return msg
287+
start_time = time.time()
288+
messages_seen = []
289+
while time.time() - start_time < timeout:
290+
msg = self.canbus.recv(timeout=0.01) # Short timeout for polling
291+
if msg:
292+
messages_seen.append(f"0x{msg.arbitration_id:02X}")
293+
# If no filter specified, return any message
294+
if expected_recv_id is None:
295+
return msg
296+
# Otherwise, only return if it matches the expected recv_id
297+
if msg.arbitration_id == expected_recv_id:
298+
return msg
299+
else:
300+
logger.debug(f"Ignoring message from CAN ID 0x{msg.arbitration_id:02X}, expected 0x{expected_recv_id:02X}")
301+
302+
# Log what we saw for debugging
303+
if messages_seen:
304+
logger.warning(f"Received {len(messages_seen)} message(s) from IDs {set(messages_seen)}, but expected 0x{expected_recv_id:02X}")
305+
else:
306+
logger.warning(f"No CAN messages received (expected from 0x{expected_recv_id:02X})")
273307
except Exception as e:
274308
logger.debug(f"Failed to receive CAN message: {e}")
275309
return None
@@ -325,7 +359,8 @@ def _mit_control(
325359

326360
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False)
327361
self.canbus.send(msg)
328-
self._recv_motor_response()
362+
recv_id = self._get_motor_recv_id(motor)
363+
self._recv_motor_response(expected_recv_id=recv_id)
329364

330365
def _float_to_uint(self, x: float, x_min: float, x_max: float, bits: int) -> int:
331366
"""Convert float to unsigned integer for CAN transmission."""
@@ -384,12 +419,15 @@ def read(
384419
raise DeviceNotConnectedError(f"{self} is not connected.")
385420

386421
# Refresh motor to get latest state
387-
self._refresh_motor(motor)
388-
389-
# Read response
390-
msg = self._recv_motor_response()
422+
msg = self._refresh_motor(motor)
391423
if msg is None:
392-
raise ConnectionError(f"No response from motor {motor}")
424+
motor_id = self._get_motor_id(motor)
425+
recv_id = self._get_motor_recv_id(motor)
426+
raise ConnectionError(
427+
f"No response from motor '{motor}' (send ID: 0x{motor_id:02X}, recv ID: 0x{recv_id:02X}). "
428+
f"Check that: 1) Motor is powered (24V), 2) CAN wiring is correct, "
429+
f"3) Motor IDs are configured correctly using Damiao Debugging Tools"
430+
)
393431

394432
motor_type = self._motor_types.get(motor, MotorType.DM4310)
395433
position_degrees, velocity_deg_per_sec, torque, t_mos, t_rotor = self._decode_motor_state(msg.data, motor_type)
@@ -578,6 +616,14 @@ def _get_motor_name(self, motor: NameOrID) -> str:
578616
if m.id == motor:
579617
return name
580618
raise ValueError(f"Unknown motor ID: {motor}")
619+
620+
def _get_motor_recv_id(self, motor: NameOrID) -> Optional[int]:
621+
"""Get motor recv_id from name or ID."""
622+
motor_name = self._get_motor_name(motor)
623+
motor_obj = self.motors.get(motor_name)
624+
if motor_obj and hasattr(motor_obj, "recv_id"):
625+
return motor_obj.recv_id
626+
return None
581627

582628
@cached_property
583629
def is_calibrated(self) -> bool:

‎tests/conftest.py‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,39 @@
3232
]
3333

3434

35+
def pytest_addoption(parser):
36+
"""Add custom command line option for hardware tests."""
37+
parser.addoption(
38+
"--run-hardware",
39+
action="store_true",
40+
default=False,
41+
help="Run hardware tests that require actual motors connected",
42+
)
43+
parser.addoption(
44+
"--can-port",
45+
action="store",
46+
default=None,
47+
help="CAN interface port (e.g., 'can0' for Linux, '/dev/cu.usbmodem*' for macOS)",
48+
)
49+
50+
51+
def pytest_configure(config):
52+
"""Register custom marker for hardware tests."""
53+
config.addinivalue_line("markers", "hardware: mark test as requiring hardware")
54+
55+
56+
def pytest_collection_modifyitems(config, items):
57+
"""Skip hardware tests unless --run-hardware flag is provided."""
58+
if config.getoption("--run-hardware"):
59+
# --run-hardware given in cli: do not skip hardware tests
60+
return
61+
62+
skip_hardware = pytest.mark.skip(reason="need --run-hardware option to run")
63+
for item in items:
64+
if "hardware" in item.keywords:
65+
item.add_marker(skip_hardware)
66+
67+
3568
def pytest_collection_finish():
3669
print(f"\nTesting with {DEVICE=}")
3770

0 commit comments

Comments
 (0)