多指灵巧手MoveIt!与Gazebo联合仿真框架搭建
至于为什么叫框架，一是因为灵巧手的3维模型没有按照基本的设计要求画，正常来说，设计机器人机构之前应该设计好机构需要多少个自由度/DOF，每个自由度是旋转/revolute类型还是滑移/prismatic类型，各个关节的自由度限位/limits，各个连杆的长度/a，每个自由度之间的扭角/α，偏移/d，整个机器人的工作空间/workspace等等；二是当前搭建的框架能实现的function和控制很基础，灵巧手其实是一个多机器人协同的系统，需要在MoveIt!的代码上进行更改，至少抓取矩阵这种概念在MoveIt!上是没有的，还有很多地方需要修改。
总的来说，当前的框架不够deep，但基本的模样已经有了。
该博客同时将帮助更加顺利的完成以下工作，希望能对各位有所帮助，源码链接见文末：
-
单纯的通过ROS控制Gazebo中的机器人模型
-
通过MoveIt!直接控制现实中的机器人
-
MoveIt!与Gazebo联合仿真
博客目录：
-
使用Solidworks设计灵巧手，通过sw2urdf插件生成ROS包
-
机器人MoveIt!配置文件生成
-
URDF适配Gazebo的简单修改
-
ROS节点控制Gazebo中机器人模型
-
Moveit!控制现实中的机器人
-
MoveIt!与Gazebo联合仿真
Ⅰ. 使用Solidworks设计灵巧手，通过sw2urdf插件生成ROS包
用Solidwork设计，一是这样相对于自己搭URDF更专业，二是有插件支持，方便生成URDF。
几个需要注意的点：
-
每个link都单独放入总装配体中，最好不要用子装配体
-
设计好装配体后，使用sw2urdf插件定义各个关节连杆的从属关系，在这之前，使用D-H方法自己画出各个轴和坐标系，不要自动生成，这样生成的URDF99%会错位。另外旋转轴的类型对于机械臂这种机器人一般选择revolute，移动的小车这种机器人就选择continous，以上两者的区别在于如果选择前者会在URDF文件中定义关节限位，固定的关节选择fixed。
-
如果你的机器人底座是固定的，那么你最好把base_link整体z坐标大于0，不然到时放到Gazebo的时候会与地面干涉，造成不必要的麻烦。
Ⅱ. 机器人MoveIt!配置文件生成
现在从Windows转到Ubuntu，将前面生成的URDF包放在工作空间， 前提是已经装好了MoveIt!，建议使用源码安装，方便阅读和修改源码．
打开MoveIt_Setup_assitants，参照官方文档设置
roslaunch moveit_setup_assistant setup_assistant.launch
加载URDF文件，按照顺序一一设置即可，后面要修改也再次可以加载ＭoveIt!配置文件修改．其中有几个要注意的地方：
-
干涉检测那一部分自动生成后转到矩阵表示，检查是否符合自己的要求
-
virtual_joint的目的一般是为了将base_link和world坐标系相固连，在后面修改URDF的部分中为适配Ｇazebo已经添加了base_link和world的虚拟关节，所以在这里不再需要添加，各位自行决定．
-
planning_group按照每个手指分组．
-
Robot_Pose这一部分可以检测你的机器人关节的限位，如果动不了，一般是URDF中关节硬限位和软限位没有设置好．
-
ROS_Control这块根据具体的控制器类型选择，先按组自动添加，然后再修改控制器类型，如果和Gazebo联合仿真的话，选择position_controllers/JointTrajectoryController这一类型，原因是由MoveIt!和仿真环境或实际机器人的通信方式所决定，具体可在MoveIt!官网介绍．当然以上两者联合仿真的话也不一定就要position_controllers/JointTrajectoryController这种控制器，你也可以选择其它控制器，然后将MoveIt!的client发出的action消息写个Server端进行解析，然后用过ROS_Control进行Gazebo控制．
move_group talks to the controllers on the robot using the FollowJointTrajectoryAction interface. This is a ROS action interface. A server on the robot needs to service this action - this server is not provided by move_group itself. move_group will only instantiate a client to talk to this controller action server on your robot.
-
Simulation这部分生成适配Gazebo的URDF文件，在原来基础上添加了传动/Transmission和连杆惯量以及gazebo_ros_control插件，这样才能真实的模拟真实物理环境，把这个URDF文件作为后面用的机器人描述文件．
Ⅲ. URDF适配Gazebo的简单修改
-
添加一个virtual_link用以连接base_link和world，类型为固定．
-
设置好关节的软硬限位和旋转方向．
-
检查惯性等属性是否正确，gazebo_ros_control可以设置你的机器人的命名空间，注意这个命名需要和之后的controller.yaml文件和MoveIt!配置生成的ros_control.launch中的命名空间一致，不然到时候节点会找不到另一个命名空间的控制器．
Ⅳ. ROS节点控制Gazebo中机器人模型
要实现ROS节点控制Gazebo中机器人模型只需要运行gazebo.launch和一个自定义的关节角发布节点．
MoveIt!配置生成的ROBOT_moveit_config包中含有诸多launch文件和config文件，建议都看看以了解其原理．
gazebo.launch负责加载Gazebo中的空世界模型和机器人URDF模型，以及controller_manager，运行该文件会加载/config/ros_controllers.yaml中的参数到ROS的参数服务器，ros_controllers.yaml是较为关键的一环，格式不对会导致运行出错．比如我之前配置MoveIt!的时候，如果选择控制器类型为position_controllers/JointPositionController，可以这样写（部分代码）:
mutifingerhand:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
J11_controller:
type: position_controllers/JointPositionController
joint: J11
pid: {p: 100.0, i: 0.01, d: 1}
J12_controller:
type: position_controllers/JointPositionController
joint: J12
pid: {p: 100.0, i: 0.01, d: 1}
以上的控制器名就是之前在MoveIt!中配置的名字，也和ros_control.launch中的控制器名一致，注意我这里的命名空间是mutifingerhand，所以在ros_control.launch和URDF中的命名空间也当是如此．
接下来就是写一个发布各关节控制的消息发布节点，根据Gazebo的官方示例，消息类型为std_msgs::Float64，话题名为/mutifingerhand/J11_controller/command，参照这样的格式写即可．
Ⅴ. Moveit!控制现实中的机器人
要实现ROS节点控制控制现实中的机器人只需要运行demo.launch和一个自定义的基于moveit的规划节点．
demo.launch加载机器人RVIZ配置文件和move_group等节点，其中在下面代码中有个fake_execution决定了控制实际机器人或者仿真，还是直接进行fake_control，在这里把它改为false.
<include file="$(find mutifingerhand_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="$(arg pipeline)"/>
</include>
另外，上文中讲到了MoveIt!通讯的机制，MoveIt!作为client需要一个机器人端的Server与其对应，同时ros_controllers.yaml控制器类型需要修改成position_controllers/JointTrajectoryController，注意这些都要写在controller_list下，具体结构参考下面的代码：
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
controller_list:
- name: finger1_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- J11
- J12
- J13
- J14
- J15
和前面Gazebo端的ros_controller.yaml有点类似，但是不同．大部分博文中这两个yaml文件被分成了两个，但其实放在同一个文件里也可以，各取所需，反正都是要加载到ROS参数服务器的．
Server端如何写，我这里有个参考，完整代码移步文末的Github链接．
#include <ros/ros.h>
#include "actionlib/server/action_server.h"
#include "actionlib/server/server_goal_handle.h"
#include <control_msgs/FollowJointTrajectoryAction.h>
class ControllerServerAction
{
protected:
ros::NodeHandle nh_;
actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> action_server;
actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
control_msgs::FollowJointTrajectoryResult result_;
public:
ControllerServerAction(std::string name) :
action_server(nh_, name, boost::bind(&ControllerServerAction::pubGoalJoints, this, _1), false)
{
action_server.start();
至于自定义的基于moveit的规划节点，参照MoveIt!官方教程写就行，记得加上代码：
// Moving to a pose goal /* Uncomment below line when working with a real robot */
move_group.move();
这是让机器人发送规划的轨迹的关键一步．
Ⅵ. MoveIt!与Gazebo联合仿真
关键在于ros_controller.yaml的配置，Gazebo端和MoveIt!端的controller.yaml文件可以放在一个文件，两者的格式分别就是上文中第四第五节中讲到的格式，运行demo_gazebo.launch文件同时加载Gazebo和MoveIt!，不需要写Server了，Gazebo这端默认作为了Server端．要控制的话写一个上文中第五节一样的节点就行．
控制器类型得是position_controllers/JointTrajectoryController．
如图我这里是控制了第四根手指弯曲，把PID参数调到机器人没有抖动，先调P，然后调ID参数，I负责消除稳态累计误差，D负责噪声影响．
Github项目链接
如有问题可在评论区留言，或者私信，或者本人主页咨询或者Github提出，欢迎交流探讨．
不过这种东西也就是工具，精髓在于MoveIt!的源码，所以是建议大家源码安装．
