ROS开发笔记（11）——Solidworks机械臂模型转化为URDF+mimic关节联动+gazebo中运动控制测试

URDF（Universal Robot Description Format——通用机器人描述格式）是ROS里使用的一种机器人的描述文件，包含的内容有：连杆、关节，运动学和动力学参数、可视化模型、碰撞检测模型等。简单的机器人模型可以人工编写，但是对于复杂的模型，转动惯量等参数计算复杂，人工编写很费时费力，好在ROS wiki 中提供一款将Solidworks模型转换为URDF文件的插件sw2urdfSetup。最近从淘宝上购买了一款开源的六自由度的机械臂，卖家提供了完整的Solidworks装配图及零件图（如下图示），但是并没有提供该机械臂的ROS驱动，也没有URDF模型，打算自行完成ROS相关驱动支持，并开展基于该机械臂的强化学习控制研究。

本机械臂看似简单，也包含了近90个零件，其中机械抓部分相对特殊与复杂，因为机械抓虽然包含有6个关节，但是只有一个关节直接受舵机驱动，其余的关节由这个关节联动，sw2urdfSetup插件不能直接处理这种情况，必须对生成的URDF文件进行调整。

本文完成了机械臂机械抓部分的URDF模型建立以及gazebo运动仿真的支持，具体包含以下几个过程：

（1）solidworks中将机械抓部分装配体的建立与调整

这部分工作是将机械抓部分的零件拿出来建立一个单独的装配体,如下：

然后在装配体内部根据URDF中每个link包含的部分聚合成不同的装配体，各部分如下（依次为base_link、left_knuckle_link、right_knuckle_link、left_inner_knuckle_link、right_inner_knuckle_link、left_finger_link、right_finger_link）：

注意每一个装配体link上要建立坐标系与基准轴（编辑link装配体时菜单操作：插入——参考几何体——基准轴或坐标系），其中基准轴用于选择表示为joint的运动轴，子link的坐标系原点在父link对应的基准轴方向上（一个父link对应几个子link就有几个基准轴），坐标系的一个轴应该与基准轴平行。这些坐标系与基准轴在sw2urdfSetup插件生成URDF文件时需要。

基准轴与坐标系应该在link的子装配体中添加，如上面第一幅图示，第二副图是带坐标系与基准轴显示的总的装配体模型，将材料改成了黄铜，所以颜色变了，我这里没有进一步调整了。

（2）基于sw2urdfSetup插件生成机械抓URDF文件

可以参考下面的博客文章和ROS wiki，注意有三个地方需要修改：  

https://blog.csdn.net/gpeng832/article/details/73917487

我这里的版本中实际操作与上面链接中有所不同：加载插件后，插件菜单位置在 工具——file——export as URDF，而不是教程里说的工具栏。

配置每个link，主要是坐标系与基准轴不要搞错了：

点击 “preview and export”,配置每个关节jiont，主要是配置限制 limit ,一个都不能空，一开始我的 速度与力矩 没有设置，自动生成了零，会导致gazebo中关节没法运动：

点击 next,大部分都是自动生成:

最后点完成生成相关文件，保存对话框文件名称中的SolidWorks装配体后缀要去掉。参考下面链接修改三个地方。

 https://blog.csdn.net/gpeng832/article/details/73917487 

生成文件目录如下:

（3）基于mimic_joint_plugin等在gazebo中实现关节联动控制

SolidWorks插件自动生成的urdf模型不具有关节联动控制的效果，需要进行修改，下面将修改后的源码贴出来。

le_arm_gripper.transmission.xacro：

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
  
  <xacro:property name="PI" value="3.14159265359" />
  <xacro:macro name="le_arm_gripper_transmission" params="prefix">

      <transmission name="${prefix}left_knuckle_joint_transmission">
          <type>transmission_interface/SimpleTransmission</type>
          <joint name="${prefix}left_knuckle_joint">
            <hardwareInterface>PositionJointInterface</hardwareInterface>
          </joint>
          <actuator name="${prefix}left_knuckle_motor">	 
            <mechanicalReduction>1</mechanicalReduction>
            <hardwareInterface>PositionJointInterface</hardwareInterface>
          </actuator>
      </transmission>

      <gazebo>
        <plugin name="${prefix}MimicJointPlugin_right_finger" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
            <joint>${prefix}left_knuckle_joint</joint>
            <mimicJoint>${prefix}right_finger_joint</mimicJoint>
        </plugin>

          <plugin name="${prefix}MimicJointPlugin_right_inner_knuckle" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
            <joint>${prefix}left_knuckle_joint</joint>
            <mimicJoint>${prefix}right_inner_knuckle_joint</mimicJoint>
            <multiplier>-1</multiplier>
            <offset>0.0</offset>
          </plugin>

          <plugin name="${prefix}MimicJointPlugin_left_finger" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
            <joint>${prefix}left_knuckle_joint</joint>
            <mimicJoint>${prefix}left_finger_joint</mimicJoint>
            <multiplier>-1</multiplier>
            <offset>0.0</offset>	
          </plugin>
            
            <plugin name="${prefix}MimicJointPlugin_left_inner_knuckle" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
            <joint>${prefix}left_knuckle_joint</joint>
            <mimicJoint>${prefix}left_inner_knuckle_joint</mimicJoint>
          </plugin>

            <plugin name="${prefix}Mimic_right_knuckle" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
            <joint>${prefix}left_knuckle_joint</joint>
            <mimicJoint>${prefix}right_knuckle_joint</mimicJoint>
            <multiplier>-1</multiplier>
            <offset>0.0</offset>	
          </plugin>  
      </gazebo> 
  </xacro:macro>

