摘要

本文基于松灵PIPER机械臂实现了连续轨迹录制与播放功能。通过对时间序列数据的记录和重现,实现机械臂复杂运动轨迹的完美复制。本文将分析代码实现机制,并提供完整代码、使用指南和步骤演示。

代码仓库

github链接

功能演示

1.使用前准备

1.1.准备工作

  • 工作空间内无障碍物,为机械臂提供充足的运动空间
  • 确认机械臂电源正常,所有指示灯显示正常状态
  • 照明条件良好,便于观察机械臂位置和状态
  • 如果配备夹爪,检查夹爪动作是否正常
  • 地面平稳,避免振动影响录制精度
  • 验证示教按钮功能正常

1.2.环境配置

  • 操作系统:Ubuntu(推荐Ubuntu 18.04或更高版本)
  • Python环境:Python 3.6或更高版本
  • git代码管理工具:用于克隆远程的代码仓库
sudo apt install git
  • pip包管理器:用于安装Python依赖包
sudo apt install python3-pip
  • 安装CAN工具
sudo apt install can-utils ethtool
  • 安装官方提供的Python SDK包,其中1_0_0_beta是带API的版本
git clone -b 1_0_0_beta https://github.com/agilexrobotics/piper_sdk.git
cd piper_sdk
pip3 install .

2.连续轨迹录制与播放功能操作步骤

2.1.操作步骤

  1. 机械臂上电,将USB转CAN模块与电脑连接(确保只连接一个CAN模块)

  2. 打开终端,激活CAN模块
    sudo ip link set can0 up type can bitrate 1000000

  3. 克隆远程代码仓库
    git clone https://github.com/agilexrobotics/Agilex-College.git

  4. 切换至recordAndPlayTraj目录
    cd Agilex-College/piper/recordAndPlayTraj/

  5. 运行录制程序
    python3 recordTrajectory.py

  6. 短按示教按钮进入示教模式

  7. 摆放好机械臂的初始位置,终端回车后,拖动机械臂即可进行轨迹的录制

  8. 录制结束后,再次短按示教按钮退出示教模式

  9. 播放前须知:
    初次退出示教模式时,需要经过特定的初始化过程才能从示教模式切换到CAN模式,因此播放程序会自动执行复位操作,将2、3、5号关节回到安全位置(零点),防止机械臂在重力作用下突然下落造成损坏,在特殊情况下需要人工辅助2、3、5关节回到零点

  10. 运行播放程序
    python3 playTrajectory.py

  11. 使能成功后,终端回车即可播放轨迹

2.2.录制技巧与策略

运动规划策略:
在开始录制前,应该对要录制的轨迹进行规划:

  1. 起始位置选择:
    • 选择机械臂的安全位置作为起始点
    • 确保起始位置便于后续播放时的初始化
    • 避免选择接近关节限位的位置
  2. 轨迹路径设计:
    • 规划平滑的运动路径,避免急剧的方向变化
    • 考虑机械臂的运动学约束,避免奇异位置
    • 预留足够的安全余量,防止碰撞
  3. 速度控制:
    • 保持适中的运动速度,既要保证录制质量,又要避免过慢
    • 在关键位置可以适当放慢速度,提高精度
    • 避免突然的加速或减速

3.过程中遇到的问题及解决方法

问题1:没有Piper类。

原因:当前安装的SDK不是带API的版本。
解决方法:执行pip3 uninstall piper_sdk卸载当前的SDK,然后按照1.2.环境配置的方法安装1_0_0_beta版本的SDK。


问题2:机械臂不动,终端输出如下。

原因:程序运行过程中,短按了示教按钮。
解决方法:检查示教按钮的指示灯是否处于熄灭状态,如果是重新运行程序即可,不是则需先短按示教按钮退出示教模式再运行程序。

4.轨迹录制程序的实现

轨迹录制程序是系统的数据采集模块,负责在示教模式下捕获机械臂的关节连续运动的位置信息。

4.1.程序初始化与配置

4.1.1.参数配置设计

