目的地の座標から Dobot MG400 の各joint の値を逆算【ROS2-Foxy】

はじめに
前提条件
目的地の座標について
逆運動学のライブラリ
ikpy のサンプルプログラム
URDF の修正
逆運動学を実行
おわりに

はじめに

前回は、gazebo_ros2_control の設定方法について説明しました。

今回は、逆運動学を用いて Dobot MG400 の joint の目標値を算出していきます。

前提条件

前提条件は、以下の通りです。

MG400 を Gazebo 上で表示できる

目的地の座標について

ロボットを動かす際は、目標地点を決めてから動かします。
当たり前なのですが、シミュレーションでも当たり前です。

目標地点へ移動する際に、人の手で joint1 は 0.3、joint2 は 0.15 … と指定することは非常に難しいですが、目標地点の xyz + θ 座標を人の手で設定することは簡単です。

各軸のイメージ図です。

やりたいこととしては、目標地点の xyz+θ 座標から、逆運動学を使用してロボット各軸の数値を指定します。

これには大変難しい方程式を用意しないといけないのですが、素晴らしい技術者の方々が逆運動学の方程式を簡単に解決する方法を提供してくれています。

逆運動学のライブラリ

その方法は Python のライブラリで提供されています。
PyPI のページはこちら、github はこちらのページ、ドキュメントはこちらです。

pip3 install ikpy

このライブラリを使用すると、URDF ファイルから逆運動学を算出することが可能になります。

ikpy のサンプルプログラム

ikpy のプログラムを作成していきます。

cd ~/mg400_workspace/src/mg400_training
touch mg400_training/kinematic_solver.py

ikpy を動かすサンプルプログラムは、以下のようになります。

import ikpy.chain
import numpy as np
import os

active_links_mask = [False, True, True, True, True, False, True]
urdf_file = os.path.join("urdf", "mg400_training_gazebo.urdf")
kuka_robot = ikpy.chain.Chain.from_urdf_file(urdf_file, active_links_mask=active_links_mask)
T = kuka_robot.forward_kinematics([0]*7)

print("Transformation matrix", T)
angles = kuka_robot.inverse_kinematics([0.2, 0.5, -0.5])
print("Angles Computer", angles)
angles = np.delete(angles, [0])
print("Corrected angles", list(angles))

早速、動かしてみましょう。

python3 mg400_training/kinematic_solver.py

すると、以下のようなエラーが出ます。

ValueError: Error: link base_link given but not found in the URDF

base_link というリンクが存在しません　というエラーです。

URDF を修正して、エラーが出ないようにしていきましょう。

URDF の修正

mg400_training.urdf を mg400_training_gazebo.urdf として、コピーします。

cp urdf/mg400_training.urdf urdf/mg400_training_gazebo.urdf