</robot>

le_arm_gripper.xacro：

<?xml version="1.0" ?>

<robot name="le_arm_gripper" xmlns:xacro="http://ros.org/wiki/xacro">
    <xacro:include filename="$(find gripper)/robots/le_arm_gripper.urdf.xacro" />
    
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> 
            <robotNamespace>/</robotNamespace>
        </plugin> 
    </gazebo>
    
    <link name="world"/>
    <joint name="world_fixed" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <parent link="world"/>
        <child link="gripper_root_link"/>
    </joint>

    <link name="gripper_root_link">
        <visual>
            <origin xyz="0 0 0.5" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 1.0"/>
            </geometry>
        </visual>
        <collision>
            <origin xyz="0 0 0.5" rpy="0 0 0"/>
            <geometry>
                <box size="0.1 0.1 1.0"/>
            </geometry>
        </collision>
        <inertial>
            <mass value="1e2" />
            <origin xyz="0.0 0.0 0.0" />
            <inertia ixx = "0.000001" ixy = "0.000000" ixz = "0.000000"
                     iyx = "0.000000" iyy = "0.000001" iyz = "0.000000"
                     izx = "0.000000" izy = "0.000000" izz = "0.000001" />
        </inertial>
    </link>
    
    <xacro:le_arm_gripper prefix="" parent="gripper_root_link" >
        <origin xyz="0.05 0 0.85" rpy="0 1.57 0"/>
    </xacro:le_arm_gripper>
    
</robot>

le_arm_gripper.urdf.xacro文件太长，这里给出了部分源码：

  <xacro:macro name="le_arm_gripper" params="prefix parent *origin">         
        <joint name="${prefix}gripper_base_joint" type="fixed">
            <parent link="${parent}"/>
            <child link="${prefix}base_link"/>
            <xacro:insert_block name="origin"/>
        </joint>
        <link
            name="base_link">
            <inertial>
                <origin
                    xyz="-0.016093 0.0024206 0.03061"
                    rpy="0 0 0" />
                <mass
                    value="0.23873" />
                <inertia
                    ixx="2.6536E-05"
                    ixy="-2.1569E-06"
                    ixz="7.2581E-06"
                    iyy="5.5255E-05"
                    iyz="-1.0197E-06"
                izz="6.0895E-05" />
            </inertial>
            <visual>
            <origin
                xyz="0 0 0"
                rpy="0 0 0" />
            <geometry>
                <mesh
                filename="package://gripper/meshes/base_link.STL" />
            </geometry>
            <material
                name="">
                <color
                rgba="0.75294 0.75294 0.75294 1" />
            </material>
            </visual>
            <collision>
            <origin
                xyz="0 0 0"
                rpy="0 0 0" />
            <geometry>
                <mesh
                filename="package://gripper/meshes/base_link.STL" />
            </geometry>
            </collision>
        </link>
        <gazebo reference="${prefix}base_link">
            <implicitSpringDamper>1</implicitSpringDamper>
            <mu1>100000</mu1>
            <mu2>100000</mu2>
            <kp>100000000.0</kp>
            <kd>1.0</kd>
        </gazebo> 
        <link
            name="left_knuckle_link">
            <inertial>
            <origin
                xyz="0.0004174 0.0081181 0.00092142"
                rpy="0 0 0" />
            <mass
                value="0.0024316" />
            <inertia
                ixx="1.2495E-07"
                ixy="2.3126E-09"
                ixz="-5.8721E-10"
                iyy="9.2991E-09"
                iyz="2.9378E-08"
                izz="1.1574E-07" />
            </inertial>
            <visual>
            <origin
                xyz="0 0 0"
                rpy="0 0 0" />
            <geometry>
                <mesh
                filename="package://gripper/meshes/left_knuckle_link.STL" />
            </geometry>
            <material
                name="">
                <color
                rgba="0.75294 0.75294 0.75294 1" />
            </material>
            </visual>
            <collision>
            <origin
                xyz="0 0 0"
                rpy="0 0 0" />
            <geometry>
                <mesh
                filename="package://gripper/meshes/left_knuckle_link.STL" />
            </geometry>
            </collision>
        </link>

        <gazebo reference="${prefix}left_knuckle_link">
            <implicitSpringDamper>1</implicitSpringDamper>
            <mu1>100000</mu1>
            <mu2>100000</mu2>
            <kp>100000000.0</kp>
            <kd>1.0</kd>
        </gazebo> 

        <joint
            name="left_knuckle_joint"
            type="revolute">
            <origin
            xyz="0.0045 -0.014 0.0325"
            rpy="1.5708 -0.59692 1.5708" />
            <parent
            link="base_link" />
            <child
            link="left_knuckle_link" />
            <axis
            xyz="0 0 1" />
            <limit
            lower="-1.57"
            upper="1.57"
            effort="1000"
            velocity="2.0" />
        </joint>
        <link
            name="right_knuckle_link">
            <inertial>
            <origin
                xyz="0.00045059 0.0080041 0.0020288"
                rpy="0 0 0" />
            <mass
                value="0.0024778" />
            <inertia
                ixx="1.411E-07"
                ixy="1.9567E-09"
                ixz="5.9037E-10"
                iyy="9.5283E-09"
                iyz="-3.0312E-08"
                izz="1.3178E-07" />
            </inertial>
            <visual>
            <origin
                xyz="0 0 0"
                rpy="0 0 0" />
            <geometry>
                <mesh
                filename="package://gripper/meshes/right_knuckle_link.STL" />
            </geometry>
            <material
                name="">
                <color
                rgba="0.75294 0.75294 0.75294 1" />
            </material>
            </visual>
            <collision>
            <origin
                xyz="0 0 0"
                rpy="0 0 0" />
            <geometry>
                <mesh
                filename="package://gripper/meshes/right_knuckle_link.STL" />
            </geometry>
            </collision>
        </link>

        <gazebo reference="${prefix}right_knuckle_link">
            <implicitSpringDamper>1</implicitSpringDamper>
            <mu1>100000</mu1>
            <mu2>100000</mu2>
            <kp>100000000.0</kp>
            <kd>1.0</kd>
        </gazebo> 

        <joint
            name="right_knuckle_joint"
            type="revolute">
            <origin
            xyz="0.0075 0.014 0.0325"
            rpy="1.5708 -0.69513 -1.5708" />
            <parent
            link="base_link" />
            <child
            link="right_knuckle_link" />
            <axis
            xyz="0 0 -1" />
            <limit
            lower="-1.57"
            upper="1.57"
            effort="1000"
            velocity="2.0" />
            <mimic joint="${prefix}left_knuckle_joint" multiplier="-1"/>
        </joint>

