机器人操作进阶系列文章
ROS MoveIt 控制机械臂速度的方法汇总——机器人操作进阶系列 · 速度控制篇
机械臂末端位姿和力传感器数据的提取与绘制——机器人操作进阶系列 · 数据提取篇
状态机SMACH相关教程介绍与应用案例分析——机器人操作进阶系列 · 状态机篇
(未完待续)
温馨提示：点击跳转机器人抓取系统基础系列文章

概要

本文概述了机器人系统中的数据提取，处理与绘制的整个流程，具体的目标是获取机械臂抓取过程中末端位姿和接触力的同步变化数据，为类似的任务提供参考。

1 数据的提取（关节角和力传感数据）

本文是基于UR的ROS driver实现机械臂的控制的，所以想要获取机械臂末端位姿和力传感器的数值，就要先得到和位姿和传感器通讯的相关数据。可以使用以下指令查询相关话题：

$ rostopic list # 查看所有的话题
$ rostopic echo <topic_name> # 查看特定话题的消息内容

经过查询可以得知/joint_states记录了六个关节角的变化，关节角可以通过正运动学计算出末端位姿，/wrench记录了末端力传感器的数据，可以直接使用。

根据博客ROS中数据提取与导出，我们可以利用rosbag功能包录制话题消息，其指令如下所示：

# 同时录制/wrench和/joint_states话题
rosbag record /wrench /joint_states -o wrench_position.bag

当实验过程中录制完成之后，可以在离线状态下回放和提取数据，如下所示：

# terminal 1
roscore

# terminal 2，先指定话题保存路径，避免遗漏数据
rostopic echo -p /wrench/wrench/force/z > force_z.txt 
rostopic echo -p /joint_states/position > joint_angle.txt

# terminal 3，回放rosbag文件
rosbag play wrench_position.bag

获得关节角位置数据之后，想要转换为想要的末端位姿，需要对关节角进行正运动学计算，见下一小节。

2 关节角到末端位姿的正运动学计算

由机械臂的关节角求末端位姿为经典的机器人正运动学计算问题，对于ur5e这类经典的机械臂，我们可以在github上查询并获得ur5e机械臂的正运动学的计算程序，地址：https://github.com/Shaluols/ur5e-fk.

其中，在主函数中的重要函数如下：

    # Obtain the homogeneous matrix from six joint angles
    np_T = forward(q)

    # transfer the homogeneous matrix format into ros msg type
    ros_T = matrix2ros(np_T)

如果我们只是想获得其末端执行器在Z轴上的位移分量，可以使用如下指令

	# 末端执行器位姿Z轴位移的获取
	position_z = matrix2ros(forward(q)).position.z

3 数据的绘制

至此，我们已经获得了机械臂末端执行器的位姿和接触力的数据，然后就可以对数据文件进行绘制，这里以末端Z向接触力为例，给出基于Python的文件读取与绘制程序如下所示：

import matplotlib.pyplot as plt
import numpy as np

filename = "force_z3" # 指定数据源文件

#替换txt文档中的逗号
import re  # research
f=open(filename + ".txt", 'r')
alllines=f.readlines()
f.close()
f=open(filename + ".txt",'w+')
for eachline in alllines:
    a=re.sub(',',' ',eachline)
    f.writelines(a)
f.close()

# download data
file = filename + ".txt"
b = np.loadtxt(file)

# 指定时间（x）和Z向接触力(y)数据
for i in range(len(b)):
    b[i, 0] = i/500 # 消息频率为500Hz，将其转换为真实时间尺度
x = b[:, 0]
y = b[:,1]  # 读取Z向接触力

plt.ylabel('force_z', fontsize = 20)
plt.xlabel('time', fontsize = 20)
plt.yticks(fontsize = 15)
plt.xticks(fontsize = 15)

plt.plot(x, y)
plt.show()

按照以上程序最终获得的Z向位移和接触力的同步变化如下图所示：

然后基于位姿和接触力的同步分析，就可以看到抓取过程中详细的历程。

小结

本文给出了如何获取机械臂末端位姿和接触力的同步变化数据的详细过程，同样的，对于其他机器人系统的数据提取和绘制也可以参考本文的思路和程度。