MoveItでPandaを動かそう：後編（WMD 2019 in Tokyo 準備編）

著者:yuki.onishi

MoveItでPandaを動かそう：後編（WMD 2019 in Tokyo 準備編）

はじめに

本記事では，World MoveIt Day 2019 in Tokyo（WMD 2019 in Tokyo） の会場にて動かすことのできるロボットアーム Panda を，実際にPCと接続して動かす方法を紹介します．
後編では，Panda のパッケージ群である franka_ros の中身について紹介します．
当日実際に機体を動かしてみたい方は，必見です！

Panda を動かすPCの環境構築と，PCと実機の接続方法を説明した前編はこちらになります．
MoveItでPandaを動かそう：前編（WMD 2019 in Tokyo 準備編）

1. franka_ros の構成

franka_ros は，libfranka を ROS1 に対応させたパッケージ群です．
構成は次の図のようになっています．

※ panda_moveit_config は別パッケージです．

1.1. franka_description 

franka_description は Panda のロボットモデル (urdf xacro) を提供する ROS パッケージです．
RViz や MoveIt，Gazebo 等と連携することで，運動学計算や動力学計算，描画やプランニングなどを可能にします．

1.2. franka_gripper 

franka_gripper は Panda のグリッパのインターフェースの役割を担う ROS パッケージです．franka_gripper_node という名前のノードとして実装されています．
このノードは，グリッパの状態を publish するほか，下記の actions server に従ってグリッパを制御します．

• franka_gripper::MoveAction(width, speed):
  目標グリッパ幅 width に，速度 speed で動きます．
• franka_gripper::GraspAction(width, epsilon_inner, epsilon_outer, speed, force):
  目標グリッパ幅 width において，力 force で物を掴むようにします．目標グリッパ幅へは，速度 speed で動作します．epsilon_inner ，epsilon_outer はそれぞれ内側，外側へのグリッパ幅の許容誤差を表し，この許容誤差の中で収まった場合，成功と判定します．
• franka_gripper::HomingAction():
  グリッパを元の状態に戻します．
• franka_gripper::StopAction():
  現在実行中の action を中断します．力制御モードの場合は，脱力もします．
• control_msgs::GripperCommandAction(width, max_effort):
  MoveIt 標準の指令です．

次のコマンドでノードを実行できます．
<fci-ip>には，コントローラのIPアドレス（前編での設定では 172.16.0.2）を入れてください．

roslaunch franka_gripper franka_gripper.launch robot_ip:=<fci-ip>

1.3. franka_hw 

franka_hw は libfranka の制御 API をベースにした，制御インターフェースのパッケージです．

ハードウェアインターフェースのクラスである franka_hw::FrankaHWは，次のインターフェースをサポートしています．

• hardware_interface::JointStateInterface:
  関節毎に，JointState を取得する．
• hardware_interface::VelocityJointInterface:
  関節毎に，JointState を取得しつつ，回転速度の指令で動かす．
• hardware_interface::PositionJointInterface:
  関節毎に，JointState を取得しつつ，角度（位置）指令で動かす．
• hardware_interface::EffortJointInterface:
  関節毎に，JointState を取得しつつ，トルク指令で動かす．
• franka_hw::FrankaStateInterface:
  ロボットの全関節のJointState を取得する．
• franka_hw::FrankaPoseCartesianInterface:
  ロボットの全関節のJointState を取得しつつ，手先位置の指令で動かす．
• franka_hw::FrankaVelocityCartesianInterface:
  ロボットの全関節のJointState を取得しつつ，手先速度の指令で動かす．
• franka_hw::FrankaModelInterface:
  ロボットの運動学モデル・動力学モデルを取得する．

hardware_interfaceを利用する場合は，<arm_id>_joint1, … , <arm_id>_joint7といったように，ジョイントの名前を指定する必要があります．
一方，franka_hwを利用する場合は，<arm_id>_robotのようにロボット名を一度指定すればよいです．
デフォルトでは，<arm_id>はpandaに設定されています．

また，ここでは解説しませんが，franka_hwには，

• 動作開始
• 動作中断
• 制御インターフェースの切り替え

といった，便利な API も用意されています．

1.4. franka_control 

franka_control は libfranka の API を ROS 上で最大限利用するためのパッケージです．次のような機能が提供されています．

• franka_control::SetJointImpedance:
  ロボットの内部コントローラによる各関節のインピーダンスを設定します．
• franka_control::SetCartesianImpedance:
  ロボットの内部コントローラによる手先のインピーダンスを設定します．
• franka_control::SetEEFrame:
  エンドエフェクタ と link8 の相対座標を指定します．
• franka_control::SetKFrame:
  あるリンクとエンドエフェクタの相対座標を指定します．
• franka_control::SetForceTorqueCollisionBehavior:
  加わった手先外力が，剛体の衝突によるものだと判断する際の閾値を設定します．
• franka_control::SetFullCollisionBehavior:
  上記に加え，各リンク単位での衝突判定の判断のための閾値を設定します．
• franka_control::SetLoad:
  把持対象の重さなど，補償に使うための負荷を設定します．

franka_control::ErrorRecoveryAction は，エラー状態からの復帰を可能にします．
トピックを利用した簡単な復帰が可能です．

rostopic pub -1 /franka_control/error_recovery/goal franka_control/ErrorRecoveryActionGoal "{}"

復帰後，franka_control_node が動作している場合は，ロボットのコントローラを自動的に再起動させます．
ノードの立ち上げ方は次の通りです．

