用MoveIT与gazebo实现机器人轨迹规划
【2020.6.19更新】之前配置的仿真中，关节是通过位置控制器控制的，虽然位移层面没有问题，但是速度与加速度层面无法实现三次曲线或者五次曲线的运动轨迹。解决这个问题也很简单，就是将位置控制器换成力控制器。
1.在原来的第3步中将
hardware_interface/PositionJointInterface
改成
hardware_interface/EffortJointInterface；
同时在ros_controllers.yaml中类型
type: position_controllers/JointTrajectoryController
也要改成
type: effort_controllers/JointTrajectoryController
2.在ros_controllers.yaml文件中添加PID的设置,可以参考这个link，同时也可以学习里面调节pid的方法，十分便捷。
修改这两处就没有问题了。现在的仿真可以说是完全的物理仿真了，从加速度一直到位移都可以符合五次曲线的运动。不过这同时带来一个缺点，因为pid调节不可能完美，所以关节的实际位置跟期望位置总会有点误差，这跟实体机械臂的控制是一样的。
----------旧文分割线----------
前言
gazebo可以仿真机械臂的运动，但是用其自带的插件系统来做机械臂的运动规划很不方便，而moveit自带各种规划算法，把moveit跟gazebo结合起来就可以很方便的仿真机械臂的各种操作了。
在网上找了很多的教程，感觉写的都不是很详细，大部分都只是说了下步骤，甚至连完整的可以运行的代码也没有给。找到的所有教程教的方法都需要自己写各种配置文件，简单的机械臂还好，要是碰到复杂的机械臂自己写配置文件根本不现实，容易出错。我经过几天的研究，终于找到了一个特别方便的方法，只需要自己写一点代码就可以，分享给大家。
博客里所有的源码都可以在这下载，已经在我的电脑上验证过了，下载后应该能直接运行，前提是环境得配好，下面贴出的代码可能不全，有不清楚的可以看源代码。
我的环境：ubuntu18.04+ros melodic+gazebo+moveit.
先介绍下步骤
- 新建一个robot_description包，用来放机器人模型文件
- 用moveit_setup_assistant图形界面工具配置moveit包
- 修改moveit包
- 运行moveit包里的demo_gazebo.launch，完成！
新建robot_description 包
ros中包的创建、urdf文件这些基础知识应该都会了，不然也不会看到这篇博客，不会的可以找资料学习。
１．在catkin工作目录（~/catkin_ws）下创建一个robot_description包，名字任意。后面的urdf是包的依赖。然后编译一下。
cd ~/catkin_ws/src
catkin_create_pkg robot_description urdf
cd ~/catkin_ws
catkin_make
２．新建urdf文件夹跟meshes文件
cd ~/catkin_ws/src/robot_description
mkdir urdf
mkdir meshes
并把自己的机器人模型拷进来，urdf文件放在urdf文件夹，mesh文件放在meshes文件夹。注意在urdf文件里引用所有mesh文件的路径要相应改到新建的meshes文件夹里。例如我给的ur5.urdf里就是这样：
<geometry>
<mesh filename="package://robot_description/meshes/ur5/visual/base.dae"/>
geometry>
3.添加gazebo tag，不知道gazebo tag的点这。详情见robot_description/urdf/ur5.urdf，这里就简单提一下，下面两类tag是必须的：
#1.transmission tag，每一个主动关节都需要
<transmission name="shoulder_pan_trans">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="shoulder_pan_joint">
<hardwareInterface>hardware_interface/PositionJointInterfacehardwareInterface>
joint>
<actuator name="shoulder_pan_motor">
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
#2.ros_control 插件，一个就够了
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
plugin>
新建robot_description 包这一步很简单，也可以不用新建，保证ros能正确找到你的包里的urdf模型就行。
配置moveit包
１．terminal里打开moveit_setup_assistant
roslaunch moveit_setup_assistant setup_assistant.launch
２．给robot_description/urdf/ur5.urdf配置moveit包
接下来就是常见的配置moveit文件了，这里就不多说，不会的点这
给几个关键过程截图：
start
sefl-collisions不用改参数，直接点generate collision matrix；virtual joints跳过不需要，因为ur5自带基座固定关节；
planning groups:
选这６个关节就行，ur5是一个６自由度的机械臂。这个planning groups命名为arm,名字任意，但是要记住，后面要用到。
robot pose没什么特殊的，平时怎么来就怎么来；end effectors跳过，passive joints跳过；
robot control:
这一步很重要，点一下箭头指的按钮就可以。注意到controller的名字是arm_controller,前半部分是刚才你命名的planning group，后半部分是程序自己加的。
simulation 3d perception跳过，author information填上自己的信息，最后在catkin工作目录生成moveit包：
修改moveit包
最后一步比较关键，虽然要改的代码不多，还是要细心点，不要出错。
１．找到刚才配置的moveit包，我配置的包名是ur5robot_moveit_config，放在~/catkin_ws/src/下，打开 ~/catkin_ws/src/ur5robot_moveit_config/config/ros_controllers.yaml，在最末尾添加如下语句：
arm_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint : {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 1
需要注意的几点：
（１）数组名要和刚才命名的planning group对应，比如我的planning group是arm，这里就是arm_controller
（２）关节名要与planning group里添加的joint相对应
（３）格式不能错，该对齐的对齐，该缩进的缩进
２．打开~/catkin_ws/src/ur5robot_moveit_config/launch/ros_controllers.launch，把这一句命令
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args=""/>
改成
<node name="controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="spawn joint_state_controller arm_controller"/>
运行demo_gazebo.launch
到这里就完成了，在terminal里运行一下看看
roslaunch ur5robot_moveit_config demo_gazebo.launch
出现问题：gazebo里机器人一直在抖动。最后发现是机械臂放的太低，碰到了地面。解决办法：在~/catkin_ws/src/ur5robot_moveit_config/launch/gazebo.launch里修改如下语句，把机器人抬高0.1m.虽然改的是gazebo.launch，其实demo_gazebo.launch在启动gazebo的时候是通过include gazebo.launch完成的。
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0"
respawn="false" output="screen" />
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -x 0 -y 0 -z 0.1"
respawn="false" output="screen" />
再次运行demo_gazebo.launch，结果如下，在rviz里面点击plan规划，点击execute,gazebo里的机械臂也跟着动了。
是不是很简单？
结语
总结一下，网上大部分教程都是说原理，怎么写配置文件，但是自己写配置文件不仅耗时，还容易出错，我最开始尝试的时候就碰到过各种各样奇怪的报错，终于被我找到了一个非常便捷的方法。
gazebo跟moveit集成原理很简单，就是通过action消息实现gazebo跟moveit的通信，本篇博客主要是分享实现方法，原理就不多说了，自己看demo_gazebo.launch文件分析，网上也有很多帖子，例如这个。
一些可能出现的bug
1.melodic版本的ros有可能会出现ros controll插件没安装的情况，碰到相关的报错可百度melodic安装ros controll
2.如果是自己写的urdf文件，注意urdf文件中joint的属性配置很重要，要是配置的不合适会导致仿真出错。如果不知道怎么配置就参考我给的源码里ur5.urdf文件。