另外，要实现关节位置控制，还需要添加相关控制器配置文件：

gripper_controller.yaml 

gripper_controller:
  type: position_controllers/JointTrajectoryController
  joints:
     - left_knuckle_joint
  constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      left_knuckle_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10

joint_state_controller.yaml 

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

启动文件如下：

controller_utils.launch 

<?xml version="1.0"?>
<launch>

  <!-- Robot state publisher -->
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="50.0" />
    <param name="tf_prefix" type="string" value="" />
  </node>

  <!-- Fake Calibration -->
  <node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
        args="pub /calibrated std_msgs/Bool true" />
  
  <!-- joint_state_controller -->
  <rosparam file="$(find gripper)/controller/joint_state_controller.yaml" command="load"/>
  <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller" />
 
</launch>

upload_le_arm_gripper.launch 

<launch>
    <param  name="robot_description"  command="$(find xacro)/xacro --inorder '$(find gripper)/robots/le_arm_gripper.xacro'" />  
</launch>

le_arm_gripper_gazebo.launch 

<?xml version="1.0"?>
<launch>
  <!-- <arg name="limited" default="false"/> -->
  <arg name="paused" default="false"/>
  <arg name="gui" default="true"/>
  
  <!-- startup simulated world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="worlds/empty.world"/>
    <arg name="paused" value="$(arg paused)"/>
    <arg name="gui" value="$(arg gui)"/>
  </include>

  <!-- send robot urdf to param server -->
  <include file="$(find gripper)/launch/upload_le_arm_gripper.launch"/>

  <!-- push robot_description to factory and spawn robot in gazebo -->
  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot" respawn="false" output="screen" />

  <include file="$(find gripper)/launch/controller_utils.launch"/>
  <rosparam file="$(find gripper)/controller/gripper_controller.yaml" command="load"/> 
  <node name="gripper_controller_spawner" pkg="controller_manager" type="spawner" args="gripper_controller --shutdown-timeout 0.5" />

</launch>

 

（4）效果展示

执行测试命令为：

rostopic  pub /gripper_controller/command trajectory_msgs/JointTrajectory '{joint_names:["left_knuckle_joint"],points: [{positions:[0.5], time_from_start: [1.0,0.0]}]}' -1

 

说明控制指令已经正确执行，这里没有纹理，将mesh文件将stl格式的文件由dae文件替代可以展示出纹理。

下一步将机械臂的其它部分都添加完毕后一同进行moveit的配置。