3自由度机械臂正逆运动学计算末端连杆丢失问题排查

我搭建了一台3自由度机械臂，想用逆运动学控制末端执行器。用URDF创建了机械臂模型，在URDF Widget Factory中预览结构正常，但用ikpy在Jupyter Lab中运行逆运动学代码并绘图时，看不到最后一段连杆(link2)。

我的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定义本身没有错误，问题出在目标位置的选择：

计算机械臂完全伸展的最大高度：base_link高度0.05 + base_to_link1高度0.074965 + link1长度0.299 + link2长度0.299 = 0.722965，刚好和你设置的target_position一致。此时link2会和link1完全重合在一条竖直线上，3D绘图中视觉上重叠，看起来像是消失了。

修复方法： #

1. 修改目标位置：将目标位置改为非完全伸展状态，比如：

   target_position = [0.3, 0, 0.5]
   

   运行后机械臂会弯曲，link2就会显示出来。

2. 调整视觉区分度：修改link2的视觉属性，比如增大半径或更换颜色，即使重叠也能看到轮廓：

   <geometry>
       <cylinder length="0.299" radius="0.03"/>
   </geometry>
   

内容的提问来源于stack exchange，提问作者Katherine Williams