環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	16.04
ROS	Kinetic
Gazebo	7.14.0
Qt	5.5.1

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

オリジナルで作ったロボットアームをmoveitで動かします。

moveitの設定

moveitで設定ファイルを作るためのアプリとしてroslaunch moveit_setup_assistant setup_assistant.launchというものがありますが、これでは完全な設定はできません。追加のファイルを作る必要があります。

setup_assistantでの設定ファイルの作成

roslaunch moveit_setup_assistant setup_assistant.launchを実行します。
ここで「Create New MoveIt Configuration Package」を選択します。

「Browsw」を押してurdfファイルを選びます。

この後に「Add Joints」押して次のページでアームの動作に関するすべての軸を選択します。

「Self-Colisions」で「Generate Collision matrix」を選びます。これは軌道計画をするときにどのリンク同士は衝突計算を行うのい、どの組み合わせは無視するのかを決定します。

「Planning Groups」では

Kinematics
- Group Name：アームのグループの名前を自由に決定します。
- Kinematic Solver：「kdl_kinematics_plugin/KDLKinematicsPlugin」を選択します。
OMPL Planning
- Group Default Planner：「RRTConnect」を選択します。

上記の選択をすると以下のような画面になります。

コントローラーの設定をします。「Add Controller」を押します。

Controller NameではTrajectory controllerのtopic名になる名前を入れます。
Controller Typeではeffort_controller/JointTrajectoryControllerを選択します。

この後に「Add Planning Group Joints」を選択してgroup名を選択します。

「Auther Information」で項目を埋めます。

最後に「Configuration Files」で「Generate Package」を選択します。

ファイルのコピー

以下のファイルをarm_lecture_moveit_config/configからarm_lecture/config/moveitにコピーします。

コピーするファイルのリスト

arm_robot.srdf  # アームのメタ情報
joint_limits.yaml　# jointの速度、加速度の上限
kinematics.yaml  #逆運動学のソルバーの設定
ompl_planning.yaml  # 軌道計画のソルバーの設定

追加のファイル

moveitに出力側のコントローラーの設定をするためのファイルです。

arm_lecture/config/moveit/controllers.yaml

controller_manager_ns: controller_manager
controller_list:
  - name: arm_robot_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - arm1_joint
      - arm2_joint
      - arm3_joint
      - arm4_joint
      - arm5_joint
      - arm6_joint

launchファイル

arm_lecture/launch/arm_robot_moveit.launch

<?xml version="1.0" ?>
<launch>
  <arg name="model" default="$(find arm_lecture)/urdf/arm_robot_sim.urdf" />
  <param name="robot_description" textfile="$(arg model)" />

  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>

  <!-- gazebo model -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model arm_robot" />
  <rosparam file="$(find arm_lecture)/config/trajectory_controller.yaml" command="load"/>
  <node name="arm_robot_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="arm_robot_trajectory_controller joint_state_controller"/>

  <!-- common -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- moveit setting -->
  <param name="robot_description_semantic" textfile="$(find arm_lecture)/config/moveit/arm_robot.srdf" />
  <group ns="robot_description_planning">
    <rosparam command="load" file="$(find arm_lecture)/config/moveit/joint_limits.yaml"/>
  </group>
  <group ns="robot_description_kinematics">
    <rosparam command="load" file="$(find arm_lecture)/config/moveit/kinematics.yaml"/>
  </group>

  <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
                       default_planner_request_adapters/FixWorkspaceBounds
                       default_planner_request_adapters/FixStartStateBounds
                       default_planner_request_adapters/FixStartStateCollision
                       default_planner_request_adapters/FixStartStatePathConstraints" />

  <node name="move_group"  pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="">
    <!-- Set the display variable, in case OpenGL code is used internally -->
    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />

    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="true" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="true" />
    <param name="planning_scene_monitor/publish_state_updates" value="true" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="true" />

    <!-- controller config -->
    <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
    <rosparam file="$(find arm_lecture)/config/moveit/controllers.yaml"/>

    <!-- planning config -->
    <param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
    <param name="request_adapters" value="$(arg planning_adapters)" />
    <param name="start_state_max_bounds_error" value="0.1" />
    <rosparam command="load" file="$(find arm_lecture)/config/moveit/ompl_planning.yaml"/>
  </node>

  <!-- Rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find arm_lecture)/config/display.rviz"/>
