myCobotでほんとうにmoveitする

# 意味深なタイトル

先日、[myCobotでmoveitする](https://qiita.com/nisshan_/items/de06464dbac84408d188)というタイトルで記事を投稿しました。とりあえず動かして、とりあえず記事投稿しましたが「本当にこれで合ってるの？　なんか違う気がする」ともやもやと気になってました。moveitについて調べ、先人の記事を片っ端から読んでいくと、あれ？全然違う。。。恥ずかしい。。。

良かった点をあえて挙げると、myCobotの公式のリポジトリにはまだmoveit対応されたものがありませんので、記事投稿することで「moveitで動かせた」実績が残せたことに意味があったと思います(なんちゃってなんだけど)。

とういうわけで、moveitのお作法に正しく従ったパッケージを作成しました。

# パッケージのリポジトリ

こちらに置きました。使用方法などはreadmeを読んでください。
[nisshan-x / mycobot_moveit](https://github.com/nisshan-x/mycobot_moveit)

# 作成の裏側

moveitのお作法にしたがって書いたら動きました。

・・・だと話しが終わってしまうので、「moveit対応していないロボットアームをどうやって対応させたか？」を自分なりに書いてみたいと思います。

## 前提条件

今回のmyCobotは以下の条件が整っていました。このおかげでmoveit対応ができました。
- ハードウェアインタフェースが用意されている。
- URDFで書かれた3Dモデルが用意されている
どちらも自分で対応するのは骨が折れますので、揃っててラッキーです。

## 手順1. URDFの書き直し

[公式リポジトリ](https://github.com/elephantrobotics/myCobotROS)にURDFと3Dモデル(Colladaフォーマット)が用意されていますが、これを使わずにURDFを書き直しました。Rvizで表示するだけなら良いのですが、Gazeboで表示できる書き方になっていないです。あと、公式のURDFはリンクとジョイントが混ざってて正直「あかんー」とおもいました(笑)

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">(linkの名前や構造は全部変えました) <a href="https://t.co/63uerBiPzi">pic.twitter.com/63uerBiPzi</a></p>&mdash; nisshan にっしゃん (@nisshan_) <a href="https://twitter.com/nisshan_/status/1350494146067197952?ref_src=twsrc%5Etfw">January 16, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

なお、なぜか公式のColladaファイル(dae拡張子)はGazeboで表示できませんでした。先にGazebo表示に成功した[たいりょーくん](https://twitter.com/Tiryoh)のファイルを利用させていただきました。ありがとうございます！

## 手順2. moveitのsetup_assistant

URDFができたので、moveitのsetup_assistantを立ち上げて読み込みます。やり方はやはり[moveit公式のチュートリアル](http://docs.ros.org/en/melodic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html)の通りです。前回なんちゃって対応だったので、今回は入念に読みました。

## 手順3. HardwareInterfaceの記述

moveitはHardwareInterfaceという実機を抽象化するコンセプトを取り入れており、それゆえに様々な会社のロボットアームやオリジナルアームを制御することができます。また、前述のGazeboといったPC内のシミュレータと実機が同じインタフェースで切り替えができるようになっています。

このHardwareInterfaceやros_controlの使い方がさっぱり理解できず、途方に暮れていたのですが、 @MoriKen さんの記事 [Controller と HardwareInterface との間の処理の仕組み（1. ロボットモデルの定義と登録）](https://qiita.com/MoriKen/items/613635b90f3a98042dc5) のおかげでなんとか理解し、組むことができました。[srcの下のコード](https://github.com/nisshan-x/mycobot_moveit/tree/main/src)を見ると分かるのですが、全部で100行にも満たないです。HardwareInterfaceさまさまです。

## 完成

ラズパイやAI本で有名な[からあげ](https://twitter.com/karaage0703)さんが私のパッケージを使ってくださいました！！！
(しかもデバッグさせてしまった…)

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr"><a href="https://twitter.com/hashtag/myCobot?src=hash&amp;ref_src=twsrc%5Etfw">#myCobot</a> <a href="https://twitter.com/nisshan_?ref_src=twsrc%5Etfw">@nisshan_</a> さんのリポジトリでMoveIt制御できた！ <a href="https://t.co/uCHCbKu8gb">pic.twitter.com/uCHCbKu8gb</a></p>&mdash; からあげ (@karaage0703) <a href="https://twitter.com/karaage0703/status/1353737953273470976?ref_src=twsrc%5Etfw">January 25, 2021</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

# 最後に反省： 前回、なぜ間違えたのか？

myCobot公式のpythonスクリプトの中に、アームの関節の角度を指定して動かすROSノードがあります。このノードが`joint_state`をsubscribeしているのを見て「これでいいじゃん」と無理矢理合わせてmoveitを動かしてしまいました。`joint_state` は本来アームの状態を示すトピックなので「指示する用途には使ってはいけない」ですね。良く調べもせずに現物合わせで作ってはいけない、と反省です。

# 参考
* [Controller と HardwareInterface との間の処理の仕組み（1. ロボットモデルの定義と登録）](https://qiita.com/MoriKen/items/613635b90f3a98042dc5)
* [ROS講座104 moveitでオリジナルのアームを使う](https://qiita.com/srs/items/5501a5d5059e4596f8f0)
* [ROS講座103 armロボットのsim環境を作る](https://qiita.com/srs/items/42828b2d556dae146b20)
* [「ROS + MoveIt! でハンドリングしたい！ 1. Moveit!ってなに？](https://qiita.com/RyodoTanaka/items/1a2f3a427c8800b5cecd)
* [yoneken/minimum_ros_control](https://github.com/yoneken/minimum_ros_control)
* [hardware_interface](https://github.com/ros-controls/ros_control/wiki/hardware_interface)
* [二足歩行ロボット研修（kora編）[20] ros_controlについて](https://rt-net.jp/humanoid/archives/3415)