一、引言

此文续上节Solidworks导出URDF模型
系统版本与ROS版本：Ubuntu18.04、Melodic
内容：（1）Setup Assistant配置自己的机械臂的URDF模型（2）实现机械臂在rviz与gazebo之中的联调仿真，即通过调节rviz参数可以使rviz与gazebo中的机械臂一起动
本文主要用KUKA机械臂模型（当然是项目需要啦）
目前还没有给它夹爪，所以暂时不管抓取的部分，先能控制机械臂动
还是费了一番心血害，写个详细点的配置过程，让做毕设的小伙子们舒服点~~（吐槽一下，这方面教程太少了，找到的很多教程讲的好像没那么小白，算是误打误撞+站在各位大佬的基础上弄出来的）

二、使用Setup Assistant配置机械臂

（1）下载机械臂模型

git clone https://github.com/ros-industrial/kuka_experimental.git

下载后可以把整个包放到工作空间下进行编译~~（这个大家应该都会吧就不详细说了）

本文选用的是kuka_kr16_support这一款机械臂模型，当然大家有需要可以用其他的，本质上都是一样的啦
（2）下载相关依赖包（可能还是会缺一些，大家到时候根据情况再补充）

sudo apt-get install ros-melodic-ros-control ros-melodic-ros-controllers ros-melodic-gazebo-ros ros-melodic-gazebo-ros-control ros-melodic-hector-gazebo-plugins 
sudo apt-get install ros-melodic-joint-trajectory-controller
sudo apt-get install ros-melodic-moveit*

（3）Setup Assistant配置机械臂

首先需要启动MoveIt Setup Assistant

roslaunch moveit_setup_assistant setup_assistant.launch

第一步->Start：创建新的 MoveIt 配置包或编辑现有的 MoveIt 配置包~~
因为是第一次配置，所以我们选择创建新的Moveit配置包（Create New MoveIt Configuration Package），如果是已经有用MoveIt Setup Assistant配置好的包可以选择编辑已有配置包（Edit Existing Moveit Configuration Package）
点击Browse，选中机械臂的URDF文件（如图箭头所示）
点击Load Files，可以看到右侧已经成功加载机械臂模型了
第二步->Self-Collision Matrix：生成自碰撞矩阵~~
这一步没啥，就不介绍是个啥了，看名字就那个意思，直接根据默认的来~~
点击Generate Collision Matrix
第三步->Virtual Joint：虚拟关节~~
就是为了保证机械臂在gazebo环境里面被固定在原地不会乱飞（话是这么说，但是我配置了反正一点用都没，不如自己后面在URDF里面添加一个fixed关节把base_link和world固定死）
这一步直接跳过好啦，后面我们手动配置~
第四步->Planning Groups：添加规划组~~
点击Add Group
其中Group name为arm，Kinematic Solver（运动学求解器）选kdl_kinematics_plugin/KDLKinematicsPlugin，OMPL Planning为RRT，其他默认~
点击Add Joints，将所有属于机械臂arm组的Joint添加到右侧，如果有夹爪需要创建一个gripper组，当然组的名字可以自定义的
点击save
第五步->Robot Poses：机器人位姿~~
点击Add Poses
可以根据自己需要任意移动机械臂的姿态然后他会保存下来机械臂的位姿，后面可以在rviz里面直接根据该姿态的名称移动机械臂，可以添加好几个姿态，这里我就只创建一个home姿态啦
点击Save
第六步->End Effectors：标记末端执行器~~
这一步因为没有机械手，所以先跳过，后续博客如果添加了机械手就再更新
第七步->Passive Joints：被动关节~~
这个配置也可以跳过，基本用不到
第八步->ROS Control：ROS控制~~
这部分必须有，而且很重要，如果没有这部分的话后续会报错
点击 Add Controller
Controller Name为 arm_position_controller，控制器类型为position_controllers/JointTrajectoryController
点击Add Planning Group Joints
将arm组添加到右边
点击Save
第九步->Simulation：仿真~~
点击Generate URDF

这一步主要是生成一个新的urdf模型，当然也可以选择用自己的，但是自己要手动添加一下gazebo的一些插件，什么transmission啊那些，为了方便我们用它自动生成的这个，这里我们先新建一个test.urdf文件然后把对应的自动生成的内容copy进去，后面我们要对里面一些内容进行修改
第十步->3D Preception：3D感知~~
这一步跳过，咱没有要添加摄像头
第十一步->Author information：作者信息~~
就输入姓名和邮箱就可以了
第十二步->Configuration Files：配置信息~~
这一步就是保存功能包，需要我们预先在工作空间src下面创建一个空文件夹，名字自己取，然后点击Generation Package就可以了

