具身智能专题（2）-主从臂零位校准及摄像头数据获取与检验

1. 主从臂零位校准

标定文件解读：
homing offset: 表示每个电机的零点偏移（Homing Offset），单位为步数（Steps）。它是将实际电机读数调整到标定目标位置（通常为0°或90°）所需的修正量。
drive mode:表示每个电机的驱动方向（DriveMode），0表示原始方向（无需反转），1表示反转方向。
start pos:表示标定过程中手动移到“零位”（Zero Position）时的电机读数（单位：步数），即 zero_pos。
end pos:表示标定过程中手动移到“旋转位”（Rotated Position，90°）时的电机读数（单位：步数），即 rotated_pos。
calib mode:表示每个电机的标定模式，DEGREE表示旋转关节（单位：度，范围[-180,180]），LINEAR表示线性关节（范围[0,100]）。
motor names: 机械臂电机的名称列表，与其他参数一一对应。指定参数的适用对象，表示6个自由度（DOF）的机械臂：

• shoulder pan: 肩部水平旋转
• shoulder lift: 肩部俯仰
• elbow flex: 肘部弯曲
• wrist flex: 腕部俯仰
• wrist roll: 腕部滚动
• gripper: 夹爪

1.1 方案1：终端代码

from dynamixel_motors_bus import DynamixelMotorsBusleader_port = "/dev/ttyACM10"
follower_port = "/dev/ttyACM11"leader_arm = DynamixelMotorsBus(port=leader_port,motors={"shoulder_pan": (1, "xl330-m077"),"shoulder_lift": (2, "xl330-m077"),"elbow_flex": (3, "xl330-m077"),"wrist_flex": (4, "xl330-m077"),"wrist_roll": (5, "xl330-m077"),"gripper": (6, "xl330-m077")}
)follower_arm = DynamixelMotorsBus(port=follower_port,motors={"shoulder_pan": (1, "xl430-w250"),"shoulder_lift": (2, "xl430-w250"),"elbow_flex": (3, "xl330-m288"),"wrist_flex": (4, "xl330-m288"),"wrist_roll": (5, "xl330-m288"),"gripper": (6, "xl330-m288")}
)from lerobot.common.robot.devices.robots.manipulator import ManipulatorRobotrobot = ManipulatorRobot(robot_type="koch",leader_arms={"main": "leader_arm"},follower_arms={"main": "follower_arm"},calibration_dir=".cache/calibration/koch"
)robot.connect()

1.2 方案1：脚本（推荐）

cd lerobot
vim ./lerobot/scripts/calibration.py

代码如下所示：