mg400_training_gazebo.urdf を編集していきます。
変更後の urdf ファイルを以下に示します。

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from mg400.urdf.xacro               | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="mg400">
  <!-- links -->
  <link name="base_footprint"/>
  <!-- links -->
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.000600637001378879 -2.53487071137343E-07 0.0560927888040551"/>
      <mass value="3.62164811695094"/>
      <inertia ixx="0.0126582175296405" ixy="6.66832041405407E-08" ixz="-3.52548281362717E-06" iyy="0.0145891535260188" iyz="5.15901657460475E-08" izz="0.0195045116656827"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/base_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/base_link.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="origin_link"/>
  <link name="link1">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0046209286967387 -6.88107982318643E-05 0.0709919358465827"/>
      <mass value="1.77056222753813"/>
      <inertia ixx="0.00605885293080088" ixy="0.000156081820035162" ixz="-0.00021785654850453" iyy="0.00579381982249127" iyz="6.20876898015621E-05" izz="0.00741058257251941"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link1.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link2_1">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00321788524756698 0.0352926570826593 0.0596131328734945"/>
      <mass value="0.0866115884541459"/>
      <inertia ixx="0.000338551627157209" ixy="-2.78858228577314E-09" ixz="1.05212346076681E-05" iyy="0.000307789653713489" iyz="-1.36109590152488E-06" izz="3.89949940178023E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link2_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link2_1.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link2_2">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0201111131723385 0.0304999635268521 0.073632809340276"/>
      <mass value="0.0662546292985346"/>
      <inertia ixx="0.000218973873589181" ixy="3.0147933007673E-10" ixz="2.51402553227571E-06" iyy="0.000200510070261609" iyz="-1.41837669904595E-09" izz="3.74316777806776E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link2_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link2_2.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link3_1">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0821679417596488 0.000113044685842874 -0.00333758753063973"/>
      <mass value="0.0375214831045061"/>
      <inertia ixx="2.54761091812684E-06" ixy="3.73102654491378E-07" ixz="-2.02625705871199E-06" iyy="0.000164234314177935" iyz="-1.44731096825347E-08" izz="0.000164331500854172"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link3_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link3_1.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link3_2">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0338566736074436 0.0240208873857428 0.00452229895926692"/>
      <mass value="0.0366692789294813"/>
      <inertia ixx="7.24011830695354E-06" ixy="7.93542746250724E-10" ixz="-1.69163254472566E-06" iyy="2.34967008246727E-05" iyz="4.55507111386579E-10" izz="1.66831643179488E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link3_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link3_2.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link4_1">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0387878182928232 0.0170848333998282 -0.0024610990490942"/>
      <mass value="0.0628979533925801"/>
      <inertia ixx="4.78483605712844E-05" ixy="1.42081909356751E-07" ixz="8.32917079044309E-06" iyy="5.42362047732042E-05" iyz="-2.90608446019989E-08" izz="3.36120409842199E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link4_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link4_1.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link4_2">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0876391591012518 0.023499867452275 0.00831492252441945"/>
      <mass value="0.0453569016333092"/>
      <inertia ixx="2.00037596001325E-05" ixy="3.34474706250153E-09" ixz="2.17594982354756E-07" iyy="0.000132049841023893" iyz="5.36978669669685E-10" izz="0.000141813978894428"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link4_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link4_2.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link4_3">
    <inertial>
      <origin rpy="0 0 0" xyz="0.003174 0.0 -0.028198"/>
      <mass value="0.01907"/>
      <inertia ixx="3.28164E-05" ixy="3.15E-10" ixz="2.42378E-06" iyy="3.17191E-05" iyz="-1.27E-10" izz="1.08143E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link4_3.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link4_3.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="link5">
    <inertial>
      <origin rpy="0 0 0" xyz="4.24246458685662E-07 3.08507851181768E-14 -0.0349688816440933"/>
      <mass value="0.0965276919819365"/>
      <inertia ixx="4.98589992160928E-05" ixy="-1.534865545378E-12" ixz="3.46424449605658E-10" iyy="4.98589881725446E-05" iyz="-5.64890953414983E-17" izz="2.130463008359E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/link5.dae"/>
      </geometry>
    </collision>
  </link>
  <link name="end_effector_flange">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 5.0e-3"/>
      <mass value="24.0e-3"/>
      <inertia ixx="2.058e-6" ixy="0.0" ixz="0.0" iyy="2.058e-6" iyz="0.0" izz="3.335e-6"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/flange.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/flange.dae"/>
      </geometry>
    </collision>
  </link>
  <!-- joints -->
  <joint name="base_joint" type="fixed">
    <parent link="arm_frame_link_offset"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- <joint name="joint_0" type="fixed">
    <origin rpy="0 0 0" xyz="-0.005 0 0.224"/>
    <parent link="base_link"/>
    <child link="origin_link"/>
  </joint> -->
  <joint name="joint_1" type="revolute">
    <origin rpy="0 0 0" xyz="-0.005 0 0.109"/>
    <parent link="base_link"/>
    <child link="link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="0" lower="-3.141592653589793" upper="3.141592653589793" velocity="0"/>
  </joint>
  <joint name="joint_2" type="revolute">
    <origin rpy="0 0 0" xyz="0.0435 -0.035775 0.119"/>
    <parent link="link1"/>
    <child link="link2_1"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-3.14" upper="3.14" velocity="0"/>
  </joint>
  <joint name="joint_3" type="revolute">
    <origin rpy="0 0 0" xyz="0.00452885682964267 -0.0305 0.141500000000001"/>
    <parent link="link1"/>
    <child link="link2_2"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-3.14" upper="3.14" velocity="0"/>
  </joint>
  <joint name="joint_4" type="revolute">
    <origin rpy="0 0 0" xyz="-0.0010512570171516 0.0357748756218898 0.175001164344716"/>
    <parent link="link2_1"/>
    <child link="link3_1"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-3.14" upper="3.14" velocity="0"/>
  </joint>
  <joint name="joint_5" type="revolute">
    <origin rpy="0 0 0" xyz="-0.00105 0.0065 0.175"/>
    <parent link="link2_2"/>
    <child link="link3_2"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-3.14" upper="3.14" velocity="0"/>
  </joint>
  <joint name="joint_6" type="revolute">
    <origin rpy="0 0 0" xyz="0.175 -0.017 0.00325"/>
    <parent link="link3_1"/>
    <child link="link4_1"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-3.14" upper="3.14" velocity="0"/>
  </joint>
  <joint name="joint_7" type="revolute">
    <origin rpy="0 0 0" xyz="0.0679 0.0005 0.011972"/>
    <parent link="link3_2"/>
    <child link="link4_2"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-3.14" upper="3.14" velocity="0"/>
  </joint>
  <!-- <joint name="joint_8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.002"/>
    <parent link="link5"/>
    <child link="link4_3"/>
  </joint> -->
  <joint name="joint_9" type="fixed">
    <origin rpy="0 0 0" xyz="0.066 0.017 0.031"/>
    <parent link="link4_1"/>
    <child link="link5"/>
  </joint>
  <joint name="joint_10" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 -0.084"/>
    <parent link="link5"/>
    <child link="end_effector_flange"/>
    <axis xyz="0 0 1"/>
    <limit effort="0" lower="-3.141592653589793" upper="3.141592653589793" velocity="0"/>
  </joint>

  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
      <parameters>/home/wsl-ubuntu/mg400_workspace/src/mg400_training/config/jtc.yaml</parameters>
    </plugin>
  </gazebo>

  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

    <!-- <joint name="joint_0">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint> -->
    
    <joint name="joint_1">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>
    <joint name="joint_2">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">-1.57</param>   
    </joint>
    <joint name="joint_3">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>
    <joint name="joint_4">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">-1.57</param>   
    </joint>
    <joint name="joint_5">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>
    <joint name="joint_6">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

    <joint name="joint_7">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

    <!-- <joint name="joint_8">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint> -->

    <joint name="joint_9">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

    <joint name="joint_10">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

  </ros2_control>

</robot>

逆運動学を実行

URDFの修正が終わったら、動かしてみましょう。

python3 mg400_training/kinematic_solver.py

特にエラーがなければ、以下のような出力が得られます。

Corrected angles [1.181699520287901, 2.5486868958687228, -1.5582073086991044, -0.695196103055658, 0.0, 0.0]

左から、joint_1, joint_2, joint_4, joint_6, joint_9(fixed), joint_10(end_effector) となります。

この出力を、各軸に与えると、目標位置へ移動することが可能になります。

ここら辺は次回とりかかります。

おわりに

今回は、URDF から逆運動学を解くライブラリの使い方を説明しました。

次回は、目標位置への移動方法について簡単に説明します。