myCobotをOpenRRで動かしてみた

# myCobotとは

[TEGAKARI](https://www.tegakari.net/2021/01/mycobot/)より

> 中国 Elephant Robotics 社 と M5Stack 社が共同開発する、小さく軽い、モジュラーデザインの6軸 協働ロボット。

> 本体重量850gと軽量ながらも250gのペイロード能力をもち、アームの長さは350mm、さまざまなエンドエフェクタを取り付けることで、科学研究や教育、スマートホーム、軽工業など多分野のアプリケーションに対応します。

> コントローラにはM5Stack Basicが採用されており、大量のアプリケーションケースを直接使用することができます。
ビジュアルプログラミングソフトウェア UIFlow による Blockyプログラミングに対応しており、また ElephantRobotics 社の産業ロボット用ソフトウェア RoboFlow によりROSオープンソースシステムにも対応します。

# myCobotをMoveItに対応させる

### ファームウェアの焼き込み

[公式サイト](https://www.elephantrobotics.com/myCobot/)が提供するmyStudioというアプリケーションを使いました。Linux版は使い方がよく分からなかったためMac版をインストールしました。ただこれも英語版のウェブサイトからだと”This site can’t be reached”エラーが出るため中国語版のウェブサイトからインストーラーをダウンロードする必要がありました。（アプリ内で英語に変更できます）

myStudioがインストールできたら、myCobotとPCをUSB接続して、ToolsからTransponderを焼き込みます。必要に応じて先にキャリブレーションを行ってもいいかもしれません。

### MoveItを使う

ROS-melodicのワークスペースを作った後、基本は[nisshan-xさんのパッケージ](https://github.com/nisshan-x/mycobot_moveit)に従うだけでMoveItできてしまいます。
但し2021/03/05の時点で一点だけ問題があって、moveitが出す各関節角とmyCobot内の関節角のスケールが異なっているようです。具体的にはmyCobotの関節角のスケールがmoveitのそれの1 / 2πになっていて、上のリンクに従ってそのまま実行すると例えば関節を10°動かそうとすると60°ほど動いてしまいます。myCobotをROSで操作する他のパッケージを用いても同じ症状がみられたため、おそらくはmyStudioのアップデートに伴って関節角のスケールが変更されてしまっているのではないかと思います。
myStudioをダウングレードさせることはできなかったため、今回はパッケージ内のファイルを変更することで対応しました。具体的にはmycobot_moveit/scripts/mycobot_hw_interface.pyのjointCommandCallback関数内のvalueを2πで割ってあげれば大丈夫です。

```diff_python
def jointCommandCallback(self, msg):
   data_list = []
   for index, value in enumerate(msg.data):
       if index != 2:
           value /= -1
+  value /= (2 * math.pi)
   data_list.append(value)
```

修正を加えたリポジトリは[こちら](https://github.com/Masa0u0/mycobot_moveit)です。

# OpenRRとは

オープンソースのRust用ロボット開発フレームワークです。現状(2021/03/19時点)ではROS1と後述するurdf-vizに対応しています。

# OpenRRを触ってみる

[OTLさんの記事](https://qiita.com/OTL/items/3df938f61a4520c10b70)通りにやればサンプルを動かすことができました。

# OpenRRとurdf-vizでmyCobotをシミュレーションしてみる

上の記事を参考に、シミュレーション空間内でmyCobotに対してOpenRRのJoint Position Senderを使ってみます。Joint Position SenderはMoveItが動けば動くように設計されているらしいので、上でMoveItができていれば設定用のtomlファイルを1つ作るだけで動くはずです。

### 設定ファイルの作成

[設定ファイル](https://github.com/Masa0u0/openrr-mycobot/blob/main/config/mycobot.toml)を以下のように作成しました。

```toml
[[openrr_clients_configs]]
name = "arm"
joint_names = [
   "arm1_joint",
   "arm2_joint",
   "arm3_joint",
   "arm4_joint",
   "arm5_joint",
   "arm6_joint",
]
controller_name = "mycobot_trajectory_controller"
complete_allowable_errors = [0.02, 0.02, 0.02, 0.02, 0.02, 0.02]
 
[openrr_clients_config]
urdf_path = "../urdf/mycobot.urdf"
self_collision_check_pairs = [
   "body_link:arm1_link",
   "body_link:arm2_link",
   "body_link:arm3_link",
   "body_link:arm4_link",
   "body_link:arm5_link",
   "body_link:arm6_link",
]
 
[[openrr_clients_config.collision_check_clients_configs]]
name = "arm_collision_checked"
client_name = "arm"
 
[[openrr_clients_config.ik_clients_configs]]
name = "arm_ik"
client_name = "arm_collision_checked"
solver_name = "arm_ik_solver"
 
[[openrr_clients_config.joints_poses]]
pose_name = "zero"
client_name = "arm_collision_checked"
positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
[[openrr_clients_config.joints_poses]]
pose_name = "pose1"
client_name = "arm_collision_checked"
positions = [0.0, 0.5, 0.0, -0.8, 0.0, 0.3]
 
[openrr_clients_config.ik_solvers_configs.arm_ik_solver]
ik_target = "arm6_joint"
```

URDFへのパスは記事には絶対パス or 相対パスと書いてありますが、手元で実行した際は相対パスでないと動きませんでした。
self_collision_check_pairsはテキトーです。

### 実行

```terminal
$ urdf-viz <urdfファイルへのパス>
$ openrr_apps_joint_position_sender <tomlファイルへのパス>
```

を実行すればmyCobotをurdf-viz上で操作することができます。URDFはMoveItの際に使用したものと同じで大丈夫です。

# OpenRRとROSでmyCobotの実機を動かしてみる

今度はmyCobotの実機に対してJoint Position Senderを使ってみます。

### 設定ファイルの作成

[設定ファイル](openrr-mycobot/config/mycobot_ros.toml)を以下のように作成しました。

```diff_toml
+ use_move_base_urdf_viz_web_client = false
+ use_navigation_urdf_viz_web_client = false
 
- [[openrr_clients_configs]]
+ [[ros_clients_configs]]
name = "arm"
joint_names = [
   "arm1_joint",
   "arm2_joint",
   "arm3_joint",
   "arm4_joint",
   "arm5_joint",
   "arm6_joint",
]
controller_name = "mycobot_trajectory_controller"
complete_allowable_errors = [0.02, 0.02, 0.02, 0.02, 0.02, 0.02]
 
[openrr_clients_config]
urdf_path = "../urdf/mycobot.urdf"
self_collision_check_pairs = [
   "body_link:arm1_link",
   "body_link:arm2_link",
   "body_link:arm3_link",
   "body_link:arm4_link",
   "body_link:arm5_link",
   "body_link:arm6_link",
]
 
[[openrr_clients_config.collision_check_clients_configs]]
name = "arm_collision_checked"
client_name = "arm"
 
[[openrr_clients_config.ik_clients_configs]]
name = "arm_ik"
client_name = "arm_collision_checked"
solver_name = "arm_ik_solver"
 
[[openrr_clients_config.joints_poses]]
pose_name = "zero"
client_name = "arm_collision_checked"
positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
[[openrr_clients_config.joints_poses]]
pose_name = "pose1"
client_name = "arm_collision_checked"
positions = [0.0, 0.5, 0.0, -0.8, 0.0, 0.3]
 
[openrr_clients_config.ik_solvers_configs.arm_ik_solver]
ik_target = "arm6_joint"
```

前述のものとほぼ同じですが、少しだけ変更点があります。
まず、1, 2行目に

```toml
use_move_base_urdf_viz_web_client = false
use_navigation_urdf_viz_web_client = false
```

を追加する必要があります。
また、4行目は

```toml
[[openrr_clients_config]]
```

ではなく

```toml
[[ros_clients_config]]
```

でなければならないことに注意してください。

### 実行、デバッグ

本来ならばこれでmyCobotをUSB接続して

```terminal
$ roslaunch mycobot_moveit mycobot_moveit_control.launch
$ openner_apps_joint_position_sender <tomlファイルへのパス>
```

とすればJoint Position SenderのGUIからmyCobotの実機を操作することができるはずなのですが、手元で実行した際にはいくつかエラーが発生しました。全ては書ききれないので、一番手強かったものだけ対処法を記しておきます。
上のコマンドを実行すると

```terminal
Dropping all 1 trajectory point(s), as they occur before the current time last time 33.02s...
```

のような警告文が出て、時間差がどんどん広がっていきました。Joint Trajectory内の時刻が現在時刻よりも前になっているために到達不可能という趣旨の警告なのですが、調べてみると、mycobot_moveit/src/mycobot_control.cppのwhile文内でprev_timeの更新が行われていなかったためにシミュレーション空間内の現在時刻がズレていたことが原因であることがわかりました。よってwhile文の1週ごとにprev_timeにtimeを代入するように修正すると改善しました。

```diff_cpp
   while (ros::ok())
   {
       const ros::Time time = ros::Time::now();
       const ros::Duration period = time - prev_time;
+      prev_time = time;
       robot.read(time, period);
       cm.update(time, period);
       robot.write(time, period);
 
       rate.sleep();
   }
```

修正を加えたリポジトリは[こちら](https://github.com/Masa0u0/mycobot_moveit)です。

# リンク先

- [nissyan-xさんのリポジトリに修正を加えたリポジトリ](https://github.com/Masa0u0/mycobot_moveit)
- [設定用のtomlファイルを含むリポジトリ](https://github.com/Masa0u0/openrr-mycobot)