# 是否有夹爪
have_gripper = True
# 最大录制时间,单位:秒,0表示无限长(强制关闭程序可结束录制)
record_time = 10.0
# 示教模式检测超时时间,单位:秒
timeout = 10.0
# 保存轨迹的CSV文件路径
CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")

配置参数分析:

have_gripper参数类型为布尔类型,True时代表有夹爪。

record_time参数为最大录制时间设定,默认为10s,也就是录制10s的轨迹,当该参数设置为0时,系统进入无限录制模式。

timeout参数为设定示教模式检测超时时间,启动程序后,如果10s没有进入示教模式,程序会退出。

CSV_path参数为设定轨迹文件保存路径,默认为与程序当前同路径,文件名为trajectory.csv

4.1.2.机械臂连接与初始化

# 初始化并连接机械臂
piper = Piper("can0")
interface = piper.init()
piper.connect()
time.sleep(0.1)

连接机制分析:
Piper()是API的核心类,该类在interface的基础上简化了通用的一些方法。
init()会创建并返回interface实例,可用于调用Piper的一些特殊方法。
connect()会开启一个线程来连接CAN端口以及处理CAN数据。
time.sleep(0.1)的添加是为了确保连接完全建立。在嵌入式系统中,硬件初始化通常需要一定的时间,这个短暂的延时确保了后续操作的可靠性。

4.1.3.位置获取与数据存储

4.1.3.1.位置获取函数
def get_pos():
    '''获取机械臂当前关节弧度和夹爪张开距离'''
    joint_state = piper.get_joint_states()[0]
    if have_gripper:
        return joint_state + (piper.get_gripper_states()[0][0], )
    return joint_state
4.1.3.2.位置变化检测
current_pos = get_pos()
if current_pos != last_pos: # 发生变化时记录
    wait_time = round(time.time() - last_time, 4)
    print(f"INFO: 等待时间:{wait_time : 0.4f}s,当前位置: {current_pos}")
    last_pos = current_pos
    last_time = time.time()
    csv.write(f"{wait_time}," + ",".join(map(str, current_pos)) + "\n")

位置处理:
current_pos != last_pos
当机械臂静止或连续两次获取的位置相同时,通过位置变化处理可以大大减少录制的数据

时间处理:
round(time.time() - last_time, 4)

  • 0.1毫秒的精度对于机械臂控制已经足够
  • 缩短时间的长度,可以减少占用的存储空间

4.1.4.模式检测与切换

print("step 1: 请点击示教按钮进入示教模式")
over_time = time.time() + timeout
while interface.GetArmStatus().arm_status.ctrl_mode != 2:
    if over_time < time.time():
        print("ERROR: 示教模式检测超时,请检查示教模式是否开启")
        exit()
    time.sleep(0.01)

状态轮询策略:
程序采用轮询方式检测控制模式,这种方法的特点:

  • 实现简单,逻辑清晰
  • 对系统资源要求较低

超时保护机制:
10秒的超时设置考虑了实际操作的需要:

  • 用户理解提示信息的时间
  • 找到并按下示教按钮的时间
  • 系统响应和状态切换的时间
  • 异常情况下的容错处理

示教模式的安全特性:

  • 关节力矩释放,允许手动操作
  • 保持位置反馈,实时监控位置变化

4.1.5.数据存储

csv = open(CSV_path, "w")
# ... 录制循环 ...
csv.write(f"{wait_time}," + ",".join(map(str, current_pos)) + "\n")
# ... 录制结束 ...
csv.close()

数据完整性保证:
每次录制后立即写入文件并刷新缓冲区,确保数据不会因为程序异常退出而丢失。

数据格式选择:
选择CSV格式存储数据的原因包括:

  • 通用性强,几乎所有数据处理工具都支持CSV格式
  • 人类可读性强,便于调试和验证
  • 结构简单,解析效率高
  • 广泛支持,便于与其他工具集成

数据列属性:

  • 第1列:时间间隔,单位秒
  • 第2~7列:关节电机弧度
  • 第8列:夹爪张开距离,单位m

4.1.6.录制控制逻辑

input("step 2: 回车开始录制轨迹")
csv = open(CSV_path, "w")
last_pos = get_pos()
last_time = time.time()
over_time = last_time + record_time
while record_time == 0 or time.time() < over_time:
    # 录制逻辑
    time.sleep(0.01)
