多指灵巧手MoveIt!与Gazebo联合仿真框架搭建

至于为什么叫框架，一是因为灵巧手的3维模型没有按照基本的设计要求画，正常来说，设计机器人机构之前应该设计好机构需要多少个自由度/DOF，每个自由度是旋转/revolute类型还是滑移/prismatic类型，各个关节的自由度限位/limits，各个连杆的长度/a，每个自由度之间的扭角/α，偏移/d，整个机器人的工作空间/workspace等等；二是当前搭建的框架能实现的function和控制很基础，灵巧手其实是一个多机器人协同的系统，需要在MoveIt!的代码上进行更改，至少抓取矩阵这种概念在MoveIt!上是没有的，还有很多地方需要修改。

 

总的来说，当前的框架不够deep，但基本的模样已经有了。

 

该博客同时将帮助更加顺利的完成以下工作，希望能对各位有所帮助，源码链接见文末：

• 单纯的通过ROS控制Gazebo中的机器人模型
• 通过MoveIt!直接控制现实中的机器人
• MoveIt!与Gazebo联合仿真

---

博客目录：

1. 使用Solidworks设计灵巧手，通过sw2urdf插件生成ROS包
2. 机器人MoveIt!配置文件生成
3. URDF适配Gazebo的简单修改
4. ROS节点控制Gazebo中机器人模型
5. Moveit!控制现实中的机器人
6. 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项目链接

https://github.com/ShieldQiQi/MutiFingerHandControl

如有问题可在评论区留言，或者私信，或者本人主页咨询或者Github提出，欢迎交流探讨．

不过这种东西也就是工具，精髓在于MoveIt!的源码，所以是建议大家源码安装．