import os
import sys
## 添加根目录到系统路径
# current_dir = os.path.dirname(os.path.abspath(__file__))
# root_dir = os.path.abspath(os.path.join(current_dir, "..", "..", "."))
# print('root_dir:', root_dir)
# sys.path.insert(0, root_dir)# 标定脚本。进行双臂的关节角度标定。再输出标定验证结果。
from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus
from pathlib import Pathleader_port = "/dev/ttyACM10"  # 主臂端口
follower_port = "/dev/ttyACM11"  # 从臂端口leader_arm = DynamixelMotorsBus(port=leader_port,motors={# name: (index, model)"shoulder_pan": (1, "xl330-m077"),"shoulder_lift": (2, "xl330-m077"),"elbow_flex": (3, "xl330-m077"),"wrist_flex": (4, "xl330-m077"),"wrist_roll": (5, "xl330-m077"),"gripper": (6, "xl330-m077"),},
)follower_arm = DynamixelMotorsBus(port=follower_port,motors={# name: (index, model)"shoulder_pan": (1, "xl430-w250"),"shoulder_lift": (2, "xl430-w250"),"elbow_flex": (3, "xl330-m288"),"wrist_flex": (4, "xl330-m288"),"wrist_roll": (5, "xl330-m288"),"gripper": (6, "xl330-m288"),},
)# 操作机器人
from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot# 实例化机器人
robot = ManipulatorRobot(robot_type="koch",leader_arms={"main": leader_arm},follower_arms={"main": follower_arm},calibration_dir=".cache/calibration/koch",
)
# 连接机器人
robot.connect()# 注意，下面代码会读取calibration_dir文件夹下是否有main_leader.json和main_follower.json文件，如果有的则不会进行calibration
calibration_dir = ".cache/calibration/koch"
# 判断文件夹下是否有main_leader.json和main_follower.json文件
if os.path.exists(os.path.join(calibration_dir, "main_leader.json")) and os.path.exists(os.path.join(calibration_dir, "main_follower.json")):print("已经标定过了, 请检查标定是否成功")leader_pos = robot.leader_arms["main"].read("Present_Position")follower_pos = robot.follower_arms["main"].read("Present_Position")print(leader_pos)print(follower_pos)print("对比leader和follower的关节参数,相差不大则说明标定成功")

执行命令：

python ./lerobot/scripts/calibration.py

（1. 从臂零位
此时需要将从臂放置到零位，夹爪和机械臂水平放置，夹爪微张，一指宽即可（如下图所示），摆放好位置后，敲击回车键：

（2. 从臂旋转位
这里需要将机械臂树立起来旋转90度，做到横平竖直，夹爪开口到最大，摆放好位置后，敲击回车键：
（3. 从臂休息位
这里需要将从臂调整到休息位，使得电机不受力，靠自身的硬件属性就可以停放。此时的夹爪微张，大概一指宽。摆放好位置后，敲击回车键

（4. 主臂零位

摆放同从臂零位，敲击回车。
（5. 主臂旋转位
摆放同从臂旋转位，敲击回车。

（6. 主臂休息位
摆放同从臂休息位，敲击回车。

（7. 标定结果检验
标定结束，终端会输出leader和follower的关节参数，需要人工对比，如果两者差距不是很大，则说明标定成功

标定成功后，项目路径下会保存标定后的数据

2. 主从臂跟随性测试

主从臂零位校准成功后，就需要测试一下主臂操作，从臂的跟随性。

vim ./lerobot/scripts/teleopration.py

写入代码

import os
import sys
## 添加根目录到系统路径
# current_dir = os.path.dirname(os.path.abspath(__file__))
# root_dir = os.path.abspath(os.path.join(current_dir, "..", "..", "."))
# print('root_dir:', root_dir)
# sys.path.insert(0, root_dir)
# 遥操纵机器人
from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus
from lerobot.common.robot_devices.robots.manipulator import ManipulatorRobot
from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera, find_cameras
from pathlib import Path
import os
import tqdmleader_port = "/dev/ttyACM10"  # 主臂端口
follower_port = "/dev/ttyACM11"  # 从臂端口leader_arm = DynamixelMotorsBus(port=leader_port,motors={# name: (index, model)"shoulder_pan": (1, "xl330-m077"),"shoulder_lift": (2, "xl330-m077"),"elbow_flex": (3, "xl330-m077"),"wrist_flex": (4, "xl330-m077"),"wrist_roll": (5, "xl330-m077"),"gripper": (6, "xl330-m077"),},
)follower_arm = DynamixelMotorsBus(port=follower_port,motors={# name: (index, model)"shoulder_pan": (1, "xl430-w250"),"shoulder_lift": (2, "xl430-w250"),"elbow_flex": (3, "xl330-m288"),"wrist_flex": (4, "xl330-m288"),"wrist_roll": (5, "xl330-m288"),"gripper": (6, "xl330-m288"),},
)# 将摄像头连接到机器人 调试获取摄像头信息
# mock=False  # 取值决定了函数在未找到摄像头时是静默返回一个空列表还是主动抛出异常
# camera_infos = find_cameras(mock=mock)  # find_cameras 查找并返回计算机上可用摄像头的索引和对应端口信息
# camera_ids = [cam["index"] for cam in camera_infos]
# print(camera_ids)# 实例化遥控机器人 对象
robot = ManipulatorRobot(leader_arms={"main": leader_arm},follower_arms={"main": follower_arm},calibration_dir=".cache/calibration/koch",# cameras={#     # "front": OpenCVCamera(0, fps=30, width=640, height=480),#     # "side": OpenCVCamera(2, fps=30, width=640, height=480),#     # "top": OpenCVCamera(4, fps=30, width=640, height=480),#     "laptop": OpenCVCamera(2, fps=30, width=640, height=480),#     "phone": OpenCVCamera(4, fps=30, width=640, height=480),#     # "wrist":OpenCVCamera(6, fps=30, width=640, height=480),# },
)robot.connect()seconds = 100  # 运行时间
frequency = 200 # 运行频率  
for _ in tqdm.tqdm(range(seconds*frequency)):leader_pos = robot.leader_arms["main"].read("Present_Position")robot.follower_arms["main"].write("Goal_Position", leader_pos)# for _ in tqdm.tqdm(range(seconds*frequency)):  # 每秒运行200次，运行30秒
#     robot.teleop_step()# leader_pos = robot.leader_arms["main"].read("Present_Position")# follower_pos = robot.follower_arms["main"].read("Present_Position")# observation, action = robot.teleop_step(record_data=True)# print(follower_pos)# print(observation)# print(leader_pos)# print(action)robot.disconnect()
print("遥操结束！！！.")

执行程序，即可手动去操控主臂，来检查从臂的跟随性，通过旋转每个关节的电机来一一检查。

python ./lerobot/scripts/teleopration.py

备注：
不要让机械臂长时间上力矩，需要及时执行卸力代码。

vim ./lerobot/scripts/release_torque.py

写入代码

import os
import sys
## 添加根目录到系统路径
# current_dir = os.path.dirname(os.path.abspath(__file__))
# root_dir = os.path.abspath(os.path.join(current_dir, "..", "..", "."))
# print('root_dir:', root_dir)
# sys.path.insert(0, root_dir)
# 机械臂释放力矩
from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBusleader_port = "/dev/ttyACM10"  # 主臂端口
follower_port = "/dev/ttyACM11"  # 从臂端口leader_arm = DynamixelMotorsBus(port=leader_port,motors={# name: (index, model)"shoulder_pan": (1, "xl330-m077"),"shoulder_lift": (2, "xl330-m077"),"elbow_flex": (3, "xl330-m077"),"wrist_flex": (4, "xl330-m077"),"wrist_roll": (5, "xl330-m077"),"gripper": (6, "xl330-m077"),},
)follower_arm = DynamixelMotorsBus(port=follower_port,motors={# name: (index, model)"shoulder_pan": (1, "xl430-w250"),"shoulder_lift": (2, "xl430-w250"),"elbow_flex": (3, "xl330-m288"),"wrist_flex": (4, "xl330-m288"),"wrist_roll": (5, "xl330-m288"),"gripper": (6, "xl330-m288"),},
)leader_arm.connect()
follower_arm.connect()# 用于控制 Dynamixel 马达的扭矩模式
from lerobot.common.robot_devices.motors.dynamixel import TorqueMode
follower_arm.write("Torque_Enable", TorqueMode.DISABLED.value)      # TorqueMode.DISABLED：禁用扭矩（马达松开，不受主臂控）
leader_arm.write("Torque_Enable", TorqueMode.DISABLED.value)
print("Torque disabled")

执行程序，即可手动去操控主臂，来检查从臂的跟随性，通过旋转每个关节的电机来一一检查。

python ./lerobot/scripts/release_torque.py

3.摄像头数据获取以及位置校准

整个套件有两个摄像头，一个顶部摄像头，一个侧面摄像头。相机布局如图所示

使用下面代码调整摄像头位置，使其可以完整的拍摄画面。

'''python lerobot/common/robot_devices/cameras/opencv.py \
--images-dir /home/cheng/Documents/workspace_hjc/data'''# from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera
# import cv2# # 查找所有摄像头端口号
# from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera, find_cameras
# camera_infos = find_cameras(False)  
# camera_ids = [cam["index"] for cam in camera_infos]
# print(camera_ids)# cam = cv2.VideoCapture(index=2) # 0 side 2 top
# while True:
#     ret, frame = cam.read()
#     cv2.imshow("camera", frame)
#     if cv2.waitKey(1) & 0xFF == ord('q'):
#         break
# cam.release()
# cv2.destroyAllWindows()###################################  优化的代码  ####################################
import cv2
from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera, find_camerasdef get_camera(camera_id, window_name="Camera Preview"):"""测试并显示单个摄像头画面"""try:# 初始化摄像头cam = cv2.VideoCapture(camera_id)if not cam.isOpened():print(f"无法打开摄像头 {camera_id}")return False# 显示操作提示print(f"\n正在测试摄像头 {camera_id}:")print("  - 按 'n' 切换到下一个摄像头")print("  - 按 'q' 退出程序")while True:ret, frame = cam.read()if not ret:print(f"摄像头 {camera_id} 读取帧失败")break# 显示画面cv2.imshow(window_name, frame)# 按 'q' 退出当前摄像头，'n' 切换到下一个key = cv2.waitKey(1) & 0xFFif key == ord('q'):return False  # 退出整个程序elif key == ord('n'):return True  # 切换到下一个摄像头return False  # 默认退出except Exception as e:print(f"摄像头 {camera_id} 发生错误: {e}")return Falsefinally:cam.release()cv2.destroyWindow(window_name)def main():# 初始提示print("欢迎使用摄像头测试程序！")print("操作说明：请你将鼠标光标点击弹出图像然后按照下面提示切换摄像头")print("  - 按 'n' 切换到下一个摄像头")print("  - 按 'q' 随时退出程序")print("正在搜索可用摄像头...\n")# 查找所有可用摄像头try:camera_infos = find_cameras()except Exception as e:print(f"查找摄像头失败: {e}")returncamera_ids = [cam["index"] for cam in camera_infos]print(f"找到 {len(camera_ids)} 个摄像头端口: {camera_ids}")if not camera_ids:print("未找到任何可用摄像头")returnprint(f"找到以下摄像头端口: {camera_ids}")print(f"共检测到 {len(camera_ids)} 个摄像头，开始测试...\n")# 依次测试每个摄像头for idx, cam_id in enumerate(camera_ids):window_name = f"Camera {cam_id} Preview"continue_testing = get_camera(cam_id, window_name)if not continue_testing:break  # 用户按 'q' 或发生错误，退出循环print("\n所有摄像头测试完成或已退出")if __name__ == "__main__":main()

整体画面如下图所示：

顶部相机

侧边相机

修改配置文件
lerobot/configs/robot/koch.yaml 中摄像头的参数 camera_index 修改成对应的序号