csv.close()

时间控制策略:
系统支持两种录制模式:

  1. 定时录制:record_time > 0时,录制指定时长后自动停止
  2. 无限录制:record_time == 0时,需要手动关闭程序

这种设计的灵活性体现在:

  • 适应不同长度的轨迹录制需求
  • 提供了安全的自动停止机制
  • 支持长时间的连续录制任务

4.1.7.轨迹录制程序的完整代码实现

#!/usr/bin/env python3
# -*-coding:utf8-*-
# 录制连续轨迹
import os, time
from piper_sdk import *

if __name__ == "__main__":
    # 是否有夹爪
    have_gripper = True
    # 最大录制时间,单位:秒,0表示无限长(强制关闭程序可结束录制)
    record_time = 10.0
    # 示教模式检测超时时间,单位:秒
    timeout = 10.0
    # 保存轨迹的CSV文件路径
    CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")
    # 初始化并连接机械臂
    piper = Piper("can0")
    interface = piper.init()
    piper.connect()
    time.sleep(0.1)

    def get_pos():
        '''获取机械臂当前关节弧度和夹爪张开距离'''
        joint_state = piper.get_joint_states()[0]
        if have_gripper:
            return joint_state + (piper.get_gripper_states()[0][0], )
        return joint_state
    
    print("step 1: 请点击示教按钮进入示教模式")
    over_time = time.time() + timeout
    while interface.GetArmStatus().arm_status.ctrl_mode != 2:
        if over_time < time.time():
            print("ERROR: 示教模式检测超时,请检查示教模式是否开启")
            exit()
        time.sleep(0.01)

    input("step 2: 回车开始录制轨迹")
    csv = open(CSV_path, "w")
    last_pos = get_pos()
    last_time = time.time()
    over_time = last_time + record_time
    while record_time == 0 or time.time() < over_time:
        current_pos = get_pos()
        if current_pos != last_pos: # 发生变化时记录
            wait_time = round(time.time() - last_time, 4)
            print(f"INFO: 等待时间:{wait_time : 0.4f}s,当前位置: {current_pos}")
            last_pos = current_pos
            last_time = time.time()
            csv.write(f"{wait_time}," + ",".join(map(str, current_pos)) + "\n")
        time.sleep(0.01)
    csv.close()
    print("INFO: 录制结束,再次点击示教按钮退出示教模式")

5.轨迹播放程序的实现

轨迹播放程序是系统的执行模块,负责读取录制的位置数据并控制机械臂重现这些位置。

5.1.参数配置与数据加载

播放参数配置:

# 播放次数 0表示无限循环
play_times = 1
# 播放间隔,单位:秒
play_interval = 1.0
# 运动速度百分比,建议范围:10-100
move_spd_rate_ctrl = 100
# 播放倍速,建议范围:0.1-2
play_speed = 1.0

播放次数控制:
play_times参数支持三种播放模式:

  • 单次播放(play_times = 1):适用于演示和测试
  • 多次播放(play_times > 1):适用于重复性任务
  • 无限循环(play_times = 0):适用于连续生产作业

速度控制的双重机制:
系统提供了两种速度控制方式:

  1. move_spd_rate_ctrl:控制机械臂的整体运动速度
  2. play_speed:控制轨迹播放的时间缩放

这种双重控制的优势:

  • 硬件层面的速度控制确保安全性
  • 软件层面的时间缩放提供灵活性
  • 两者结合可以实现精确的速度控制

播放间隔的作用:
play_interval参数在连续播放中起到重要作用:

  • 给系统提供恢复时间
  • 便于观察和分析播放效果
  • 在生产环境中可以用于节拍控制

5.2.数据加载:数据文件读取

try:
    with open(CSV_path, 'r', encoding='utf-8') as f:
        track = list(csv.reader(f))
        if not track:
            print("ERROR: 轨迹文件为空")
            exit()
        track = [[float(j) for j in i] for i in track]    # 转换为浮点数列表
