最近在琢磨别的事情，Gazebo探索上面进展不大，但也有一些收获，秉承慢慢写的佛系态度记录一下：
1、pigot四足项目的步态改进，前行换成了摆线步态，加入了斜向步态
2、Gazebo-IMU(惯性测量单元)插件的用法
3、利用rqt_plot绘制数据验证步态改进的效果
步态改进
简单来说，就是将每一步的足端轨迹规划为一条摆线。摆线轨迹的两端方向垂直于地面，足端与地面在其它方向的冲击力更小，机器人行进更加稳定。
摆线
由于我们已经有了四足机器人足端逆解方法，因此规划摆线轨迹没有什么困难。轨迹代码见pig_control功能包的traj_data文件中的forward_gait()函数。详细的参考资料推荐一个：
在该文的3.3节中讲到摆线步态以及优化方法。
惯性测量单元(IMU)插件
惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置，在机器人导航中有着很重要的应用[1]。
Gazebo中提供了IMU插件libgazebo_ros_imu_sensor.so，官方教程十分简单。这里介绍基本的使用步骤：
1. 在模型文件中建立一个link用以放置IMU插件
既可以添加一个单独的link代表IMU传感器，图方便也可以直接固连在已有的link中。在pigot中便直接使用机体连杆body_link放置IMU。
2. 在xacro文件中配置IMU
添加如下代码以配置IMU，一般只需修改<bodyName>的值为想要连接IMU的连杆名称即可
<!-- IMU plugin for 'body_link' -->
<gazebo reference="body_link">
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu</topicName>
<bodyName>body_link</bodyName>
<updateRateHZ>100.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link</frameName>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
</gazebo>
对于大型工程，建议单独建立一个xacro文件以配置所有的插件（传感器、控制插件等），再在主文件中引用。pigot项目中的插件配置文件为pigot.gazebo。
3.建立一个订阅者用以接收IMU数据
上述两步完成以后，运行仿真模型时，topic列表中就会出现一个imu话题，不停地有imu消息发送到这个话题上。pigot项目中的话题为/pigot/imu，前缀/pigot来源于机器人运行在单独的命名空间中，在pigot_world.launch中进行设置，设置方式见我的这篇文章。
订阅者代码如下，网络上很容易找到c++的代码，这里用python来写：
#!/usr/bin/env python
# license removed for brevity
import rospy
from sensor_msgs.msg import Imu
import math
def imu_cb(imu_data):
# Read the quaternion of the robot IMU
x = imu_data.orientation.x
y = imu_data.orientation.y
z = imu_data.orientation.z
w = imu_data.orientation.w
# Read the angular velocity of the robot IMU
w_x = imu_data.angular_velocity.x
w_y = imu_data.angular_velocity.y
w_z = imu_data.angular_velocity.z
# Read the linear acceleration of the robot IMU
a_x = imu_data.linear_acceleration.x
a_y = imu_data.linear_acceleration.y
a_z = imu_data.linear_acceleration.z
# Convert Quaternions to Euler-Angles
rpy_angle = [0, 0, 0]
rpy_angle[0] = math.atan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2))
rpy_angle[1] = math.asin(2 * (w * y - z * x))
rpy_angle[2] = math.atan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2))
return
if __name__ == '__main__':
rospy.init_node('imu_node', anonymous=True)
rospy.Subscriber("/pigot/imu", Imu, imu_cb)
rospy.spin()
需要注意的地方有：
消息类型Imu
from sensor_msgs.msg import Imu
导入一种消息类型Imu，这是一种标准的传感器消息类型，官方文档sensor_msgs/Imu 消息类型中重要部分摘录如下：
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z
文档描述了Imu的消息结构，其中姿态（orientation）类型为四元数（geometry_msgs/Quaternion）；角速度（angular_velocity）和线加速度（linear_acceleration）的类型为三维向量（geometry_msgs/Vector3）。
四元数（Quaternion）转欧拉角（Euler-Angles）
四元数是一种姿态的表达方式，与欧拉角相比，它规避了“万向节锁”的问题。
这篇文章讲解了四元数与欧拉角的相互转换
这个视频形象地介绍了万向节锁的问题
4. 利用rqt_plot绘制数据曲线
rqt_plot是ROS内置的一款用于绘图的可视化插件。它可以读取话题中发布的数据，将类型允许的数据绘制成为图像。在终端中运行以下节点可打开rqt_plot:
rosrun rqt_plot rqt_plot
如果Imu插件启动成功，那么此时的话题列表中就应该有相应的话题了。在Topic栏中输入相应的话题即可进行绘图。注意，根据官方文档说明，绘图需要指定出数值所在的完整的地址，例如想要绘制加速度linear_acceleration，它是imu话题中消息的一个成员，那么应当指明的地址为：
/pigot/imu/linear_acceleration
利用rqt_plot验证步态改进的效果
在pigot项目中，改进前的前行步态只是简单指明了轨迹点后进行了线性插值，改进后采用了摆线步态。那么，步态的改进对于改善机器人前行的平稳性究竟有没有好处呢？可以通过rqt_plot采集机器人前行过程中的线性加速度予以验证。
在线性插值的步态下，采集线性加速度图像如下：
线性插值步态的加速度
改用摆线步态后，采集线性加速度图像如下：
摆线步态的加速度
以上两次实验的步幅、步频、抬腿高度和落脚点均相同，可以看出，采用摆线步态时，落步时的冲击一定程度上减小了。这也说明了摆线步态更具有平稳性。
以上现象也可以直接从动作中观察到：
ROS四足机器人步态仿真对比
项目地址
参考文献
[2] 刘蕊. 基于力传感器的四足机器人多步态规划及初步维稳控制[D]. 南京航空航天大学.
