solarian-dynamixel 0.1.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,7 @@
1
+ """
2
+ Solarian-Dynamixel Plugin
3
+
4
+ Dynamixel servo actuator and bus implementation for Solarian robotics framework.
5
+ """
6
+
7
+ __version__ = "0.1.0"
@@ -0,0 +1,33 @@
1
+ """
2
+ Dynamixel Servo Actuator
3
+
4
+ Actuator class for Dynamixel motors (e.g., XL330, XM430, etc.).
5
+ """
6
+
7
+ from solarian.core.servo import Servo
8
+ from solarian.core.data import PosData, SpeedData
9
+
10
+ from solarian_dynamixel.bus import DynamixelBus
11
+
12
+
13
+ class DynamixelServo(Servo):
14
+ """
15
+ Servo actuator for Dynamixel motors (e.g., XL330, XM430, etc.).
16
+
17
+ Retrieve this class from the plugin registry:
18
+
19
+ from solarian.plugins import actuators
20
+ DynamixelServo = actuators.get("dynamixel")
21
+
22
+ class MyArmModule(Module):
23
+ shoulder = DynamixelServo(model="xl330-m288")
24
+ elbow = DynamixelServo(model="xl330-m288")
25
+
26
+ The corresponding DynamixelBus will be automatically used for communication.
27
+ """
28
+
29
+ bus_class = DynamixelBus
30
+ default_mode = "range_m100_100"
31
+
32
+ pos = PosData()
33
+ speed = SpeedData()
@@ -0,0 +1,283 @@
1
+ """
2
+ Dynamixel Actuator Bus
3
+
4
+ Bus implementation for communicating with Dynamixel servos.
5
+ """
6
+
7
+ import logging
8
+ import time
9
+ from typing import Dict, Any
10
+
11
+ import serial.tools.list_ports
12
+
13
+ from .lerobot.dynamixel import DynamixelMotorsBus
14
+ from .lerobot.dynamixel.dynamixel import NORMALIZED_DATA
15
+ from .lerobot.dynamixel.tables import MODEL_NUMBER_TABLE, MODEL_RESOLUTION
16
+ from solarian.core.calibration import Motor, MotorCalibration, MotorNormMode
17
+
18
+ from solarian.core.component import Component
19
+ from solarian.core.data import PosData, SpeedData, TorqueEnableData, Data
20
+ from solarian.core.actuator_bus import ActuatorBus
21
+
22
+ logger = logging.getLogger(__name__)
23
+
24
+ #: Position control. X-series Operating_Mode 3 = joint (single-turn) position.
25
+ OPERATING_MODE_POSITION = 3
26
+
27
+ WRITE_MAP = {
28
+ PosData: "Goal_Position",
29
+ SpeedData: "Goal_Speed",
30
+ TorqueEnableData: "Torque_Enable",
31
+ }
32
+
33
+ READ_MAP = {
34
+ PosData: "Present_Position",
35
+ SpeedData: "Present_Speed",
36
+ }
37
+
38
+ def map_write(data_type: type[Data]):
39
+ if data_type in WRITE_MAP:
40
+ return WRITE_MAP[data_type]
41
+ else:
42
+ raise ValueError(f"Unsupported data type: {data_type}")
43
+
44
+ def map_read(data_type: type[Data]):
45
+ if data_type in READ_MAP:
46
+ return READ_MAP[data_type]
47
+ else:
48
+ raise ValueError(f"Unsupported data type: {data_type}")
49
+
50
+ def move_to_mid_encoder(
51
+ bus: DynamixelMotorsBus,
52
+ motor: str,
53
+ resolution: int,
54
+ *,
55
+ settle_s: float = 3.0,
56
+ tol: int = 8,
57
+ ) -> int:
58
+ """Drive *motor* to mid-encoder, leave torque off, return the reading reached.
59
+
60
+ Homing_Offset is cleared and the position limits opened first, so Goal mid
61
+ means *absolute* mid. Only for a free shaft: the goal can be most of a turn
62
+ away from where the shaft sits, so a mounted linkage would hit a hard stop.
63
+ """
64
+ half = (resolution - 1) // 2
65
+ bus.disable_torque([motor])
66
+ bus.write("Homing_Offset", motor, 0, normalize=False)
67
+ bus.write("Min_Position_Limit", motor, 0, normalize=False)
68
+ bus.write("Max_Position_Limit", motor, resolution - 1, normalize=False)
69
+ bus.write("Operating_Mode", motor, OPERATING_MODE_POSITION, normalize=False)
70
+
71
+ bus.enable_torque([motor])
72
+ try:
73
+ bus.write("Goal_Position", motor, half, normalize=False)
74
+ deadline = time.monotonic() + settle_s
75
+ while time.monotonic() < deadline:
76
+ if abs(int(bus.read("Present_Position", motor, normalize=False)) - half) <= tol:
77
+ break
78
+ time.sleep(0.05)
79
+ finally:
80
+ bus.disable_torque([motor])
81
+
82
+ return int(bus.read("Present_Position", motor, normalize=False))
83
+
84
+
85
+ def map_motor_mode(mode: str):
86
+ if mode == "range_0_100":
87
+ return MotorNormMode.RANGE_0_100
88
+ elif mode == "range_m100_100":
89
+ return MotorNormMode.RANGE_M100_100
90
+ elif mode == "degrees":
91
+ return MotorNormMode.DEGREES
92
+ else:
93
+ raise ValueError(f"Unknown motor mode: {mode}")
94
+
95
+
96
+ class DynamixelBus(ActuatorBus):
97
+ """
98
+ Actuator bus for Dynamixel servos.
99
+
100
+ Handles communication with Dynamixel motors via serial port.
101
+ Automatically used when DynamixelServo actuators are defined in a module.
102
+ """
103
+
104
+ def __init__(self, port: str, components: Dict[str, Component], calibration: Dict[str, MotorCalibration]):
105
+ super().__init__(port, components, calibration)
106
+
107
+ self.bus = DynamixelMotorsBus(
108
+ port=port,
109
+ motors={
110
+ name: Motor(component.cfg.id, component.cfg.model, map_motor_mode(component.cfg.mode))
111
+ for name, component in components.items()
112
+ },
113
+ calibration=calibration
114
+ )
115
+
116
+ def is_connected(self) -> bool:
117
+ return self.bus.is_connected
118
+
119
+ def connect(self):
120
+ self.bus.connect()
121
+
122
+ def disconnect(self):
123
+ self.bus.disconnect()
124
+
125
+ def read(self, data_type: type[Data], component_names: list[str], normalize: bool = True):
126
+ # Retry a few times: the XL330 chain occasionally drops a single status
127
+ # packet at high baud, and without retries one glitch kills the whole
128
+ # control loop.
129
+ return self.bus.sync_read(
130
+ map_read(data_type), motors=component_names, normalize=normalize, num_retry=3
131
+ )
132
+
133
+ def write(self, values: Dict[str, Data], normalize: bool = True):
134
+ # group values by class type, extracting raw values from Data objects
135
+ values_by_class = {}
136
+ for name, value in values.items():
137
+ if value.__class__ not in values_by_class:
138
+ values_by_class[value.__class__] = {}
139
+ values_by_class[value.__class__][name] = value.value
140
+ for class_type, class_values in values_by_class.items():
141
+ data_name = map_write(class_type)
142
+ if not normalize or data_name not in NORMALIZED_DATA:
143
+ class_values = {k: int(v) for k, v in class_values.items()}
144
+ self.bus.sync_write(data_name, values=class_values, normalize=normalize)
145
+
146
+ def counts_per_revolution(self, name: str) -> int | None:
147
+ """Encoder resolution (counts/rev) for motor ``name`` from its model.
148
+
149
+ XL330/XM430 are 4096 counts/rev per :data:`MODEL_RESOLUTION`.
150
+ """
151
+ try:
152
+ model = self.bus.motors[name].model
153
+ return int(self.bus.model_resolution_table[model])
154
+ except Exception:
155
+ return None
156
+
157
+ def center_offset_in_hardware(self, name: str) -> bool:
158
+ del name
159
+ return True
160
+
161
+ def is_calibrated(self) -> bool:
162
+ return self.bus.is_calibrated
163
+
164
+ def reset_calibration(self, component_names: list[str]):
165
+ self.bus.reset_calibration(component_names)
166
+
167
+ def read_calibration(self) -> dict[str, MotorCalibration]:
168
+ return self.bus.read_calibration()
169
+
170
+ def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> dict[str, MotorCalibration]:
171
+ # bound min and max to range of motion
172
+ for name, calibration in calibration_dict.items():
173
+ calibration.range_min = max(calibration.range_min, 0)
174
+ calibration.range_max = min(calibration.range_max, 4095)
175
+ self.bus.write_calibration(calibration_dict)
176
+ return calibration_dict
177
+
178
+ def configure(self, params: dict[str, int], component_names: list[str]):
179
+ for name in component_names:
180
+ for param_name, value in params.items():
181
+ self.bus.write(param_name, name, value, normalize=False)
182
+
183
+ def enable_torque(self, component_names: list[str]):
184
+ self.bus.enable_torque(component_names)
185
+
186
+ def disable_torque(self, component_names: list[str]):
187
+ self.bus.disable_torque(component_names)
188
+
189
+ def start_calibration_mode(self):
190
+ """Sets the actuators where the arm can be moved manually freely"""
191
+ self.bus.disable_torque(list(self.components.keys()))
192
+
193
+ def end_calibration_mode(self):
194
+ pass
195
+
196
+ def set_half_turn_homings(self, component_names: list[str]) -> Dict[str, int]:
197
+ """Set half turn homings for the actuators. Read present position and set/store"""
198
+ return self.bus.set_half_turn_homings(component_names)
199
+
200
+ def drive_to_encoder_center(self, component_names: list[str]) -> Dict[str, int]:
201
+ reached: Dict[str, int] = {}
202
+ for name in component_names:
203
+ resolution = self.counts_per_revolution(name) or 4096
204
+ present = move_to_mid_encoder(self.bus, name, resolution)
205
+ reached[name] = present
206
+ half = (resolution - 1) // 2
207
+ if abs(present - half) > 8:
208
+ logger.warning(
209
+ "%s: reached %d, wanted ~%d. Shaft may be blocked.", name, present, half
210
+ )
211
+ return reached
212
+
213
+ # -- provisioning (no declared components: servos are addressed by ID) --
214
+ @classmethod
215
+ def available_ports(cls) -> list[str]:
216
+ return [p.device for p in serial.tools.list_ports.comports()]
217
+
218
+ @classmethod
219
+ def scan_port(cls, port: str) -> list[dict]:
220
+ model_by_number = {nb: name for name, nb in MODEL_NUMBER_TABLE.items()}
221
+ bus = DynamixelMotorsBus(port, {})
222
+ bus._connect(handshake=False)
223
+ try:
224
+ for baudrate in bus.available_baudrates:
225
+ bus.set_baudrate(baudrate)
226
+ ids_models = bus.broadcast_ping() or {}
227
+ if not ids_models:
228
+ continue
229
+ # Motors always share one baud-rate, so the first hit is the bus.
230
+ return [
231
+ {
232
+ "id": motor_id,
233
+ "model": model_by_number.get(model_nb, f"unknown({model_nb})"),
234
+ "baudrate": baudrate,
235
+ }
236
+ for motor_id, model_nb in sorted(ids_models.items())
237
+ ]
238
+ finally:
239
+ bus.port_handler.closePort()
240
+ return []
241
+
242
+ @classmethod
243
+ def provision_servo(cls, port: str, found: dict) -> None:
244
+ model = str(found["model"])
245
+ if model not in MODEL_RESOLUTION:
246
+ raise ValueError(f"Unknown Dynamixel model {model!r}; cannot provision it.")
247
+
248
+ current_id = int(found["id"])
249
+ raw = input(f"New ID for the servo at ID {current_id} (Enter to keep it): ").strip()
250
+ target_id = int(raw) if raw.isdigit() else current_id
251
+
252
+ # setup_motor writes the ID declared here, so the motor is named at its
253
+ # target ID and found at its current one.
254
+ bus = DynamixelMotorsBus(
255
+ port, {"servo": Motor(target_id, model, MotorNormMode.RANGE_M100_100)}
256
+ )
257
+ bus._connect(handshake=False)
258
+ try:
259
+ bus.setup_motor(
260
+ "servo", initial_baudrate=int(found["baudrate"]), initial_id=current_id
261
+ )
262
+ print(f" Servo is at ID {target_id}, baud-rate {bus.default_baudrate}.")
263
+
264
+ resolution = MODEL_RESOLUTION[model]
265
+ half = (resolution - 1) // 2
266
+ answer = input(
267
+ f"Drive shaft to mid-encoder ({half}) for horn mounting?\n"
268
+ " WARNING: Only if the output is free to rotate (no linkage / hard "
269
+ "stops). [Y/n] "
270
+ ).strip().lower()
271
+ if answer in ("", "y", "yes"):
272
+ present = move_to_mid_encoder(bus, "servo", resolution)
273
+ print(f" Present={present} (wanted ~{half}).")
274
+ finally:
275
+ bus.port_handler.closePort()
276
+
277
+ def setup_component(self, component_name: str, configs: Dict[str, Any]):
278
+ """Setup config for a component"""
279
+ print("Setting the following configs for component:", component_name)
280
+ print(configs)
281
+ input("Connect the servo to the controller, make sure only the servo to be configured is connected. Press Enter when ready.")
282
+ self.bus.setup_motor(component_name, initial_baudrate=configs["baudrate"], initial_id=configs["id"])
283
+ print("Configuration completed.")
File without changes
File without changes
@@ -0,0 +1,43 @@
1
+ # Copyright 2024 The HuggingFace Inc. team. All rights reserved.
2
+ #
3
+ # Licensed under the Apache License, Version 2.0 (the "License");
4
+ # you may not use this file except in compliance with the License.
5
+ # You may obtain a copy of the License at
6
+ #
7
+ # http://www.apache.org/licenses/LICENSE-2.0
8
+ #
9
+ # Unless required by applicable law or agreed to in writing, software
10
+ # distributed under the License is distributed on an "AS IS" BASIS,
11
+ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12
+ # See the License for the specific language governing permissions and
13
+ # limitations under the License.
14
+
15
+
16
+ class DeviceNotConnectedError(ConnectionError):
17
+ """Exception raised when the device is not connected."""
18
+
19
+ def __init__(self, message="This device is not connected. Try calling `connect()` first."):
20
+ self.message = message
21
+ super().__init__(self.message)
22
+
23
+
24
+ class DeviceAlreadyConnectedError(ConnectionError):
25
+ """Exception raised when the device is already connected."""
26
+
27
+ def __init__(
28
+ self,
29
+ message="This device is already connected. Try not calling `connect()` twice.",
30
+ ):
31
+ self.message = message
32
+ super().__init__(self.message)
33
+
34
+
35
+ class InvalidActionError(ValueError):
36
+ """Exception raised when an action is already invalid."""
37
+
38
+ def __init__(
39
+ self,
40
+ message="The action is invalid. Check the value follows what it is expected from the action space.",
41
+ ):
42
+ self.message = message
43
+ super().__init__(self.message)
File without changes
@@ -0,0 +1,67 @@
1
+ # Copyright 2024 The HuggingFace Inc. team. All rights reserved.
2
+ #
3
+ # Licensed under the Apache License, Version 2.0 (the "License");
4
+ # you may not use this file except in compliance with the License.
5
+ # You may obtain a copy of the License at
6
+ #
7
+ # http://www.apache.org/licenses/LICENSE-2.0
8
+ #
9
+ # Unless required by applicable law or agreed to in writing, software
10
+ # distributed under the License is distributed on an "AS IS" BASIS,
11
+ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12
+ # See the License for the specific language governing permissions and
13
+ # limitations under the License.
14
+
15
+
16
+ def encode_sign_magnitude(value: int, sign_bit_index: int):
17
+ """
18
+ https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
19
+ """
20
+ max_magnitude = (1 << sign_bit_index) - 1
21
+ magnitude = abs(value)
22
+ if magnitude > max_magnitude:
23
+ raise ValueError(f"Magnitude {magnitude} exceeds {max_magnitude} (max for {sign_bit_index=})")
24
+
25
+ direction_bit = 1 if value < 0 else 0
26
+ return (direction_bit << sign_bit_index) | magnitude
27
+
28
+
29
+ def decode_sign_magnitude(encoded_value: int, sign_bit_index: int):
30
+ """
31
+ https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
32
+ """
33
+ direction_bit = (encoded_value >> sign_bit_index) & 1
34
+ magnitude_mask = (1 << sign_bit_index) - 1
35
+ magnitude = encoded_value & magnitude_mask
36
+ return -magnitude if direction_bit else magnitude
37
+
38
+
39
+ def encode_twos_complement(value: int, n_bytes: int):
40
+ """
41
+ https://en.wikipedia.org/wiki/Signed_number_representations#Two%27s_complement
42
+ """
43
+
44
+ bit_width = n_bytes * 8
45
+ min_val = -(1 << (bit_width - 1))
46
+ max_val = (1 << (bit_width - 1)) - 1
47
+
48
+ if not (min_val <= value <= max_val):
49
+ raise ValueError(
50
+ f"Value {value} out of range for {n_bytes}-byte two's complement: [{min_val}, {max_val}]"
51
+ )
52
+
53
+ if value >= 0:
54
+ return value
55
+
56
+ return (1 << bit_width) + value
57
+
58
+
59
+ def decode_twos_complement(value: int, n_bytes: int) -> int:
60
+ """
61
+ https://en.wikipedia.org/wiki/Signed_number_representations#Two%27s_complement
62
+ """
63
+ bits = n_bytes * 8
64
+ sign_bit = 1 << (bits - 1)
65
+ if value & sign_bit:
66
+ value -= 1 << bits
67
+ return value
@@ -0,0 +1,35 @@
1
+ #!/usr/bin/env python
2
+
3
+ # Copyright 2024 The HuggingFace Inc. team. All rights reserved.
4
+ #
5
+ # Licensed under the Apache License, Version 2.0 (the "License");
6
+ # you may not use this file except in compliance with the License.
7
+ # You may obtain a copy of the License at
8
+ #
9
+ # http://www.apache.org/licenses/LICENSE-2.0
10
+ #
11
+ # Unless required by applicable law or agreed to in writing, software
12
+ # distributed under the License is distributed on an "AS IS" BASIS,
13
+ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14
+ # See the License for the specific language governing permissions and
15
+ # limitations under the License.
16
+
17
+ import platform
18
+ import sys
19
+ import select
20
+
21
+ def enter_pressed() -> bool:
22
+ if platform.system() == "Windows":
23
+ import msvcrt
24
+
25
+ if msvcrt.kbhit():
26
+ key = msvcrt.getch()
27
+ return key in (b"\r", b"\n") # enter key
28
+ return False
29
+ else:
30
+ return select.select([sys.stdin], [], [], 0)[0] and sys.stdin.readline().strip() == ""
31
+
32
+
33
+ def move_cursor_up(lines):
34
+ """Move the cursor up by a specified number of lines."""
35
+ print(f"\033[{lines}A", end="")
@@ -0,0 +1,2 @@
1
+ from .dynamixel import DynamixelMotorsBus
2
+ from .tables import *