パラレルリンクロボットを自作してみた（ソフト編）

# パラレルリンクロボット製作（ソフト編）

パラレルリンクロボット、別名デルタロボットの製作メモ。ソフト編。

メカ編では3D CADや3Dプリントサービスを活用して、３つのサーボモータからなるパラレルリンクロボットの本体を作りました。今回はサーボモータをPythonから制御して、ロボットとして動かすために作ったソフトについて書きます。
![fusion_rendering_3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/126781/ee033ad9-361c-f2ee-f1aa-c23996ce2465.png)

## 要求仕様
ロボットを制御する場合、一般的に「ティーチング」と呼ばれるロボットに動作を教示する行為を行い、その後で教示した通りにロボットを自動実行させるという手順を踏みます。ティーチングには、大きく下記の３種類があります。今回はこれら全てについて実装してみることにしました。

| ティーチングの種類 | 説明 |
|:--|:--|
| オンラインティーチング | ロボットを制御するリモコンを使って、実際にロボットを動かしながら動作を教示する。 |
| オフラインティーチング | シミュレータ上でロボットを動かして動作を教示する。 |
| ダイレクトティーチング | ロボットを素手でさわって直接動作を教示する。 |

## クラス構成
Pythonで書いたモジュールのクラス図です。
![class_diagram.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/126781/f66edb72-a31f-8bd8-f7a6-1ff499571d79.png)


各クラスの役割です。

| クラス名 | 役割 |
|:--|:--|
| PS4Controller | pygameモジュールを使って、PS4のDualShock4コントローラから各種操作情報を取得するクラス。アナログスティックの情報はデルタロボットの３次元動作の指示に用いる。○△□×ボタンはデルタロボットのシミュレーションモデルの描画領域のビューモードを切り替えるために用いる。 |
| AX12Driver | dynamixel_sdkモジュールを使って、Dynamixel AX12サーボモータを制御するクラス。デルタロボットを構成する３つのサーバモーターの位置決め制御を行うことで、デルタロボットが上下左右に動作する。 |
| DeltaModel | デルタロボットの運動学、逆運動学の計算ロジックを提供するクラス。 |
| DeltaBase | デルタロボットの制御に共通的に必要な機能を提供する抽象クラス。各種クラスの初期化のほか、matplotlibを用いてロボットの形状を3Dグラフ常に描画する機能を提供する。 |
| DeltaTeaching | デルタロボットのティーチングに共通的に必要になる機能を提供する抽象クラス。ティーチング操作中のサーボモーターの位置情報をメモリ上に記録し、プログラムの終了時にファイルに出力する。出力フォーマットはJSON。 |
| DeltaTeachingOffline | デルタロボットのオフラインティーチングを行うクラス。PS4Controllerから得られる操作情報から、各軸方向の移動速度を取得し、ロボットの現在位置を演算。DeltaModelの逆運動学の計算機能を使って、位置情報をモーターの角度情報に変換する。DeltaBaseから継承した機能で、更新したロボットの状態を3Dグラフ上の仮想モデルに描画する。 |
| DeltaTeachingOnline | デルタロボットのオンラインティーチングを行うクラス。DeltaTeachingOfflineクラスから継承した機能を用いて仮想モデルを制御し、仮想モデルと実機が同じ状態になるように、AX12Driverに対してサーボモータの位置制御指示を行う。 |
| DeltaTeachingDirect | デルタロボットのダイレクトティーチングを行うクラス。AX12Driverからサーボモータの現在位置情報（角度）を取得して、記録する。同時に、DeltaBaseから継承した機能で、3Dグラフ上の仮想モデルに同情報を反映する。 |
| DeltaPlayerOffline | デルタロボットのオフライン自動実行を行うクラス。各種ティーチング時に記録したサーボモータの位置情報の時系列データを入力として、ティーチングした動きを3Dグラフ上の仮想モデルで再現する。 |
| DeltaPlayerOnline | デルタロボットのオンライン自動実行を行うクラス。DeltaPlayerOfflineから継承した機能を用いて仮想モデルを制御し、仮想モデルと実機が同じ状態になるように、AX12Driverに対してサーボモータの位置制御指示を行う。 |

## ソースコード
https://github.com/kizitorashiro/deltarobot

## 参考にしたサイト
実装する際に参考にしたサイトです。

| 項目 | URL |
|:--|:--|
| デルタロボットの逆運動学 | https://tony-mooori.blogspot.com/2016/09/kinematics.html |
| デルタロボットの運動学 | http://hypertriangle.com/~alex/delta-robot-tutorial/  | 
| デルタロボットの運動学 | https://github.com/awesomebytes/delta_robot/blob/master/src/delta_kinematics.py | 
| PS4 DualShock4 | https://gist.github.com/claymcleod/028386b860b75e4f5472 |
| Dynamixelサーボモータの仕様 | http://support.robotis.com/jp/product/dynamixel/ax_series/dxl_ax_actuator.htm |
| Dynamixelサーボモータのドライバ | https://github.com/ROBOTIS-GIT/DynamixelSDK |

## 動作デモ
![demo1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/126781/b25d9fb1-cba5-bdae-ee93-1468111f4a15.png)


### オンラインティーチング
PS4のコントローラをリモコンにして、デルタロボットを動かします。PC上の仮想モデルと実機が同期して動作します。仮想モデルはビューモードに切り替えによって視点を変えて表示することができます。
[![オンラインティーチング動画](http://img.youtube.com/vi/cfyof1CeysQ/0.jpg)](http://www.youtube.com/watch?v=cfyof1CeysQ)

### オフラインティーチング
PS4のコントローラをリモコンにして、デルタロボットの仮想モデルを制御すると同時に、位置情報を時系列データとして記録します。その後、同じ動作をロボットが自動リプレイします。
[![オフラインティーチング動画](http://img.youtube.com/vi/9cVzLlweHJY/0.jpg)](http://www.youtube.com/watch?v=9cVzLlweHJY)

### ダイレクトティーチング
ロボットを直接手で触って動かします。ロボットの位置が変わるごとにその情報を時系列データとして記録します。その後、同じ動作をロボットが自動リプレイします。
[![ダイレクトティーチング動画](http://img.youtube.com/vi/4xpo1VL3fhg/0.jpg)](http://www.youtube.com/watch?v=4xpo1VL3fhg)