roslaunch franka_control franka_control.launch robot_ip:=<fci-ip> load_gripper:=<true|false>

この launch は franka_control_node だけでなく， franka_control::FrankaStateController も起動させます．
これはロボットの状態や推定手先外力など，RViz 上での可視化に必要な情報を読み取り，topic として publish します．（※ 描画の際には，RViz だけでなく，robot_state_publisher の立ち上げが必要です．）

1.5. franka_visualization

 franka_visualization はロボット（グリッパを含む）の状態を RViz 上で可視化するためのパッケージです．
次のコマンドで起動できます．

roslaunch franka_visualization franka_visualization.launch robot_ip:=<fci-ip> \
  load_gripper:=<true|false>

このパッケージは可視化をするだけで，ロボットを実際に動かすことはできません．

また，単一の franka::Robot のみが実際にロボットと接続できることに注意が必要です．複数の franka_joint_state_publisher は共存できないため，注意してください．

1.6. franka_example_controllers

franka_example_controllers は franka_hw::FrankaHW を利用した ROS コントローラの実装例です．

例えば，各関節をインピーダンス制御する例は，

roslaunch franka_example_controllers joint_impedance_example_controller.launch \
  robot_ip:=<fci-ip> load_gripper:=<true|false>

World MoveIt Day のハッカソンで実際に機体を動かしたい場合は，このパッケージを改造するのが便利かもしれません．

1.7. panda_moveit_config

panda_moveit_config は，Panda で MoveIt を使うために必要な設定ファイルが格納されています．
次のコマンドで立ち上がります．

roslaunch panda_moveit_config panda_control_moveit_rviz.launch load_gripper:=<true|false> \
robot_ip:=<fci-ip>

このパッケージの詳細な説明は，MoveIt のチュートリアル（英語）で行われています．また現在，このチュートリアルを日本語訳する作業が始まっています．

2. ROS コントローラを自作する

2.1. 継承クラス

franka_example_controllers の中に実装されている例は全て，controller_interface::MultiInterfaceControllerクラスを継承しています．
このクラスは，最大4種類のインターフェースを1つのコントローラに集約させることができます．

class NameOfYourControllerClass : controller_interface::MultiInterfaceController <
                              my_mandatory_first_interface,
                              my_possible_second_interface,
                              my_possible_third_interface,
                              my_possible_fourth_interface> {
  bool init (hardware_interface::RobotHW* hw, ros::NodeHandle& nh); // mandatory
  void update (const ros::Time& time, const ros::Duration& period); // mandatory
  void starting (const ros::Time& time) // optional
  void stopping (const ros::Time& time); // optional
  ...
}

franka_hw::FrankaHW において利用可能なインターフェースの組み合わせは，次の通りです．

• 1種類の単独利用
• EffortJointInterface + PositionJointInterface
• EffortJointInterface + VelocityJointInterface
• EffortJointInterface + FrankaCartesianPoseInterface
• EffortJointInterface + FrankaCartesianVelocityInterface

また，init 関数と update 関数は必ず定義しなければなりません．
更にオプションで，starting関数と stopping 関数を定義できます．

init 関数はコントローラの読み込み時にしか呼ばれませんが， starting関数は再起動が発生した場合でも呼び出されます．
初期姿勢などの代入は starting関数内で行うのが良いでしょう．

もし速度制御のインターフェースを利用している場合は，安全のため，stopping 関数で目標速度をゼロにする必要があります．

2.2. コントローラのエクスポート

作成したコントローラのクラスは，pluginlib を用いてエクスポートする必要があります．
C++ソースファイルの末尾に，次のようなコードを加えてください．

#include <pluginlib/class_list_macros.h>
// Implementation ..
PLUGINLIB_EXPORT_CLASS(name_of_your_controller_package::NameOfYourControllerClass,
                       controller_interface::ControllerBase)

更に，クラスのエクスポートのために，次のように plugin.xml を記述する必要があります．

<library path="lib/lib<name_of_your_controller_library>">
  <class name="name_of_your_controller_package/NameOfYourControllerClass"
         type="name_of_your_controller_package::NameOfYourControllerClass"
         base_class_type="controller_interface::ControllerBase">
    <description>
      Some text to describe what your controller is doing
    </description>
  </class>
</library>

catkin にエクスポートさせるため，package.xml には，以下を書き足してください．

<export>
  <controller_interface plugin="${prefix}/plugin.xml"/>
</export>

そして，コントローラを起動する際には，パラメーターサーバへの登録が必要です．
yaml ファイルに，次のようなパラメータを記述しておくと便利です．

your_custom_controller_name:
  type: name_of_your_controller_package/NameOfYourControllerClass
  additional_example_parameter: 0.0
  # ..

これで，自作したコントローラを使う準備が整いました．
controller_spawner や hardware_manager を利用してコントローラを起動できます．

参考までになりますが，controller_spawner と franka_control_node は同じ名前空間で動作することに注意しましょう．

おわりに

この記事では，前編・後編を通じて，実際に Panda を動かす準備を解説しました．

World MoveIt Day 2019 in Tokyo では，Panda が会場に用意されています．ハッカソンでロボットアームを実際に動かすことのできる貴重な機会を，是非とも有効活用していただけたらと思います．

参考リンク

• Franka Control Interface Documentation
  • franka_ros
• ROS Documentation
  • ROS control tutorials
• MoveItでPandaを動かそう：前編（WMD 2019 in Tokyo 準備編）