MuJoCo仿真（3）将aubo-i5模型导入mujoco环境

在前述 MuJoCO仿真（1） MuJoCo210 Win10安装以及 MuJoCo仿真（2）MuJoCo210 Linux安装中讲述了使用 ur5.xml模型进行导入，对MuJoCo和mujoco-py进行了验证。由于实际需要用到的机器人为 aubo-i5，在 aubo的官方github中仅提供了 aubo-i5的 urdf模型，无法直接导入进行使用，因此需要对 urdf进行转换。

1 模型准备

从aubo的官方github中下载项目：

git clone https://github.com/AuboRobot/aubo_robot.git

进入项目文件夹，我们可以看到：

1.1 urdf

首先从aubo_description/urdf中找到aubo_i5.urdf文件,复制到自己的项目目录(“/home/xxx/program/aubo”, 根据自己的需要确定)中，或者在自己的项目目录下创建一个新的urdf文件，直接复制以下内容到文件中：

<?xml version="1.0"?>
<robot  name="aubo_i5">
  <link name="base_link">
    <inertial>
      <origin xyz="-1.4795E-13 0.0015384 0.020951" rpy="0 0 0" />
      <mass value="0.83419" />
      <inertia ixx="0.0014414" ixy="7.8809E-15" ixz="8.5328E-16" iyy="0.0013542" iyz="-1.4364E-05" izz="0.0024659" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/base_link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/base_link.STL" />
      </geometry>
    </collision>
  </link>

  <link name="shoulder_Link">
    <inertial>
      <origin xyz="3.2508868974735E-07 0.00534955349296065 -0.00883689325611056" rpy="0 0 0" />
      <mass value="1.57658348693929" />
      <inertia ixx="0.0040640448663128" ixy="0" ixz="0" iyy="0.00392863238466817" iyz="-0.000160151642851425" izz="0.0030869857349184" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/shoulder_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/shoulder_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="shoulder_joint" type="revolute">
    <origin xyz="0 0 0.122" rpy="0 0 3.1416" />
    <parent link="base_link" />
    <child link="shoulder_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-3.04" upper="3.04" effort="0" velocity="0" />
  </joint>

  <link name="upperArm_Link">
    <inertial>
      <origin xyz="0.203996646979614 2.01304585036544E-10 0.0127641545395984" rpy="0 0 0" />
      <mass value="4.04175782265494" />
      <inertia ixx="0.00965399211106204" ixy="0" ixz="0" iyy="0.144993869035655" iyz="0" izz="0.142607184038966" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/upperArm_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/upperArm_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="upperArm_joint" type="revolute">
    <origin xyz="0 0.1215 0" rpy="-1.5708 -1.5708 0" />
    <parent link="shoulder_Link" />
    <child link="upperArm_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-3.04" upper="3.04" effort="0" velocity="0" />
  </joint>

  <link name="foreArm_Link">
    <inertial>
      <origin xyz="0.188922115560337 6.78882434739072E-07 0.0981026740461557" rpy="0 0 0" />
      <mass  value="2.27145669098343" />
      <inertia ixx="0.00214322284946289" ixy="0" ixz="-0.00073120631553383" iyy="0.0443926090878205" iyz="0" izz="0.0441273797128365" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/foreArm_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/foreArm_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="foreArm_joint" type="revolute">
    <origin xyz="0.408 0 0" rpy="-3.1416 -5.1632E-18 -5.459E-16" />
    <parent link="upperArm_Link" />
    <child link="foreArm_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-3.04" upper="3.04" effort="0" velocity="0" />
  </joint>

  <link name="wrist1_Link">
    <inertial>
      <origin xyz="7.54205137428592E-07 0.0062481254331257 -0.00392367464072373" rpy="0 0 0" />
      <mass value="0.500477539188764" />
      <inertia ixx="0.00071194605962081" ixy="0" ixz="0" iyy="0.00040588242872958" iyz="-2.30808694377512E-05" izz="0.000685574004861334" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/wrist1_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/wrist1_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="wrist1_joint" type="revolute">
    <origin xyz="0.376 0 0" rpy="3.1416 -1.8323E-15 1.5708" />
    <parent link="foreArm_Link" />
    <child link="wrist1_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-3.04" upper="3.04" effort="0" velocity="0" />
  </joint>

  <link name="wrist2_Link">
    <inertial>
      <origin xyz="-7.54207620578635E-07 -0.00624812542617262 -0.00392367464115684" rpy="0 0 0" />
      <mass value="0.500477539245988" />
      <inertia ixx="0.00071194605981829" ixy="0" ixz="0" iyy="0.000405882428755442" iyz="2.30808694515886E-05" izz="0.000685574005112107" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/wrist2_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/wrist2_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="wrist2_joint" type="revolute">
    <origin xyz="0 0.1025 0" rpy="-1.5708 -1.8709E-15 -1.6653E-16" />
    <parent link="wrist1_Link" />
    <child link="wrist2_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-3.04" upper="3.04" effort="0" velocity="0" />
  </joint>

  <link name="wrist3_Link">
    <inertial>
      <origin xyz="3.92048778449938E-10 0.000175788057281467 -0.0213294490706684" rpy="0 0 0" />
      <mass value="0.158309554874285" />
      <inertia ixx="7.31376196034769E-05" ixy="0" ixz="0" iyy="7.19528188876563E-05" iyz="0" izz="0.000108772439051422" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/visual/wrist3_Link.DAE" />
      </geometry>
      <material name="">
        <color rgba="1 1 1 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://aubo_description/meshes/aubo_i5/collision/wrist3_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint name="wrist3_joint" type="revolute">
    <origin xyz="0 -0.094 0" rpy="1.5708 0 1.7907E-15" />
    <parent link="wrist2_Link" />
    <child link="wrist3_Link" />
    <axis xyz="0 0 1" />
    <limit lower="-3.04" upper="3.04" effort="0" velocity="0" />
  </joint>

  <link name="world" />

  <joint name="world_joint" type="fixed">
    <parent link="world" />
    <child link = "base_link" />
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
  </joint>