except FileNotFoundError:
    print("ERROR: 轨迹文件不存在")
    exit()

异常处理策略:
程序采用了完善的异常处理机制,覆盖了常见的文件操作错误:

  • FileNotFoundError:处理文件不存在的情况
  • 空文件检查:防止读取空数据文件
  • 数据格式验证:确保数据能够正确转换为数值类型

数据类型转换: 将字符串数据转换为浮点数的过程中,程序使用了列表推导式。

5.3.安全停止函数

def stop():
    '''停止机械臂;初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂'''
    interface.EmergencyStop(0x01)
    time.sleep(1.0)
    limit_angle = [0.1745, 0.7854, 0.2094]  # 2、3、5关节角度在限制范围内时才恢复机械臂,防止大角度直接掉落造成损坏
    pos = get_pos()
    while not (abs(pos[1]) < limit_angle[0] and abs(pos[2]) < limit_angle[0] and pos[4] < limit_angle[1] and pos[4] > limit_angle[2]):
        time.sleep(0.01)
        pos = get_pos()
    # 恢复机械臂
    piper.disable_arm()
    time.sleep(1.0)

分阶段停止策略:
停止函数采用了分阶段的安全停止策略:

  1. 紧急停止阶段:EmergencyStop(0x01)发送紧急停止指令,立即停止所有关节运动(关节带阻抗)
  2. 安全位置等待:等待关键关节(2、3、5号关节)移动到安全范围内
  3. 系统恢复阶段:发送恢复指令,重新激活控制系统

安全范围设计:
程序特别关注2、3、5号关节的位置,这是基于PIPER机械臂的机械结构特点:

  • 2号关节(肩关节):控制大臂的俯仰运动,影响整体稳定性
  • 3号关节(肘关节):控制小臂角度,直接影响末端位置
  • 5号关节(腕关节):控制末端姿态,影响夹爪方向

安全角度范围的设定(10°、45°、12°)基于以下考虑:

  • 避免机械臂在重力作用下快速下落
  • 确保关节不会碰撞到机械限位
  • 为后续运动提供足够的操作空间

实时监控机制: 程序采用实时轮询方式监控关节位置,确保只有在安全条件满足时才进行下一步操作。

5.4.系统使能函数

def enable():
    '''使能机械臂和夹爪'''
    while not piper.enable_arm():
        time.sleep(0.01)
    if have_gripper:
        time.sleep(0.01)
        piper.enable_gripper()
    interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
    print("INFO: 使能成功")

机械臂使能:enable_arm()

夹爪使能:enable_gripper()

控制模式设置:
ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)控制模式参数:

  • 第一个参数(0x01):设置为CAN指令控制模式
  • 第二个参数(0x01):设置为关节运动模式
  • 第三个参数(0至100):设置运动速度百分比
  • 第四个参数(0x00):设置为位置速度模式

5.5.播放控制逻辑

count = 0
input("step 2: 回车开始播放轨迹")
while play_times == 0 or abs(play_times) != count:
    for n, pos in enumerate(track):
        piper.move_j(pos[1:-1], move_spd_rate_ctrl)
        if have_gripper and len(pos) == 8:
            piper.move_gripper(pos[-1], 1)
        print(f"INFO: 第{count + 1}次播放,等待时间:{pos[0] / play_speed : 0.4f}s,目标位置: {pos[1:]}")
        if n == len(track) - 1:
            time.sleep(play_interval)
        else:
            time.sleep(pos[0] / play_speed)
    count += 1

关节控制:move_j()

  • 第一个参数:包含六个关节电机弧度的元组
  • 第二个参数:运动速度百分比,范围0~100

夹爪控制:move_gripper()

  • 第一个参数:夹爪张开距离,单位m
  • 第二个参数:夹爪力矩,单位N/m

播放速度调整机制:
pos[0] / play_speed实现了轨迹播放速度的动态调整:

  • play_speed > 1.0:加速播放,时间间隔缩短
  • play_speed < 1.0:减速播放,时间间隔延长
  • play_speed = 1.0:原速播放

这种实现方式的好处:

  • 保持轨迹的相对时间关系
  • 简单直观的速度控制
  • 不需要重新计算整个轨迹

