こんにちはアールティのクアンです。前回の円弧補間に続き、チーム開発研修ブログの最終回です。本ブログは前回のブログとは違い、今回はMoveItを使わずに、独自のROSパッケージ（DYNAMIXEL SDKのROSラッパー）を実装してピックアンドプレースを行う事について解説していこうと思います。よろしくお願いします。
MoveItを使わなくなった理由
MoveItを使用せずに、独自のROSパッケージでCRANE-X7を制御する事になったきっかけは以下の二点です。
- カスタマイズしやすさ
- 拡張しやすさ
一点目の理由は、Cartesianパスで軌道を計画した際の問題です。第1回のブログで述べたように、Cartesianパスを使用するとマニピュレータの動きが速すぎ、安全性を確保するために速度を落としたいと考えました。しかし、MoveItではCartesianパスの速度を直接変更する設定がないことがわかりました。この問題を解決するためには、生成された軌道に対して時間パラメータ化を行う必要があり、単純な設定でもMoveItがサポートしていない場合があります。
二点目は、前回のブログで、Pilzによる手先の円弧補間を実装していた際、ある問題に直面しました。MoveIt2の資料が非常に少なく、C++又はPython等での使用し方などのマニュアルがMoveIt1と比べてほとんど整備されていません。また、CRANE-X7の制御を担当している中村さんとの話し合いの中で、位置制御に加えてトルク制御や重力補償などの電流制御も実装したいという要望がありました。MoveIt2でこれらの機能の実現方法を調べましたが、ドキュメントが不十分であると判断し、独自のCRANE-X7制御用のROSパッケージを実装することにしました。
DYNAMIXEL SDKのROS2ラッパー
CRANE-X7制御用のROSパッケージはCRANE-X7のサーボを制御するためのDYNAMIXEL SDKのROSラッパーとなっています。C++でDYNAMIXEL SDKライブラリを使用し、サーボの設定を変更したり、角度を書き込んだりしてCRANE-X7の八個のサーボを制御します。ROSパッケージの構成は以下の図のようになります。
制御の中村さんが実装してくれたCRANE-X7の逆運動学による手先の位置制御処理やCRANE-X7の各ジョイントの角度を制御するためのトピックを用いて、公式のCRANE-X7のROSパッケージ同様の機能を実装しました。
各サーボの角度を指定し、CRANE-X7を制御する機能
キーボード入力で各ジョイントの角度を制御する機能
グリッパーの閉じ開き機能
決まった位置の物体のピッキング機能
これら機能はROSのアクションサーバを経由し、各ジョイントの角度・グリッパーの閉じ開き・手先の位置をコントロールしました。DYNAMIXEL SDKの各サーボの設定（電流制御・位置制御）から各ジョイントの速度まで簡単に変更可能になりました。したがって、今後電流制御による新しい制御法を実装する際、サーボの設定変更やROSアクションの作成を行うだけで簡単に追加可能です。
独自のROSパッケージによるピックアンドプレースの実装
独自のROSパッケージを解説する前に、現在のピックアンドプレースシステムの全体の流れについて説明したいと思います。MoveItで制御するピックアンドプレースシステムの全体図は以下のようになります。カメラで撮ったRGB画像で物体を検出し、tf変換で検出された物体の位置・姿勢をMoveItに渡し、ピックアンドプレースの軌道計画を行います。
新しい制御用のROSパッケージではURDF情報なしで起動計画を行うため、MoveItの処理だけでなく、物体検出の処理も修正する必要があります。URDF情報がないと、カメラの位置・姿勢情報を得られないため、物体が検出されてもtf変換できなくなります。したがって、要修正の箇所は以下の図のようになります。
上記のURDFがない問題は、独自のROSパッケージをURDFに対応させる事で解決できます。CRANE-X7のURDF情報はアールティが公開しているcrane_x7_descriptionというROSパッケージがあるため、このパッケージを使って問題を解決します。display.launch.pyファイルの25~28行目を以下のように修正すると、RViz内で表示されるURDFを別のトピックから各ジョイントの角度を読み込ませる事ができます。このトピック通信を利用すると、DYNAMIXEL SDKのROSラッパーをURDFに対応させる事ができます。
jsp = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
remappings=[('/joint_states', '/dummy_joint_states')],
output='screen'
次は独自のROSノードでCRANE-X7の現在の角度情報を用いてRViz内のURDFを制御します。RViz内のCRANE-X7は/joint_statesというトピックから各ジョイントの角度を読み込んで表示されるため、launchファイルを上記のように修正すると、角度情報を独自のROSノードからパブリッシュされる情報だけを読み込ませられます。キーボード入力機能によるURDFを制御する様子は以下のようになります。
キーボード入力でRViz内のCRANE-X7を制御
これで、検出された物体の把持位置・姿勢を取得できたため、tfの情報を軌道計画パッケージに渡し、セクション「DYNAMIXEL SDKのROS2ラッパー」でお話ししたピッキング機能と組み合わせてピックアンドプレースデモを実装できました。ピックアンドプレースデモの様子は以下のようになります。
おわりに
チーム開発研修の最後のブログでは、独自のDYNAMIXEL SDKのROS2ラッパーについて解説しました。また、また、独自のROSパッケージを用いてCRANE-X7の各ジョイントの角度制御機能、ピッキング機能、キーボード入力による角度制御機能、ピックアンドプレースデモを実装しました。このシステムを期限通りに納品し、チーム開発研修を終えました。
本研修を通じて、MoveIt2やマニピュレータの制御について学ぶことができました。また、社内のチーム開発の流れについても学べました。今までほとんど意識しなかった安全の基準や非技術者のための使用書なども非常にためになりました。本研修で得た知識を生かし、今後のプロジェクトに挑戦したいと思います。
