11.4.2 人形机器人运动控制
在本实例中，人形机器人运动控制以统一接口协议为基础，支持真实硬件与虚拟调试双模式，通过关节配置（ID、偏移量、符号修正）、扭矩开关、角度范围映射及数据滤波优化，实现关节状态精准读写与动作指令下发；同时适配双机械臂协同控制、相机挂载机器人姿态调整等场景，配套参数校验与初始化校准机制，为上层遥操作、主动视觉控制等逻辑提供稳定、灵活的底层运动控制支撑。
（1）文件teleop/dynamixel/robot.py定义了一套用于控制机器人（尤其适配人形机器人灵巧手、双机械臂）的抽象协议与具体实现类，基于Protocol实现接口规范，核心提供机器人关节自由度查询、关节状态获取、关节状态指令下发以及机器人多维度观测数据收集的核心功能；同时实现了PrintRobot（用于打印关节指令状态的调试/演示机器人）和BimanualRobot（组合左右单机器人的双机械臂机器人，支持联合关节控制与观测数据合并），为后续人形机器人遥操作中的实体机器人控制提供了统一的接口规范和基础实现框架。
python
from abc import abstractmethod
from typing import Dict, Protocol
import numpy as np
class Robot(Protocol):
"""机器人协议。
一个用于定义可被控制的机器人的协议接口。
"""
@abstractmethod
def num_dofs(self) -> int:
"""获取机器人的关节自由度数量。
返回:
int: 机器人的关节自由度数量。
"""
raise NotImplementedError
@abstractmethod
def get_joint_state(self) -> np.ndarray:
"""获取主机器人的当前关节状态。
返回:
np.ndarray: 主机器人的当前关节状态。
"""
raise NotImplementedError
@abstractmethod
def command_joint_state(self, joint_state: np.ndarray) -> None:
"""指令主机器人达到指定的关节状态。
参数:
joint_state (np.ndarray): 要指令主机器人达到的目标关节状态。
"""
raise NotImplementedError
@abstractmethod
def get_observations(self) -> Dict[str, np.ndarray]:
"""获取机器人的当前所有观测数据。
该方法用于提取机器人可获取的所有信息，
例如关节位置、关节速度等。也可能包含来自其他附加传感器的信息，
如相机、力传感器等。
返回:
Dict[str, np.ndarray]: 包含各类观测数据的字典。
"""
raise NotImplementedError
class PrintRobot(Robot):
"""一个用于打印被指令关节状态的机器人（调试/演示用）。"""
def __init__(self, num_dofs: int, dont_print: bool = False):
self._num_dofs = num_dofs
self._joint_state = np.zeros(num_dofs)
self._dont_print = dont_print
def num_dofs(self) -> int:
return self._num_dofs
def get_joint_state(self) -> np.ndarray:
return self._joint_state
def command_joint_state(self, joint_state: np.ndarray) -> None:
assert len(joint_state) == (self._num_dofs), (
f"期望的关节状态长度为 {self._num_dofs}，"
f"实际得到 {len(joint_state)}。"
)
self._joint_state = joint_state
if not self._dont_print:
print(self._joint_state)
def get_observations(self) -> Dict[str, np.ndarray]:
joint_state = self.get_joint_state()
pos_quat = np.zeros(7)
return {
"joint_positions": joint_state, # 关节位置
"joint_velocities": joint_state, # 关节速度
"ee_pos_quat": pos_quat, # 末端执行器位置与四元数
"gripper_position": np.array(0), # 夹爪位置
}
class BimanualRobot(Robot):
def __init__(self, robot_l: Robot, robot_r: Robot):
self._robot_l = robot_l
self._robot_r = robot_r
def num_dofs(self) -> int:
return self._robot_l.num_dofs() + self._robot_r.num_dofs()
def get_joint_state(self) -> np.ndarray:
return np.concatenate(
(self._robot_l.get_joint_state(), self._robot_r.get_joint_state())
)
def command_joint_state(self, joint_state: np.ndarray) -> None:
self._robot_l.command_joint_state(joint_state[: self._robot_l.num_dofs()])
self._robot_r.command_joint_state(joint_state[self._robot_l.num_dofs() :])
def get_observations(self) -> Dict[str, np.ndarray]:
l_obs = self._robot_l.get_observations()
r_obs = self._robot_r.get_observations()
assert l_obs.keys() == r_obs.keys()
return_obs = {}
for k in l_obs.keys():
try:
return_obs[k] = np.concatenate((l_obs[k], r_obs[k]))
except Exception as e:
print(e)
print(k)
print(l_obs[k])
print(r_obs[k])
raise RuntimeError()
return return_obs
def main():
pass
if __name__ == "__main__":
main()（2）文件teleop/dynamixel/driver.py针对人形机器人中Dynamixel舵机的控制需求，实现了一套完整的舵机驱动接口与具体实现：首先定义了Dynamixel舵机通信所需的核心寄存器常量（扭矩使能、目标位置、当前位置等地址与数据长度）；接着通过DynamixelDriverProtocol协议规范了舵机驱动的核心接口（设置关节角度、查询扭矩使能状态、切换扭矩模式、获取当前关节角度、关闭驱动）；然后分别实现了FakeDynamixelDriver（无硬件依赖的虚拟驱动，用于调试和仿真，不进行实际串口通信）和DynamixelDriver（真实硬件驱动，基于串口通信实现舵机扭矩控制、关节角度同步读写，通过独立线程持续采集当前关节角度，保障数据实时性）；最后提供main函数完成驱动实例化、扭矩模式切换与关节角度实时读取的演示，为后续人形机器人灵巧手等搭载Dynamixel舵机的部件提供底层控制支撑。
python
ADDR_TORQUE_ENABLE = 64
ADDR_GOAL_POSITION = 116
LEN_GOAL_POSITION = 4
#! rewrite
ADDR_PRESENT_POSITION = 132
ADDR_PRESENT_POSITION = 140 # 位置轨迹(寄存器地址140)
LEN_PRESENT_POSITION = 4
TORQUE_ENABLE = 1
TORQUE_DISABLE = 0
class DynamixelDriverProtocol(Protocol):
def set_joints(self, joint_angles: Sequence[float]):
"""设置Dynamixel舵机的关节角度。
参数:
joint_angles (Sequence[float]): 关节角度列表。
"""
...
def torque_enabled(self) -> bool:
"""检查Dynamixel舵机的扭矩是否已使能。
返回:
bool: 扭矩已使能返回True，未使能返回False。
"""
...
def set_torque_mode(self, enable: bool):
"""设置Dynamixel舵机的扭矩模式。
参数:
enable (bool): 为True时使能扭矩，为False时关闭扭矩。
"""
...
def get_joints(self) -> np.ndarray:
"""获取当前的关节角度（单位：弧度）。
返回:
np.ndarray: 包含各关节角度的数组。
"""
...
def close(self):
"""关闭舵机驱动。"""
...
class FakeDynamixelDriver(DynamixelDriverProtocol):
def __init__(self, ids: Sequence[int]):
self._ids = ids
self._joint_angles = np.zeros(len(ids), dtype=int)
self._torque_enabled = False
def set_joints(self, joint_angles: Sequence[float]):
if len(joint_angles) != len(self._ids):
raise ValueError(
"关节角度列表的长度必须与舵机数量匹配"
)
if not self._torque_enabled:
raise RuntimeError("必须先使能扭矩才能设置关节角度")
self._joint_angles = np.array(joint_angles)
def torque_enabled(self) -> bool:
return self._torque_enabled
def set_torque_mode(self, enable: bool):
self._torque_enabled = enable
def get_joints(self) -> np.ndarray:
return self._joint_angles.copy()
def close(self):
pass
class DynamixelDriver(DynamixelDriverProtocol):
def __init__(
self, ids: Sequence[int], port: str = "/dev/ttyUSB0", baudrate: int = 57600
):
"""初始化DynamixelDriver类。
参数:
ids (Sequence[int]): Dynamixel舵机的ID列表。
port (str): 连接机械臂的USB串口端口。
baudrate (int): 通信波特率。
"""
self._ids = ids
self._joint_angles = None
#!
self._lock = Lock()
#! 目前无需了解该部分细节
# 初始化端口处理器、数据包处理器以及同步读写组
self._portHandler = PortHandler(port)
self._packetHandler = PacketHandler(2.0)
self._groupSyncRead = GroupSyncRead(
self._portHandler,
self._packetHandler,
ADDR_PRESENT_POSITION,
LEN_PRESENT_POSITION,
)
self._groupSyncWrite = GroupSyncWrite(
self._portHandler,
self._packetHandler,
ADDR_GOAL_POSITION,
LEN_GOAL_POSITION,
)
# 打开串口并设置波特率
if not self._portHandler.openPort():
raise RuntimeError("串口打开失败")
if not self._portHandler.setBaudRate(baudrate):
raise RuntimeError(f"波特率 {baudrate} 设置失败")
# 为每个Dynamixel舵机向同步读取组添加参数
for dxl_id in self._ids:
if not self._groupSyncRead.addParam(dxl_id):
raise RuntimeError(
f"ID为 {dxl_id} 的Dynamixel舵机参数添加失败"
)
# 关闭所有Dynamixel舵机的扭矩
self._torque_enabled = False
try:
self.set_torque_mode(self._torque_enabled)
except Exception as e:
print(f"端口: {port}, 异常信息: {e}")
# 控制读取线程
self._stop_thread = Event()
self._start_reading_thread()
def set_joints(self, joint_angles: Sequence[float]):
if len(joint_angles) != len(self._ids):
raise ValueError(
"关节角度列表的长度必须与舵机数量匹配"
)
if not self._torque_enabled:
raise RuntimeError("必须先使能扭矩才能设置关节角度")
for dxl_id, angle in zip(self._ids, joint_angles):
# 将角度转换为舵机可识别的数值
position_value = int(angle * 2048 / np.pi)
# 将目标位置值分配到字节数组中
param_goal_position = [
DXL_LOBYTE(DXL_LOWORD(position_value)),
DXL_HIBYTE(DXL_LOWORD(position_value)),
DXL_LOBYTE(DXL_HIWORD(position_value)),
DXL_HIBYTE(DXL_HIWORD(position_value)),
]
# 将目标位置值添加到同步写入参数存储区
dxl_addparam_result = self._groupSyncWrite.addParam(
dxl_id, param_goal_position
)
if not dxl_addparam_result:
raise RuntimeError(
f"ID为 {dxl_id} 的Dynamixel舵机关节角度设置失败"
)
# 同步写入目标位置
dxl_comm_result = self._groupSyncWrite.txPacket()
if dxl_comm_result != COMM_SUCCESS:
raise RuntimeError("目标位置同步写入失败")
# 清空同步写入参数存储区
self._groupSyncWrite.clearParam()
def torque_enabled(self) -> bool:
return self._torque_enabled
def set_torque_mode(self, enable: bool):
torque_value = TORQUE_ENABLE if enable else TORQUE_DISABLE
with self._lock:
for dxl_id in self._ids:
dxl_comm_result, dxl_error = self._packetHandler.write1ByteTxRx(
self._portHandler, dxl_id, ADDR_TORQUE_ENABLE, torque_value
)
if dxl_comm_result != COMM_SUCCESS or dxl_error != 0:
print(dxl_comm_result)
print(dxl_error)
raise RuntimeError(
f"ID为 {dxl_id} 的Dynamixel舵机扭矩模式设置失败"
)
self._torque_enabled = enable
def _start_reading_thread(self):
self._reading_thread = Thread(target=self._read_joint_angles)
# self._reading_thread.daemon = True
# self._reading_thread.start()
def _read_joint_angles(self):
# 持续读取关节角度并更新joint_angles数组
while not self._stop_thread.is_set():
time.sleep(0.001)
with self._lock:
_joint_angles = np.zeros(len(self._ids), dtype=int)
dxl_comm_result = self._groupSyncRead.txRxPacket()
if dxl_comm_result != COMM_SUCCESS:
print(f"警告，通信失败: {dxl_comm_result}")
continue
for i, dxl_id in enumerate(self._ids):
if self._groupSyncRead.isAvailable(
dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION
):
angle = self._groupSyncRead.getData(
dxl_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION
)
angle = np.int32(np.uint32(angle))
_joint_angles[i] = angle
else:
raise RuntimeError(
f"ID为 {dxl_id} 的Dynamixel舵机关节角度获取失败"
)
self._joint_angles = _joint_angles
# self._groupSyncRead.clearParam() # 待办：这个方法的作用是什么？是否需要添加？
def get_joints(self) -> np.ndarray:
# 返回关节角度数组的副本，避免竞态条件
while self._joint_angles is None:
time.sleep(0.1)
with self._lock:
_j = self._joint_angles.copy()
return _j / 2048.0 * np.pi
def close(self):
self._stop_thread.set()
self._reading_thread.join()
self._portHandler.closePort()
def main():
# 设置串口端口、波特率和舵机ID
ids = [1,2]
# 创建DynamixelDriver实例
try:
driver = DynamixelDriver(ids)
except FileNotFoundError:
driver = DynamixelDriver(ids, port="/dev/cu.usbserial-FT7WBMUB")
driver.set_torque_mode(True)
driver.set_torque_mode(False)
# 打印关节角度
try:
while True:
joint_angles = driver.get_joints()
print(f"ID为 {ids} 的舵机关节角度: {joint_angles}")
# print(f"ID为 {ids[1]} 的舵机关节角度: {joint_angles[1]}")
except KeyboardInterrupt:
driver.close()
# driver.set_torque_mode(True)
# joint_angles = driver.get_joints()
# print(f"ID为 {ids} 的舵机关节角度: {joint_angles}")
# driver.set_joints([6, 6])
# driver.set_joints([7, 8])
if __name__ == "__main__":
main()（3）文件teleop/dynamixel/dynamixel_robot.py定义了DynamixelRobot类（继承自Robot协议），是针对搭载Dynamixel舵机的人形机器人（如灵巧手、机械臂）的高层控制封装类。该类核心支持关节配置（关节ID、偏移量、符号修正）与可选夹爪配置，能够根据real参数切换真实硬件驱动（DynamixelDriver）和虚拟调试驱动（FakeDynamixelDriver）；实现了关节状态获取（含关节坐标修正、夹爪位置归一化映射、指数平滑滤波优化）、关节指令下发（有效角度范围映射、偏移量补偿）、扭矩模式切换以及观测数据封装的核心功能，同时在初始化时支持基于起始关节姿态校准关节偏移量，保障机器人启动姿态准确，为上层遥操作逻辑提供了简洁、统一的机器人舵机控制接口，是连接底层舵机驱动与上层人形机器人控制逻辑的关键桥梁。
python
class DynamixelRobot(Robot):
"""一个代表Dynamixel舵机机器人的类"""
def __init__(
self,
joint_ids: Sequence[int],
joint_offsets: Optional[Sequence[float]] = None,
joint_signs: Optional[Sequence[int]] = None,
real: bool = False,
port: str = "/dev/ttyUSB0",
baudrate: int = 57600,
gripper_config: Optional[Tuple[int, float, float]] = None,
start_joints: Optional[np.ndarray] = None,
):
print(f"尝试连接串口端口: {port}")
# 用于设置夹爪，预留供后续使用
self.gripper_open_close: Optional[Tuple[float, float]]
if gripper_config is not None:
assert joint_offsets is not None
assert joint_signs is not None
# 将夹爪ID、偏移量、符号修正补充到关节配置中
joint_ids = tuple(joint_ids) + (gripper_config[0],)
joint_offsets = tuple(joint_offsets) + (0.0,)
joint_signs = tuple(joint_signs) + (1,)
# 夹爪开闭合角度转换为弧度制
self.gripper_open_close = (
gripper_config[1] * np.pi / 180,
gripper_config[2] * np.pi / 180,
)
else:
self.gripper_open_close = None
# 所有实例变量均以下划线开头命名（私有变量规范）
# 配置关节参数
self._joint_ids = joint_ids
self._driver: DynamixelDriverProtocol
# 初始化关节偏移量（无传入则默认全为0）
if joint_offsets is None:
self._joint_offsets = np.zeros(len(joint_ids))
else:
self._joint_offsets = np.array(joint_offsets)
# 初始化关节符号修正值（无传入则默认全为1）
if joint_signs is None:
self._joint_signs = np.ones(len(joint_ids))
else:
self._joint_signs = np.array(joint_signs)
# 参数合法性校验
assert len(self._joint_ids) == len(self._joint_offsets), (
f"关节ID数量: {len(self._joint_ids)}, "
f"关节偏移量数量: {len(self._joint_offsets)}"
)
assert len(self._joint_ids) == len(self._joint_signs), (
f"关节ID数量: {len(self._joint_ids)}, "
f"关节符号修正值数量: {len(self._joint_signs)}"
)
assert np.all(
np.abs(self._joint_signs) == 1
), f"关节符号修正值必须为1或-1，当前值: {self._joint_signs}"
# 当在gello_agent中调用时，real参数为True（用于真实硬件测试），否则使用虚拟驱动
if real:
self._driver = DynamixelDriver(joint_ids, port=port, baudrate=baudrate)
# 默认关闭扭矩模式
self._driver.set_torque_mode(False)
else:
self._driver = FakeDynamixelDriver(joint_ids)
self._torque_on = False # 记录当前扭矩使能状态
self._last_pos = None # 记录上一次的关节位置（用于滤波）
self._alpha = 0.99 # 指数平滑滤波系数
#! 这部分逻辑很重要，需要校验机器人是否从不同的关节姿态启动
if start_joints is not None:
# 遍历所有关节，通过增减2π的整数倍调整关节偏移量，使当前关节姿态尽可能接近起始目标姿态
new_joint_offsets = []
current_joints = self.get_joint_state()
assert current_joints.shape == start_joints.shape, "当前关节姿态与起始目标姿态形状不匹配"
# 若配置了夹爪，忽略夹爪相关参数（仅校准机械关节）
if gripper_config is not None:
current_joints = current_joints[:-1]
start_joints = start_joints[:-1]
for c_joint, s_joint, joint_offset in zip(
current_joints, start_joints, self._joint_offsets
):
new_joint_offsets.append(
np.pi * 2 * np.round((s_joint - c_joint) / (2 * np.pi))
+ joint_offset
)
# 若配置了夹爪，补充夹爪的偏移量（保持不变）
if gripper_config is not None:
new_joint_offsets.append(self._joint_offsets[-1])
self._joint_offsets = np.array(new_joint_offsets)
#! 后续可能修改为@property装饰器（属性化）
def num_dofs(self) -> int:
"""获取机器人关节自由度数量"""
return len(self._joint_ids)
#! 目前关节数量已发生变化（可能包含夹爪）
def get_joint_state(self) -> np.ndarray:
"""获取当前关节状态（含滤波与坐标修正）"""
# 读取原始关节位置，进行偏移量补偿与符号修正
pos = (self._driver.get_joints() - self._joint_offsets) * self._joint_signs
assert len(pos) == self.num_dofs(), "关节状态长度与自由度数量不匹配"
# 若配置了夹爪，将夹爪位置归一化映射到[0, 1]区间
if self.gripper_open_close is not None:
g_pos = (pos[-1] - self.gripper_open_close[0]) / (
self.gripper_open_close[1] - self.gripper_open_close[0]
)
g_pos = min(max(0, g_pos), 1) # 限制取值范围在[0, 1]
pos[-1] = g_pos
# 指数平滑滤波，优化关节位置数据的稳定性
if self._last_pos is None:
self._last_pos = pos
else:
pos = self._last_pos * (1 - self._alpha) + pos * self._alpha
self._last_pos = pos
# 补充一个0值，扩展关节状态数组（预留后续功能使用）
new_pos = np.append(pos, 0)
return new_pos
def map_to_valid_range(self, radians_array):
"""将角度数组映射到[0, 2π]的有效范围"""
mapped_radians = np.mod(radians_array, 2 * np.pi)
return mapped_radians
def command_joint_state(self, joint_state: np.ndarray) -> None:
"""下发关节目标状态指令（含有效范围映射与偏移量补偿）"""
# print("指令关节状态 : ", [f"{x:.3f}" for x in joint_state])
# 补偿关节偏移量，转换为舵机可识别的数值
set_value = (joint_state + self._joint_offsets).tolist()
# print("补偿偏移量后的值 : ", [f"{x:.3f}" for x in set_value])
# 映射到[0, 2π]的有效角度范围
set_value = self.map_to_valid_range(set_value)
# print("映射有效范围后的值 : ", [f"{x:.3f}" for x in set_value])
# 下发指令到舵机驱动
self._driver.set_joints(set_value)
def set_torque_mode(self, mode: bool):
"""设置机器人扭矩模式（使能/关闭扭矩）"""
# 若目标模式与当前状态一致，直接返回（避免重复操作）
if mode == self._torque_on:
return
self._driver.set_torque_mode(mode)
self._torque_on = mode
def get_observations(self) -> Dict[str, np.ndarray]:
"""获取机器人观测数据（封装关节状态）"""
return {"joint_state": self.get_joint_state()}（4）文件teleop/dynamixel/agent.py针对人形机器人（尤其是双机械臂/双灵巧手）的动作决策需求，定义了一套智能体（Agent）动作决策接口与具体实现：首先通过Agent协议规范了智能体的核心接口act，要求实现该接口的智能体能够接收环境观测数据并返回对应的动作指令数组；随后实现了DummyAgent（哑元智能体，根据指定自由度返回全零动作数组，用于调试、基准测试或临时占位）和BimanualAgent（双端智能体，组合左右两个独立智能体，自动将输入的双端观测数据按维度对半拆分，分别调用左右智能体的动作决策方法，最后合并双端动作结果），为后续人形机器人双机械臂/双灵巧手的协同遥操作与动作决策提供了统一的智能体接口框架，支撑双端设备的协同控制逻辑。
python
class Agent(Protocol):
def act(self, obs: Dict[str, Any]) -> np.ndarray:
"""根据输入的观测数据返回对应的动作指令。
参数:
obs: 来自环境的观测数据字典。
返回:
要在环境中执行的动作指令数组。
"""
raise NotImplementedError
class DummyAgent(Agent):
def __init__(self, num_dofs: int):
self.num_dofs = num_dofs
def act(self, obs: Dict[str, Any]) -> np.ndarray:
"""返回全零动作数组（用于调试、占位）"""
return np.zeros(self.num_dofs)
class BimanualAgent(Agent):
def __init__(self, agent_left: Agent, agent_right: Agent):
self.agent_left = agent_left
self.agent_right = agent_right
def act(self, obs: Dict[str, Any]) -> np.ndarray:
"""拆分双端观测数据，分别调用左右智能体决策，合并动作结果"""
left_obs = {}
right_obs = {}
for key, val in obs.items():
L = val.shape[0]
half_dim = L // 2
assert L == half_dim * 2, f"观测数据{key}的维度必须为偶数，当前数据存在异常"
left_obs[key] = val[:half_dim] # 拆分左半部分观测数据给左侧智能体
right_obs[key] = val[half_dim:] # 拆分右半部分观测数据给右侧智能体
# 合并左右智能体的动作结果并返回
return np.concatenate(
[self.agent_left.act(left_obs), self.agent_right.act(right_obs)]
)（5）文件teleop/dynamixel/active_cam.py封装了人形机器人主动视觉系统中相机挂载Dynamixel机器人的配置、代理控制与调试功能：通过配置类和端口映射字典快速创建机器人实例，实现DynamixelAgent代理封装关节状态获取逻辑，最后通过主函数循环下发指令、验证机器人运动有效性，支撑相机姿态调整。
python
@dataclass
class DynamixelRobotConfig:
joint_ids: Sequence[int]
"""Dynamixel机器人的关节ID列表。通常格式为(1, 2, 3, ...)。"""
joint_offsets: Sequence[float]
"""机器人的关节偏移量列表。每个关节ID都必须对应一个关节偏移量，且偏移量应为π/2的整数倍。"""
joint_signs: Sequence[int]
"""Dynamixel机器人的关节符号修正值。所有关节的符号修正值均为-1。"""
gripper_config: Tuple[int, int, int]
"""夹爪配置参数（预留供后续开发使用）"""
# 该方法在类实例初始化完成后执行，用于做初始化参数校验
def __post_init__(self):
# 校验关节ID与偏移量数量一致
assert len(self.joint_ids) == len(self.joint_offsets)
# 校验关节ID与符号修正值数量一致
assert len(self.joint_ids) == len(self.joint_signs)
def make_robot(
self, port: str = "/dev/ttyUSB0", start_joints: Optional[np.ndarray] = None
) -> DynamixelRobot:
"""基于当前配置创建DynamixelRobot实例"""
return DynamixelRobot(
joint_ids=self.joint_ids,
joint_offsets=list(self.joint_offsets),
real=True,
joint_signs=list(self.joint_signs),
port=port,
gripper_config=self.gripper_config,
start_joints=start_joints,
)
# 可在该字典中配置多个机器人，注意：机器人的标定信息也应存储在此处
PORT_CONFIG_MAP: Dict[str, DynamixelRobotConfig] = {
#! 用于相机挂载机器人的配置
"/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT8IT033-if00-port0": DynamixelRobotConfig(
joint_ids=(1, 2),
joint_offsets=(
2*np.pi/2,
2*np.pi/2,
),
joint_signs=(-1, -1),
gripper_config=None, # 注：原配置此处为None，与数据类注解类型略有冲突，保留项目原始逻辑
),
}
# 通常只需向该类传入串口端口即可，其他配置信息均存储在上述配置字典中
class DynamixelAgent(Agent):
def __init__(
self,
port: str,
dynamixel_config: Optional[DynamixelRobotConfig] = None,
start_joints: Optional[np.ndarray] = None,
cap_num: int = 42,
):
#! 初始化Dynamixel机器人配置
# 方式一：使用手动传入的配置创建机器人
if dynamixel_config is not None:
self._robot = dynamixel_config.make_robot(
port=port, start_joints=start_joints
)
# 方式二：自动通过串口端口匹配配置字典中的信息，创建机器人
else:
# 校验串口端口是否存在
assert os.path.exists(port), f"串口端口 {port} 不存在"
# 校验串口端口是否在配置字典中
assert port in PORT_CONFIG_MAP, f"串口端口 {port} 未在配置字典中注册"
# 通过端口获取对应的机器人配置
config = PORT_CONFIG_MAP[port]
self._robot = config.make_robot(port=port, start_joints=start_joints)
def act(self, obs: Dict[str, np.ndarray]) -> np.ndarray:
"""根据环境观测数据返回机器人当前关节状态（实现Agent协议接口）"""
return self._robot.get_joint_state()
if __name__ == "__main__":
# 初始化DynamixelAgent代理（关联相机挂载机器人）
agent = DynamixelAgent(port="/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FT8IST6E-if00-port0")
# 使能机器人扭矩（允许关节运动）
agent._robot.set_torque_mode(True)
# 定义关节运动的角度范围与步长（单位：弧度）
min_radians = -1.57
max_radians = 1.57
interval = 0.1
# 循环下发关节角度指令，调试相机挂载机器人的运动
current_radian = 0
while current_radian <= max_radians:
action = agent.act({}) # 传入空字典满足接口要求，实际未使用观测数据
print("当前机器人关节状态 : ", [f"{x:.3f}" for x in action])
# 构造关节指令（第一个关节固定为0，第二个关节按当前步长更新）
command = [0, 0]
agent._robot.command_joint_state([0, current_radian])
# 延时等待关节运动到位
time.sleep(0.1)
# 读取舵机驱动返回的真实关节状态
true_value = agent._robot._driver.get_joints()
print("舵机真实关节状态 : ", [f"{x:.3f}" for x in true_value])
# 更新下一步的关节角度
current_radian += interval