ROSによるロボットアームの制御 (ROS) | ROS、ROS2 (ロボットオペレーティングシステム）

概要

オジリナルのロボットアームを設計・製作し、それをROSで動かします。

１）オリジナルの６軸ロボットアームを3D CADで設計します。
２）ROSの環境でロボットアームを表示したりシミュレーションしたりします。
３）ROSからロボットアームの実機を動かします。

動画

ロボットアームの設計

３次元CADのSolidWorksを使用して、6軸ロボットアームを設計します。サーボモータにはROBOTIS社のダイナミクセルRX64（第1,2軸）,RX28（第3〜6軸とハンド開閉）を使用しました。ロボットアームのフレームは、3Dプリンタで出力できるように樹脂で設計しました。

３D CADで設計中のロボットアーム（SolidWorks画面）

ROSではロボットをURDFで表現します。URDFにロボットのデータを渡すために、SolidWorksからパーツをSTL形式で出力します。出力フォーマットはバイナリです。
このとき、オプションにて、単位をメートル、座標軸をROSで組み立てやすい場所に設定してから、STLファイルを出力します。

SolidWorksからSTLで出力するときのオプション画面

visual要素には、パーツのSTLファイルを指定します。

シミュレーションにおける衝突検出の計算負荷を減らすために、簡略化したパーツもSolidWorksで作成して、
collision要素には、その簡略化したパーツのSTLファイルを指定しました。

inertial要素の、重量や慣性モーメントには、SolidWorksで計算される値を用いました。オプションで単位をメートルにし、座標系を設定します。

SolidWorksで計算される慣性モーメント

link要素とjoint要素を使用して、SolidWorksで出力したパーツを組み立てていき、ロボットアームのURDFを完成させます。

ロボットアームのURDFファイル hajimearm.urdf

<?xml version="1.0"?>
<robot name="hajimearm">
  <link name="world"/>
  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="package://hajimearm_description/meshes/hr56_base.stl" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://hajimearm_description/meshes/hr56_base.stl" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.164985"/>
      <origin xyz="-0.011 0 0.019" rpy="0 0 0"/>
      <inertia ixx="0.000126" iyy="0.000176" izz="0.000125" ixy="0" ixz="0" iyz="-4.3e-5"/>
    </inertial>
  </link>
  <joint name="base_link_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="world"/>
    <child link="base_link"/>
  </joint>
  <link name="arm1_link">
    <visual>
      <geometry>
        <mesh filename="package://hajimearm_description/meshes/hr56_arm1.stl" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://hajimearm_description/meshes/hr56_arm1.stl" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.012945"/>
      <origin xyz="0 0 0.012" rpy="0 0 0"/>
      <inertia ixx="1.0e-5" iyy="5.0e-6" izz="6.0e-6" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="joint_1" type="revolute">
    <origin xyz="0 0 0.046" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="arm1_link"/>
    <limit effort="5" lower="-2.617" upper="2.617" velocity="1.571" />
  </joint>
  <link name="arm2_link">
    <visual>
      <geometry>
        <mesh filename="package://hajimearm_description/meshes/hr56_arm2.stl" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://hajimearm_description/meshes/hr56_arm2.stl" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
    </collision>
    <inertial>
      <mass value="0.143134"/>
      <origin xyz="-0.005 0 0.066" rpy="0 0 0"/>
      <inertia ixx="1.15e-4" iyy="0.001161" izz="4.9e-5" ixy="0" ixz="-9.2e-5" iyz="0"/>
    </inertial>
  </link>
  <joint name="joint_2" type="revolute">
    <origin xyz="0 0 0.03" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="arm1_link"/>
    <child link="arm2_link"/>
    <limit effort="5" lower="-1.571" upper="1.571" velocity="1.571" />
  </joint>

（省略）

Gazeboシミュレーション用にtransmission要素とros_controlを追加します。hajimearm.urdfの続き

  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint_2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

（省略）

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>
  <gazebo>
    <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
      <jointName>joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7</jointName>
    </plugin>
  </gazebo>
</robot>

STLファイルの中身にsolidの文字があれば警告が出ます。先頭にあるsolidの文字を、例えばsssssに置換します。

sed -i 's/^solid/sssss/' *

URDFをチェックします。

check_urdf urdf/hajimearm.urdf

ROSを起動しておきます。

roscore
cd hajimerobot6_ws
source devel/setup.bash

URDFチェックで問題なければrvizで表示して確認します。

cd src/hajimearm_description
roslaunch urdf_tutorial display.launch model:=urdf/hajimearm.urdf gui:=true

joint_state_publisherのスライダーを動かすとロボットアームのjointが動きます。

rvizにロボットアーム表示

ROSでロボットアームのシミュレーション

ロボットアームをMoveItで動かすために、以下のMoveItセットアップアシスタントで設定を行います。

roslaunch moveit_setup_assistant setup_assistant.launch

多数のファイルが自動作成されるが、手動で修正しなければならないファイルがあります。

controllers.yaml

# Hand-made
controller_manager_ns: /
controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6
  - name: hand_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint_7

hajimearm_moveit_controller_manager.launch

<launch>
  <param name="moveit_controller_manager"
         value="moveit_simple_controller_manager/MoveItSimpleControllerManager"/>
  <param name="controller_manager_name" value="/" />
  <param name="use_controller_manager" value="true" />
  <rosparam file="$(find hajimearm_moveit_config)/config/controllers.yaml"/>
</launch>

次に、シミュレータのGazeboにロボットアームを表示させるために、以下をまとめたファイル(hajimearm.launch)を作成します。
1) gazebo_rosでhajimearm.urdfを生成します。
2) controller_managerでarm_controllerとhand_controllerを生成します。
3) robot_state_publisherでロボットの状態を配信します。

