Dobot MG400 の各jointへ数値を送信準備編【ROS2-Foxy】

はじめに
前提条件
joint へ publish するプログラム
1. コードの説明
  1. def __init__
    1. JointTrajectory型について
  2. timer_callback
    1. JointTrajectoryPoint型について
setup.py の編集
jtc_controller.yaml の作成
おわりに

はじめに

前回は逆運動学を用いて Dobot MG400 の各joint の目標値を算出しました。

今回は DobotMG400 の各jointへ数値を送信する準備をしていきます。

前提条件

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

ikpy を用いて、各joint の目標値を算出できる

joint へ publish するプログラム

始めに、プログラムを作成します。

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

import rclpy
from rclpy.node import Node
from builtin_interfaces.msg import Duration
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

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"]
        self.goal_positions = [0.1, 0.21, 0.22, 0.31, 0.32]

    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("callback ...", point, self.trajectory_publisher)

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()

コードの説明

publish_trajectory.py の説明をしていきます。

def init

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"]
self.goal_positions = [0.1, 0.21, 0.22, 0.31, 0.32]

まず、MG400 を動かすには、jtc.yaml もしくは下で説明する jtc_controller.yaml で指定した

joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController

に、JointTrajectory 型のメッセージを送信する必要があります。

今回は、/joint_trajectory_controller/joint_trajectory に送信します。

create_publisher で JointTrajectory型を送信していきます。

create_timer で 1s 毎にパブリッシュします。

JointTrajectory型について

JointTrajectory型の説明はこちらにあります。

# The header is used to specify the coordinate frame and the reference time for
# the trajectory durations

std_msgs/Header header

# The names of the active joints in each trajectory point. These names are
# ordered and must correspond to the values in each trajectory point.

string[] joint_names

# Array of trajectory points, which describe the positions, velocities,
# accelerations and/or efforts of the joints at each time point.

JointTrajectoryPoint[] points

色々ありますが、要は joint 名称と各joint の値を格納している型になります。

timer_callback

bazu_trajectory_msg = JointTrajectory()
bazu_trajectory_msg.joint_names = self.joints

JointTrajectory型の joint_names に joint 名を代入します。

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("callback ...", point, self.trajectory_publisher)

JointTrajectoryPoint型にゴール位置と何秒で移動するか(Duration)を代入します。

それをJointTrajectory型に append で追加し、publish します。

JointTrajectoryPoint型について

JointTrajectoryPoint型の説明はこちらを参考にしてください。

# Each trajectory point specifies either positions[, velocities[, accelerations]]
# or positions[, effort] for the trajectory to be executed.
# All specified values are in the same order as the joint names in JointTrajectory.msg.
# Single DOF joint positions for each joint relative to their "0" position.
# The units depend on the specific joint type: radians for revolute or
# continuous joints, and meters for prismatic joints.
float64[] positions

# The rate of change in position of each joint. Units are joint type dependent.
# Radians/second for revolute or continuous joints, and meters/second for
# prismatic joints.
float64[] velocities

# Rate of change in velocity of each joint. Units are joint type dependent.
# Radians/second^2 for revolute or continuous joints, and meters/second^2 for
# prismatic joints.
float64[] accelerations

# The torque or the force to be applied at each joint. For revolute/continuous
# joints effort denotes a torque in newton-meters. For prismatic joints, effort
# denotes a force in newtons.
float64[] effort

# Desired time from the trajectory start to arrive at this trajectory point.
builtin_interfaces/Duration time_from_start

positions 以外にも、velocities, accelerrations, effort が指定できます。

time_from_start は何秒で到着するか、処理が完了するかを設定します。

setup.py の編集

次に、setup.py の以下の部分を変更します。

'console_scripts': [
     ],
↓
'console_scripts': [
     "publish_trajectory = mg400_training.publish_trajectory:main",
],

jtc_controller.yaml の作成

準備編の最後として、どの joint がコントローラから値を受け取るか設定していきます。

cd ~/mg400_workspace/src/mg400_training
touch config/jtc_controller.yaml

controller_manager:
  ros__parameters:
    update_rate: 100
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint_1
      - joint_2
      # - joint_3
      - joint_4
      # - joint_5
      - joint_6
      # - joint_7
      # - joint_8
      # - joint_9
      - joint_10
      # - camera_joint

    command_interfaces:
      - position

    state_interfaces:
      - position

    state_publish_rate: 50.0
    action_monitor_rate: 20.0

    allow_partial_joints_goal: false
    open_loop_control: true
    constraints:
      stopped_velocity_tolerance: 0.01
      goal_time: 0.0
      joint1:
        trajectory: 0.05
        goal: 0.03

jtc_controller.yaml の説明は別の記事で実施しています。

おわりに

今回は MG400 の各joint へ値を送信するために、送信プログラムと yaml ファイルを作成しました。
jtc_controller.yaml を作成することで簡単に値が送信できることが分かったと思います。

次回は、launch ファイルと URDF の修正をします。その後、ようやく動かせるようになります。