镜像自地址
https://github.com/MINT-SJTU/Evo-RL.git
已同步 2026-11-20 18:17:06 +00:00
snapshot: sync mint main from current workspace 2026-03-03 16:32:56 +0800
这个提交包含在:
21
src/lerobot/motors/__init__.py
普通文件
21
src/lerobot/motors/__init__.py
普通文件
@ -0,0 +1,21 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from .motors_bus import (
|
||||
Motor,
|
||||
MotorCalibration,
|
||||
MotorNormMode,
|
||||
)
|
||||
@ -0,0 +1,403 @@
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import math
|
||||
import os
|
||||
from dataclasses import dataclass
|
||||
|
||||
os.environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1"
|
||||
|
||||
from .motors_bus import MotorCalibration, MotorsBus
|
||||
|
||||
BAR_LEN, BAR_THICKNESS = 450, 8
|
||||
HANDLE_R = 10
|
||||
BRACKET_W, BRACKET_H = 6, 14
|
||||
TRI_W, TRI_H = 12, 14
|
||||
|
||||
BTN_W, BTN_H = 60, 22
|
||||
SAVE_W, SAVE_H = 80, 28
|
||||
LOAD_W = 80
|
||||
DD_W, DD_H = 160, 28
|
||||
|
||||
TOP_GAP = 50
|
||||
PADDING_Y, TOP_OFFSET = 70, 60
|
||||
FONT_SIZE, FPS = 20, 60
|
||||
|
||||
BG_COLOR = (30, 30, 30)
|
||||
BAR_RED, BAR_GREEN = (200, 60, 60), (60, 200, 60)
|
||||
HANDLE_COLOR, TEXT_COLOR = (240, 240, 240), (250, 250, 250)
|
||||
TICK_COLOR = (250, 220, 40)
|
||||
BTN_COLOR, BTN_COLOR_HL = (80, 80, 80), (110, 110, 110)
|
||||
DD_COLOR, DD_COLOR_HL = (70, 70, 70), (100, 100, 100)
|
||||
|
||||
|
||||
def dist(a, b):
|
||||
return math.hypot(a[0] - b[0], a[1] - b[1])
|
||||
|
||||
|
||||
@dataclass
|
||||
class RangeValues:
|
||||
min_v: int
|
||||
pos_v: int
|
||||
max_v: int
|
||||
|
||||
|
||||
class RangeSlider:
|
||||
"""One motor = one slider row"""
|
||||
|
||||
def __init__(self, motor, idx, res, calibration, present, label_pad, base_y):
|
||||
import pygame
|
||||
|
||||
self.motor = motor
|
||||
self.res = res
|
||||
self.x0 = 40 + label_pad
|
||||
self.x1 = self.x0 + BAR_LEN
|
||||
self.y = base_y + idx * PADDING_Y
|
||||
|
||||
self.min_v = calibration.range_min
|
||||
self.max_v = calibration.range_max
|
||||
self.pos_v = max(self.min_v, min(present, self.max_v))
|
||||
|
||||
self.min_x = self._pos_from_val(self.min_v)
|
||||
self.max_x = self._pos_from_val(self.max_v)
|
||||
self.pos_x = self._pos_from_val(self.pos_v)
|
||||
|
||||
self.min_btn = pygame.Rect(self.x0 - BTN_W - 6, self.y - BTN_H // 2, BTN_W, BTN_H)
|
||||
self.max_btn = pygame.Rect(self.x1 + 6, self.y - BTN_H // 2, BTN_W, BTN_H)
|
||||
|
||||
self.drag_min = self.drag_max = self.drag_pos = False
|
||||
self.tick_val = present
|
||||
self.font = pygame.font.Font(None, FONT_SIZE)
|
||||
|
||||
def _val_from_pos(self, x):
|
||||
return round((x - self.x0) / BAR_LEN * self.res)
|
||||
|
||||
def _pos_from_val(self, v):
|
||||
return self.x0 + (v / self.res) * BAR_LEN
|
||||
|
||||
def set_tick(self, v):
|
||||
self.tick_val = max(0, min(v, self.res))
|
||||
|
||||
def _triangle_hit(self, pos):
|
||||
import pygame
|
||||
|
||||
tri_top = self.y - BAR_THICKNESS // 2 - 2
|
||||
return pygame.Rect(self.pos_x - TRI_W // 2, tri_top - TRI_H, TRI_W, TRI_H).collidepoint(pos)
|
||||
|
||||
def handle_event(self, e):
|
||||
import pygame
|
||||
|
||||
if e.type == pygame.MOUSEBUTTONDOWN and e.button == 1:
|
||||
if self.min_btn.collidepoint(e.pos):
|
||||
self.min_x, self.min_v = self.pos_x, self.pos_v
|
||||
return
|
||||
if self.max_btn.collidepoint(e.pos):
|
||||
self.max_x, self.max_v = self.pos_x, self.pos_v
|
||||
return
|
||||
if dist(e.pos, (self.min_x, self.y)) <= HANDLE_R:
|
||||
self.drag_min = True
|
||||
elif dist(e.pos, (self.max_x, self.y)) <= HANDLE_R:
|
||||
self.drag_max = True
|
||||
elif self._triangle_hit(e.pos):
|
||||
self.drag_pos = True
|
||||
|
||||
elif e.type == pygame.MOUSEBUTTONUP and e.button == 1:
|
||||
self.drag_min = self.drag_max = self.drag_pos = False
|
||||
|
||||
elif e.type == pygame.MOUSEMOTION:
|
||||
x = e.pos[0]
|
||||
if self.drag_min:
|
||||
self.min_x = max(self.x0, min(x, self.pos_x))
|
||||
elif self.drag_max:
|
||||
self.max_x = min(self.x1, max(x, self.pos_x))
|
||||
elif self.drag_pos:
|
||||
self.pos_x = max(self.min_x, min(x, self.max_x))
|
||||
|
||||
self.min_v = self._val_from_pos(self.min_x)
|
||||
self.max_v = self._val_from_pos(self.max_x)
|
||||
self.pos_v = self._val_from_pos(self.pos_x)
|
||||
|
||||
def _draw_button(self, surf, rect, text):
|
||||
import pygame
|
||||
|
||||
clr = BTN_COLOR_HL if rect.collidepoint(pygame.mouse.get_pos()) else BTN_COLOR
|
||||
pygame.draw.rect(surf, clr, rect, border_radius=4)
|
||||
t = self.font.render(text, True, TEXT_COLOR)
|
||||
surf.blit(t, (rect.centerx - t.get_width() // 2, rect.centery - t.get_height() // 2))
|
||||
|
||||
def draw(self, surf):
|
||||
import pygame
|
||||
|
||||
# motor name above set-min button (right-aligned)
|
||||
name_surf = self.font.render(self.motor, True, TEXT_COLOR)
|
||||
surf.blit(
|
||||
name_surf,
|
||||
(self.min_btn.right - name_surf.get_width(), self.min_btn.y - name_surf.get_height() - 4),
|
||||
)
|
||||
|
||||
# bar + active section
|
||||
pygame.draw.rect(surf, BAR_RED, (self.x0, self.y - BAR_THICKNESS // 2, BAR_LEN, BAR_THICKNESS))
|
||||
pygame.draw.rect(
|
||||
surf, BAR_GREEN, (self.min_x, self.y - BAR_THICKNESS // 2, self.max_x - self.min_x, BAR_THICKNESS)
|
||||
)
|
||||
|
||||
# tick
|
||||
tick_x = self._pos_from_val(self.tick_val)
|
||||
pygame.draw.line(
|
||||
surf,
|
||||
TICK_COLOR,
|
||||
(tick_x, self.y - BAR_THICKNESS // 2 - 4),
|
||||
(tick_x, self.y + BAR_THICKNESS // 2 + 4),
|
||||
2,
|
||||
)
|
||||
|
||||
# brackets
|
||||
for x, sign in ((self.min_x, +1), (self.max_x, -1)):
|
||||
pygame.draw.line(
|
||||
surf, HANDLE_COLOR, (x, self.y - BRACKET_H // 2), (x, self.y + BRACKET_H // 2), 2
|
||||
)
|
||||
pygame.draw.line(
|
||||
surf,
|
||||
HANDLE_COLOR,
|
||||
(x, self.y - BRACKET_H // 2),
|
||||
(x + sign * BRACKET_W, self.y - BRACKET_H // 2),
|
||||
2,
|
||||
)
|
||||
pygame.draw.line(
|
||||
surf,
|
||||
HANDLE_COLOR,
|
||||
(x, self.y + BRACKET_H // 2),
|
||||
(x + sign * BRACKET_W, self.y + BRACKET_H // 2),
|
||||
2,
|
||||
)
|
||||
|
||||
# triangle ▼
|
||||
tri_top = self.y - BAR_THICKNESS // 2 - 2
|
||||
pygame.draw.polygon(
|
||||
surf,
|
||||
HANDLE_COLOR,
|
||||
[
|
||||
(self.pos_x, tri_top),
|
||||
(self.pos_x - TRI_W // 2, tri_top - TRI_H),
|
||||
(self.pos_x + TRI_W // 2, tri_top - TRI_H),
|
||||
],
|
||||
)
|
||||
|
||||
# numeric labels
|
||||
fh = self.font.get_height()
|
||||
pos_y = tri_top - TRI_H - 4 - fh
|
||||
txts = [
|
||||
(self.min_v, self.min_x, self.y - BRACKET_H // 2 - 4 - fh),
|
||||
(self.max_v, self.max_x, self.y - BRACKET_H // 2 - 4 - fh),
|
||||
(self.pos_v, self.pos_x, pos_y),
|
||||
]
|
||||
for v, x, y in txts:
|
||||
s = self.font.render(str(v), True, TEXT_COLOR)
|
||||
surf.blit(s, (x - s.get_width() // 2, y))
|
||||
|
||||
# buttons
|
||||
self._draw_button(surf, self.min_btn, "set min")
|
||||
self._draw_button(surf, self.max_btn, "set max")
|
||||
|
||||
# external
|
||||
def values(self) -> RangeValues:
|
||||
return RangeValues(self.min_v, self.pos_v, self.max_v)
|
||||
|
||||
|
||||
class RangeFinderGUI:
|
||||
def __init__(self, bus: MotorsBus, groups: dict[str, list[str]] | None = None):
|
||||
import pygame
|
||||
|
||||
self.bus = bus
|
||||
self.groups = groups if groups is not None else {"all": list(bus.motors)}
|
||||
self.group_names = list(self.groups)
|
||||
self.current_group = self.group_names[0]
|
||||
|
||||
if not bus.is_connected:
|
||||
bus.connect()
|
||||
|
||||
self.calibration = bus.read_calibration()
|
||||
self.res_table = bus.model_resolution_table
|
||||
self.present_cache = {
|
||||
m: bus.read("Present_Position", m, normalize=False)
|
||||
for motors in self.groups.values()
|
||||
for m in motors
|
||||
}
|
||||
|
||||
pygame.init()
|
||||
self.font = pygame.font.Font(None, FONT_SIZE)
|
||||
|
||||
label_pad = max(self.font.size(m)[0] for ms in self.groups.values() for m in ms)
|
||||
self.label_pad = label_pad
|
||||
width = 40 + label_pad + BAR_LEN + 6 + BTN_W + 10 + SAVE_W + 10
|
||||
self.controls_bottom = 10 + SAVE_H
|
||||
self.base_y = self.controls_bottom + TOP_GAP
|
||||
height = self.base_y + PADDING_Y * len(self.groups[self.current_group]) + 40
|
||||
|
||||
self.screen = pygame.display.set_mode((width, height))
|
||||
pygame.display.set_caption("Motors range finder")
|
||||
|
||||
# ui rects
|
||||
self.save_btn = pygame.Rect(width - SAVE_W - 10, 10, SAVE_W, SAVE_H)
|
||||
self.load_btn = pygame.Rect(self.save_btn.left - LOAD_W - 10, 10, LOAD_W, SAVE_H)
|
||||
self.dd_btn = pygame.Rect(width // 2 - DD_W // 2, 10, DD_W, DD_H)
|
||||
self.dd_open = False # dropdown expanded?
|
||||
|
||||
self.clock = pygame.time.Clock()
|
||||
self._build_sliders()
|
||||
self._adjust_height()
|
||||
|
||||
def _adjust_height(self):
|
||||
import pygame
|
||||
|
||||
motors = self.groups[self.current_group]
|
||||
new_h = self.base_y + PADDING_Y * len(motors) + 40
|
||||
if new_h != self.screen.get_height():
|
||||
w = self.screen.get_width()
|
||||
self.screen = pygame.display.set_mode((w, new_h))
|
||||
|
||||
def _build_sliders(self):
|
||||
self.sliders: list[RangeSlider] = []
|
||||
motors = self.groups[self.current_group]
|
||||
for i, m in enumerate(motors):
|
||||
self.sliders.append(
|
||||
RangeSlider(
|
||||
motor=m,
|
||||
idx=i,
|
||||
res=self.res_table[self.bus.motors[m].model] - 1,
|
||||
calibration=self.calibration[m],
|
||||
present=self.present_cache[m],
|
||||
label_pad=self.label_pad,
|
||||
base_y=self.base_y,
|
||||
)
|
||||
)
|
||||
|
||||
def _draw_dropdown(self):
|
||||
import pygame
|
||||
|
||||
# collapsed box
|
||||
hover = self.dd_btn.collidepoint(pygame.mouse.get_pos())
|
||||
pygame.draw.rect(self.screen, DD_COLOR_HL if hover else DD_COLOR, self.dd_btn, border_radius=6)
|
||||
|
||||
txt = self.font.render(self.current_group, True, TEXT_COLOR)
|
||||
self.screen.blit(
|
||||
txt, (self.dd_btn.centerx - txt.get_width() // 2, self.dd_btn.centery - txt.get_height() // 2)
|
||||
)
|
||||
|
||||
tri_w, tri_h = 12, 6
|
||||
cx = self.dd_btn.right - 14
|
||||
cy = self.dd_btn.centery + 1
|
||||
pygame.draw.polygon(
|
||||
self.screen,
|
||||
TEXT_COLOR,
|
||||
[(cx - tri_w // 2, cy - tri_h // 2), (cx + tri_w // 2, cy - tri_h // 2), (cx, cy + tri_h // 2)],
|
||||
)
|
||||
|
||||
if not self.dd_open:
|
||||
return
|
||||
|
||||
# expanded list
|
||||
for i, name in enumerate(self.group_names):
|
||||
item_rect = pygame.Rect(self.dd_btn.left, self.dd_btn.bottom + i * DD_H, DD_W, DD_H)
|
||||
clr = DD_COLOR_HL if item_rect.collidepoint(pygame.mouse.get_pos()) else DD_COLOR
|
||||
pygame.draw.rect(self.screen, clr, item_rect)
|
||||
t = self.font.render(name, True, TEXT_COLOR)
|
||||
self.screen.blit(
|
||||
t, (item_rect.centerx - t.get_width() // 2, item_rect.centery - t.get_height() // 2)
|
||||
)
|
||||
|
||||
def _handle_dropdown_event(self, e):
|
||||
import pygame
|
||||
|
||||
if e.type == pygame.MOUSEBUTTONDOWN and e.button == 1:
|
||||
if self.dd_btn.collidepoint(e.pos):
|
||||
self.dd_open = not self.dd_open
|
||||
return True
|
||||
if self.dd_open:
|
||||
for i, name in enumerate(self.group_names):
|
||||
item_rect = pygame.Rect(self.dd_btn.left, self.dd_btn.bottom + i * DD_H, DD_W, DD_H)
|
||||
if item_rect.collidepoint(e.pos):
|
||||
if name != self.current_group:
|
||||
self.current_group = name
|
||||
self._build_sliders()
|
||||
self._adjust_height()
|
||||
self.dd_open = False
|
||||
return True
|
||||
self.dd_open = False
|
||||
return False
|
||||
|
||||
def _save_current(self):
|
||||
for s in self.sliders:
|
||||
self.calibration[s.motor].range_min = s.min_v
|
||||
self.calibration[s.motor].range_max = s.max_v
|
||||
|
||||
with self.bus.torque_disabled():
|
||||
self.bus.write_calibration(self.calibration)
|
||||
|
||||
def _load_current(self):
|
||||
self.calibration = self.bus.read_calibration()
|
||||
for s in self.sliders:
|
||||
s.min_v = self.calibration[s.motor].range_min
|
||||
s.max_v = self.calibration[s.motor].range_max
|
||||
s.min_x = s._pos_from_val(s.min_v)
|
||||
s.max_x = s._pos_from_val(s.max_v)
|
||||
|
||||
def run(self) -> dict[str, MotorCalibration]:
|
||||
import pygame
|
||||
|
||||
while True:
|
||||
for e in pygame.event.get():
|
||||
if e.type == pygame.QUIT:
|
||||
pygame.quit()
|
||||
return self.calibration
|
||||
|
||||
if self._handle_dropdown_event(e):
|
||||
continue
|
||||
|
||||
if e.type == pygame.MOUSEBUTTONDOWN and e.button == 1:
|
||||
if self.save_btn.collidepoint(e.pos):
|
||||
self._save_current()
|
||||
elif self.load_btn.collidepoint(e.pos):
|
||||
self._load_current()
|
||||
|
||||
for s in self.sliders:
|
||||
s.handle_event(e)
|
||||
|
||||
# live goal write while dragging
|
||||
for s in self.sliders:
|
||||
if s.drag_pos:
|
||||
self.bus.write("Goal_Position", s.motor, s.pos_v, normalize=False)
|
||||
|
||||
# tick update
|
||||
for s in self.sliders:
|
||||
pos = self.bus.read("Present_Position", s.motor, normalize=False)
|
||||
s.set_tick(pos)
|
||||
self.present_cache[s.motor] = pos
|
||||
|
||||
# ─ drawing
|
||||
self.screen.fill(BG_COLOR)
|
||||
for s in self.sliders:
|
||||
s.draw(self.screen)
|
||||
|
||||
self._draw_dropdown()
|
||||
|
||||
# load / save buttons
|
||||
for rect, text in ((self.load_btn, "LOAD"), (self.save_btn, "SAVE")):
|
||||
clr = BTN_COLOR_HL if rect.collidepoint(pygame.mouse.get_pos()) else BTN_COLOR
|
||||
pygame.draw.rect(self.screen, clr, rect, border_radius=6)
|
||||
t = self.font.render(text, True, TEXT_COLOR)
|
||||
self.screen.blit(t, (rect.centerx - t.get_width() // 2, rect.centery - t.get_height() // 2))
|
||||
|
||||
pygame.display.flip()
|
||||
self.clock.tick(FPS)
|
||||
@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from .damiao import DamiaoMotorsBus
|
||||
from .tables import *
|
||||
859
src/lerobot/motors/damiao/damiao.py
普通文件
859
src/lerobot/motors/damiao/damiao.py
普通文件
@ -0,0 +1,859 @@
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
# Portions of this file are derived from DM_Control_Python by cmjang.
|
||||
# Licensed under the MIT License; see `LICENSE` for the full text:
|
||||
# https://github.com/cmjang/DM_Control_Python
|
||||
|
||||
import logging
|
||||
import time
|
||||
from contextlib import contextmanager
|
||||
from copy import deepcopy
|
||||
from functools import cached_property
|
||||
from typing import TYPE_CHECKING, Any, TypedDict
|
||||
|
||||
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
|
||||
from lerobot.utils.import_utils import _can_available
|
||||
|
||||
if TYPE_CHECKING or _can_available:
|
||||
import can
|
||||
else:
|
||||
|
||||
class can: # noqa: N801
|
||||
Message = object
|
||||
interface = None
|
||||
|
||||
|
||||
import numpy as np
|
||||
|
||||
from lerobot.utils.robot_utils import precise_sleep
|
||||
from lerobot.utils.utils import enter_pressed, move_cursor_up
|
||||
|
||||
from ..motors_bus import Motor, MotorCalibration, MotorsBusBase, NameOrID, Value
|
||||
from .tables import (
|
||||
AVAILABLE_BAUDRATES,
|
||||
CAN_CMD_DISABLE,
|
||||
CAN_CMD_ENABLE,
|
||||
CAN_CMD_REFRESH,
|
||||
CAN_CMD_SET_ZERO,
|
||||
CAN_PARAM_ID,
|
||||
DEFAULT_BAUDRATE,
|
||||
DEFAULT_TIMEOUT_MS,
|
||||
MIT_KD_RANGE,
|
||||
MIT_KP_RANGE,
|
||||
MOTOR_LIMIT_PARAMS,
|
||||
MotorType,
|
||||
)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
LONG_TIMEOUT_SEC = 0.1
|
||||
MEDIUM_TIMEOUT_SEC = 0.01
|
||||
SHORT_TIMEOUT_SEC = 0.001
|
||||
PRECISE_TIMEOUT_SEC = 0.0001
|
||||
|
||||
|
||||
class MotorState(TypedDict):
|
||||
position: float
|
||||
velocity: float
|
||||
torque: float
|
||||
temp_mos: float
|
||||
temp_rotor: float
|
||||
|
||||
|
||||
class DamiaoMotorsBus(MotorsBusBase):
|
||||
"""
|
||||
The Damiao implementation for a MotorsBus using CAN bus communication.
|
||||
|
||||
This class uses python-can for CAN bus communication with Damiao motors.
|
||||
For more info, see:
|
||||
- python-can documentation: https://python-can.readthedocs.io/en/stable/
|
||||
- Seedstudio documentation: https://wiki.seeedstudio.com/damiao_series/
|
||||
- DM_Control_Python repo: https://github.com/cmjang/DM_Control_Python
|
||||
"""
|
||||
|
||||
# CAN-specific settings
|
||||
available_baudrates = deepcopy(AVAILABLE_BAUDRATES)
|
||||
default_baudrate = DEFAULT_BAUDRATE
|
||||
default_timeout = DEFAULT_TIMEOUT_MS
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str,
|
||||
motors: dict[str, Motor],
|
||||
calibration: dict[str, MotorCalibration] | None = None,
|
||||
can_interface: str = "auto",
|
||||
use_can_fd: bool = True,
|
||||
bitrate: int = 1000000,
|
||||
data_bitrate: int | None = 5000000,
|
||||
):
|
||||
"""
|
||||
Initialize the Damiao motors bus.
|
||||
|
||||
Args:
|
||||
port: CAN interface name (e.g., "can0" for Linux, "/dev/cu.usbmodem*" for macOS)
|
||||
motors: Dictionary mapping motor names to Motor objects
|
||||
calibration: Optional calibration data
|
||||
can_interface: CAN interface type - "auto" (default), "socketcan" (Linux), or "slcan" (macOS/serial)
|
||||
use_can_fd: Whether to use CAN FD mode (default: True for OpenArms)
|
||||
bitrate: Nominal bitrate in bps (default: 1000000 = 1 Mbps)
|
||||
data_bitrate: Data bitrate for CAN FD in bps (default: 5000000 = 5 Mbps), ignored if use_can_fd is False
|
||||
"""
|
||||
super().__init__(port, motors, calibration)
|
||||
self.port = port
|
||||
self.can_interface = can_interface
|
||||
self.use_can_fd = use_can_fd
|
||||
self.bitrate = bitrate
|
||||
self.data_bitrate = data_bitrate
|
||||
self.canbus: can.interface.Bus | None = None
|
||||
self._is_connected = False
|
||||
|
||||
# Map motor names to CAN IDs
|
||||
self._motor_can_ids: dict[str, int] = {}
|
||||
self._recv_id_to_motor: dict[int, str] = {}
|
||||
self._motor_types: dict[str, MotorType] = {}
|
||||
|
||||
for name, motor in self.motors.items():
|
||||
if motor.motor_type_str is None:
|
||||
raise ValueError(f"Motor '{name}' is missing required 'motor_type'")
|
||||
self._motor_types[name] = getattr(MotorType, motor.motor_type_str.upper().replace("-", "_"))
|
||||
|
||||
# Map recv_id to motor name for filtering responses
|
||||
if motor.recv_id is not None:
|
||||
self._recv_id_to_motor[motor.recv_id] = name
|
||||
|
||||
# State cache for handling packet drops safely
|
||||
self._last_known_states: dict[str, MotorState] = {
|
||||
name: {
|
||||
"position": 0.0,
|
||||
"velocity": 0.0,
|
||||
"torque": 0.0,
|
||||
"temp_mos": 0.0,
|
||||
"temp_rotor": 0.0,
|
||||
}
|
||||
for name in self.motors
|
||||
}
|
||||
|
||||
# Dynamic gains storage
|
||||
# Defaults: Kp=10.0 (Stiffness), Kd=0.5 (Damping)
|
||||
self._gains: dict[str, dict[str, float]] = {name: {"kp": 10.0, "kd": 0.5} for name in self.motors}
|
||||
|
||||
@property
|
||||
def is_connected(self) -> bool:
|
||||
"""Check if the CAN bus is connected."""
|
||||
return self._is_connected and self.canbus is not None
|
||||
|
||||
@check_if_already_connected
|
||||
def connect(self, handshake: bool = True) -> None:
|
||||
"""
|
||||
Open the CAN bus and initialize communication.
|
||||
|
||||
Args:
|
||||
handshake: If True, ping all motors to verify they're present
|
||||
"""
|
||||
|
||||
try:
|
||||
# Auto-detect interface type based on port name
|
||||
if self.can_interface == "auto":
|
||||
if self.port.startswith("/dev/"):
|
||||
self.can_interface = "slcan"
|
||||
logger.info(f"Auto-detected slcan interface for port {self.port}")
|
||||
else:
|
||||
self.can_interface = "socketcan"
|
||||
logger.info(f"Auto-detected socketcan interface for port {self.port}")
|
||||
|
||||
# Connect to CAN bus
|
||||
kwargs = {
|
||||
"channel": self.port,
|
||||
"bitrate": self.bitrate,
|
||||
"interface": self.can_interface,
|
||||
}
|
||||
|
||||
if self.can_interface == "socketcan" and self.use_can_fd and self.data_bitrate is not None:
|
||||
kwargs.update({"data_bitrate": self.data_bitrate, "fd": True})
|
||||
logger.info(
|
||||
f"Connected to {self.port} with CAN FD (bitrate={self.bitrate}, data_bitrate={self.data_bitrate})"
|
||||
)
|
||||
else:
|
||||
logger.info(f"Connected to {self.port} with {self.can_interface} (bitrate={self.bitrate})")
|
||||
|
||||
self.canbus = can.interface.Bus(**kwargs)
|
||||
self._is_connected = True
|
||||
|
||||
if handshake:
|
||||
self._handshake()
|
||||
|
||||
logger.debug(f"{self.__class__.__name__} connected via {self.can_interface}.")
|
||||
except Exception as e:
|
||||
self._is_connected = False
|
||||
raise ConnectionError(f"Failed to connect to CAN bus: {e}") from e
|
||||
|
||||
def _handshake(self) -> None:
|
||||
"""
|
||||
Verify all motors are present and populate initial state cache.
|
||||
Raises ConnectionError if any motor fails to respond.
|
||||
"""
|
||||
logger.info("Starting handshake with motors...")
|
||||
|
||||
# Drain any pending messages
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
while self.canbus.recv(timeout=0.01):
|
||||
pass
|
||||
|
||||
missing_motors = []
|
||||
for motor_name in self.motors:
|
||||
motor_id = self._get_motor_id(motor_name)
|
||||
recv_id = self._get_motor_recv_id(motor_name)
|
||||
|
||||
# Send enable command
|
||||
data = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, CAN_CMD_ENABLE]
|
||||
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False, is_fd=self.use_can_fd)
|
||||
self.canbus.send(msg)
|
||||
|
||||
# Wait for response with longer timeout
|
||||
response = None
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < 0.1:
|
||||
response = self.canbus.recv(timeout=0.1)
|
||||
if response and response.arbitration_id == recv_id:
|
||||
break
|
||||
response = None
|
||||
|
||||
if response is None:
|
||||
missing_motors.append(motor_name)
|
||||
else:
|
||||
self._process_response(motor_name, msg)
|
||||
time.sleep(MEDIUM_TIMEOUT_SEC)
|
||||
|
||||
if missing_motors:
|
||||
raise ConnectionError(
|
||||
f"Handshake failed. The following motors did not respond: {missing_motors}. "
|
||||
"Check power (24V) and CAN wiring."
|
||||
)
|
||||
logger.info("Handshake successful. All motors ready.")
|
||||
|
||||
@check_if_not_connected
|
||||
def disconnect(self, disable_torque: bool = True) -> None:
|
||||
"""
|
||||
Close the CAN bus connection.
|
||||
|
||||
Args:
|
||||
disable_torque: If True, disable torque on all motors before disconnecting
|
||||
"""
|
||||
|
||||
if disable_torque:
|
||||
try:
|
||||
self.disable_torque()
|
||||
except Exception as e:
|
||||
logger.warning(f"Failed to disable torque during disconnect: {e}")
|
||||
|
||||
if self.canbus:
|
||||
self.canbus.shutdown()
|
||||
self.canbus = None
|
||||
self._is_connected = False
|
||||
logger.debug(f"{self.__class__.__name__} disconnected.")
|
||||
|
||||
def configure_motors(self) -> None:
|
||||
"""Configure all motors with default settings."""
|
||||
# Damiao motors don't require much configuration in MIT mode
|
||||
# Just ensure they're enabled
|
||||
for motor in self.motors:
|
||||
self._send_simple_command(motor, CAN_CMD_ENABLE)
|
||||
time.sleep(MEDIUM_TIMEOUT_SEC)
|
||||
|
||||
def _send_simple_command(self, motor: NameOrID, command_byte: int) -> None:
|
||||
"""Helper to send simple 8-byte commands (Enable, Disable, Zero)."""
|
||||
motor_id = self._get_motor_id(motor)
|
||||
motor_name = self._get_motor_name(motor)
|
||||
recv_id = self._get_motor_recv_id(motor)
|
||||
data = [0xFF] * 7 + [command_byte]
|
||||
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False, is_fd=self.use_can_fd)
|
||||
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
self.canbus.send(msg)
|
||||
if msg := self._recv_motor_response(expected_recv_id=recv_id):
|
||||
self._process_response(motor_name, msg)
|
||||
else:
|
||||
logger.debug(f"No response from {motor_name} after command 0x{command_byte:02X}")
|
||||
|
||||
def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
"""Enable torque on selected motors."""
|
||||
target_motors = self._get_motors_list(motors)
|
||||
for motor in target_motors:
|
||||
for _ in range(num_retry + 1):
|
||||
try:
|
||||
self._send_simple_command(motor, CAN_CMD_ENABLE)
|
||||
break
|
||||
except Exception as e:
|
||||
if _ == num_retry:
|
||||
raise e
|
||||
time.sleep(MEDIUM_TIMEOUT_SEC)
|
||||
|
||||
def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
"""Disable torque on selected motors."""
|
||||
target_motors = self._get_motors_list(motors)
|
||||
for motor in target_motors:
|
||||
for _ in range(num_retry + 1):
|
||||
try:
|
||||
self._send_simple_command(motor, CAN_CMD_DISABLE)
|
||||
break
|
||||
except Exception as e:
|
||||
if _ == num_retry:
|
||||
raise e
|
||||
time.sleep(MEDIUM_TIMEOUT_SEC)
|
||||
|
||||
@contextmanager
|
||||
def torque_disabled(self, motors: str | list[str] | None = None):
|
||||
"""
|
||||
Context manager that guarantees torque is re-enabled.
|
||||
|
||||
This helper is useful to temporarily disable torque when configuring motors.
|
||||
"""
|
||||
self.disable_torque(motors)
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
self.enable_torque(motors)
|
||||
|
||||
def set_zero_position(self, motors: str | list[str] | None = None) -> None:
|
||||
"""Set current position as zero for selected motors."""
|
||||
target_motors = self._get_motors_list(motors)
|
||||
for motor in target_motors:
|
||||
self._send_simple_command(motor, CAN_CMD_SET_ZERO)
|
||||
time.sleep(MEDIUM_TIMEOUT_SEC)
|
||||
|
||||
def _refresh_motor(self, motor: NameOrID) -> can.Message | None:
|
||||
"""Refresh motor status and return the response."""
|
||||
motor_id = self._get_motor_id(motor)
|
||||
recv_id = self._get_motor_recv_id(motor)
|
||||
data = [motor_id & 0xFF, (motor_id >> 8) & 0xFF, CAN_CMD_REFRESH, 0, 0, 0, 0, 0]
|
||||
msg = can.Message(arbitration_id=CAN_PARAM_ID, data=data, is_extended_id=False, is_fd=self.use_can_fd)
|
||||
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
self.canbus.send(msg)
|
||||
return self._recv_motor_response(expected_recv_id=recv_id)
|
||||
|
||||
def _recv_motor_response(
|
||||
self, expected_recv_id: int | None = None, timeout: float = 0.001
|
||||
) -> can.Message | None:
|
||||
"""
|
||||
Receive a response from a motor.
|
||||
|
||||
Args:
|
||||
expected_recv_id: If provided, only return messages from this CAN ID
|
||||
timeout: Timeout in seconds (default: 1ms for high-speed operation)
|
||||
Returns:
|
||||
CAN message if received, None otherwise
|
||||
"""
|
||||
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
try:
|
||||
start_time = time.time()
|
||||
messages_seen = []
|
||||
while time.time() - start_time < timeout:
|
||||
msg = self.canbus.recv(timeout=PRECISE_TIMEOUT_SEC)
|
||||
if msg:
|
||||
messages_seen.append(f"0x{msg.arbitration_id:02X}")
|
||||
if expected_recv_id is None or msg.arbitration_id == expected_recv_id:
|
||||
return msg
|
||||
logger.debug(
|
||||
f"Ignoring message from 0x{msg.arbitration_id:02X}, expected 0x{expected_recv_id:02X}"
|
||||
)
|
||||
|
||||
if logger.isEnabledFor(logging.DEBUG):
|
||||
if messages_seen:
|
||||
logger.debug(
|
||||
f"Received {len(messages_seen)} msgs from {set(messages_seen)}, expected 0x{expected_recv_id:02X}"
|
||||
)
|
||||
else:
|
||||
logger.debug(f"No CAN messages received (expected 0x{expected_recv_id:02X})")
|
||||
except Exception as e:
|
||||
logger.debug(f"Failed to receive CAN message: {e}")
|
||||
return None
|
||||
|
||||
def _recv_all_responses(
|
||||
self, expected_recv_ids: list[int], timeout: float = 0.002
|
||||
) -> dict[int, can.Message]:
|
||||
"""
|
||||
Efficiently receive responses from multiple motors at once.
|
||||
Uses the OpenArms pattern: collect all available messages within timeout.
|
||||
|
||||
Args:
|
||||
expected_recv_ids: List of CAN IDs we expect responses from
|
||||
timeout: Total timeout in seconds (default: 2ms)
|
||||
|
||||
Returns:
|
||||
Dictionary mapping recv_id to CAN message
|
||||
"""
|
||||
responses: dict[int, can.Message] = {}
|
||||
expected_set = set(expected_recv_ids)
|
||||
start_time = time.time()
|
||||
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
try:
|
||||
while len(responses) < len(expected_recv_ids) and (time.time() - start_time) < timeout:
|
||||
# 100us poll timeout
|
||||
msg = self.canbus.recv(timeout=PRECISE_TIMEOUT_SEC)
|
||||
if msg and msg.arbitration_id in expected_set:
|
||||
responses[msg.arbitration_id] = msg
|
||||
if len(responses) == len(expected_recv_ids):
|
||||
break
|
||||
except Exception as e:
|
||||
logger.debug(f"Error receiving responses: {e}")
|
||||
|
||||
return responses
|
||||
|
||||
def _encode_mit_packet(
|
||||
self,
|
||||
motor_type: MotorType,
|
||||
kp: float,
|
||||
kd: float,
|
||||
position_degrees: float,
|
||||
velocity_deg_per_sec: float,
|
||||
torque: float,
|
||||
) -> list[int]:
|
||||
"""Helper to encode control parameters into 8 bytes for MIT mode."""
|
||||
# Convert degrees to radians
|
||||
position_rad = np.radians(position_degrees)
|
||||
velocity_rad_per_sec = np.radians(velocity_deg_per_sec)
|
||||
|
||||
# Get motor limits
|
||||
pmax, vmax, tmax = MOTOR_LIMIT_PARAMS[motor_type]
|
||||
|
||||
# Encode parameters
|
||||
kp_uint = self._float_to_uint(kp, *MIT_KP_RANGE, 12)
|
||||
kd_uint = self._float_to_uint(kd, *MIT_KD_RANGE, 12)
|
||||
q_uint = self._float_to_uint(position_rad, -pmax, pmax, 16)
|
||||
dq_uint = self._float_to_uint(velocity_rad_per_sec, -vmax, vmax, 12)
|
||||
tau_uint = self._float_to_uint(torque, -tmax, tmax, 12)
|
||||
|
||||
# Pack data
|
||||
data = [0] * 8
|
||||
data[0] = (q_uint >> 8) & 0xFF
|
||||
data[1] = q_uint & 0xFF
|
||||
data[2] = dq_uint >> 4
|
||||
data[3] = ((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)
|
||||
data[4] = kp_uint & 0xFF
|
||||
data[5] = kd_uint >> 4
|
||||
data[6] = ((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)
|
||||
data[7] = tau_uint & 0xFF
|
||||
return data
|
||||
|
||||
def _mit_control(
|
||||
self,
|
||||
motor: NameOrID,
|
||||
kp: float,
|
||||
kd: float,
|
||||
position_degrees: float,
|
||||
velocity_deg_per_sec: float,
|
||||
torque: float,
|
||||
) -> None:
|
||||
"""Send MIT control command to a motor."""
|
||||
motor_id = self._get_motor_id(motor)
|
||||
motor_name = self._get_motor_name(motor)
|
||||
motor_type = self._motor_types[motor_name]
|
||||
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
data = self._encode_mit_packet(motor_type, kp, kd, position_degrees, velocity_deg_per_sec, torque)
|
||||
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False, is_fd=self.use_can_fd)
|
||||
self.canbus.send(msg)
|
||||
|
||||
recv_id = self._get_motor_recv_id(motor)
|
||||
if msg := self._recv_motor_response(expected_recv_id=recv_id):
|
||||
self._process_response(motor_name, msg)
|
||||
else:
|
||||
logger.debug(f"No response from {motor_name} after MIT control command")
|
||||
|
||||
def _mit_control_batch(
|
||||
self,
|
||||
commands: dict[NameOrID, tuple[float, float, float, float, float]],
|
||||
) -> None:
|
||||
"""
|
||||
Send MIT control commands to multiple motors in batch.
|
||||
Sends all commands first, then collects responses.
|
||||
|
||||
Args:
|
||||
commands: Dict mapping motor name/ID to (kp, kd, position_deg, velocity_deg/s, torque)
|
||||
Example: {'joint_1': (10.0, 0.5, 45.0, 0.0, 0.0), ...}
|
||||
"""
|
||||
if not commands:
|
||||
return
|
||||
|
||||
recv_id_to_motor: dict[int, str] = {}
|
||||
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
# Step 1: Send all MIT control commands
|
||||
for motor, (kp, kd, position_degrees, velocity_deg_per_sec, torque) in commands.items():
|
||||
motor_id = self._get_motor_id(motor)
|
||||
motor_name = self._get_motor_name(motor)
|
||||
motor_type = self._motor_types[motor_name]
|
||||
|
||||
data = self._encode_mit_packet(motor_type, kp, kd, position_degrees, velocity_deg_per_sec, torque)
|
||||
msg = can.Message(arbitration_id=motor_id, data=data, is_extended_id=False, is_fd=self.use_can_fd)
|
||||
self.canbus.send(msg)
|
||||
|
||||
recv_id_to_motor[self._get_motor_recv_id(motor)] = motor_name
|
||||
|
||||
# Step 2: Collect responses and update state cache
|
||||
responses = self._recv_all_responses(list(recv_id_to_motor.keys()), timeout=SHORT_TIMEOUT_SEC)
|
||||
for recv_id, motor_name in recv_id_to_motor.items():
|
||||
if msg := responses.get(recv_id):
|
||||
self._process_response(motor_name, msg)
|
||||
|
||||
def _float_to_uint(self, x: float, x_min: float, x_max: float, bits: int) -> int:
|
||||
"""Convert float to unsigned integer for CAN transmission."""
|
||||
x = max(x_min, min(x_max, x)) # Clamp to range
|
||||
span = x_max - x_min
|
||||
data_norm = (x - x_min) / span
|
||||
return int(data_norm * ((1 << bits) - 1))
|
||||
|
||||
def _uint_to_float(self, x: int, x_min: float, x_max: float, bits: int) -> float:
|
||||
"""Convert unsigned integer from CAN to float."""
|
||||
span = x_max - x_min
|
||||
data_norm = float(x) / ((1 << bits) - 1)
|
||||
return data_norm * span + x_min
|
||||
|
||||
def _decode_motor_state(
|
||||
self, data: bytearray | bytes, motor_type: MotorType
|
||||
) -> tuple[float, float, float, int, int]:
|
||||
"""
|
||||
Decode motor state from CAN data.
|
||||
Returns: (position_deg, velocity_deg_s, torque, temp_mos, temp_rotor)
|
||||
"""
|
||||
if len(data) < 8:
|
||||
raise ValueError("Invalid motor state data")
|
||||
|
||||
# Extract encoded values
|
||||
q_uint = (data[1] << 8) | data[2]
|
||||
dq_uint = (data[3] << 4) | (data[4] >> 4)
|
||||
tau_uint = ((data[4] & 0x0F) << 8) | data[5]
|
||||
t_mos = data[6]
|
||||
t_rotor = data[7]
|
||||
|
||||
# Get motor limits
|
||||
pmax, vmax, tmax = MOTOR_LIMIT_PARAMS[motor_type]
|
||||
|
||||
# Decode to physical values
|
||||
position_rad = self._uint_to_float(q_uint, -pmax, pmax, 16)
|
||||
velocity_rad_per_sec = self._uint_to_float(dq_uint, -vmax, vmax, 12)
|
||||
torque = self._uint_to_float(tau_uint, -tmax, tmax, 12)
|
||||
|
||||
return np.degrees(position_rad), np.degrees(velocity_rad_per_sec), torque, t_mos, t_rotor
|
||||
|
||||
def _process_response(self, motor: str, msg: can.Message) -> None:
|
||||
"""Decode a message and update the motor state cache."""
|
||||
try:
|
||||
motor_type = self._motor_types[motor]
|
||||
pos, vel, torque, t_mos, t_rotor = self._decode_motor_state(msg.data, motor_type)
|
||||
|
||||
self._last_known_states[motor] = {
|
||||
"position": pos,
|
||||
"velocity": vel,
|
||||
"torque": torque,
|
||||
"temp_mos": float(t_mos),
|
||||
"temp_rotor": float(t_rotor),
|
||||
}
|
||||
except Exception as e:
|
||||
logger.warning(f"Failed to decode response from {motor}: {e}")
|
||||
|
||||
@check_if_not_connected
|
||||
def read(self, data_name: str, motor: str) -> Value:
|
||||
"""Read a value from a single motor. Positions are always in degrees."""
|
||||
|
||||
# Refresh motor to get latest state
|
||||
msg = self._refresh_motor(motor)
|
||||
if msg is None:
|
||||
motor_id = self._get_motor_id(motor)
|
||||
recv_id = self._get_motor_recv_id(motor)
|
||||
raise ConnectionError(
|
||||
f"No response from motor '{motor}' (send ID: 0x{motor_id:02X}, recv ID: 0x{recv_id:02X}). "
|
||||
f"Check that: 1) Motor is powered (24V), 2) CAN wiring is correct, "
|
||||
f"3) Motor IDs are configured correctly using Damiao Debugging Tools"
|
||||
)
|
||||
|
||||
self._process_response(motor, msg)
|
||||
return self._get_cached_value(motor, data_name)
|
||||
|
||||
def _get_cached_value(self, motor: str, data_name: str) -> Value:
|
||||
"""Retrieve a specific value from the cache."""
|
||||
state = self._last_known_states[motor]
|
||||
mapping: dict[str, Any] = {
|
||||
"Present_Position": state["position"],
|
||||
"Present_Velocity": state["velocity"],
|
||||
"Present_Torque": state["torque"],
|
||||
"Temperature_MOS": state["temp_mos"],
|
||||
"Temperature_Rotor": state["temp_rotor"],
|
||||
}
|
||||
if data_name not in mapping:
|
||||
raise ValueError(f"Unknown data_name: {data_name}")
|
||||
return mapping[data_name]
|
||||
|
||||
@check_if_not_connected
|
||||
def write(
|
||||
self,
|
||||
data_name: str,
|
||||
motor: str,
|
||||
value: Value,
|
||||
) -> None:
|
||||
"""
|
||||
Write a value to a single motor. Positions are always in degrees.
|
||||
Can write 'Goal_Position', 'Kp', or 'Kd'.
|
||||
"""
|
||||
|
||||
if data_name in ("Kp", "Kd"):
|
||||
self._gains[motor][data_name.lower()] = float(value)
|
||||
elif data_name == "Goal_Position":
|
||||
kp = self._gains[motor]["kp"]
|
||||
kd = self._gains[motor]["kd"]
|
||||
self._mit_control(motor, kp, kd, float(value), 0.0, 0.0)
|
||||
else:
|
||||
raise ValueError(f"Writing {data_name} not supported in MIT mode")
|
||||
|
||||
def sync_read(
|
||||
self,
|
||||
data_name: str,
|
||||
motors: str | list[str] | None = None,
|
||||
) -> dict[str, Value]:
|
||||
"""
|
||||
Read the same value from multiple motors simultaneously.
|
||||
"""
|
||||
target_motors = self._get_motors_list(motors)
|
||||
self._batch_refresh(target_motors)
|
||||
|
||||
result = {}
|
||||
for motor in target_motors:
|
||||
result[motor] = self._get_cached_value(motor, data_name)
|
||||
return result
|
||||
|
||||
def sync_read_all_states(
|
||||
self,
|
||||
motors: str | list[str] | None = None,
|
||||
*,
|
||||
num_retry: int = 0,
|
||||
) -> dict[str, MotorState]:
|
||||
"""
|
||||
Read ALL motor states (position, velocity, torque) from multiple motors in ONE refresh cycle.
|
||||
|
||||
Returns:
|
||||
Dictionary mapping motor names to state dicts with keys: 'position', 'velocity', 'torque'
|
||||
Example: {'joint_1': {'position': 45.2, 'velocity': 1.3, 'torque': 0.5}, ...}
|
||||
"""
|
||||
target_motors = self._get_motors_list(motors)
|
||||
self._batch_refresh(target_motors)
|
||||
|
||||
result = {}
|
||||
for motor in target_motors:
|
||||
result[motor] = self._last_known_states[motor].copy()
|
||||
return result
|
||||
|
||||
def _batch_refresh(self, motors: list[str]) -> None:
|
||||
"""Internal helper to refresh a list of motors and update cache."""
|
||||
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
|
||||
# Send refresh commands
|
||||
for motor in motors:
|
||||
motor_id = self._get_motor_id(motor)
|
||||
data = [motor_id & 0xFF, (motor_id >> 8) & 0xFF, CAN_CMD_REFRESH, 0, 0, 0, 0, 0]
|
||||
msg = can.Message(
|
||||
arbitration_id=CAN_PARAM_ID, data=data, is_extended_id=False, is_fd=self.use_can_fd
|
||||
)
|
||||
self.canbus.send(msg)
|
||||
|
||||
# Collect responses
|
||||
expected_recv_ids = [self._get_motor_recv_id(m) for m in motors]
|
||||
responses = self._recv_all_responses(expected_recv_ids, timeout=MEDIUM_TIMEOUT_SEC)
|
||||
|
||||
# Update cache
|
||||
for motor in motors:
|
||||
recv_id = self._get_motor_recv_id(motor)
|
||||
msg = responses.get(recv_id)
|
||||
if msg:
|
||||
self._process_response(motor, msg)
|
||||
else:
|
||||
logger.warning(f"Packet drop: {motor} (ID: 0x{recv_id:02X}). Using last known state.")
|
||||
|
||||
@check_if_not_connected
|
||||
def sync_write(self, data_name: str, values: dict[str, Value]) -> None:
|
||||
"""
|
||||
Write values to multiple motors simultaneously. Positions are always in degrees.
|
||||
"""
|
||||
|
||||
if data_name in ("Kp", "Kd"):
|
||||
key = data_name.lower()
|
||||
for motor, val in values.items():
|
||||
self._gains[motor][key] = float(val)
|
||||
|
||||
elif data_name == "Goal_Position":
|
||||
# Step 1: Send all MIT control commands
|
||||
recv_id_to_motor: dict[int, str] = {}
|
||||
if self.canbus is None:
|
||||
raise RuntimeError("CAN bus is not initialized.")
|
||||
for motor, value_degrees in values.items():
|
||||
motor_id = self._get_motor_id(motor)
|
||||
motor_name = self._get_motor_name(motor)
|
||||
motor_type = self._motor_types[motor_name]
|
||||
|
||||
kp = self._gains[motor]["kp"]
|
||||
kd = self._gains[motor]["kd"]
|
||||
|
||||
data = self._encode_mit_packet(motor_type, kp, kd, float(value_degrees), 0.0, 0.0)
|
||||
msg = can.Message(
|
||||
arbitration_id=motor_id, data=data, is_extended_id=False, is_fd=self.use_can_fd
|
||||
)
|
||||
self.canbus.send(msg)
|
||||
precise_sleep(PRECISE_TIMEOUT_SEC)
|
||||
|
||||
recv_id_to_motor[self._get_motor_recv_id(motor)] = motor_name
|
||||
|
||||
# Step 2: Collect responses and update state cache
|
||||
responses = self._recv_all_responses(list(recv_id_to_motor.keys()), timeout=MEDIUM_TIMEOUT_SEC)
|
||||
for recv_id, motor_name in recv_id_to_motor.items():
|
||||
if msg := responses.get(recv_id):
|
||||
self._process_response(motor_name, msg)
|
||||
else:
|
||||
# Fall back to individual writes
|
||||
for motor, value in values.items():
|
||||
self.write(data_name, motor, value)
|
||||
|
||||
def read_calibration(self) -> dict[str, MotorCalibration]:
|
||||
"""Read calibration data from motors."""
|
||||
# Damiao motors don't store calibration internally
|
||||
# Return existing calibration or empty dict
|
||||
return self.calibration if self.calibration else {}
|
||||
|
||||
def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None:
|
||||
"""Write calibration data to motors."""
|
||||
# Damiao motors don't store calibration internally
|
||||
# Just cache it in memory
|
||||
if cache:
|
||||
self.calibration = calibration_dict
|
||||
|
||||
def record_ranges_of_motion(
|
||||
self,
|
||||
motors: str | list[str] | None = None,
|
||||
display_values: bool = True,
|
||||
) -> tuple[dict[str, Value], dict[str, Value]]:
|
||||
"""
|
||||
Interactively record the min/max values of each motor in degrees.
|
||||
|
||||
Move the joints by hand (with torque disabled) while the method streams live positions.
|
||||
Press Enter to finish.
|
||||
"""
|
||||
target_motors = self._get_motors_list(motors)
|
||||
|
||||
self.disable_torque(target_motors)
|
||||
time.sleep(LONG_TIMEOUT_SEC)
|
||||
|
||||
start_positions = self.sync_read("Present_Position", target_motors)
|
||||
mins = start_positions.copy()
|
||||
maxes = start_positions.copy()
|
||||
|
||||
print("\nMove joints through their full range of motion. Press ENTER when done.")
|
||||
user_pressed_enter = False
|
||||
|
||||
while not user_pressed_enter:
|
||||
positions = self.sync_read("Present_Position", target_motors)
|
||||
|
||||
for motor in target_motors:
|
||||
if motor in positions:
|
||||
mins[motor] = min(positions[motor], mins.get(motor, positions[motor]))
|
||||
maxes[motor] = max(positions[motor], maxes.get(motor, positions[motor]))
|
||||
|
||||
if display_values:
|
||||
print("\n" + "=" * 50)
|
||||
print(f"{'MOTOR':<20} | {'MIN (deg)':>12} | {'POS (deg)':>12} | {'MAX (deg)':>12}")
|
||||
print("-" * 50)
|
||||
for motor in target_motors:
|
||||
if motor in positions:
|
||||
print(
|
||||
f"{motor:<20} | {mins[motor]:>12.1f} | {positions[motor]:>12.1f} | {maxes[motor]:>12.1f}"
|
||||
)
|
||||
|
||||
if enter_pressed():
|
||||
user_pressed_enter = True
|
||||
|
||||
if display_values and not user_pressed_enter:
|
||||
move_cursor_up(len(target_motors) + 4)
|
||||
|
||||
time.sleep(LONG_TIMEOUT_SEC)
|
||||
|
||||
self.enable_torque(target_motors)
|
||||
|
||||
for motor in target_motors:
|
||||
if (motor in mins) and (motor in maxes) and (int(abs(maxes[motor] - mins[motor])) < 5):
|
||||
raise ValueError(f"Motor {motor} has insufficient range of motion (< 5 degrees)")
|
||||
|
||||
return mins, maxes
|
||||
|
||||
def _get_motors_list(self, motors: str | list[str] | None) -> list[str]:
|
||||
"""Convert motor specification to list of motor names."""
|
||||
if motors is None:
|
||||
return list(self.motors.keys())
|
||||
elif isinstance(motors, str):
|
||||
return [motors]
|
||||
elif isinstance(motors, list):
|
||||
return motors
|
||||
else:
|
||||
raise TypeError(f"Invalid motors type: {type(motors)}")
|
||||
|
||||
def _get_motor_id(self, motor: NameOrID) -> int:
|
||||
"""Get CAN ID for a motor."""
|
||||
if isinstance(motor, str):
|
||||
if motor in self.motors:
|
||||
return self.motors[motor].id
|
||||
else:
|
||||
raise ValueError(f"Unknown motor: {motor}")
|
||||
else:
|
||||
return motor
|
||||
|
||||
def _get_motor_name(self, motor: NameOrID) -> str:
|
||||
"""Get motor name from name or ID."""
|
||||
if isinstance(motor, str):
|
||||
return motor
|
||||
else:
|
||||
for name, m in self.motors.items():
|
||||
if m.id == motor:
|
||||
return name
|
||||
raise ValueError(f"Unknown motor ID: {motor}")
|
||||
|
||||
def _get_motor_recv_id(self, motor: NameOrID) -> int:
|
||||
"""Get motor recv_id from name or ID."""
|
||||
motor_name = self._get_motor_name(motor)
|
||||
motor_obj = self.motors.get(motor_name)
|
||||
if motor_obj and motor_obj.recv_id is not None:
|
||||
return motor_obj.recv_id
|
||||
else:
|
||||
raise ValueError(f"Motor {motor_obj} doesn't have a valid recv_id (None).")
|
||||
|
||||
@cached_property
|
||||
def is_calibrated(self) -> bool:
|
||||
"""Check if motors are calibrated."""
|
||||
return bool(self.calibration)
|
||||
209
src/lerobot/motors/damiao/tables.py
普通文件
209
src/lerobot/motors/damiao/tables.py
普通文件
@ -0,0 +1,209 @@
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
"""Configuration tables for Damiao motors."""
|
||||
|
||||
from enum import IntEnum
|
||||
|
||||
|
||||
# Motor type definitions
|
||||
class MotorType(IntEnum):
|
||||
DM3507 = 0
|
||||
DM4310 = 1
|
||||
DM4310_48V = 2
|
||||
DM4340 = 3
|
||||
DM4340_48V = 4
|
||||
DM6006 = 5
|
||||
DM8006 = 6
|
||||
DM8009 = 7
|
||||
DM10010L = 8
|
||||
DM10010 = 9
|
||||
DMH3510 = 10
|
||||
DMH6215 = 11
|
||||
DMG6220 = 12
|
||||
|
||||
|
||||
# Control modes
|
||||
class ControlMode(IntEnum):
|
||||
MIT = 1
|
||||
POS_VEL = 2
|
||||
VEL = 3
|
||||
TORQUE_POS = 4
|
||||
|
||||
|
||||
# Motor variable IDs (RID)
|
||||
class MotorVariable(IntEnum):
|
||||
UV_VALUE = 0
|
||||
KT_VALUE = 1
|
||||
OT_VALUE = 2
|
||||
OC_VALUE = 3
|
||||
ACC = 4
|
||||
DEC = 5
|
||||
MAX_SPD = 6
|
||||
MST_ID = 7
|
||||
ESC_ID = 8
|
||||
TIMEOUT = 9
|
||||
CTRL_MODE = 10
|
||||
DAMP = 11
|
||||
INERTIA = 12
|
||||
HW_VER = 13
|
||||
SW_VER = 14
|
||||
SN = 15
|
||||
NPP = 16
|
||||
RS = 17
|
||||
LS = 18
|
||||
FLUX = 19
|
||||
GR = 20
|
||||
PMAX = 21
|
||||
VMAX = 22
|
||||
TMAX = 23
|
||||
I_BW = 24
|
||||
KP_ASR = 25
|
||||
KI_ASR = 26
|
||||
KP_APR = 27
|
||||
KI_APR = 28
|
||||
OV_VALUE = 29
|
||||
GREF = 30
|
||||
DETA = 31
|
||||
V_BW = 32
|
||||
IQ_C1 = 33
|
||||
VL_C1 = 34
|
||||
CAN_BR = 35
|
||||
SUB_VER = 36
|
||||
U_OFF = 50
|
||||
V_OFF = 51
|
||||
K1 = 52
|
||||
K2 = 53
|
||||
M_OFF = 54
|
||||
DIR = 55
|
||||
P_M = 80
|
||||
XOUT = 81
|
||||
|
||||
|
||||
# Motor limit parameters [PMAX, VMAX, TMAX]
|
||||
# PMAX: Maximum position (rad)
|
||||
# VMAX: Maximum velocity (rad/s)
|
||||
# TMAX: Maximum torque (N·m)
|
||||
MOTOR_LIMIT_PARAMS = {
|
||||
MotorType.DM3507: (12.5, 30, 10),
|
||||
MotorType.DM4310: (12.5, 30, 10),
|
||||
MotorType.DM4310_48V: (12.5, 50, 10),
|
||||
MotorType.DM4340: (12.5, 8, 28),
|
||||
MotorType.DM4340_48V: (12.5, 10, 28),
|
||||
MotorType.DM6006: (12.5, 45, 20),
|
||||
MotorType.DM8006: (12.5, 45, 40),
|
||||
MotorType.DM8009: (12.5, 45, 54),
|
||||
MotorType.DM10010L: (12.5, 25, 200),
|
||||
MotorType.DM10010: (12.5, 20, 200),
|
||||
MotorType.DMH3510: (12.5, 280, 1),
|
||||
MotorType.DMH6215: (12.5, 45, 10),
|
||||
MotorType.DMG6220: (12.5, 45, 10),
|
||||
}
|
||||
|
||||
# Motor model names
|
||||
MODEL_NAMES = {
|
||||
MotorType.DM3507: "dm3507",
|
||||
MotorType.DM4310: "dm4310",
|
||||
MotorType.DM4310_48V: "dm4310_48v",
|
||||
MotorType.DM4340: "dm4340",
|
||||
MotorType.DM4340_48V: "dm4340_48v",
|
||||
MotorType.DM6006: "dm6006",
|
||||
MotorType.DM8006: "dm8006",
|
||||
MotorType.DM8009: "dm8009",
|
||||
MotorType.DM10010L: "dm10010l",
|
||||
MotorType.DM10010: "dm10010",
|
||||
MotorType.DMH3510: "dmh3510",
|
||||
MotorType.DMH6215: "dmh6215",
|
||||
MotorType.DMG6220: "dmg6220",
|
||||
}
|
||||
|
||||
# Motor resolution table (encoder counts per revolution)
|
||||
MODEL_RESOLUTION = {
|
||||
"dm3507": 65536,
|
||||
"dm4310": 65536,
|
||||
"dm4310_48v": 65536,
|
||||
"dm4340": 65536,
|
||||
"dm4340_48v": 65536,
|
||||
"dm6006": 65536,
|
||||
"dm8006": 65536,
|
||||
"dm8009": 65536,
|
||||
"dm10010l": 65536,
|
||||
"dm10010": 65536,
|
||||
"dmh3510": 65536,
|
||||
"dmh6215": 65536,
|
||||
"dmg6220": 65536,
|
||||
}
|
||||
|
||||
# CAN baudrates supported by Damiao motors
|
||||
AVAILABLE_BAUDRATES = [
|
||||
125000, # 0: 125 kbps
|
||||
200000, # 1: 200 kbps
|
||||
250000, # 2: 250 kbps
|
||||
500000, # 3: 500 kbps
|
||||
1000000, # 4: 1 mbps (default for OpenArms)
|
||||
2000000, # 5: 2 mbps
|
||||
2500000, # 6: 2.5 mbps
|
||||
3200000, # 7: 3.2 mbps
|
||||
4000000, # 8: 4 mbps
|
||||
5000000, # 9: 5 mbps
|
||||
]
|
||||
DEFAULT_BAUDRATE = 1000000 # 1 Mbps is standard for OpenArms
|
||||
|
||||
# Default timeout in milliseconds
|
||||
DEFAULT_TIMEOUT_MS = 1000
|
||||
|
||||
# OpenArms specific configurations
|
||||
# Based on: https://docs.openarm.dev/software/setup/configure-test
|
||||
# OpenArms has 7 DOF per arm (14 total for dual arm)
|
||||
OPENARMS_ARM_MOTOR_IDS = {
|
||||
"joint_1": {"send": 0x01, "recv": 0x11}, # J1 - Shoulder pan
|
||||
"joint_2": {"send": 0x02, "recv": 0x12}, # J2 - Shoulder lift
|
||||
"joint_3": {"send": 0x03, "recv": 0x13}, # J3 - Elbow flex
|
||||
"joint_4": {"send": 0x04, "recv": 0x14}, # J4 - Wrist flex
|
||||
"joint_5": {"send": 0x05, "recv": 0x15}, # J5 - Wrist roll
|
||||
"joint_6": {"send": 0x06, "recv": 0x16}, # J6 - Wrist pitch
|
||||
"joint_7": {"send": 0x07, "recv": 0x17}, # J7 - Wrist rotation
|
||||
}
|
||||
|
||||
OPENARMS_GRIPPER_MOTOR_IDS = {
|
||||
"gripper": {"send": 0x08, "recv": 0x18}, # J8 - Gripper
|
||||
}
|
||||
|
||||
# Default motor types for OpenArms
|
||||
OPENARMS_DEFAULT_MOTOR_TYPES = {
|
||||
"joint_1": MotorType.DM8009, # Shoulder pan - high torque
|
||||
"joint_2": MotorType.DM8009, # Shoulder lift - high torque
|
||||
"joint_3": MotorType.DM4340, # Shoulder rotation
|
||||
"joint_4": MotorType.DM4340, # Elbow flex
|
||||
"joint_5": MotorType.DM4310, # Wrist roll
|
||||
"joint_6": MotorType.DM4310, # Wrist pitch
|
||||
"joint_7": MotorType.DM4310, # Wrist rotation
|
||||
"gripper": MotorType.DM4310, # Gripper
|
||||
}
|
||||
|
||||
# MIT control parameter ranges
|
||||
MIT_KP_RANGE = (0.0, 500.0)
|
||||
MIT_KD_RANGE = (0.0, 5.0)
|
||||
|
||||
# CAN frame command IDs
|
||||
CAN_CMD_ENABLE = 0xFC
|
||||
CAN_CMD_DISABLE = 0xFD
|
||||
CAN_CMD_SET_ZERO = 0xFE
|
||||
CAN_CMD_REFRESH = 0xCC
|
||||
CAN_CMD_QUERY_PARAM = 0x33
|
||||
CAN_CMD_WRITE_PARAM = 0x55
|
||||
CAN_CMD_SAVE_PARAM = 0xAA
|
||||
|
||||
# CAN ID for parameter operations
|
||||
CAN_PARAM_ID = 0x7FF
|
||||
@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from .dynamixel import DriveMode, DynamixelMotorsBus, OperatingMode, TorqueMode
|
||||
from .tables import *
|
||||
@ -0,0 +1,263 @@
|
||||
# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
# TODO(aliberts): Should we implement FastSyncRead/Write?
|
||||
# https://github.com/ROBOTIS-GIT/DynamixelSDK/pull/643
|
||||
# https://github.com/ROBOTIS-GIT/DynamixelSDK/releases/tag/3.8.2
|
||||
# https://emanual.robotis.com/docs/en/dxl/protocol2/#fast-sync-read-0x8a
|
||||
# -> Need to check compatibility across models
|
||||
|
||||
import logging
|
||||
from copy import deepcopy
|
||||
from enum import Enum
|
||||
|
||||
from ..encoding_utils import decode_twos_complement, encode_twos_complement
|
||||
from ..motors_bus import Motor, MotorCalibration, NameOrID, SerialMotorsBus, Value, get_address
|
||||
from .tables import (
|
||||
AVAILABLE_BAUDRATES,
|
||||
MODEL_BAUDRATE_TABLE,
|
||||
MODEL_CONTROL_TABLE,
|
||||
MODEL_ENCODING_TABLE,
|
||||
MODEL_NUMBER_TABLE,
|
||||
MODEL_RESOLUTION,
|
||||
)
|
||||
|
||||
PROTOCOL_VERSION = 2.0
|
||||
DEFAULT_BAUDRATE = 1_000_000
|
||||
DEFAULT_TIMEOUT_MS = 1000
|
||||
|
||||
NORMALIZED_DATA = ["Goal_Position", "Present_Position"]
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class OperatingMode(Enum):
|
||||
# DYNAMIXEL only controls current(torque) regardless of speed and position. This mode is ideal for a
|
||||
# gripper or a system that only uses current(torque) control or a system that has additional
|
||||
# velocity/position controllers.
|
||||
CURRENT = 0
|
||||
|
||||
# This mode controls velocity. This mode is identical to the Wheel Mode(endless) from existing DYNAMIXEL.
|
||||
# This mode is ideal for wheel-type robots.
|
||||
VELOCITY = 1
|
||||
|
||||
# This mode controls position. This mode is identical to the Joint Mode from existing DYNAMIXEL. Operating
|
||||
# position range is limited by the Max Position Limit(48) and the Min Position Limit(52). This mode is
|
||||
# ideal for articulated robots that each joint rotates less than 360 degrees.
|
||||
POSITION = 3
|
||||
|
||||
# This mode controls position. This mode is identical to the Multi-turn Position Control from existing
|
||||
# DYNAMIXEL. 512 turns are supported(-256[rev] ~ 256[rev]). This mode is ideal for multi-turn wrists or
|
||||
# conveyor systems or a system that requires an additional reduction gear. Note that Max Position
|
||||
# Limit(48), Min Position Limit(52) are not used on Extended Position Control Mode.
|
||||
EXTENDED_POSITION = 4
|
||||
|
||||
# This mode controls both position and current(torque). Up to 512 turns are supported (-256[rev] ~
|
||||
# 256[rev]). This mode is ideal for a system that requires both position and current control such as
|
||||
# articulated robots or grippers.
|
||||
CURRENT_POSITION = 5
|
||||
|
||||
# This mode directly controls PWM output. (Voltage Control Mode)
|
||||
PWM = 16
|
||||
|
||||
|
||||
class DriveMode(Enum):
|
||||
NON_INVERTED = 0
|
||||
INVERTED = 1
|
||||
|
||||
|
||||
class TorqueMode(Enum):
|
||||
ENABLED = 1
|
||||
DISABLED = 0
|
||||
|
||||
|
||||
def _split_into_byte_chunks(value: int, length: int) -> list[int]:
|
||||
import dynamixel_sdk as dxl
|
||||
|
||||
if length == 1:
|
||||
data = [value]
|
||||
elif length == 2:
|
||||
data = [dxl.DXL_LOBYTE(value), dxl.DXL_HIBYTE(value)]
|
||||
elif length == 4:
|
||||
data = [
|
||||
dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
|
||||
dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
|
||||
dxl.DXL_LOBYTE(dxl.DXL_HIWORD(value)),
|
||||
dxl.DXL_HIBYTE(dxl.DXL_HIWORD(value)),
|
||||
]
|
||||
return data
|
||||
|
||||
|
||||
class DynamixelMotorsBus(SerialMotorsBus):
|
||||
"""
|
||||
The Dynamixel implementation for a MotorsBus. It relies on the python dynamixel sdk to communicate with
|
||||
the motors. For more info, see the Dynamixel SDK Documentation:
|
||||
https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20
|
||||
"""
|
||||
|
||||
apply_drive_mode = False
|
||||
available_baudrates = deepcopy(AVAILABLE_BAUDRATES)
|
||||
default_baudrate = DEFAULT_BAUDRATE
|
||||
default_timeout = DEFAULT_TIMEOUT_MS
|
||||
model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
|
||||
model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
|
||||
model_encoding_table = deepcopy(MODEL_ENCODING_TABLE)
|
||||
model_number_table = deepcopy(MODEL_NUMBER_TABLE)
|
||||
model_resolution_table = deepcopy(MODEL_RESOLUTION)
|
||||
normalized_data = deepcopy(NORMALIZED_DATA)
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str,
|
||||
motors: dict[str, Motor],
|
||||
calibration: dict[str, MotorCalibration] | None = None,
|
||||
):
|
||||
super().__init__(port, motors, calibration)
|
||||
import dynamixel_sdk as dxl
|
||||
|
||||
self.port_handler = dxl.PortHandler(self.port)
|
||||
self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION)
|
||||
self.sync_reader = dxl.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0)
|
||||
self.sync_writer = dxl.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0)
|
||||
self._comm_success = dxl.COMM_SUCCESS
|
||||
self._no_error = 0x00
|
||||
|
||||
def _assert_protocol_is_compatible(self, instruction_name: str) -> None:
|
||||
pass
|
||||
|
||||
def _handshake(self) -> None:
|
||||
self._assert_motors_exist()
|
||||
|
||||
def _find_single_motor(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
|
||||
model = self.motors[motor].model
|
||||
search_baudrates = (
|
||||
[initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
|
||||
)
|
||||
|
||||
for baudrate in search_baudrates:
|
||||
self.set_baudrate(baudrate)
|
||||
id_model = self.broadcast_ping()
|
||||
if id_model:
|
||||
found_id, found_model = next(iter(id_model.items()))
|
||||
expected_model_nb = self.model_number_table[model]
|
||||
if found_model != expected_model_nb:
|
||||
raise RuntimeError(
|
||||
f"Found one motor on {baudrate=} with id={found_id} but it has a "
|
||||
f"model number '{found_model}' different than the one expected: '{expected_model_nb}'. "
|
||||
f"Make sure you are connected only connected to the '{motor}' motor (model '{model}')."
|
||||
)
|
||||
return baudrate, found_id
|
||||
|
||||
raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
|
||||
|
||||
def configure_motors(self, return_delay_time=0) -> None:
|
||||
# By default, Dynamixel motors have a 500µs delay response time (corresponding to a value of 250 on
|
||||
# the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
|
||||
for motor in self.motors:
|
||||
self.write("Return_Delay_Time", motor, return_delay_time)
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
return self.calibration == self.read_calibration()
|
||||
|
||||
def read_calibration(self) -> dict[str, MotorCalibration]:
|
||||
offsets = self.sync_read("Homing_Offset", normalize=False)
|
||||
mins = self.sync_read("Min_Position_Limit", normalize=False)
|
||||
maxes = self.sync_read("Max_Position_Limit", normalize=False)
|
||||
drive_modes = self.sync_read("Drive_Mode", normalize=False)
|
||||
|
||||
calibration = {}
|
||||
for motor, m in self.motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=int(drive_modes[motor]),
|
||||
homing_offset=int(offsets[motor]),
|
||||
range_min=int(mins[motor]),
|
||||
range_max=int(maxes[motor]),
|
||||
)
|
||||
|
||||
return calibration
|
||||
|
||||
def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None:
|
||||
for motor, calibration in calibration_dict.items():
|
||||
self.write("Homing_Offset", motor, calibration.homing_offset)
|
||||
self.write("Min_Position_Limit", motor, calibration.range_min)
|
||||
self.write("Max_Position_Limit", motor, calibration.range_max)
|
||||
|
||||
if cache:
|
||||
self.calibration = calibration_dict
|
||||
|
||||
def disable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
|
||||
def _disable_torque(self, motor: int, model: str, num_retry: int = 0) -> None:
|
||||
addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
|
||||
self._write(addr, length, motor, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
|
||||
def enable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.ENABLED.value, num_retry=num_retry)
|
||||
|
||||
def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
|
||||
for id_ in ids_values:
|
||||
model = self._id_to_model(id_)
|
||||
encoding_table = self.model_encoding_table.get(model)
|
||||
if encoding_table and data_name in encoding_table:
|
||||
n_bytes = encoding_table[data_name]
|
||||
ids_values[id_] = encode_twos_complement(ids_values[id_], n_bytes)
|
||||
|
||||
return ids_values
|
||||
|
||||
def _decode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
|
||||
for id_ in ids_values:
|
||||
model = self._id_to_model(id_)
|
||||
encoding_table = self.model_encoding_table.get(model)
|
||||
if encoding_table and data_name in encoding_table:
|
||||
n_bytes = encoding_table[data_name]
|
||||
ids_values[id_] = decode_twos_complement(ids_values[id_], n_bytes)
|
||||
|
||||
return ids_values
|
||||
|
||||
def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
|
||||
"""
|
||||
On Dynamixel Motors:
|
||||
Present_Position = Actual_Position + Homing_Offset
|
||||
"""
|
||||
half_turn_homings: dict[NameOrID, Value] = {}
|
||||
for motor, pos in positions.items():
|
||||
model = self._get_motor_model(motor)
|
||||
max_res = self.model_resolution_table[model] - 1
|
||||
half_turn_homings[motor] = int(max_res / 2) - pos
|
||||
|
||||
return half_turn_homings
|
||||
|
||||
def _split_into_byte_chunks(self, value: int, length: int) -> list[int]:
|
||||
return _split_into_byte_chunks(value, length)
|
||||
|
||||
def broadcast_ping(self, num_retry: int = 0, raise_on_error: bool = False) -> dict[int, int] | None:
|
||||
for n_try in range(1 + num_retry):
|
||||
data_list, comm = self.packet_handler.broadcastPing(self.port_handler)
|
||||
if self._is_comm_success(comm):
|
||||
break
|
||||
logger.debug(f"Broadcast ping failed on port '{self.port}' ({n_try=})")
|
||||
logger.debug(self.packet_handler.getTxRxResult(comm))
|
||||
|
||||
if not self._is_comm_success(comm):
|
||||
if raise_on_error:
|
||||
raise ConnectionError(self.packet_handler.getTxRxResult(comm))
|
||||
|
||||
return None
|
||||
|
||||
return {id_: data[0] for id_, data in data_list.items()}
|
||||
@ -0,0 +1,199 @@
|
||||
# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
# TODO(Steven): Consider doing the following:
|
||||
# from enum import Enum
|
||||
# class MyControlTableKey(Enum):
|
||||
# ID = "ID"
|
||||
# GOAL_SPEED = "Goal_Speed"
|
||||
# ...
|
||||
#
|
||||
# MY_CONTROL_TABLE ={
|
||||
# MyControlTableKey.ID.value: (5,1)
|
||||
# MyControlTableKey.GOAL_SPEED.value: (46, 2)
|
||||
# ...
|
||||
# }
|
||||
# This allows me do to:
|
||||
# bus.write(MyControlTableKey.GOAL_SPEED, ...)
|
||||
# Instead of:
|
||||
# bus.write("Goal_Speed", ...)
|
||||
# This is important for two reasons:
|
||||
# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError
|
||||
# 2. We can change the value of the MyControlTableKey enums without impacting the client code
|
||||
|
||||
|
||||
# {data_name: (address, size_byte)}
|
||||
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table
|
||||
X_SERIES_CONTROL_TABLE = {
|
||||
"Model_Number": (0, 2),
|
||||
"Model_Information": (2, 4),
|
||||
"Firmware_Version": (6, 1),
|
||||
"ID": (7, 1),
|
||||
"Baud_Rate": (8, 1),
|
||||
"Return_Delay_Time": (9, 1),
|
||||
"Drive_Mode": (10, 1),
|
||||
"Operating_Mode": (11, 1),
|
||||
"Secondary_ID": (12, 1),
|
||||
"Protocol_Type": (13, 1),
|
||||
"Homing_Offset": (20, 4),
|
||||
"Moving_Threshold": (24, 4),
|
||||
"Temperature_Limit": (31, 1),
|
||||
"Max_Voltage_Limit": (32, 2),
|
||||
"Min_Voltage_Limit": (34, 2),
|
||||
"PWM_Limit": (36, 2),
|
||||
"Current_Limit": (38, 2),
|
||||
"Acceleration_Limit": (40, 4),
|
||||
"Velocity_Limit": (44, 4),
|
||||
"Max_Position_Limit": (48, 4),
|
||||
"Min_Position_Limit": (52, 4),
|
||||
"Shutdown": (63, 1),
|
||||
"Torque_Enable": (64, 1),
|
||||
"LED": (65, 1),
|
||||
"Status_Return_Level": (68, 1),
|
||||
"Registered_Instruction": (69, 1),
|
||||
"Hardware_Error_Status": (70, 1),
|
||||
"Velocity_I_Gain": (76, 2),
|
||||
"Velocity_P_Gain": (78, 2),
|
||||
"Position_D_Gain": (80, 2),
|
||||
"Position_I_Gain": (82, 2),
|
||||
"Position_P_Gain": (84, 2),
|
||||
"Feedforward_2nd_Gain": (88, 2),
|
||||
"Feedforward_1st_Gain": (90, 2),
|
||||
"Bus_Watchdog": (98, 1),
|
||||
"Goal_PWM": (100, 2),
|
||||
"Goal_Current": (102, 2),
|
||||
"Goal_Velocity": (104, 4),
|
||||
"Profile_Acceleration": (108, 4),
|
||||
"Profile_Velocity": (112, 4),
|
||||
"Goal_Position": (116, 4),
|
||||
"Realtime_Tick": (120, 2),
|
||||
"Moving": (122, 1),
|
||||
"Moving_Status": (123, 1),
|
||||
"Present_PWM": (124, 2),
|
||||
"Present_Current": (126, 2),
|
||||
"Present_Velocity": (128, 4),
|
||||
"Present_Position": (132, 4),
|
||||
"Velocity_Trajectory": (136, 4),
|
||||
"Position_Trajectory": (140, 4),
|
||||
"Present_Input_Voltage": (144, 2),
|
||||
"Present_Temperature": (146, 1),
|
||||
}
|
||||
|
||||
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#baud-rate8
|
||||
X_SERIES_BAUDRATE_TABLE = {
|
||||
9_600: 0,
|
||||
57_600: 1,
|
||||
115_200: 2,
|
||||
1_000_000: 3,
|
||||
2_000_000: 4,
|
||||
3_000_000: 5,
|
||||
4_000_000: 6,
|
||||
}
|
||||
|
||||
# {data_name: size_byte}
|
||||
X_SERIES_ENCODINGS_TABLE = {
|
||||
"Homing_Offset": X_SERIES_CONTROL_TABLE["Homing_Offset"][1],
|
||||
"Goal_PWM": X_SERIES_CONTROL_TABLE["Goal_PWM"][1],
|
||||
"Goal_Current": X_SERIES_CONTROL_TABLE["Goal_Current"][1],
|
||||
"Goal_Velocity": X_SERIES_CONTROL_TABLE["Goal_Velocity"][1],
|
||||
"Goal_Position": X_SERIES_CONTROL_TABLE["Goal_Position"][1],
|
||||
"Present_Position": X_SERIES_CONTROL_TABLE["Present_Position"][1],
|
||||
"Present_PWM": X_SERIES_CONTROL_TABLE["Present_PWM"][1],
|
||||
"Present_Current": X_SERIES_CONTROL_TABLE["Present_Current"][1],
|
||||
"Present_Velocity": X_SERIES_CONTROL_TABLE["Present_Velocity"][1],
|
||||
}
|
||||
|
||||
MODEL_ENCODING_TABLE = {
|
||||
"x_series": X_SERIES_ENCODINGS_TABLE,
|
||||
"xl330-m077": X_SERIES_ENCODINGS_TABLE,
|
||||
"xl330-m288": X_SERIES_ENCODINGS_TABLE,
|
||||
"xl430-w250": X_SERIES_ENCODINGS_TABLE,
|
||||
"xm430-w350": X_SERIES_ENCODINGS_TABLE,
|
||||
"xm540-w270": X_SERIES_ENCODINGS_TABLE,
|
||||
"xc430-w150": X_SERIES_ENCODINGS_TABLE,
|
||||
}
|
||||
|
||||
# {model: model_resolution}
|
||||
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#specifications
|
||||
MODEL_RESOLUTION = {
|
||||
"x_series": 4096,
|
||||
"xl330-m077": 4096,
|
||||
"xl330-m288": 4096,
|
||||
"xl430-w250": 4096,
|
||||
"xm430-w350": 4096,
|
||||
"xm540-w270": 4096,
|
||||
"xc430-w150": 4096,
|
||||
}
|
||||
|
||||
# {model: model_number}
|
||||
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table-of-eeprom-area
|
||||
MODEL_NUMBER_TABLE = {
|
||||
"xl330-m077": 1190,
|
||||
"xl330-m288": 1200,
|
||||
"xl430-w250": 1060,
|
||||
"xm430-w350": 1020,
|
||||
"xm540-w270": 1120,
|
||||
"xc430-w150": 1070,
|
||||
}
|
||||
|
||||
# {model: available_operating_modes}
|
||||
# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#operating-mode11
|
||||
MODEL_OPERATING_MODES = {
|
||||
"xl330-m077": [0, 1, 3, 4, 5, 16],
|
||||
"xl330-m288": [0, 1, 3, 4, 5, 16],
|
||||
"xl430-w250": [1, 3, 4, 16],
|
||||
"xm430-w350": [0, 1, 3, 4, 5, 16],
|
||||
"xm540-w270": [0, 1, 3, 4, 5, 16],
|
||||
"xc430-w150": [1, 3, 4, 16],
|
||||
}
|
||||
|
||||
MODEL_CONTROL_TABLE = {
|
||||
"x_series": X_SERIES_CONTROL_TABLE,
|
||||
"xl330-m077": X_SERIES_CONTROL_TABLE,
|
||||
"xl330-m288": X_SERIES_CONTROL_TABLE,
|
||||
"xl430-w250": X_SERIES_CONTROL_TABLE,
|
||||
"xm430-w350": X_SERIES_CONTROL_TABLE,
|
||||
"xm540-w270": X_SERIES_CONTROL_TABLE,
|
||||
"xc430-w150": X_SERIES_CONTROL_TABLE,
|
||||
}
|
||||
|
||||
MODEL_BAUDRATE_TABLE = {
|
||||
"x_series": X_SERIES_BAUDRATE_TABLE,
|
||||
"xl330-m077": X_SERIES_BAUDRATE_TABLE,
|
||||
"xl330-m288": X_SERIES_BAUDRATE_TABLE,
|
||||
"xl430-w250": X_SERIES_BAUDRATE_TABLE,
|
||||
"xm430-w350": X_SERIES_BAUDRATE_TABLE,
|
||||
"xm540-w270": X_SERIES_BAUDRATE_TABLE,
|
||||
"xc430-w150": X_SERIES_BAUDRATE_TABLE,
|
||||
}
|
||||
|
||||
AVAILABLE_BAUDRATES = [
|
||||
9_600,
|
||||
19_200,
|
||||
38_400,
|
||||
57_600,
|
||||
115_200,
|
||||
230_400,
|
||||
460_800,
|
||||
500_000,
|
||||
576_000,
|
||||
921_600,
|
||||
1_000_000,
|
||||
1_152_000,
|
||||
2_000_000,
|
||||
2_500_000,
|
||||
3_000_000,
|
||||
3_500_000,
|
||||
4_000_000,
|
||||
]
|
||||
@ -0,0 +1,67 @@
|
||||
# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
|
||||
def encode_sign_magnitude(value: int, sign_bit_index: int):
|
||||
"""
|
||||
https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
|
||||
"""
|
||||
max_magnitude = (1 << sign_bit_index) - 1
|
||||
magnitude = abs(value)
|
||||
if magnitude > max_magnitude:
|
||||
raise ValueError(f"Magnitude {magnitude} exceeds {max_magnitude} (max for {sign_bit_index=})")
|
||||
|
||||
direction_bit = 1 if value < 0 else 0
|
||||
return (direction_bit << sign_bit_index) | magnitude
|
||||
|
||||
|
||||
def decode_sign_magnitude(encoded_value: int, sign_bit_index: int):
|
||||
"""
|
||||
https://en.wikipedia.org/wiki/Signed_number_representations#Sign%E2%80%93magnitude
|
||||
"""
|
||||
direction_bit = (encoded_value >> sign_bit_index) & 1
|
||||
magnitude_mask = (1 << sign_bit_index) - 1
|
||||
magnitude = encoded_value & magnitude_mask
|
||||
return -magnitude if direction_bit else magnitude
|
||||
|
||||
|
||||
def encode_twos_complement(value: int, n_bytes: int):
|
||||
"""
|
||||
https://en.wikipedia.org/wiki/Signed_number_representations#Two%27s_complement
|
||||
"""
|
||||
|
||||
bit_width = n_bytes * 8
|
||||
min_val = -(1 << (bit_width - 1))
|
||||
max_val = (1 << (bit_width - 1)) - 1
|
||||
|
||||
if not (min_val <= value <= max_val):
|
||||
raise ValueError(
|
||||
f"Value {value} out of range for {n_bytes}-byte two's complement: [{min_val}, {max_val}]"
|
||||
)
|
||||
|
||||
if value >= 0:
|
||||
return value
|
||||
|
||||
return (1 << bit_width) + value
|
||||
|
||||
|
||||
def decode_twos_complement(value: int, n_bytes: int) -> int:
|
||||
"""
|
||||
https://en.wikipedia.org/wiki/Signed_number_representations#Two%27s_complement
|
||||
"""
|
||||
bits = n_bytes * 8
|
||||
sign_bit = 1 << (bits - 1)
|
||||
if value & sign_bit:
|
||||
value -= 1 << bits
|
||||
return value
|
||||
@ -0,0 +1,18 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from .feetech import DriveMode, FeetechMotorsBus, OperatingMode, TorqueMode
|
||||
from .tables import *
|
||||
@ -0,0 +1,454 @@
|
||||
# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
import logging
|
||||
from copy import deepcopy
|
||||
from enum import Enum
|
||||
from pprint import pformat
|
||||
|
||||
from ..encoding_utils import decode_sign_magnitude, encode_sign_magnitude
|
||||
from ..motors_bus import Motor, MotorCalibration, NameOrID, SerialMotorsBus, Value, get_address
|
||||
from .tables import (
|
||||
FIRMWARE_MAJOR_VERSION,
|
||||
FIRMWARE_MINOR_VERSION,
|
||||
MODEL_BAUDRATE_TABLE,
|
||||
MODEL_CONTROL_TABLE,
|
||||
MODEL_ENCODING_TABLE,
|
||||
MODEL_NUMBER,
|
||||
MODEL_NUMBER_TABLE,
|
||||
MODEL_PROTOCOL,
|
||||
MODEL_RESOLUTION,
|
||||
SCAN_BAUDRATES,
|
||||
)
|
||||
|
||||
DEFAULT_PROTOCOL_VERSION = 0
|
||||
DEFAULT_BAUDRATE = 1_000_000
|
||||
DEFAULT_TIMEOUT_MS = 1000
|
||||
|
||||
NORMALIZED_DATA = ["Goal_Position", "Present_Position"]
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class OperatingMode(Enum):
|
||||
# position servo mode
|
||||
POSITION = 0
|
||||
# The motor is in constant speed mode, which is controlled by parameter 0x2e, and the highest bit 15 is
|
||||
# the direction bit
|
||||
VELOCITY = 1
|
||||
# PWM open-loop speed regulation mode, with parameter 0x2c running time parameter control, bit11 as
|
||||
# direction bit
|
||||
PWM = 2
|
||||
# In step servo mode, the number of step progress is represented by parameter 0x2a, and the highest bit 15
|
||||
# is the direction bit
|
||||
STEP = 3
|
||||
|
||||
|
||||
class DriveMode(Enum):
|
||||
NON_INVERTED = 0
|
||||
INVERTED = 1
|
||||
|
||||
|
||||
class TorqueMode(Enum):
|
||||
ENABLED = 1
|
||||
DISABLED = 0
|
||||
|
||||
|
||||
def _split_into_byte_chunks(value: int, length: int) -> list[int]:
|
||||
import scservo_sdk as scs
|
||||
|
||||
if length == 1:
|
||||
data = [value]
|
||||
elif length == 2:
|
||||
data = [scs.SCS_LOBYTE(value), scs.SCS_HIBYTE(value)]
|
||||
elif length == 4:
|
||||
data = [
|
||||
scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
|
||||
scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
|
||||
scs.SCS_LOBYTE(scs.SCS_HIWORD(value)),
|
||||
scs.SCS_HIBYTE(scs.SCS_HIWORD(value)),
|
||||
]
|
||||
return data
|
||||
|
||||
|
||||
def patch_setPacketTimeout(self, packet_length): # noqa: N802
|
||||
"""
|
||||
HACK: This patches the PortHandler behavior to set the correct packet timeouts.
|
||||
|
||||
It fixes https://gitee.com/ftservo/SCServoSDK/issues/IBY2S6
|
||||
The bug is fixed on the official Feetech SDK repo (https://gitee.com/ftservo/FTServo_Python)
|
||||
but because that version is not published on PyPI, we rely on the (unofficial) on that is, which needs
|
||||
patching.
|
||||
"""
|
||||
self.packet_start_time = self.getCurrentTime()
|
||||
self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + 50
|
||||
|
||||
|
||||
class FeetechMotorsBus(SerialMotorsBus):
|
||||
"""
|
||||
The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on the
|
||||
python feetech sdk to communicate with the motors, which is itself based on the dynamixel sdk.
|
||||
"""
|
||||
|
||||
apply_drive_mode = True
|
||||
available_baudrates = deepcopy(SCAN_BAUDRATES)
|
||||
default_baudrate = DEFAULT_BAUDRATE
|
||||
default_timeout = DEFAULT_TIMEOUT_MS
|
||||
model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
|
||||
model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
|
||||
model_encoding_table = deepcopy(MODEL_ENCODING_TABLE)
|
||||
model_number_table = deepcopy(MODEL_NUMBER_TABLE)
|
||||
model_resolution_table = deepcopy(MODEL_RESOLUTION)
|
||||
normalized_data = deepcopy(NORMALIZED_DATA)
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
port: str,
|
||||
motors: dict[str, Motor],
|
||||
calibration: dict[str, MotorCalibration] | None = None,
|
||||
protocol_version: int = DEFAULT_PROTOCOL_VERSION,
|
||||
):
|
||||
super().__init__(port, motors, calibration)
|
||||
self.protocol_version = protocol_version
|
||||
self._assert_same_protocol()
|
||||
import scservo_sdk as scs
|
||||
|
||||
self.port_handler = scs.PortHandler(self.port)
|
||||
# HACK: monkeypatch
|
||||
self.port_handler.setPacketTimeout = patch_setPacketTimeout.__get__( # type: ignore[method-assign]
|
||||
self.port_handler, scs.PortHandler
|
||||
)
|
||||
self.packet_handler = scs.PacketHandler(protocol_version)
|
||||
self.sync_reader = scs.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0)
|
||||
self.sync_writer = scs.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0)
|
||||
self._comm_success = scs.COMM_SUCCESS
|
||||
self._no_error = 0x00
|
||||
|
||||
if any(MODEL_PROTOCOL[model] != self.protocol_version for model in self.models):
|
||||
raise ValueError(f"Some motors are incompatible with protocol_version={self.protocol_version}")
|
||||
|
||||
def _assert_same_protocol(self) -> None:
|
||||
if any(MODEL_PROTOCOL[model] != self.protocol_version for model in self.models):
|
||||
raise RuntimeError("Some motors use an incompatible protocol.")
|
||||
|
||||
def _assert_protocol_is_compatible(self, instruction_name: str) -> None:
|
||||
if instruction_name == "sync_read" and self.protocol_version == 1:
|
||||
raise NotImplementedError(
|
||||
"'Sync Read' is not available with Feetech motors using Protocol 1. Use 'Read' sequentially instead."
|
||||
)
|
||||
if instruction_name == "broadcast_ping" and self.protocol_version == 1:
|
||||
raise NotImplementedError(
|
||||
"'Broadcast Ping' is not available with Feetech motors using Protocol 1. Use 'Ping' sequentially instead."
|
||||
)
|
||||
|
||||
def _assert_same_firmware(self) -> None:
|
||||
firmware_versions = self._read_firmware_version(self.ids, raise_on_error=True)
|
||||
if len(set(firmware_versions.values())) != 1:
|
||||
raise RuntimeError(
|
||||
"Some Motors use different firmware versions:"
|
||||
f"\n{pformat(firmware_versions)}\n"
|
||||
"Update their firmware first using Feetech's software. "
|
||||
"Visit https://www.feetechrc.com/software."
|
||||
)
|
||||
|
||||
def _handshake(self) -> None:
|
||||
self._assert_motors_exist()
|
||||
self._assert_same_firmware()
|
||||
|
||||
def _find_single_motor(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
|
||||
if self.protocol_version == 0:
|
||||
return self._find_single_motor_p0(motor, initial_baudrate)
|
||||
else:
|
||||
return self._find_single_motor_p1(motor, initial_baudrate)
|
||||
|
||||
def _find_single_motor_p0(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
|
||||
model = self.motors[motor].model
|
||||
search_baudrates = (
|
||||
[initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
|
||||
)
|
||||
expected_model_nb = self.model_number_table[model]
|
||||
|
||||
for baudrate in search_baudrates:
|
||||
self.set_baudrate(baudrate)
|
||||
id_model = self.broadcast_ping()
|
||||
if id_model:
|
||||
found_id, found_model = next(iter(id_model.items()))
|
||||
if found_model != expected_model_nb:
|
||||
raise RuntimeError(
|
||||
f"Found one motor on {baudrate=} with id={found_id} but it has a "
|
||||
f"model number '{found_model}' different than the one expected: '{expected_model_nb}'. "
|
||||
f"Make sure you are connected only connected to the '{motor}' motor (model '{model}')."
|
||||
)
|
||||
return baudrate, found_id
|
||||
|
||||
raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
|
||||
|
||||
def _find_single_motor_p1(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
|
||||
import scservo_sdk as scs
|
||||
|
||||
model = self.motors[motor].model
|
||||
search_baudrates = (
|
||||
[initial_baudrate] if initial_baudrate is not None else self.model_baudrate_table[model]
|
||||
)
|
||||
expected_model_nb = self.model_number_table[model]
|
||||
|
||||
for baudrate in search_baudrates:
|
||||
self.set_baudrate(baudrate)
|
||||
for id_ in range(scs.MAX_ID + 1):
|
||||
found_model = self.ping(id_)
|
||||
if found_model is not None:
|
||||
if found_model != expected_model_nb:
|
||||
raise RuntimeError(
|
||||
f"Found one motor on {baudrate=} with id={id_} but it has a "
|
||||
f"model number '{found_model}' different than the one expected: '{expected_model_nb}'. "
|
||||
f"Make sure you are connected only connected to the '{motor}' motor (model '{model}')."
|
||||
)
|
||||
return baudrate, id_
|
||||
|
||||
raise RuntimeError(f"Motor '{motor}' (model '{model}') was not found. Make sure it is connected.")
|
||||
|
||||
def configure_motors(self, return_delay_time=0, maximum_acceleration=254, acceleration=254) -> None:
|
||||
for motor in self.motors:
|
||||
# By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on
|
||||
# the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
|
||||
self.write("Return_Delay_Time", motor, return_delay_time)
|
||||
# Set 'Maximum_Acceleration' to 254 to speedup acceleration and deceleration of the motors.
|
||||
if self.protocol_version == 0:
|
||||
self.write("Maximum_Acceleration", motor, maximum_acceleration)
|
||||
self.write("Acceleration", motor, acceleration)
|
||||
|
||||
@property
|
||||
def is_calibrated(self) -> bool:
|
||||
motors_calibration = self.read_calibration()
|
||||
if set(motors_calibration) != set(self.calibration):
|
||||
return False
|
||||
|
||||
same_ranges = all(
|
||||
self.calibration[motor].range_min == cal.range_min
|
||||
and self.calibration[motor].range_max == cal.range_max
|
||||
for motor, cal in motors_calibration.items()
|
||||
)
|
||||
if self.protocol_version == 1:
|
||||
return same_ranges
|
||||
|
||||
same_offsets = all(
|
||||
self.calibration[motor].homing_offset == cal.homing_offset
|
||||
for motor, cal in motors_calibration.items()
|
||||
)
|
||||
return same_ranges and same_offsets
|
||||
|
||||
def read_calibration(self) -> dict[str, MotorCalibration]:
|
||||
offsets, mins, maxes = {}, {}, {}
|
||||
for motor in self.motors:
|
||||
mins[motor] = self.read("Min_Position_Limit", motor, normalize=False)
|
||||
maxes[motor] = self.read("Max_Position_Limit", motor, normalize=False)
|
||||
offsets[motor] = (
|
||||
self.read("Homing_Offset", motor, normalize=False) if self.protocol_version == 0 else 0
|
||||
)
|
||||
|
||||
calibration = {}
|
||||
for motor, m in self.motors.items():
|
||||
calibration[motor] = MotorCalibration(
|
||||
id=m.id,
|
||||
drive_mode=0,
|
||||
homing_offset=int(offsets[motor]),
|
||||
range_min=int(mins[motor]),
|
||||
range_max=int(maxes[motor]),
|
||||
)
|
||||
|
||||
return calibration
|
||||
|
||||
def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None:
|
||||
for motor, calibration in calibration_dict.items():
|
||||
if self.protocol_version == 0:
|
||||
self.write("Homing_Offset", motor, calibration.homing_offset)
|
||||
self.write("Min_Position_Limit", motor, calibration.range_min)
|
||||
self.write("Max_Position_Limit", motor, calibration.range_max)
|
||||
|
||||
if cache:
|
||||
self.calibration = calibration_dict
|
||||
|
||||
def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
|
||||
"""
|
||||
On Feetech Motors:
|
||||
Present_Position = Actual_Position - Homing_Offset
|
||||
"""
|
||||
half_turn_homings: dict[NameOrID, Value] = {}
|
||||
for motor, pos in positions.items():
|
||||
model = self._get_motor_model(motor)
|
||||
max_res = self.model_resolution_table[model] - 1
|
||||
half_turn_homings[motor] = pos - int(max_res / 2)
|
||||
|
||||
return half_turn_homings
|
||||
|
||||
def disable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
self.write("Lock", motor, 0, num_retry=num_retry)
|
||||
|
||||
def _disable_torque(self, motor: int, model: str, num_retry: int = 0) -> None:
|
||||
addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
|
||||
self._write(addr, length, motor, TorqueMode.DISABLED.value, num_retry=num_retry)
|
||||
addr, length = get_address(self.model_ctrl_table, model, "Lock")
|
||||
self._write(addr, length, motor, 0, num_retry=num_retry)
|
||||
|
||||
def enable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0) -> None:
|
||||
for motor in self._get_motors_list(motors):
|
||||
self.write("Torque_Enable", motor, TorqueMode.ENABLED.value, num_retry=num_retry)
|
||||
self.write("Lock", motor, 1, num_retry=num_retry)
|
||||
|
||||
def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
|
||||
for id_ in ids_values:
|
||||
model = self._id_to_model(id_)
|
||||
encoding_table = self.model_encoding_table.get(model)
|
||||
if encoding_table and data_name in encoding_table:
|
||||
sign_bit = encoding_table[data_name]
|
||||
ids_values[id_] = encode_sign_magnitude(ids_values[id_], sign_bit)
|
||||
|
||||
return ids_values
|
||||
|
||||
def _decode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
|
||||
for id_ in ids_values:
|
||||
model = self._id_to_model(id_)
|
||||
encoding_table = self.model_encoding_table.get(model)
|
||||
if encoding_table and data_name in encoding_table:
|
||||
sign_bit = encoding_table[data_name]
|
||||
ids_values[id_] = decode_sign_magnitude(ids_values[id_], sign_bit)
|
||||
|
||||
return ids_values
|
||||
|
||||
def _split_into_byte_chunks(self, value: int, length: int) -> list[int]:
|
||||
return _split_into_byte_chunks(value, length)
|
||||
|
||||
def _broadcast_ping(self) -> tuple[dict[int, int], int]:
|
||||
import scservo_sdk as scs
|
||||
|
||||
data_list: dict[int, int] = {}
|
||||
|
||||
status_length = 6
|
||||
|
||||
rx_length = 0
|
||||
wait_length = status_length * scs.MAX_ID
|
||||
|
||||
txpacket = [0] * 6
|
||||
|
||||
tx_time_per_byte = (1000.0 / self.port_handler.getBaudRate()) * 10.0
|
||||
|
||||
txpacket[scs.PKT_ID] = scs.BROADCAST_ID
|
||||
txpacket[scs.PKT_LENGTH] = 2
|
||||
txpacket[scs.PKT_INSTRUCTION] = scs.INST_PING
|
||||
|
||||
result = self.packet_handler.txPacket(self.port_handler, txpacket)
|
||||
if result != scs.COMM_SUCCESS:
|
||||
self.port_handler.is_using = False
|
||||
return data_list, result
|
||||
|
||||
# set rx timeout
|
||||
self.port_handler.setPacketTimeoutMillis((wait_length * tx_time_per_byte) + (3.0 * scs.MAX_ID) + 16.0)
|
||||
|
||||
rxpacket = []
|
||||
while not self.port_handler.isPacketTimeout() and rx_length < wait_length:
|
||||
rxpacket += self.port_handler.readPort(wait_length - rx_length)
|
||||
rx_length = len(rxpacket)
|
||||
|
||||
self.port_handler.is_using = False
|
||||
|
||||
if rx_length == 0:
|
||||
return data_list, scs.COMM_RX_TIMEOUT
|
||||
|
||||
while True:
|
||||
if rx_length < status_length:
|
||||
return data_list, scs.COMM_RX_CORRUPT
|
||||
|
||||
# find packet header
|
||||
for idx in range(0, (rx_length - 1)):
|
||||
if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF):
|
||||
break
|
||||
|
||||
if idx == 0: # found at the beginning of the packet
|
||||
# calculate checksum
|
||||
checksum = 0
|
||||
for idx in range(2, status_length - 1): # except header & checksum
|
||||
checksum += rxpacket[idx]
|
||||
|
||||
checksum = ~checksum & 0xFF
|
||||
if rxpacket[status_length - 1] == checksum:
|
||||
result = scs.COMM_SUCCESS
|
||||
data_list[rxpacket[scs.PKT_ID]] = rxpacket[scs.PKT_ERROR]
|
||||
|
||||
del rxpacket[0:status_length]
|
||||
rx_length = rx_length - status_length
|
||||
|
||||
if rx_length == 0:
|
||||
return data_list, result
|
||||
else:
|
||||
result = scs.COMM_RX_CORRUPT
|
||||
# remove header (0xFF 0xFF)
|
||||
del rxpacket[0:2]
|
||||
rx_length = rx_length - 2
|
||||
else:
|
||||
# remove unnecessary packets
|
||||
del rxpacket[0:idx]
|
||||
rx_length = rx_length - idx
|
||||
|
||||
def broadcast_ping(self, num_retry: int = 0, raise_on_error: bool = False) -> dict[int, int] | None:
|
||||
self._assert_protocol_is_compatible("broadcast_ping")
|
||||
for n_try in range(1 + num_retry):
|
||||
ids_status, comm = self._broadcast_ping()
|
||||
if self._is_comm_success(comm):
|
||||
break
|
||||
logger.debug(f"Broadcast ping failed on port '{self.port}' ({n_try=})")
|
||||
logger.debug(self.packet_handler.getTxRxResult(comm))
|
||||
|
||||
if not self._is_comm_success(comm):
|
||||
if raise_on_error:
|
||||
raise ConnectionError(self.packet_handler.getTxRxResult(comm))
|
||||
return None
|
||||
|
||||
ids_errors = {id_: status for id_, status in ids_status.items() if self._is_error(status)}
|
||||
if ids_errors:
|
||||
display_dict = {id_: self.packet_handler.getRxPacketError(err) for id_, err in ids_errors.items()}
|
||||
logger.error(f"Some motors found returned an error status:\n{pformat(display_dict, indent=4)}")
|
||||
|
||||
return self._read_model_number(list(ids_status), raise_on_error)
|
||||
|
||||
def _read_firmware_version(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, str]:
|
||||
firmware_versions = {}
|
||||
for id_ in motor_ids:
|
||||
firm_ver_major, comm, error = self._read(
|
||||
*FIRMWARE_MAJOR_VERSION, id_, raise_on_error=raise_on_error
|
||||
)
|
||||
if not self._is_comm_success(comm) or self._is_error(error):
|
||||
continue
|
||||
|
||||
firm_ver_minor, comm, error = self._read(
|
||||
*FIRMWARE_MINOR_VERSION, id_, raise_on_error=raise_on_error
|
||||
)
|
||||
if not self._is_comm_success(comm) or self._is_error(error):
|
||||
continue
|
||||
|
||||
firmware_versions[id_] = f"{firm_ver_major}.{firm_ver_minor}"
|
||||
|
||||
return firmware_versions
|
||||
|
||||
def _read_model_number(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, int]:
|
||||
model_numbers = {}
|
||||
for id_ in motor_ids:
|
||||
model_nb, comm, error = self._read(*MODEL_NUMBER, id_, raise_on_error=raise_on_error)
|
||||
if not self._is_comm_success(comm) or self._is_error(error):
|
||||
continue
|
||||
|
||||
model_numbers[id_] = model_nb
|
||||
|
||||
return model_numbers
|
||||
257
src/lerobot/motors/feetech/tables.py
普通文件
257
src/lerobot/motors/feetech/tables.py
普通文件
@ -0,0 +1,257 @@
|
||||
# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
FIRMWARE_MAJOR_VERSION = (0, 1)
|
||||
FIRMWARE_MINOR_VERSION = (1, 1)
|
||||
MODEL_NUMBER = (3, 2)
|
||||
|
||||
# TODO(Steven): Consider doing the following:
|
||||
# from enum import Enum
|
||||
# class MyControlTableKey(Enum):
|
||||
# ID = "ID"
|
||||
# GOAL_SPEED = "Goal_Speed"
|
||||
# ...
|
||||
#
|
||||
# MY_CONTROL_TABLE ={
|
||||
# MyControlTableKey.ID.value: (5,1)
|
||||
# MyControlTableKey.GOAL_SPEED.value: (46, 2)
|
||||
# ...
|
||||
# }
|
||||
# This allows me do to:
|
||||
# bus.write(MyControlTableKey.GOAL_SPEED, ...)
|
||||
# Instead of:
|
||||
# bus.write("Goal_Speed", ...)
|
||||
# This is important for two reasons:
|
||||
# 1. The linter will tell me if I'm trying to use an invalid key, instead of me realizing when I get the RunTimeError
|
||||
# 2. We can change the value of the MyControlTableKey enums without impacting the client code
|
||||
|
||||
# data_name: (address, size_byte)
|
||||
# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0
|
||||
STS_SMS_SERIES_CONTROL_TABLE = {
|
||||
# EPROM
|
||||
"Firmware_Major_Version": FIRMWARE_MAJOR_VERSION, # read-only
|
||||
"Firmware_Minor_Version": FIRMWARE_MINOR_VERSION, # read-only
|
||||
"Model_Number": MODEL_NUMBER, # read-only
|
||||
"ID": (5, 1),
|
||||
"Baud_Rate": (6, 1),
|
||||
"Return_Delay_Time": (7, 1),
|
||||
"Response_Status_Level": (8, 1),
|
||||
"Min_Position_Limit": (9, 2),
|
||||
"Max_Position_Limit": (11, 2),
|
||||
"Max_Temperature_Limit": (13, 1),
|
||||
"Max_Voltage_Limit": (14, 1),
|
||||
"Min_Voltage_Limit": (15, 1),
|
||||
"Max_Torque_Limit": (16, 2),
|
||||
"Phase": (18, 1),
|
||||
"Unloading_Condition": (19, 1),
|
||||
"LED_Alarm_Condition": (20, 1),
|
||||
"P_Coefficient": (21, 1),
|
||||
"D_Coefficient": (22, 1),
|
||||
"I_Coefficient": (23, 1),
|
||||
"Minimum_Startup_Force": (24, 2),
|
||||
"CW_Dead_Zone": (26, 1),
|
||||
"CCW_Dead_Zone": (27, 1),
|
||||
"Protection_Current": (28, 2),
|
||||
"Angular_Resolution": (30, 1),
|
||||
"Homing_Offset": (31, 2),
|
||||
"Operating_Mode": (33, 1),
|
||||
"Protective_Torque": (34, 1),
|
||||
"Protection_Time": (35, 1),
|
||||
"Overload_Torque": (36, 1),
|
||||
"Velocity_closed_loop_P_proportional_coefficient": (37, 1),
|
||||
"Over_Current_Protection_Time": (38, 1),
|
||||
"Velocity_closed_loop_I_integral_coefficient": (39, 1),
|
||||
# SRAM
|
||||
"Torque_Enable": (40, 1),
|
||||
"Acceleration": (41, 1),
|
||||
"Goal_Position": (42, 2),
|
||||
"Goal_Time": (44, 2),
|
||||
"Goal_Velocity": (46, 2),
|
||||
"Torque_Limit": (48, 2),
|
||||
"Lock": (55, 1),
|
||||
"Present_Position": (56, 2), # read-only
|
||||
"Present_Velocity": (58, 2), # read-only
|
||||
"Present_Load": (60, 2), # read-only
|
||||
"Present_Voltage": (62, 1), # read-only
|
||||
"Present_Temperature": (63, 1), # read-only
|
||||
"Status": (65, 1), # read-only
|
||||
"Moving": (66, 1), # read-only
|
||||
"Present_Current": (69, 2), # read-only
|
||||
"Goal_Position_2": (71, 2), # read-only
|
||||
# Factory
|
||||
"Moving_Velocity": (80, 1),
|
||||
"Moving_Velocity_Threshold": (80, 1),
|
||||
"DTs": (81, 1), # (ms)
|
||||
"Velocity_Unit_factor": (82, 1),
|
||||
"Hts": (83, 1), # (ns) valid for firmware >= 2.54, other versions keep 0
|
||||
"Maximum_Velocity_Limit": (84, 1),
|
||||
"Maximum_Acceleration": (85, 1),
|
||||
"Acceleration_Multiplier ": (86, 1), # Acceleration multiplier in effect when acceleration is 0
|
||||
}
|
||||
|
||||
# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SCSCL-emanual-cbcc8ab2e3384282a01d4bf3
|
||||
SCS_SERIES_CONTROL_TABLE = {
|
||||
# EPROM
|
||||
"Firmware_Major_Version": FIRMWARE_MAJOR_VERSION, # read-only
|
||||
"Firmware_Minor_Version": FIRMWARE_MINOR_VERSION, # read-only
|
||||
"Model_Number": MODEL_NUMBER, # read-only
|
||||
"ID": (5, 1),
|
||||
"Baud_Rate": (6, 1),
|
||||
"Return_Delay_Time": (7, 1),
|
||||
"Response_Status_Level": (8, 1),
|
||||
"Min_Position_Limit": (9, 2),
|
||||
"Max_Position_Limit": (11, 2),
|
||||
"Max_Temperature_Limit": (13, 1),
|
||||
"Max_Voltage_Limit": (14, 1),
|
||||
"Min_Voltage_Limit": (15, 1),
|
||||
"Max_Torque_Limit": (16, 2),
|
||||
"Phase": (18, 1),
|
||||
"Unloading_Condition": (19, 1),
|
||||
"LED_Alarm_Condition": (20, 1),
|
||||
"P_Coefficient": (21, 1),
|
||||
"D_Coefficient": (22, 1),
|
||||
"I_Coefficient": (23, 1),
|
||||
"Minimum_Startup_Force": (24, 2),
|
||||
"CW_Dead_Zone": (26, 1),
|
||||
"CCW_Dead_Zone": (27, 1),
|
||||
"Protective_Torque": (37, 1),
|
||||
"Protection_Time": (38, 1),
|
||||
# SRAM
|
||||
"Torque_Enable": (40, 1),
|
||||
"Acceleration": (41, 1),
|
||||
"Goal_Position": (42, 2),
|
||||
"Running_Time": (44, 2),
|
||||
"Goal_Velocity": (46, 2),
|
||||
"Lock": (48, 1),
|
||||
"Present_Position": (56, 2), # read-only
|
||||
"Present_Velocity": (58, 2), # read-only
|
||||
"Present_Load": (60, 2), # read-only
|
||||
"Present_Voltage": (62, 1), # read-only
|
||||
"Present_Temperature": (63, 1), # read-only
|
||||
"Sync_Write_Flag": (64, 1), # read-only
|
||||
"Status": (65, 1), # read-only
|
||||
"Moving": (66, 1), # read-only
|
||||
# Factory
|
||||
"PWM_Maximum_Step": (78, 1),
|
||||
"Moving_Velocity_Threshold*50": (79, 1),
|
||||
"DTs": (80, 1), # (ms)
|
||||
"Minimum_Velocity_Limit*50": (81, 1),
|
||||
"Maximum_Velocity_Limit*50": (82, 1),
|
||||
"Acceleration_2": (83, 1), # don't know what that is
|
||||
}
|
||||
|
||||
STS_SMS_SERIES_BAUDRATE_TABLE = {
|
||||
1_000_000: 0,
|
||||
500_000: 1,
|
||||
250_000: 2,
|
||||
128_000: 3,
|
||||
115_200: 4,
|
||||
57_600: 5,
|
||||
38_400: 6,
|
||||
19_200: 7,
|
||||
}
|
||||
|
||||
SCS_SERIES_BAUDRATE_TABLE = {
|
||||
1_000_000: 0,
|
||||
500_000: 1,
|
||||
250_000: 2,
|
||||
128_000: 3,
|
||||
115_200: 4,
|
||||
57_600: 5,
|
||||
38_400: 6,
|
||||
19_200: 7,
|
||||
}
|
||||
|
||||
MODEL_CONTROL_TABLE = {
|
||||
"sts_series": STS_SMS_SERIES_CONTROL_TABLE,
|
||||
"scs_series": SCS_SERIES_CONTROL_TABLE,
|
||||
"sms_series": STS_SMS_SERIES_CONTROL_TABLE,
|
||||
"sts3215": STS_SMS_SERIES_CONTROL_TABLE,
|
||||
"sts3250": STS_SMS_SERIES_CONTROL_TABLE,
|
||||
"scs0009": SCS_SERIES_CONTROL_TABLE,
|
||||
"sm8512bl": STS_SMS_SERIES_CONTROL_TABLE,
|
||||
}
|
||||
|
||||
MODEL_RESOLUTION = {
|
||||
"sts_series": 4096,
|
||||
"sms_series": 4096,
|
||||
"scs_series": 1024,
|
||||
"sts3215": 4096,
|
||||
"sts3250": 4096,
|
||||
"sm8512bl": 4096,
|
||||
"scs0009": 1024,
|
||||
}
|
||||
|
||||
MODEL_BAUDRATE_TABLE = {
|
||||
"sts_series": STS_SMS_SERIES_BAUDRATE_TABLE,
|
||||
"sms_series": STS_SMS_SERIES_BAUDRATE_TABLE,
|
||||
"scs_series": SCS_SERIES_BAUDRATE_TABLE,
|
||||
"sm8512bl": STS_SMS_SERIES_BAUDRATE_TABLE,
|
||||
"sts3215": STS_SMS_SERIES_BAUDRATE_TABLE,
|
||||
"sts3250": STS_SMS_SERIES_BAUDRATE_TABLE,
|
||||
"scs0009": SCS_SERIES_BAUDRATE_TABLE,
|
||||
}
|
||||
|
||||
# Sign-Magnitude encoding bits
|
||||
STS_SMS_SERIES_ENCODINGS_TABLE = {
|
||||
"Present_Load": 10,
|
||||
"Homing_Offset": 11,
|
||||
"Goal_Position": 15,
|
||||
"Goal_Velocity": 15,
|
||||
"Goal_Speed": 15,
|
||||
"Present_Position": 15,
|
||||
"Present_Velocity": 15,
|
||||
"Present_Speed": 15,
|
||||
}
|
||||
|
||||
MODEL_ENCODING_TABLE = {
|
||||
"sts_series": STS_SMS_SERIES_ENCODINGS_TABLE,
|
||||
"sms_series": STS_SMS_SERIES_ENCODINGS_TABLE,
|
||||
"scs_series": {},
|
||||
"sts3215": STS_SMS_SERIES_ENCODINGS_TABLE,
|
||||
"sts3250": STS_SMS_SERIES_ENCODINGS_TABLE,
|
||||
"sm8512bl": STS_SMS_SERIES_ENCODINGS_TABLE,
|
||||
"scs0009": {},
|
||||
}
|
||||
|
||||
SCAN_BAUDRATES = [
|
||||
4_800,
|
||||
9_600,
|
||||
14_400,
|
||||
19_200,
|
||||
38_400,
|
||||
57_600,
|
||||
115_200,
|
||||
128_000,
|
||||
250_000,
|
||||
500_000,
|
||||
1_000_000,
|
||||
]
|
||||
|
||||
MODEL_NUMBER_TABLE = {
|
||||
"sts3215": 777,
|
||||
"sts3250": 2825,
|
||||
"sm8512bl": 11272,
|
||||
"scs0009": 1284,
|
||||
}
|
||||
|
||||
MODEL_PROTOCOL = {
|
||||
"sts_series": 0,
|
||||
"sms_series": 0,
|
||||
"scs_series": 1,
|
||||
"sts3215": 0,
|
||||
"sts3250": 0,
|
||||
"sm8512bl": 0,
|
||||
"scs0009": 1,
|
||||
}
|
||||
1280
src/lerobot/motors/motors_bus.py
普通文件
1280
src/lerobot/motors/motors_bus.py
普通文件
文件差异内容过多而无法显示
加载差异
在新工单中引用
屏蔽一个用户