概要

オジリナルのロボットアームを設計・製作し、それをROSで動かします。
1)オリジナルの6軸ロボットアームを3D CADで設計します。
2)ROSの環境でロボットアームを表示したりシミュレーションしたりします。
3)ROSからロボットアームの実機を動かします。

動画

ロボットアームの設計

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

3D CADで設計中のロボットアーム(SolidWorks画面)

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

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

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

visual要素には、パーツのSTLファイルを指定します。
シミュレーションにおける衝突検出の計算負荷を減らすために、簡略化したパーツもSolidWorksで作成して、
collision要素には、その簡略化したパーツのSTLファイルを指定しました。
inertial要素の、重量や慣性モーメントには、SolidWorksで計算される値を用いました。オプションで単位をメートルにし、座標系を設定します。
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にロボットアーム表示

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でロボットアームをシミュレーション

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モデルあり)

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

MoveGroup、rvizを起動します。rviz画面からプランニングにてロボットに指令を出すことができます。
roslaunch hajimearm_moveit_config move_group.launch
roslaunch hajimearm_moveit_config moveit_rviz.launch config:=true
rvizのプランニング

rvizのプランニング

pythonプログラムで動かす場合は、例えば以下のように実行します。
python scripts/move_group.py
ROSのシミュレーション画面

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

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

ROSのノードとトピック

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

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からロボットアームの実機を動かす

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

ROSのノードとトピック

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

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で表示したロボットアーム

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