在moveit2中实现四连杆及曲柄滑块

对于一些特殊的（比如说机械构件闭环的、存在被动关节的）运动模型，该如何用urdf模型描述该模型，且在MoveIt2中进行仿真？下面提供一个思路，仅供参考。

四连杆机构

结构介绍

该机构的组成如下图所示。
粉红色、蓝色的底座为固定机构，黄色、灰色、青色的三根杠与彼此之间、与底座之间都是铰链连接，可以转动。其中黄色与灰色杆子连接部分的关节为主动轴，可以主动发起旋转（可以理解在此处装了马达）。

urdf文件

fourbar.urdf

<?xml version="1.0"?>
<robot name="fourbar">

  <!-- base link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 -0.06" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.20"/>
      </geometry>s
      <material name="wtf0">
        <color rgba="0.9 0.2 0.3 1.0" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 -0.06" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.20"/>
      </geometry>
    </collision>
  </link>

  <link name="bar1">
    <visual>
      <origin xyz="0 0 0.75" rpy="0 0 0"/>
      <geometry>
        <box size="0.15 0.1 1.5"/>
      </geometry>
      <material name="wtf1">
        <color rgba="0.5 0.5 0.3 1.0" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.75" rpy="0 0 0"/>
      <geometry>
        <box size="0.15 0.1 1.5"/>
      </geometry>
    </collision>
  </link>

  <joint name="joint1" type="continuous">
    <parent link="base_link" />
    <child link="bar1" />
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="bar2">
    <visual>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <geometry>
        <box size="0.15 0.1 1.0"/>
      </geometry>
      <material name="wtf2">
        <color rgba="0.5 0.5 0.6 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.15 0.1 1.0"/>
      </geometry>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
    </collision>
  </link>

  <joint name="joint2" type="continuous">
    <parent link="bar1" />
    <child link="bar2" />
    <origin xyz="0 -0.1 1.5" rpy="0 2.5 0" />
    <axis xyz="0 1 0"/>
  </joint>

  <link name="bar3">
    <visual>
      <origin xyz="0 0 0.75" rpy="0 0 0"/>
      <geometry>
        <box size="0.15 0.1 1.5"/>
      </geometry>
      <material name="wtf3">
        <color rgba="0.1 0.5 0.7 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.15 0.1 1.5"/>
      </geometry>
      <origin xyz="0 0 0.75" rpy="0 0 0"/>
    </collision>
  </link>

  <joint name="joint3" type="continuous">
    <parent link="bar2" />
    <child link="bar3" />
    <origin xyz="0 -0.1 1.0" rpy="0 -0.45 0" />
    <axis xyz="0 1 0"/>
  </joint>

  <link name="base_link2">
    <visual>
      <origin xyz="0 0 -0.06" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.20"/>
      </geometry>
      <material name="wtf4">
        <color rgba="0. 0.1 0.5 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.12" radius="0.20"/>
      </geometry>
      <origin xyz="0 0 -0.06" rpy="0 0 0"/>
    </collision>
  </link>

  <joint name="joint4" type="continuous">
    <parent link="bar3" />
    <child link="base_link2" />
    <origin xyz="0 0 1.5" rpy="0 -2.05 0" />
    <axis xyz="0 1 0"/>
  </joint>

</robot>

MoveIt设置

关于如何根据urdf文件搭建MoveIt2工程，可以看看我之前写的：【在ROS2中，通过MoveIt2控制Gazebo中的自定义机械手】
步骤和之前的类似,主要注意一下这几个问题。
起始link和终点link分别选base_link、base_link2:

设置好主动关节和从动关节
这里将主动关节右移为从动关节时，主动关节这边还保留着，可能是个bug，先不管。总之，总的关节减去右边的从动关节，剩下的就是主动关节了。
可以看到，像那么一回事了，虽然末端还是会抖动。

四连杆机构

主要的难点在于urdf无法描述闭环的结构树。理论上base_link2是要固定到base_link上的，但是不允许这样做。
因此只能在moveIt中控制好终端机构的位姿来见解实现了。

曲柄滑块机构

结构介绍

如何在moveit中实现下面这个曲柄滑块机构？【图像来源】

urdf文件

barSlider2.urdf

<?xml version="1.0"?>
<robot name="barSlider2">

  <!-- base link -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.12"/>
      </geometry>s
      <material name="wtf0">
        <color rgba="0.9 0.2 0.3 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.12" radius="0.12"/>
      </geometry>
      <origin xyz="0 0 0" rpy="1.57 0 0"/>
    </collision>
  </link>

  <link name="bar1">
    <visual>
      <origin xyz="0 0 0.35" rpy="0 0 0"/>
      <geometry>
        <box size="0.24 0.1 0.7"/>
      </geometry>
      <material name="wtf1">
        <color rgba="0.5 0.5 0.3 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.24 0.1 0.7"/>
      </geometry>
      <origin xyz="0 0 0.35" rpy="0 0 0"/>
    </collision>
  </link>

  <joint name="joint1" type="continuous">
    <parent link="base_link" />
    <child link="bar1" />
    <origin xyz="0 0 0.0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <link name="bar2">
    <visual>
      <origin xyz="0 0 1.1" rpy="0 0 0"/>
      <geometry>
        <box size="0.15 0.1 2.2"/>
      </geometry>
      <material name="wtf2">
        <color rgba="0.5 0.5 0.6 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.15 0.1 2.2"/>
      </geometry>
      <origin xyz="0 0 1.1" rpy="0 0 0"/>
    </collision>
  </link>

  <joint name="joint2" type="continuous">
    <parent link="bar1" />
    <child link="bar2" />
    <origin xyz="0 -0.1 0.7" rpy="0 1.9 0" />
    <axis xyz="0 1 0"/>
  </joint>


  <link name="base_link2">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.6 0.1 0.3"/>
      </geometry>
      <material name="wtf4">
        <color rgba="0. 0.1 0.5 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.6 0.1 0.3"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </collision>
  </link>

  <joint name="joint3" type="continuous">
    <parent link="bar2" />
    <child link="base_link2" />
    <origin xyz="0 -0.1 2.2" rpy="0 -1.9 0" />
    <axis xyz="0 1 0"/>
  </joint>

  <link name="base_link3">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="1.0 0.5 0.1"/>
      </geometry>
      <material name="wtf5">
        <color rgba="0. 0.5 0.5 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.6 0.1 0.3"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </collision>
  </link>

  <joint name="joint4" type="prismatic">
    <parent link="base_link2" />
    <child link="base_link3" />
    <origin xyz="0 0 -0.2" rpy="0 0 0" />
    <axis xyz="1 0 0"/>
   <limit effort="1000.0" lower="-10" upper="10" velocity="0.5"/>
  </joint>

</robot>

MoveIt设置

和前面的四连杆机构类似，
效果如下：

曲柄滑块