</launch>

注意事項を以下に書きます。

gazenoのros_controlの名前はmoveitの物をそろえましょう。
ロボットのurdfを格納するrosparamであるrobot_descriptionの名前はrobot_descriptionで固定です。moveitのコードの中に絶対パスで読んでいるところがあるので、上位の名前を付けて区別するなどはできません。
同じようにrobot_description_planningやrobot_description_kinematicsも固定の名前になります。

ソースコード（moveitへの指令を出すノード）

通信自体はactionlibなのですが、そこそこ複雑なので直接触るのは難しく、MoveGroupInterfaceというmoveitのAPI経由で使います。

arm_lecture/src/moveit_commander.cpp

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char **argv) {
  ros::init(argc, argv, "moveit_commander");
  ros::NodeHandle nh;

  ros::AsyncSpinner spinner(2);
  spinner.start();

  // Set up the arm planning interface
  moveit::planning_interface::MoveGroupInterface arm("arm0");

  // Prepare
  ROS_INFO("Moving to prepare pose");
  ROS_INFO_NAMED("tutorial", "Reference frame: %s", arm.getPlanningFrame().c_str());
  ROS_INFO_NAMED("tutorial", "End effector link: %s", arm.getEndEffectorLink().c_str());
  arm.setPlanningTime(0.050);
  arm.setPlannerId("RRTConnect");
  arm.setGoalTolerance(0.01);

  // pose1
  geometry_msgs::PoseStamped pose1;
  pose1.header.frame_id = "base_link";
  pose1.pose.position.x = 0.20;
  pose1.pose.position.y = 0.00;
  pose1.pose.position.z = 0.35;
  pose1.pose.orientation.w = 1.0;

  // pose2
  geometry_msgs::PoseStamped pose2;
  pose2.header.frame_id = "base_link";
  pose2.pose.position.x = 0.30;
  pose2.pose.position.y = 0.10;
  pose2.pose.position.z = 0.35;
  pose2.pose.orientation.w = 1.0;

  // pose3
  geometry_msgs::PoseStamped pose3;
  pose3.header.frame_id = "base_link";
  pose3.pose.position.x = 0.40;
  pose3.pose.position.y = 0.00;
  pose3.pose.position.z = 0.35;
  pose3.pose.orientation.w = 1.0;

  // pose4
  geometry_msgs::PoseStamped pose4;
  pose4.header.frame_id = "base_link";
  pose4.pose.position.x = 0.30;
  pose4.pose.position.y = -0.10;
  pose4.pose.position.z = 0.35;
  pose4.pose.orientation.w = 1.0;

  moveit::planning_interface::MoveItErrorCode ret;

  ROS_INFO("move to WP1");
  arm.setPoseTarget(pose1);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP2");
  arm.setPoseTarget(pose2);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP3");
  arm.setPoseTarget(pose3);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ROS_INFO("move to WP4");
  arm.setPoseTarget(pose4);
  ret = arm.move();
  if (!ret) {
    ROS_WARN("Fail: %i", ret.val);
  }
  ros::Duration(0.5).sleep();

  ros::shutdown();
  return 0;
}

初めのほうはいろいろな設定で、arm.setPoseTarget(pose)で対象の位置姿勢を決定します。ret = arm.move()で設定に従って問題を解き行動が開始されます。このmove()はブロッキング関数で、移動が終わるまでブロックされます。返り値は0なら成功で、失敗ならエラーコードが返ってきます。エラーの詳細はrosmgsのサイトを参照して下しさい。このデモだと-4(CONTROL_FAILED)がまれに出ます。これはgazeboのros_controlのパラメーターがうまく調整されていないので、追従性が悪いために出ます。

ビルド

cd ~/catkin_ws
catkin_make

実行

1つ目のターミナル

roslaunch arm_lecture arm_robot_moveit.launch

1つ目のターミナル

rosrun arm_lecture moveit_commander

参考

https://sites.google.com/site/robotlabo/time-tracker/ros/gazebo_mani
MoveGroupInterfaceリファレンス

目次ページへのリンク

ROS講座の目次へのリンク

ROS講座104 moveitでオリジナルのアームを使う

環境

概要