チトセロボティクスの「クルーボ」で産業用ロボットのビジュアルフィードバック技術を試してみた（２）

### クルーボについて
チトセロボティクス社の製品「クルーボ」は、産業用として利用されている6軸アームを動かすロボットのビジュアルフィードバック制御のコントローラです。製品の詳細は下記のチトセロボティクス社のサイトで見ることができます。

https://chitose-robotics.com/product

前回の記事はこちらです。ロボットアームにマーカーを取り付け、カメラで検出したマーカーの位置をカメラ画像上でクリックした位置に動かしました。全体の構成、ロボットやカメラの接続、クルーボを使った基本的な開発の進め方については前回の記事を参照してください。

https://qiita.com/mshioji/private/2477d07cdf0208298746

今回は第２弾として、２台のカメラ画像からワークの位置とロボットのツールの先端の位置を検出してロボットアームを動かします。クルーボの特徴として、今回もロボットやカメラの厳密な位置決めやキャリブレーションは不要です。

ツールとしてロボットアームに取り付けた青いピンと、ワークとしてテーブル上に置かれたオレンジ色のパイプをカメラからの映像だけで検出し、青いピンの先端をパイプにインサートするタスクを実行します。

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/4f56da28-71e3-403a-7687-6f73dce13dd4.png">

### 全体の構成

全体の配置は以下の通りです。カメラは２台使用して、Y-Z面（正面）と、X-Z面（側面）の映像を使用してリアルタイムフィードバック制御をおこないます。

2台のカメラはフレームに固定していますが、特に厳密に位置を決める訳でもなく、ただツールとワークが撮影できる場所に配置しています。ワークについても位置決めや固定はしていません。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/c43fa84c-b85a-ac11-6414-4f4c72fbbe46.png)

### コードの準備
それでは前回と同じように新規プロジェクトを作成して試していきましょう。プロジェクトフォルダに移動して`crewbo create tutorial-insert`で新規プロジェクトを作成します。作成したプロジェクトをVS Codeで開きます。
<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/90303266-b3e6-26ff-9fc3-3b6ee7aa2f58.png">

今回は`examples/tutorial/melfa`フォルダの中にあるサンプルコード、`melfa_5_2_vf_for_insertion_task.cpp`を使います。

<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/ebd210fa-5188-ad78-dcd8-caceb0bb6c49.png">

これを全選択のうえコピーして、`src/main.cpp`にペーストして入れ替えます。

<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/8c228d97-d0b5-a8eb-5633-f42a3231d3cd.png">

それでは、前回と同じようにコピペしたコードの中で設定を変更していきます。
#### カメラの設定
436,437行に2台のカメラの設定があります。Camera IDは0と2が設定されています。通常は変更する必要はありませんが、使用するカメラが接続できない場合はここを変更してカメラを接続します。

<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/810ffc33-901b-4169-1eba-f8af22cd22b9.png">

#### ロボットの設定
440行に設定があります。
<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/e5cf25b2-4abb-78f2-2c18-4233f39e5af5.png">
デフォルトではMelfa RV-2FR-Lが設定されているので今回使用するRV-5ASに変更し、IPアドレスも指定しておきます。
```
crewbo::robot::melfa::base_kit::MelfaRv5as melfa(1.f, "192.168.15.20", 10000U);
```
このサンプルコードではインサートタスクを実行する各ステップでのアーム駆動用の4つの関数にそれぞれロボットの機種を指定している箇所があります。そちらもデフォルトがMelfa RV-2FR-Lになっているので、RV-5ASに変更します。
ソースの検索・置換機能を使って4か所とも`MelfaRv2frl`を`MelfaRv5as`に置き換えておきます。

ソフトウェアを実行して最初にロボットが移動する初期位置が、こちらの実験室ではテーブルと干渉したので変更しておきます。変更部分は442行目です。
<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/0aa18f60-d7c3-3f5e-e995-e31946b58e70.png">
J2とJ5のジョイント角度を変更してロボットアームが少し上の位置に向かうようにしています。
```
const crewbo::Joint initial_joint{0_deg, 30_deg, 100_deg, 0_deg, 60_deg, 0_deg};
```
参考までに、445行に`kToolPipePose`が設定されていますが、この変数がロボットアームのフランジ面からツールとして取り付けた青いピンの先端位置（TCP:Tool Center Point）の関係を示しています。付属のツール以外を使用する場合はこのパラメータの変更が必要になります。
これを正しく設定しておくことで、XYZ座標系でツールのポジションを決定した後、ツールの先端位置を維持したままでA軸(Rx)、B軸(Ry)、C軸(Rz)を駆動してツールの姿勢だけを変更することができます。

<img width="500" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/23ecd530-21b7-7f0a-8de9-8166a5a49cc5.png">

### ソフトウェアのビルドと実行
準備ができたので、このコードをビルドして実行してみます。

実行の前に、ロボットのサーボがONになっていてロボットのプログラム「CMXT」が開始していることを確認しておきます。

<img width="200" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/b79a32fd-1d8d-32c4-62f3-663d62b629f4.png">

VS Codeの「ターミナル」メニューから「ビルドタスクの実行」を選択するか、キーボードからctrl-shift-"B"を押すとウインドウ上部にメニューが表示されます。「このソースファイルをビルドして実行する」を選択します。

<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/16bdc913-41bf-5279-1b2a-baf6d13eb684.png">