以上就是使用Setup Assistant配置机械臂全过程了，通过以上过程我们获得了一个功能包（kuka_moveit_config），同时获得了一个test.urdf文件（在第9步我们保存下来的）

三、rviz与gazebo联调

以上我们完成了功能包的生成，下面我们进行一些参数的修改以及配置，使我们的机械臂可以在rviz与gazebo中能够同时工作~

3.1 配置urdf文件

我们在之前的操作中获取了一个test.urdf文件，我们先把他放到~/catkin_ws/src/kuka_kr16_support/urdf文件夹下

（1）添加机械臂与world之间的关节，使得机械臂可以固定在gazebo地面上

<link name="world"/>
	<joint name="fixed" type="fixed">
 		 <parent link="world"/>
		 <child link="base_link"/>
	</joint>

（2）<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>这部分要注意里面的得是hardware_interface/PositionJointInterface，总之不一样的替换掉就行
（3）joint里面的effort参数不能设置为0，我这里设置的是50，0的话代表最大的力的0.。。。肯定不行，后面仿真会报错

urdf部分主要就是这三个问题，我附一下我修改后的test.urdf文件供大家参考：

<?xml version="1.0" encoding="utf-8" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/lixushi/catkin_ws/src/kuka_kr16_support/urdf/kr16_2.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--Generates a urdf from the macro in kr16_2_macro.xacro -->
<robot name="kuka_kr16_2">
    <!-- LINKS -->
 <link name="world"/>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
  </joint>
    <!-- base link -->
    <link name="base_link">
        <!--
<inertial>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <mass value="2"/>
        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
      </inertial>
-->
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/base_link.dae" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/base_link.stl" />
            </geometry>
        </collision>
        
<inertial>
            <mass value="0.1" />
            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
        </inertial>

    </link>
    <!-- link 1 (A1) -->
    <link name="link_1">
        
<inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="2" />
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        </inertial>

        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_1.dae" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_1.stl" />
            </geometry>
        </collision>
    </link>
    <!-- link 2 -->
    <link name="link_2">
        
<inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="2" />
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        </inertial>

        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_2.dae" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_2.stl" />
            </geometry>
        </collision>
    </link>
    <!-- link 3 -->
    <link name="link_3">
        
<inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="2" />
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        </inertial>

        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_3.dae" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_3.stl" />
            </geometry>
        </collision>
    </link>
    <!-- link 4 -->
    <link name="link_4">
        
<inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="2" />
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        </inertial>

        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_4.dae" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_4.stl" />
            </geometry>
        </collision>
    </link>
    <!-- link 5 -->
    <link name="link_5">
        
<inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="2" />
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        </inertial>

        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_5.dae" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_5.stl" />
            </geometry>
        </collision>
    </link>
    <!-- link 6 -->
    <link name="link_6">
        
