ROS入門 (38) - MoveItによるmycobotの制御

「MoveIt」のPythonインターフェイスによる「mycobot」の制御方法をまとめました。

・Melodic

前回

1. mycobot

「mycobot」は、「Elephant Robotics」と「M5STACK」が共同で開発した、世界最小・最軽量の6軸協働ロボットアームです。

前回までpandaを使っていた部分を、mycobotに置き換えます。mycobotの設定に関しては、以下のリポジトリを使わせてもらいます。

GitHub - nisshan-x/mycobot_moveit: Unofficial myCobot moveit repositiry

2. mycobotのパッケージのインストール

mycobotのパッケージのインストール手順は、次のとおりです。

(1) 必要なパッケージのインストール。

$ sudo apt update
$ sudo apt install -y ros-melodic-moveit
$ sudo apt install -y ros-melodic-ros-control ros-melodic-ros-controllers
$ sudo apt install -y ros-melodic-gazebo-ros
$ sudo apt install -y ros-melodic-jsk-rviz-plugins

(2) リポジトリのクローン。

$ cd ~/catkin_ws/src
$ git clone https://github.com/Tiryoh/mycobot_ros
$ git clone https://github.com/nisshan-x/mycobot_moveit

(3) ワークスペースのビルド。

$ catkin build

(4) ワークスペースのセットアップ。

$ source ~/catkin_ws/devel/setup.bash

3. rvizの起動

mycobotの動作確認のためrvizなどを起動します。

$ roslaunch mycobot_moveit mycobot_moveit_gazebo.launch

4. ロボット情報の出力

(1) 「hello_moveit.py」を以下のように編集。
変更は「panda_arm」を「mycobot_arm」に変更したのみです。

#!/usr/bin/env python
# coding: UTF-8
import sys
import moveit_commander
import rospy


def main():
    # moveit_commanderの初期化
    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの初期化
    rospy.init_node('hello_moveit')

    # RobotCommanderのインスタンス化
    robot = moveit_commander.RobotCommander()

    # MoveGroupCommanderのインスタンス化
    move_group = moveit_commander.MoveGroupCommander("mycobot_arm")

    # ロボットのプランニングフレーム名の取得
    planning_frame = move_group.get_planning_frame()
    print "planning_frame: %s" % planning_frame

    # エンドエフェクタリンク名の取得
    end_effector_link = move_group.get_end_effector_link()
    print "end_effector_link: %s" % end_effector_link

    # ロボット内のグループ名のリストの取得
    group_names = robot.get_group_names()
    print "group_names:", group_names

    # ロボットの現在の状態の取得
    current_state = robot.get_current_state()
    print "\n", current_state

    # ノード終了まで待機
    rospy.spin()


if __name__ == "__main__":
    main()

(2) 実行。

$ rosrun moveit_tutorials hello_moveit.py
[ INFO] [1639024560.065676700]: Loading robot model 'firefighter'...
[ INFO] [1639024560.067835000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1639024561.191953700]: Ready to take commands for planning group mycobot_arm.
planning_frame: base_link
end_effector_link: arm6_link
group_names: ['mycobot_arm']
[ WARN] [1639024562.204312400]: Joint values for monitored state are requested but the full state is not known

joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "base_link"
  name: [arm1_joint, arm2_joint, arm3_joint, arm4_joint, arm5_joint, arm6_joint]
  position: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  velocity: []
  effort: []
multi_dof_joint_state: 
  header: 
    seq: 0
    stamp: 
      secs: 0
      nsecs:         0
    frame_id: "base_link"
  joint_names: []
  transforms: []
  twist: []
  wrench: []
attached_collision_objects: []
is_diff: False

 5. 関節角度によるモーションプランニングと動作

(1) 「hello_moveit.py」を以下のように編集。
関節数は6つになります。

#!/usr/bin/env python
# coding: UTF-8
import moveit_commander
import rospy
import sys
import numpy as np
from math import pi


def main():
    # moveit_commanderの初期化
    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの初期化
    rospy.init_node('hello_moveit')

    # MoveGroupCommanderのインスタンス化
    move_group = moveit_commander.MoveGroupCommander("mycobot_arm")

    # 現在の関節角度を取得
    joint_goal = move_group.get_current_joint_values()
    print "from:", np.rad2deg(joint_goal)

    # 関節角度の指定
    joint_goal[0] = -90 * pi / 180
    joint_goal[1] = 0 * pi / 180
    joint_goal[2] = 0
    joint_goal[3] = 90 * pi / 180
    joint_goal[4] = 0
    joint_goal[5] = 0 * pi / 180
    print "to:", np.rad2deg(joint_goal)

    # 関節角度によるモーションプランニングと動作の実行
    move_group.go(joint_goal, wait=True)

    # 停止 (動きが残っていないことを保証)
    move_group.stop()


if __name__ == "__main__":
    main()

(2) 実行。

$ rosrun moveit_tutorials hello_moveit.py

4. 姿勢によるモーションプランニングと動作

(1) 「hello_moveit.py」を以下のように編集。

#!/usr/bin/env python
# coding: UTF-8
import geometry_msgs.msg
import moveit_commander
import rospy
import sys
from math import pi


def main():
    # moveit_commanderの初期化
    moveit_commander.roscpp_initialize(sys.argv)

    # ノードの初期化
    rospy.init_node('hello_moveit')

    # MoveGroupCommanderのインスタンス化
    move_group = moveit_commander.MoveGroupCommander("mycobot_arm")

    # 姿勢メッセージの生成
    # pose_goal = geometry_msgs.msg.Pose()

    # 現在の姿勢の取得
    pose_goal = move_group.get_current_pose().pose

    # 姿勢の指定
    pose_goal.position.x += 0.0
    pose_goal.position.y += 0.0
    pose_goal.position.z -= 0.05
    pose_goal.orientation.x += 0.0
    pose_goal.orientation.y += 0.0
    pose_goal.orientation.z += 0.0
    pose_goal.orientation.w += 0.0

    # 姿勢によるモーションプランニングと動作の実行
    move_group.set_pose_target(pose_goal)
    move_group.go(wait=True)

    # 停止 (動きが残っていないことを保証)
    move_group.stop()

    # 姿勢目標のクリア
    move_group.clear_pose_targets()


if __name__ == "__main__":
    main()

(2) 実行。

$ rosrun moveit_tutorials hello_moveit.py

5. 参考

DockerでmyCobotのROSシミュレータを動かす｜myCobotのすべて

次回

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門