如何对机械臂的关节速度进行动态绘图？

我们在做机器人实验的时候，一般需要实时地观察机械臂的各个状态信息，所以动态绘图是不可避免的，下面我们将介绍如何对机械臂的关节速度进行动态绘图。


Codes
        实现代码比较简单，我们以baxter机器人单个关节的关节角（“right_s0”）为例介绍如何对其速度信息进行实时绘图：
#! /usr/bin/python


import rospy
from sensor_msgs.msg import JointState


import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation


# global variable for vel.
vel_info = np.zeros(100)
vel_info_temp = vel_info


vel_temp = 0


# global variables for plot.
fig, ax = plt.subplots()
line, = ax.plot(vel_info)
ax.set_ylim(-6e-2, 6e-2)
ax.set_ylabel("joint vel: rad/s")
ax.set_xlabel("time")


def stateMsgReceived(state_msg):
    global vel_temp
    vel_temp = state_msg.velocity[15]
    print vel_temp


def update(frame):
    global vel_info
    global vel_info_temp
    global line


    vel_info[0:-1] = vel_info_temp[1:]
    vel_info[-1] = vel_temp
    vel_info_temp = vel_info


    line.set_ydata(vel_info)
    return line




def main():
    global vel_info
    global fig
    rospy.init_node("joint_vel_drawer")
    joint_sub = rospy.Subscriber("/robot/joint_states", JointState, stateMsgReceived)


    ani = animation.FuncAnimation(fig, update, frames=None, interval=50)
    plt.show()




if __name__ == "__main__":
    main()
代码理解
        首先是导入了ROS相关的包：


import rospy
from sensor_msgs.msg import JointState


        然后是导入数据处理包numpy以及绘图包matplotlib：


import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
        设置用于绘制速度曲线的全局变量：


# global variable for vel.
vel_curve = np.zeros(100)
vel_curve_temp = vel_curve
vel_temp = 0
        其中vel_curve表示用于绘图的点的向量，比如这里用100个点来绘图；vel_curve_temp用于暂时保存vel_curve的数据，并在后面用于对vel_curve中的数据进行更新；vel_temp保存最近时刻的速度数据。


        接着我们对所需绘制的图像进行设置，比如y值范围是(-6e-2, 6e-2)，x、y轴的label分别为time和joint vel：


# global variables for plot.
fig, ax = plt.subplots()
line, = ax.plot(vel_info)
ax.set_ylim(-6e-2, 6e-2)
ax.set_ylabel("joint vel: rad/s")
ax.set_xlabel("time")


        然后我们定义了/robot/joint_states话题的回调函数：


def stateMsgReceived(state_msg):
    global vel_temp
    vel_temp = state_msg.velocity[15]
    print vel_temp


        主要也就是取出关节“right_s0”的数据放置到vel_temp中。


        接下来定义最最最重要的动态绘图更新函数：


def update(frame):
    global vel_info
    global vel_info_temp
    global line


    vel_info[0:-1] = vel_info_temp[1:]
    vel_info[-1] = vel_temp
    vel_info_temp = vel_info


    line.set_ydata(vel_info)
    return line


        在这里，我们首先将vel_info中的数据往前移一个点，也即将最早的一个速度点扔掉，然后用vel_temp这一最新数据补充到vel_info中，最后将我们的fig要绘制的曲线line中的数据设置为vel_info即可。


        理解了上面的内容之后，主函数的内容也就不难理解了：


def main():
    global vel_info
    global fig
    rospy.init_node("joint_vel_drawer")
    joint_sub = rospy.Subscriber("/robot/joint_states", JointState, stateMsgReceived)


    ani = animation.FuncAnimation(fig, update, frames=None, interval=50)
    plt.show()


        首先是订阅/joint_states话题，因为是用的rospy，所以会自己开辟一个线程/进程去执行回调函数，如果是用C++写的话，使用ros::AsyncSpinner()即可。然后用animation.FuncAnimation来绘制曲线，其中fig表示我们要绘制的图像，update表示更新图像数据所调用的函数，interval=50表示的是每隔50ms调用一次update函数对数据进行更新，并在fig上显示出来。最后，我们使用plt.show()打开显示窗口即可~


        绘制得到的关节速度动态曲线如下（这里就不放动态图了……）：