<inertial>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <mass value="2" />
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
        </inertial>

        <visual>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/visual/link_6.dae" />
            </geometry>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0 0 0" />
            <geometry>
                <mesh filename="package://kuka_kr16_support/meshes/kr16_2/collision/link_6.stl" />
            </geometry>
        </collision>
    </link>
    <!-- tool 0 -->
    <!-- Following REP199, this frame shall be use to attach EEF or other equipment -->
    <link name="flange" />
    <!-- END LINKS -->
    <!-- JOINTS -->
    <!-- joint 1 (A1) -->
    <joint name="joint_a1" type="revolute">
        <origin rpy="0 0 0" xyz="0 0 0.675" />
        <parent link="base_link" />
        <child link="link_1" />
        <axis xyz="0 0 -1" />
        <limit effort="50" lower="-3.22885911619" upper="3.22885911619" velocity="2.72271363311" />
    </joint>
    <!-- joint 2 (A2) -->
    <joint name="joint_a2" type="revolute">
        <origin rpy="0 0 0" xyz="0.26 0 0" />
        <parent link="link_1" />
        <child link="link_2" />
        <axis xyz="0 1 0" />
        <limit effort="50" lower="-2.70526034059" upper="0.610865238198" velocity="2.72271363311" />
    </joint>
    <!-- joint 3 (A3) -->
    <joint name="joint_a3" type="revolute">
        <origin rpy="0 0 0" xyz="0.68 0 0" />
        <parent link="link_2" />
        <child link="link_3" />
        <axis xyz="0 1 0" />
        <limit effort="50" lower="-2.26892802759" upper="2.68780704807" velocity="2.72271363311" />
    </joint>
    <!-- joint 4 (A4) -->
    <joint name="joint_a4" type="revolute">
        <origin rpy="0 0 0" xyz="0.67 0 -0.035" />
        <parent link="link_3" />
        <child link="link_4" />
        <axis xyz="-1 0 0" />
        <limit effort="50" lower="-6.10865238198" upper="6.10865238198" velocity="5.75958653158" />
    </joint>
    <!-- joint 5 (A5) -->
    <joint name="joint_a5" type="revolute">
        <origin rpy="0 0 0" xyz="0 0 0" />
        <parent link="link_4" />
        <child link="link_5" />
        <axis xyz="0 1 0" />
        <limit effort="50" lower="-2.26892802759" upper="2.26892802759" velocity="5.75958653158" />
    </joint>
    <!-- joint 6 (A6) -->
    <joint name="joint_a6" type="revolute">
        <origin rpy="0 0 0" xyz="0 0 0" />
        <parent link="link_5" />
        <child link="link_6" />
        <axis xyz="-1 0 0" />
        <limit effort="50" lower="-6.10865238198" upper="6.10865238198" velocity="10.7337748998" />
    </joint>
    <!-- tool frame - fixed frame -->
    <joint name="joint_a6-flange" type="fixed">
        <parent link="link_6" />
        <child link="flange" />
        <origin rpy="0 0 0" xyz="0.158 0 0" />
    </joint>
    <!-- END JOINTS -->
    <!-- ROS base_link to KUKA $ROBROOT coordinate system transform -->
    <link name="base" />
    <joint name="base_link-base" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0" />
        <parent link="base_link" />
        <child link="base" />
    </joint>
    <!-- This frame corresponds to the $TOOL coordinate system in KUKA KRC controllers -->
    <link name="tool0" />
    <joint name="flange-tool0" type="fixed">
        <parent link="flange" />
        <child link="tool0" />
        <origin rpy="0 1.57079632679 0" xyz="0 0 0" />
    </joint>
    
<transmission name="trans_joint_a1">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint_a1">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint_a1_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_joint_a2">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint_a2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint_a2_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_joint_a3">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint_a3">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint_a3_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_joint_a4">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint_a4">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint_a4_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_joint_a5">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint_a5">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint_a5_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_joint_a6">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint_a6">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint_a6_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/</robotNamespace>
        </plugin>
    </gazebo>
</robot>

3.2 配置kuka_kr16_support功能包中的文件

（1）ros_controllers.launch文件
将<arg name="execution_type" value="$(arg execution_type)" />这一行注释掉

（2）demo_gazebo.launch文件中的urdf_path修改成我们之前配置的test.urdf

（3）ros_controllers.yaml文件
其中controller_list添加：

controller_list:
  - name: arm_position_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint_a1
      - joint_a2
      - joint_a3
      - joint_a4
      - joint_a5
      - joint_a6

具体的关节名根据自己的需要进行修改,然后一定要注意name别弄错了，要不然gazeb中机械臂也一样不会动

该文件中还有一个部分是设置pid的如下：（这一个配置不想看可以直接跳过，不影响）

其实这样子默认生成的是有问题的，就是到时候终端会报错，但是其实又不会影响机械臂的正常工作，可以忽略，但是如果大家想要解决的话，可以将该部分稍微改一下，替换成如下部分：

  /gazebo_ros_control:
    pid_gains:
       joint_a1:
        p: 100.0
        i: 0.01
        d: 10.0
       joint_a2:
        p: 100.0
        i: 0.01
        d: 10.0
       joint_a3:
        p: 100.0
        i: 0.01
        d: 10.0
       joint_a4:
        p: 100.0
        i: 0.01
        d: 10.0
       joint_a5:
        p: 100.0
        i: 0.01
        d: 10.0
       joint_a6:
        p: 100.0
        i: 0.01
        d: 10.0

但是可能会有问题，就是要调参，要不然机械臂会来回摆动，所以最好就干脆忽略算了

四、运行与效果展示

运行前不要忘记catkin_make工作空间

roslaunch kuka_moveit_config demo_gazebo.launch

通过此指令可启动gazebo与rviz的仿真，基本上如果跟着我前面的配置过程基本把网上的所有BUG都解决掉了，除非就是可能缺一些依赖包这样子~
启动以后rviz界面与gazebo界面如图，机械臂处于一个初始状态

可以看到rviz中有一个小球一样的东西，我们可以拖动它，然后随便给一个位姿态，点击Plan & Ex
cute按钮就可以同步让gazebo模型也移动到相应的位置，如下图：

动态效果就不展示了，还是非常nice的！！