5.6.轨迹播放程序的完整代码实现

#!/usr/bin/env python3
# -*-coding:utf8-*-
# 播放连续轨迹
import os, time, csv
from piper_sdk import *

if __name__ == "__main__":
    # 是否有夹爪
    have_gripper = True
    # 播放次数 0表示无限循环
    play_times = 1
    # 播放间隔,单位:秒
    play_interval = 1.0
    # 运动速度百分比,建议范围:10-100
    move_spd_rate_ctrl = 100
    # 播放倍速,建议范围:0.1-2
    play_speed = 1.0
    # 切换CAN模式超时时间,单位:秒
    timeout = 5.0
    # 保存轨迹的CSV文件路径
    CSV_path = os.path.join(os.path.dirname(__file__), "trajectory.csv")
    # 读取轨迹文件
    try:
        with open(CSV_path, 'r', encoding='utf-8') as f:
            track = list(csv.reader(f))
            if not track:
                print("ERROR: 轨迹文件为空")
                exit()
            track = [[float(j) for j in i] for i in track]    # 转换为浮点数列表
    except FileNotFoundError:
        print("ERROR: 轨迹文件不存在")
        exit()

    # 初始化并连接机械臂
    piper = Piper("can0")
    interface = piper.init()
    piper.connect()
    time.sleep(0.1)

    def get_pos():
        '''获取机械臂当前关节弧度和夹爪张开距离'''
        joint_state = piper.get_joint_states()[0]
        if have_gripper:
            return joint_state + (piper.get_gripper_states()[0][0], )
        return joint_state
    
    def stop():
        '''停止机械臂;初次退出示教模式需先调用此函数才能使用CAN模式控制机械臂'''
        interface.EmergencyStop(0x01)
        time.sleep(1.0)
        limit_angle = [0.1745, 0.7854, 0.2094]  # 2、3、5关节弧度在限制范围内时才恢复机械臂,防止大弧度直接掉落造成损坏
        pos = get_pos()
        while not (abs(pos[1]) < limit_angle[0] and abs(pos[2]) < limit_angle[0] and pos[4] < limit_angle[1] and pos[4] > limit_angle[2]):
            time.sleep(0.01)
            pos = get_pos()
        # 恢复机械臂
        piper.disable_arm()
        time.sleep(1.0)
    
    def enable():
        '''使能机械臂和夹爪'''
        while not piper.enable_arm():
            time.sleep(0.01)
        if have_gripper:
            time.sleep(0.01)
            piper.enable_gripper()
        interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
        print("INFO: 使能成功")

    print("step 1: 播放前请确保机械臂已退出示教模式")
    if interface.GetArmStatus().arm_status.ctrl_mode != 1:
        stop()  # 初次退出示教模式需先调用此函数才能切换至CAN模式
    over_time = time.time() + timeout
    while interface.GetArmStatus().arm_status.ctrl_mode != 1:
        if over_time < time.time():
            print("ERROR: CAN模式切换失败,请检查是否退出示教模式")
            exit()
        interface.ModeCtrl(0x01, 0x01, move_spd_rate_ctrl, 0x00)
        time.sleep(0.01)
    
    enable()
    count = 0
    input("step 2: 回车开始播放轨迹")
    while play_times == 0 or abs(play_times) != count:
        for n, pos in enumerate(track):
            piper.move_j(pos[1:-1], move_spd_rate_ctrl)
            if have_gripper and len(pos) == 8:
                piper.move_gripper(pos[-1], 1)
            print(f"INFO: 第{count + 1}次播放,等待时间:{pos[0] / play_speed : 0.4f}s,目标位置: {pos[1:]}")
            if n == len(track) - 1:
                time.sleep(play_interval)
            else:
                time.sleep(pos[0] / play_speed)
        count += 1

6.小结

以上基于松灵PIPER机械臂实现了连续轨迹录制与播放功能。通过对Python SDK的应用,可以实现机械臂轨迹的记录和重复执行,为教学演示和自动化操作提供了强有力的技术支撑。

有任何相关使用的疑问都可以联系我们support@agilex.ai

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