実行を開始すると、カメラ画像を表示するウインドウが2つ現れます。ロボットハンドに取り付けたツールの青いピンと、テーブル上に置かれたワークのオレンジのパイプが認識できていないと映像が表示されないので、その場合はカメラの配置を調整してください。

■カメラの配置を調整してもツールとワークが上手く認識できない場合の対応については、また別記事で細かく紹介できればと思います。

(追記)　使用するカメラのメーカーや機種にあわせて調整ができるよう追加の記事を書きました。

https://qiita.com/mshioji/items/4b8419f14b9ece495408


<img width="400" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3012948/46e5fdc1-1481-d0a2-8a5b-df6e78f08a44.png">

青いツールの"ピン"と、オレンジのワークの"パイプ"が上手く認識できていると、図のようにグリーンの矢印と、その先に赤と青の２つの制御点が表示されます。ピンとパイプのグリーンの矢印はOpenCVで色情報を元に認識したもので、その情報を元にそれぞれ赤と青の制御点を設定し、リアルタイムでビジュアルフィードバックを行いながらアームロボットを動かします。

ピンのインサートタスクの動作は次の流れで実行されています。
（この挿入アルゴリズムはチトセロボティクス社が保有する特許であり、クルーボでのみ使用が許可されているとのことです。）

1. ワークの"パイプ"の先端から少し先に目標位置として制御点を設けます。ツールの"ピン"の先端にも制御点を設けます。

2. カメラ2台の映像をリアルタイムでフィードバックしながらロボットのピンの制御点をXYZ座標系でワークに設定した制御点と一致するように動かします。

3. XYZ座標系で「位置」をあわせたら、次に「姿勢」を合わせます。ロボットのA軸(Rx), B軸(Ry)をカメラからの映像をリアルタイムフィードバックしながらワークのパイプの軸と一致させます。ロボットアームに取り付けたツールの"ピン"は、その先端をTCP(Tool Center Point)となるようにツールの設定をしていますので、先端の位置を維持したままでA軸(Rx), B軸(Ry)を動かすことができます。

4. ピンとパイプの位置と姿勢が一致したら、ロボットのXYZ座標系でアームを動かして挿入動作を行います。

 - 動画 -

実際の動作
<iframe width="500" height="320" src="https://www.youtube.com/embed/mIuSZL7NMf4" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" loading="lazy" allowfullscreen></iframe>

ロボット動作中のカメラ画像（PC画面）
<iframe width="500" height="320" src="https://www.youtube.com/embed/psIWv9Px6Uo" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" loading="lazy" allowfullscreen></iframe>

このようにカメラ映像からのリアルタイムフィードバックで動作していますが、キャリブレーション不要であることが分かりやすい映像として、起動した後にワークのパイプを初期位置から手で動かしてみます。
ワンショットでポジションを取得するのではなくカメラ映像からリアルタイムでフィードバックしながら動作しているので、ワークが移動しても追従して動作します。

ワーク位置を起動後に変更した場合
<iframe width="500" height="320" src="https://www.youtube.com/embed/dfhOcOyh9mA" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" loading="lazy" allowfullscreen></iframe>

### まとめ
今回はカメラ映像だけを使ってロボットハンドのツールの制御点、テーブル上のワークの制御点を使ってロボットを動作させるチュートリアルを試した内容を紹介しました。

このように、カメラで捉えた情報のみを使って目的の動作を行うのは、ちょうど人間が自分の目でモノを見て、「ココとココを合わせる」といった動作をロボットでも実行していることになります。この方法ではロボットの台座の位置が変わっても、カメラやワークの厳密な位置決めやキャリブレーションをしなくても、2つの制御点の偏差さえ分かれば高精度な動作ができるという事で従来の手法とは大きく異なります。

#### 色情報での認識の課題
今回の事例のようにカメラ映像からターゲットの「色」の情報を元にツールやワークを認識する場合、ターゲットの「色」の選択、そしてその「色」をどの範囲で捉えるかという設定も大変重要です。

まず基本的にはカメラで撮影する画角内には、「ターゲットと同じ色の物体が映り込まない」という事が必要なので、実際の作業環境で背景に映り込むような色を避ける必要があります。例えば、オレンジのワークをターゲットとするのに、よく似たオレンジ色のマーキングや別部材が映り込む場所では正しく認識ができません。

また、ターゲット色の上限・下限をセットして認識するようにしていますが、この色の範囲を広く設定すれば本来のターゲット以外の物体もターゲットとしてとらえる恐れが出てきますし、逆に色の範囲を狭く設定すれば少し条件が変わるだけでターゲットの識別ができなくなります。

そもそも、ターゲットの「色」は特定の波長の光を強く反射することでその色に見えていますが、作業場所の照明条件（自然光や人工光源による環境光）やカメラ内部の自動露出、ホワイトバランスなど、実際に使用するカメラで取得した映像の特性の影響を受けるのも確かです。

こういった点については、別記事でもう少し記述できればと思います。

#### マーカーによる識別
色情報でのターゲット識別が困難な場合、安定して制御点をセットする方法としては前記事のような「マーカー」を使用する方法も考えられます。しかし、マーカーを設置する場所がない場合などもありますので、作業場所に応じた方法を上手く組み合わせるなどの工夫も必要です。

### 今後の予定
今後はtutorialから離れて、実際にNode-REDと連動してネットワーク経由で動作した事例、Node-RED MCUで周辺機器制御をやった事例などを紹介できればと思います。

クルーボやビジュアルフィードバックにご興味ある方、Node-RED、Node-RED MCUと組み合わせている方、コメント等いただけますと嬉しいです。