逆向和正向运动学忽略计算中的最后一个环节

Question

我已经构建了一个 3 DOF 机器人手臂，现在我想借助逆运动学来控制末端执行器。我使用 urdf 扩展创建了我的机器人手臂的基本版本，并使用 Jupyter Lab 借助 ikpy 创建了逆向运动学代码。

当我检查 URDF Widget Factory 时，机器人手臂的结构看起来不错。但是当我运行逆运动学代码并绘制机器人手臂时，我看不到最后一个链接。

这是我的 urdf 文件：

<robot name="My Robot">
    
    <link name="base_link">
        <visual>
            <origin xyz="0 0 0.0225" rpy="0 0 0"/>
            <geometry>
                <cylinder length="0.050" radius="0.089"/>
            </geometry>
            <material name="base_color">
                <color rgba="1 0 0 1"/>
            </material>
        </visual>   
    </link>

    <link name="base_to_link1">
        <visual>
            <geometry>
                <box size="0.05 0.05 0.074965" />
            </geometry>
            <material name="base_to_link1_color">
                <color rgba="0 1 0 1"/>
            </material>
        </visual>   
    </link>
    
    <joint name="base_joint" type="revolute">
      <parent link="base_link"/>
      <child link="base_to_link1"/>
      <origin xyz="0 0 0.0874825" rpy="0 0 0" />
      <axis xyz="0 0 -1"/>
      <limit lower="-3.1" upper="3.1" />
    </joint>
    
    <link name="link1">
        <visual>
          <origin xyz="0 0 0.1495" rpy="0 0 0" />
            <geometry>
                <cylinder length="0.299" radius="0.022"/>
            </geometry>
            <material name="link1_color">
                <color rgba="0 0 1 1"/>
            </material>
        </visual>   
    </link>
    
    <joint name="second_joint" type="revolute">
      <parent link="base_to_link1"/>
      <child link="link1"/>
      <origin xyz="0 0 0.0374825" rpy="0 0 0" />
      <axis xyz="1 0 0"/>
      <limit lower="-1.57" upper="1.57" />
    </joint>
    
    <link name="link2">
        <visual>
          <origin xyz="0 0 0.1495" rpy="0 0 0" />
            <geometry>
                <cylinder length="0.299" radius="0.022"/>
            </geometry>
            <material name="link2_color">
                <color rgba="1 1 0 1"/>
            </material>
        </visual>   
    </link>

    <joint name="third_joint" type="revolute">
      <parent link="link1"/>
      <child link="link2"/>
      <origin xyz="0 0 0.299" rpy="0 0 0" />
      <axis xyz="1 0 0"/>
        <limit lower="-1.92" upper="1.92" />
    </joint>
    
</robot>

这是我的 Jupyter Notebook 代码：

import ikpy.chain
import ikpy.utils.plot as plot_utils

import numpy as np
import time
import math

import ipywidgets as widgets
import serial

my_chain = ikpy.chain.Chain.from_urdf_file("arm_urdf.urdf", active_links_mask=[False, True, True, True])

target_position = [ 0, 0, 0.722965]

ik = my_chain.inverse_kinematics(target_position)

computed_position = my_chain.forward_kinematics(ik)

%matplotlib widget
import matplotlib.pyplot as plt
fig, ax = plot_utils.init_3d_figure()
fig.set_figheight(9)  
fig.set_figwidth(13)  
my_chain.plot(ik, ax, target=target_position)
plt.xlim(-0.5, 0.5)
plt.ylim(-0.5, 0.5)
ax.set_zlim(0, 1)
plt.ion()

这是 URDF 小部件工厂：

URDF 小部件工厂

这是图表的输出：

代码图表

我相信问题出在我的 urdf 文件上，我尝试检查有关如何创建模型的文档，但我找不到我的错误。

Answer 1

我花了更多时间来解决这个问题，我想分享我的解决方案，以防其他人遇到同样的问题。

似乎最后一个链接在这个过程中总是被忽略（或者至少，我找不到另一种方法来解决这个问题）。当我添加末端执行器（只是一个简单的球体）时，它也被忽略，因为它是创建的最后一个“链接”，而其余链接在计算和图表中都被考虑在内。

因此，我只是添加了一个末端执行器并将其设置为自身与最后一个链接 (link2) 之间的固定关节（固定是因为它只是两个部件之间的牢固连接）。通过此设置，IKPy 中的逆运动学函数现在可以完美运行！

这是带有末端执行器的新 urdf 文件：

<robot name="My Robot">

<material name="red">
    <color rgba="1 0 0 1"/>
</material>

<material name="green">
    <color rgba="0 1 0 1"/>
</material>

<material name="blue">
    <color rgba="0 0 1 1"/>
</material>

<material name="yellow">
    <color rgba="1 1 0 1"/>
</material>

<link name="base_link">
    <visual>
        <origin xyz="0 0 0.025" rpy="0 0 0"/>
        <geometry>
            <cylinder length="0.05" radius="0.089"/>
        </geometry>
        <material name="red">
        </material>
    </visual>   
</link>

<joint name="base_joint" type="revolute">
  <parent link="base_link"/>
  <child link="base_to_link1"/>
  <origin xyz="0 0 0.05" rpy="0 0 0" />
  <axis xyz="0 0 -1"/>
  <limit lower="-3.1" upper="3.1" />
</joint>

<link name="base_to_link1">
    <visual>
        <origin xyz="0 0 0.0374825" rpy="0 0 0"/>
        <geometry>
            <box size="0.05 0.05 0.074965" />
        </geometry>
        <material name="green">
        </material>
    </visual>   
</link>

<joint name="second_joint" type="revolute">
  <parent link="base_to_link1"/>
  <child link="link1"/>
  <origin xyz="0 0 0.074965" rpy="0 0 0" />
  <axis xyz="1 0 0"/>
  <limit lower="-1.57" upper="1.57" />
</joint>

<link name="link1">
    <visual>
      <origin xyz="0 0 0.1495" rpy="0 0 0" />
        <geometry>
            <cylinder length="0.299" radius="0.022"/>
        </geometry>
        <material name="blue">
        </material>
    </visual>   
</link>

<joint name="third_joint" type="revolute">
  <parent link="link1"/>
  <child link="link2"/>
  <origin xyz="0 0 0.299" rpy="0 0 0" />
  <axis xyz="1 0 0"/>
    <limit lower="-1.92" upper="1.92" />
</joint>

<link name="link2">
    <visual>
      <origin xyz="0 0 0.1495" rpy="0 0 0" />
        <geometry>
            <cylinder length="0.299" radius="0.022"/>
        </geometry>
        <material name="yellow">
        </material>
    </visual>   
</link>

<joint name="link2_to_endeffector" type="fixed">
  <parent link="link2"/>
  <child link="endeffector"/>
  <origin xyz="0 0 0.299" rpy="0 0 0" />
  <axis xyz="1 0 0"/>
</joint>

<link name="endeffector">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
            <sphere radius="0.022"/>
        </geometry>
        <material name="red">
        </material>
    </visual>   
</link>
</robot>

不要忘记编辑主代码中的active_links_mask！现在我们多了一个连杆（末端执行器），而且它是一个固定连杆！

my_chain = ikpy.chain.Chain.from_urdf_file("arm_urdf.urdf", active_links_mask=[False, True, True, True, False])

最后，这是图表的结果！

图表结果