具身智能专题(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 DynamixelMotorsBus

leader_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 ManipulatorRobot

robot = 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 Path


leader_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 tqdm

leader_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 DynamixelMotorsBus

leader_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_cameras

def 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) & 0xFF
            if key == ord('q'):
                return False  # 退出整个程序
            elif key == ord('n'):
                return True  # 切换到下一个摄像头
        
        return False  # 默认退出
    
    except Exception as e:
        print(f"摄像头 {camera_id} 发生错误: {e}")
        return False
    
    finally:
        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}")
        return
    
    camera_ids = [cam["index"] for cam in camera_infos]
    print(f"找到 {len(camera_ids)} 个摄像头端口: {camera_ids}")
    
    if not camera_ids:
        print("未找到任何可用摄像头")
        return
    
    print(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 修改成对应的序号
在这里插入图片描述

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

BigCabbageFy

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值