</robot>

1.2 .stl

我们还需将aubo-i5本体对应的mesh文件复制到自己的项目中（其实不复制，直接从其文件夹中进行索引也可，但是那样路径就会有点长，不是很方便），在自己的项目目录下创建mesh文件夹，同时在aubo项目目录下的aubo_description/meshes/collision/找到对应的STL文件，将其移动至刚刚创建的mesh文件夹中。

至此就完成了转换的基本准备。

2 aubo_i5.urdf转aubo_i5.xml

MuJoCo提供了模型的转换工具，执行文件为MuJoCo项目的bin文件夹下的compile：

compile的使用语法为：

./compile   "source_urdf_path"   "target_xml_path"

但是不能直接对aubo-i5的urdf进行转换，需要做一些修改，可参考urdf转xml，即在原urdf文件中加入下述节点：

  <mujoco>
        <compiler 
        meshdir="mesh_path" 
        balanceinertia="true" 
        discardvisual="false" />
  </mujoco>

这里的mesh_path即刚刚自己创建的mesh文件夹的路径，对于这点网上还是很多相关参考资料的，这里我不做过多的阐述，但是我想强调的是，我们要注意mujoco节点添加的位置应该在robot节点下，如下：

对于剩下两个参数正常情况保持默认即可。
此外在urdf中还需要需改的地方有为每个轴的STL文件的路径：

修改成自己的mesh文件的路径，总共有六个关节，六个都需要修改。
进而我们就可以使用MuJoCo提供的原生的compile工具进行转化了：

cd ~/mujoco210/bin #进入mujoco项目的可执行文件目录，根据自己的进行修改
# ./compile   "source_urdf_path"   "target_xml_path"
./compile ~/Program/force_control/aubo_i5/aubo_i5.urdf ~/Program/force_control/aubo_i5/aubo_i5.xml 

转换完成后，我们使用MuJoCo提供的原生的simulate工具进行验证：

/simulate ~/Program/force_control/aubo_i5/aubo_i5.xml

验证结果如下：

这里由于重力的影响，且关节尚没有加上驱动力，故机器人就自己缩成一团了，至此即完成了对模型的导入。