hajimearm.launch

<?xml version="1.0"?>
<launch>
  <!-- Load the HajimeArm URDF model into the parameter server -->
  <param name="robot_description" textfile="$(find hajimearm_description)/urdf/hajimearm.urdf" />
  <!-- Start Gazebo with an empty world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
  <!-- Spawn a HajimeArm in Gazebo, taking the description from the
       parameter server -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
        args="-param robot_description -urdf -model hajimearm" output="screen"/>
  <rosparam file="$(find hajimearm_description)/config/controllers.yaml" command="load"/>
  <node name="controller_spawner" pkg="controller_manager" type="spawner"
        args="arm_controller hand_controller" output="screen"/>
  <!-- Convert /joint_states messages published by Gazebo to /tf messages,
       e.g., for rviz-->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
        type="robot_state_publisher" output="screen"/>
</launch>

シミュレーションで動かしてみます。
Gazeboを起動する前に、gazebo用モデルファイルの場所を定義しておく必要があります。シミュレーション用に作成したtableとboxはmodelsの中です。
（指定なしだとgazeboのディレクトリは、~/.gazebo、変更するときはGAZEBO_MODEL_PATHで設定することもできる。）
（参考　http://gazebosim.org/tutorials?tut=ros_roslaunch）
package.xml (hajimearm_gazebo)　（一部）

  <exec_depend>gazebo_ros</exec_depend>
  <export>
    <gazebo_ros gazebo_model_path="${prefix}/models"/>
    <gazebo_ros gazebo_media_path="${prefix}/models"/>
  </export>

Gazeboを起動します。

roslaunch hajimearm_description hajimearm.launch

Gazeboでロボットアームをシミュレーション

シミュレーション用に作成したtableとboxありの場合です。

hajimearm_world.world

<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://table1</uri>
      <name>table1</name>
      <pose>0.2 0 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://box1</uri>
      <name>box1</name>
      <pose>0.2 0 0.22 0 0 0</pose>
    </include>
  </world>
</sdf>

Gazeboでロボットアームをシミュレーション（tableとboxモデルあり）

MoveGroup、rvizを起動します。rviz画面からプランニングにてロボットに指令を出すことができます。

roslaunch hajimearm_moveit_config move_group.launch
roslaunch hajimearm_moveit_config moveit_rviz.launch config:=true

rvizのプランニング

pythonプログラムで動かす場合は、例えば以下のように実行します。

python scripts/move_group.py

ROSのシミュレーション画面

ROSのノードとトピックを表示します。

rqt_graph

ROSのノードとトピック

ROSの座標変換TFを表示します。

rosrun tf view_frames

ROSの座標変換TF

ROSからロボットアームの実機を動作

ロボットアームの実機を動作さする場合は、hajimearm.urdfからGazeboに関わる部分を削除したものhajimearm_robothw.urdfを使います。

hajimearm.urdfからtransmission要素とros_controlを削除しました。

roslaunch hajimearm_description hajimearm_robothw.launch

PCとサーボモータとの接続は、USB-RS485変換器を使用します。

ROBOTIS社ダイナミクセルのサーボモータを動かすためのドライバを起動します。

roslaunch hajimearm_driver hajimearm_controller.launch
roslaunch hajimearm_ros_control hajimearm_ros_control.launch

MoveGroup、rvizを起動する。rviz画面からロボットに指令を出すことができます。rvizのプランニングで実機が動かせます。

roslaunch hajimearm_moveit_config move_group.launch
roslaunch hajimearm_moveit_config moveit_rviz.launch config:=true

pythonプログラムで動かす場合は、例えば以下を実行します。

cd src/hajimearm_moveit_config 
python scripts/move_group.py

実機の動きに合わせて、rviz上のロボットアームも動きます。

ROSからロボットアームの実機を動かす

ROSのノードとトピックを表示します。

rqt_graph

ROSのノードとトピック

ROSの座標変換TFを表示します。

rosrun tf view_frames

ROSの座標変換TF

逆運動学計算IKFast

以下は参考情報です。

MoveItでロボットアーム等の逆運動学計算を行う際に、デフォルトのKDL(Kinematics Dynamics Library)を、より高速のIKFastに置き換えます。OpenRAVEを使用します。

rosrun collada_urdf urdf_to_collada hajimearm.urdf hajimearm.dae
openrave-robot.py hajimearm.dae --info links
openrave hajimearm.dae

OpenRAVEで表示したロボットアーム

export BASE_LINK="0"
export EEF_LINK="7"
export IKFAST_OUTPUT_PATH=`pwd`/ikfast61_arm.cpp
python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=hajimearm.dae --iktype=transform6d --baselink="$BASE_LINK" --eelink="$EEF_LINK" --savefile="$IKFAST_OUTPUT_PATH"

catkin_create_pkg hajimearm_ikfast_arm_plugin
catkin_make

rosrun moveit_kinematics create_ikfast_moveit_plugin.py hajimearm "arm" hajimearm_ikfast_arm_plugin "$IKFAST_OUTPUT_PATH"

hajimearm_moveit_config config/kinematics.yamlが以下のように自動修正されました。
kinematics.yaml修正後

arm:
  kinematics_solver: hajimearm_arm_kinematics/IKFastKinematicsPlugin
  kinematics_solver_attempts: 3
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005

kinematics.yaml修正前

arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3