Dobot MG400 に目標値を送信して移動【ROS2-Foxy】

はじめに
前提条件
動作用のプログラム作成
プログラムの説明
おわりに

はじめに

前回は Dobot MG400 の各 joint に値を送信し、MG400 を動かしました。

今回はまとめとして、目標値から逆運動学で算出した各 joint の値を送信し、MG400 を動かします。

前提条件

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

逆運動学のライブラリ ikpy がインストールされている
前回までの記事で、joint に値を送信して MG400 の動作が確認できている

動作用のプログラム作成

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

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

今作成した ikpy_publish.py を以下のようにしてください。

import rclpy
from rclpy.node import Node
from builtin_interfaces.msg import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from ament_index_python.packages import get_package_share_directory
import ikpy.chain
import numpy as np
import os
import sys

class Trajectory_publisher(Node):
    def __init__(self):
        super().__init__("trajectory_publisher_node")
        publisher_topic = "/joint_trajectory_controller/joint_trajectory"
        self.trajectory_publisher = self.create_publisher(JointTrajectory, publisher_topic, 10)
        timer_period = 1
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.joints = ["joint_1", "joint_2", "joint_4", "joint_6", "joint_10"]
        
        package_share_dir = get_package_share_directory("mg400_training")
        urdf_file = os.path.join(package_share_dir, "urdf", "mg400_training_gazebo.urdf")

        self.robot_initialize(urdf_file)
        argv = sys.argv[1:]
        self.inverse_kinematics_solution(float(argv[0]), float(argv[1]), float(argv[2]), float(argv[3]), float(argv[4]))

    def timer_callback(self):
        bazu_trajectory_msg = JointTrajectory()
        bazu_trajectory_msg.joint_names = self.joints
        point = JointTrajectoryPoint()
        point.positions = self.goal_positions
        point.time_from_start = Duration(sec=2)
        bazu_trajectory_msg.points.append(point)
        self.trajectory_publisher.publish(bazu_trajectory_msg)
        print("Trajectory Sent :", point)

    def robot_initialize(self, urdf_file):
        self.kuka_robot = ikpy.chain.Chain.from_urdf_file(urdf_file)

    def inverse_kinematics_solution(self, x, y, z, wrist, tool):
        angles = self.kuka_robot.inverse_kinematics([x, y, z])
        angles = np.delete(angles, [0, 4, 5, 6])
        self.goal_positions = list(np.append(angles, [wrist, tool]))
        print("Inverse Kinematics Solution: ", self.goal_positions)

def main(args=None):
    rclpy.init(args=args)
    joint_trajectory_object = Trajectory_publisher()

    rclpy.spin(joint_trajectory_object)
    joint_trajectory_object.destroy_node()
    rclpy.shutdown()

if __name__ == "__main__":
    main()

ビルドしたら、まずはシミュレーション環境を起動します。

cd ~/mg400_workspace
colcon build --packages-select mg400_training
source ~/.bashrc
ros2 launch mg400_training controller.launch.py

Gazebo が起動するので、別のターミナルでikpy_publish.py を起動しましょう。

ros2 run mg400_training ikpy_publish.py 1 1 1 0 0

以下のようにロボットが動作します。

x, y, z を指定して移動させることができました。
(デモ gif は関節の限界まで動いているだけです。)

プログラムの説明

プログラムを説明していきます。

robot_initialize

def robot_initialize(self, urdf_file):
    self.kuka_robot = ikpy.chain.Chain.from_urdf_file(urdf_file)

urdf ファイルから、逆運動学の関数を作成します。
これはプログラムの起動時一度のみ呼び出されます。

inverse_kinematics_solution

def inverse_kinematics_solution(self, x, y, z, wrist, tool):
    angles = self.kuka_robot.inverse_kinematics([x, y, z])
    angles = np.delete(angles, [0, 4, 5, 6])
    self.goal_positions = list(np.append(angles, [wrist, tool]))
    print("Inverse Kinematics Solution: ", self.goal_positions)

x, y, z, の目標値を inverse_kinematics_solution 関数に渡すと、逆運動学を用いて算出された各 joint の角度が、self.goal_positions に格納されます。

これはプログラムの起動時一度のみ呼び出されます。

timer_callback

def timer_callback(self):
    bazu_trajectory_msg = JointTrajectory()
    bazu_trajectory_msg.joint_names = self.joints
    point = JointTrajectoryPoint()
    point.positions = self.goal_positions
    point.time_from_start = Duration(sec=2)
    bazu_trajectory_msg.points.append(point)
    self.trajectory_publisher.publish(bazu_trajectory_msg)
    print("Trajectory Sent :", point)

timer_callback 関数は、プログラム実行中に一定間隔で呼び出されます。
目標値への指令をパブリッシュします。

ここで、現在地と目標値との差分を取得すると、ゴールに到着したかどうか判定し、プログラムを終了することができます。