Dobot MG400 の各jointへ数値を送信 URDF修正編【ROS2-Foxy】

ROS2
2023.01.28
スポンサーリンク

スポンサーリンク

はじめに

前回は、MG400 の各joint へ値を送信する準備をしました。

Dobot MG400 の各jointへ数値を送信 準備編【ROS2-Foxy】
Dobot MG400 の各jointへ数値を送信 準備編【ROS2-Foxy】 逆運動学で算出した XYZ 座標へ Dobot MG400 を移動させるための準備をしていきます。
nutritionfoodtech.com
2023.01.26

今回は、launch ファイルの作成と URDF を修正していきます。

前提条件

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

  • 前回の記事で joint publish の準備が完了している

gazebo と controller を起動する launch ファイル

publish するプログラムは作成しましたが、このままでは受け取り側の準備ができていません。
なので、launch ファイルを作成していきます。

cd ~/mg400_workspace/src/mg400_training
touch launch/controller.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node
from scripts import GazeboRosPaths

def generate_launch_description():
    package_share_dir = get_package_share_directory("mg400_training")
    urdf_file = os.path.join(package_share_dir, "urdf", "mg400_training_gazebo.urdf")
    
    controller_file = os.path.join(package_share_dir, "config", "jtc_controller.yaml")
    robot_description = {"robot_description": urdf_file}
    return LaunchDescription(
        [
            ExecuteProcess(
                cmd=["gazebo","-s","libgazebo_ros_factory.so",],
                output="screen",
            ),
            Node(
                package="gazebo_ros",
                executable="spawn_entity.py",
                arguments=["-entity","mg400_training", "-b", "-file", urdf_file,],
            ),
            Node(
                package="robot_state_publisher",
                executable="robot_state_publisher",
                output="screen",
                arguments=[urdf_file],
            ),
            
            Node(
                package="controller_manager",
                executable="ros2_control_node",
                parameters=[robot_description, controller_file],
                output={
                    "stdout": "screen",
                    "stderr": "screen",
                },
            ),
            Node(
                package="controller_manager",
                executable="spawner.py",
                arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
                ),

            Node(
                package="controller_manager",
                executable="spawner.py",
                arguments=["joint_trajectory_controller", "-c", "/controller_manager"],
            ),
        ]
    )

URDF ファイルの修正

修正と書きましたが、バックアップのためにファイル名を変更して作成します。

cd ~/mg400_workspace/src/mg400_training
touch urdf/mg400_training_gazebo.urdf

URDFは長くなるので後で載せますが、先に注意点を載せておきます。

Gazebo で URDF がうまく表示されないとき

  • 未使用の Link がないか確認
  • ikpy を使用する際は joint名が joint_x となってるか、Link 名が Link_x となっているか確認
  • ikpy は URDF の上から順に計算されるので、fixed されている joint や 関係ない joint は、joint 設定の最後に回す

他の要因でエラーになる可能性もありますが、上記条件は必須です。

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="link_1">
    <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="link_5">
    <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="base_footprint"/>
    <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="link_1"/>
    <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="link_1"/>
    <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="link_1"/>
    <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_9" type="fixed">
    <origin rpy="0 0 0" xyz="0.066 0.017 0.031"/>
    <parent link="link4_1"/>
    <child link="link_5"/>
  </joint>
  <joint name="joint_10" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 -0.084"/>
    <parent link="link_5"/>
    <child link="end_effector_flange"/>
    <axis xyz="0 0 1"/>
    <limit effort="0" lower="-3.141592653589793" upper="3.141592653589793" velocity="0"/>
  </joint>
  <joint name="joint_8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.002"/>
    <parent link="link_5"/>
    <child link="link4_3"/>
  </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_controller.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>

おわりに

今回は launch ファイルの作成と URDF を修正しました。

次回はようやく、MG400 を動かしていきます。
その他にも、パラメータ変更した際の挙動等を説明できればと思います。

Dobot MG400 の各jointへ数値を送信 実践編【ROS2-Foxy】
Dobot MG400 の各jointへ数値を送信 実践編【ROS2-Foxy】 今回は、今までのまとめとして、実際にGazebo上で MG400 が動いていることを確認していきます。
nutritionfoodtech.com
2023.01.30
スポンサーリンク

コメント

タイトルとURLをコピーしました