mycobotをTouchDesigner(python)で動かす

# ■はじめに
先日、作品制作の中でプログラマブルロボットアームmycobotを触る機会がありまして備忘録的に書きます。
作った作品としては、簡単に言うならば骨董品である長火鉢に眠る記憶を呼び起こすというもので、5/2~4にSICF24（https://www.sicf.jp/information/) に出展してきました。
mycobotの役目としては、長火鉢を実際に扱う際、客人をもてなす際に行われたという「灰ならし」という所作を再現するといったものでした。

https://www.youtube.com/watch?v=_7nAYkA7h5o

https://www.youtube.com/watch?v=9D_KKQjDpWU


mycobotに関する記事はROSでの制御やpythonの制御の記事はいくつかありつつも、TouchDesigner上で動かしている参考がなく、今回の作品ではAzureKinectなど他のデバイスとも制御の連携が必要だったため、このような実装としました。
どなたかの参考になれば幸いです。

# ■開封の儀
購入したものはこちらの「mycobot 280 M5」。
台座部分にM5Stack Basic、頭の部分にATOM Matrixが組み込まれています。
スイッチサイエンスで注文してから3日ほどで届きました。
https://www.switch-science.com/products/7141?_pos=9&_sid=cf39015dd&_ss=r
![IMG_8514.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/d578c5ca-fdc4-e900-6f44-7f30bd6c6d66.jpeg)
配送時の折りたたまれた状態ではiPadほどのサイズ感で、あれサイズ間違えた？と焦りました・・。
公式ページによると全長350mmほどで、全てのアームを展開するとそれなりのサイズ感となります。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/c15e4ff1-809b-925e-d1f3-4ad084ff2154.png)

標準でレコード・再生機能がついているので、電源させばすぐに動かせます。滑らかな動きがキモチイィィ

https://twitter.com/tsunk66/status/1641454889766367235?s=20

# ■Pythonで制御する
### mycobotモード切り替え
mycobotをPCからPythonで制御するには、本体側でモードを切り替える必要があります。PCとmycobot(M5Stack部分)をUSBケーブルで接続しトップ画面から、Transponder>USB UART を選択。
Connect testの表示で「Atom：ok」となっていれば正常です。
![mycobotMode.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/bd27ac6b-d081-75e0-bc13-349c9c0a5403.png)

### Python環境構築
mycobotにPython経由でアクセスするためのライブラリpyserialとpymycobotをインストール。
(執筆時点:pyserial-3.5、pymycobot-3.0.9)
https://github.com/elephantrobotics/pymycobot

```console
$ pip3 install pyserial
$ pip3 install pymycobot
```
### Atom部分のLEDを制御
```python
from pymycobot.mycobot import MyCobot
from pymycobot import Coord
import serial
import serial.tools.list_ports

# Conection
port = "/dev/tty.usbserial-hogehoge"
baud = 115200
DEBUG = True
mycobot = MyCobot(port, baud, debug=DEBUG)

#カラー変更 (0~255でそれぞれRGBを指定)
mycobot.set_color(255,0,0)
```
portの指定はWindowsの場合はCOM〜。
MacでシリアルデバイスのUSBポートを知りたい際はターミナル上で、下記を打つことで一覧を取得できる。
```console
$ ls /dev/tty.*
```
### アーム部分の制御
```python
from pymycobot.mycobot import MyCobot
from pymycobot.genre import Angle
from pymycobot import Coord
import serial
import serial.tools.list_ports

# Conection
port = "/dev/tty.usbserial-hogehohe"
baud = 115200
DEBUG = True
mycobot = MyCobot(port, baud, debug=DEBUG)

#アーム制御(J1~J6の角度を-160〜160度で指定、またスピードを0~100で指定)
mycobot.send_angles([20,0,20,0,20,0], 30)
```

J1が台座部分、J6が頭部分。詳細は上記に載せた寸法図を参考。

またmycobotのPythonAPIは他にも各モータのステータス取得や、現在の角度から＋10°のように制御する方法など幅広くあるので、詳しくは下記リンクのAPI一覧をご覧ください。

https://github.com/elephantrobotics/pymycobot/blob/main/docs/README.md



# ■TouchDesigner上で動かす
といってもTouchDesigner上でPythonで動かすというだけなので、慣れている人は当たり前な話かと思いますが、一応。
Setting>Preference>Generalの
Add External Python to Search Pathからpythonライブラリの環境構築したパスを指定。
これでtextDATなどでLEDやアーム制御のスクリプトをrunすれば制御可能です。
![preference.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/d279405e-d66b-33b1-8a7d-501130d6a261.png)


https://twitter.com/tsunk66/status/1647970319435698177?s=20


### mycobotシミュレーターを作る
今回ロボットアームに模様を描かせるといった、それなりに複雑な動きを考えていたためシミュレータをTouchDesigner上で作ることにしました。

https://twitter.com/tsunk66/status/1654505569955516416?s=20

mycobotシリーズはElephantRoboticsから3Dモデル（URDFファイル）が配布されています。

https://github.com/elephantrobotics/mycobot_ros/tree/noetic/mycobot_description/urdf

僕はURDF？はて？という状態だったのですが、調べてみると
>URDF (Unified Robotics Description Format) は、製造業の組み立てライン用ロボット マニピュレーター アームや遊園地用のアニマトロニクス ロボットなどのマルチボディ システムをモデル化するために、学術界や産業界で使用される XML 仕様です。URDF は ROS (Robotics Operating System) のユーザーに特によく使用されています。
とのこと。

参考：https://jp.mathworks.com/help/sm/ug/urdf-model-import.html;jsessionid=1d3c73c784f023e93234a87fbb20

恐らく便利な人にとっては便利な形式なんだろう、と思いつつも、制作時間が限られていたこともあり、自分は無理やりfbxデータに変換して使用。
ここら辺はえいやで突き進んだので、あまり参考にならないかと思いますが、
Unity上でURDFをimportできるプラグインをつっこみ、ゴニョゴニョした後、fbxに書き出しました。
ここでは詳しく触れませんが参考リンクだけ貼っておきます。
UnityでURDFを使う：https://github.com/Unity-Technologies/URDF-Importer
Unityオブジェクトをfbxで書き出す：https://docs.unity3d.com/ja/Packages/com.unity.formats.fbx@4.1/manual/exporting.html

あとは実際にpythonでmycobotに書き込む値を、TouchDesigner上に取り込んだ3Dモデルの各軸に代入するだけです。
またGUI上で3D空間のシミュレータを表示するには、OPViewerCOMPを利用するにカメラなどのオペレータを入れるだけですが、
その際のTIPSとして、カメラを向けたいモデルの中心あたりに空白のGeometoryなどを置いておき、CamCOMPのLookAtに、そのGeometoryを代入しておくと、軸がずれず扱いやすいです。

![mycobotSim.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/240544d4-63b9-6e31-7aa6-8a8434b1c12d.gif)

またオペレーターがActiveな状態で右クリックし、「Home All」　を選択することで初期位置に戻ることができます。



### アーム制御のレコーディング・再生機能を作る
mycobotのAPIには、ロボットアームのサーボの現在の角度を取得できるものがあります。
ただし通常mycobotは電源接続時、各サーボは固定モードになっているため、一度それをリリースし、自由稼働できる状態にする必要があります。
```python
#　サーボモータをリリース状態に
mycobot.release_all_servos()
# 現在の各サーボモータの角度を取得
getAngles = mycobot.get_angles()
```
取得したgetAnglesをConstantCHOPなどに代入することでタッチのネットワーク上でCHOPデータとして扱えるので、数値の保存や、再生するのに便利です。

これでタッチとmycobotを常時接続して、リアルタイムに角度取得も制御もやり放題！と思いきや
pythonスクリプトをmycobotを書き込む際に（シリアル通信する際に？）処理が重くなり、高速に書き込むとmycobotが接続エラーになることがあっため、0.5秒に1度くらいの接続でリアルタイムの角度を取得する方針としました。
最終的に、取得した角度でキーフレームを打つことでアームの各角度を動きをレコーディングし、再生するシステムとしました。
![GUI.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/77487440-f408-f80d-61b4-eadb4ee444b2.png)
get_anglesで取得した値はシンプルにConstantCHOPに記録し複数点分の角度データをShuffleCHOPで配列に変換する形としました。
![スクリーンショット 2023-05-29 4.53.17.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/8505ad02-801e-6683-a605-c0019bc8e929.png)
またその際、GUI表示自体は3Dモデルのシミュレータ同様OPViewerCOMPにつっ込むだけですが、
CHOPデータをGUIで扱うTIPSとして、CHOPデータはActive状態で右クリックのプロパティから様々に表示方法を変えることができます。（上のNullCHOPが初期表示、下が変更したもの）
今回は各タイミングの角度が一目で見えるようにしたかったため、同一の縦軸上にドット表示する形にしています。
![スクリーンショット 2023-05-29 4.52.26.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/115ab440-8094-82f6-3b19-d11502efa8b2.png)


### 全体像
汎用的に使えそうな部分で整理した、最終的な全体像とスクリプトは下記です。
![TDGUI.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/320190/9243772c-4724-8cb1-6e85-337daaf8bc09.png)

```python
from pymycobot.mycobot import MyCobot
from pymycobot.genre import Angle
from pymycobot import Coord
import serial
import serial.tools.list_ports

# Conection
port = "COM6"
baud = 115200
DEBUG = True
mycobot = MyCobot(port, baud, debug=DEBUG)

# 変数
table = op('table_status')
setAngles = op('null_motor')
getAngles = mycobot.get_angles()
positions = mycobot.get_coords()
colorLED =  op('null_color')
motorSpeed = op('null_speed')
RecordAngle = op('constant_RecordAngle')
realTimeAngle = op('constant_realTimeAngle')
Log = op('table_log')
modeRecAngle = op('null_modeRecAngle')
playModeAngle = op('playModeAngle')

# Error判定
Log[0,0] = "電源ステータス：" + str(mycobot.is_power_on())
Log[1,0] = "ロボットステータス：" + str(mycobot.read_next_error())
Log[2,0] = "サーボステータス：" + str(mycobot.is_all_servo_enable())
Log[3,0] = "Atomステータス：" + str(mycobot.is_controller_connected())

if op('null_mode_manual')[0] == 1:
    # LED
    mycobot.set_color(int(colorLED[0]),int(colorLED[1]),int(colorLED[2]))
    # //Servo
    mycobot.send_angles([int(setAngles[0]),
                        int(setAngles[1]),
                        int(setAngles[2]),
                        int(setAngles[3]),
                        int(setAngles[4]),
                        int(setAngles[5])],
                        int(motorSpeed[0])
                        )

if op('null_mode_record')[0] == 1:
    # free mode
    if op('null_move')[0] == 1:
        mycobot.release_all_servos()
        getAngles = mycobot.get_angles()
        realTimeAngle.par.value0 = int(getAngles[0])
        realTimeAngle.par.value1 = int(getAngles[1])
        realTimeAngle.par.value2 = int(getAngles[2])
        realTimeAngle.par.value3 = int(getAngles[3])
        realTimeAngle.par.value4 = int(getAngles[4])
        realTimeAngle.par.value5 = int(getAngles[5])

        # LED
        mycobot.set_color(0,255,0)
    # record mode
    if op('null_record')[0] == 1:
        mycobot.release_all_servos()
        getAngles = mycobot.get_angles()
        realTimeAngle.par.value0 = int(getAngles[0])
        realTimeAngle.par.value1 = int(getAngles[1])
        realTimeAngle.par.value2 = int(getAngles[2])
        realTimeAngle.par.value3 = int(getAngles[3])
        realTimeAngle.par.value4 = int(getAngles[4])
        realTimeAngle.par.value5 = int(getAngles[5])
        # 角度取得
        getAngles = mycobot.get_angles()
        RecordAngle.par.value0 = int(getAngles[0])
        RecordAngle.par.value1 = int(getAngles[1])
        RecordAngle.par.value2 = int(getAngles[2])
        RecordAngle.par.value3 = int(getAngles[3])
        RecordAngle.par.value4 = int(getAngles[4])
        RecordAngle.par.value5 = int(getAngles[5])

        # LED
        mycobot.set_color(0,0,255)
    # play mode
    if op('null_play')[0] == 1:
        mycobot.send_angles([int(modeRecAngle[0]),
                            int(modeRecAngle[1]),
                            int(modeRecAngle[2]),
                            int(modeRecAngle[3]),
                            int(modeRecAngle[4]),
                            int(modeRecAngle[5])],
                            int(motorSpeed[0])
                            )
        # LED
        mycobot.set_color(255,0,0)

Log[4,0] = "角度取得：" + str(getAngles)
Log[5,0] = "位置取得：" + str(positions)
sys.stdout = op('text_log')
```
#  ■プロジェクトファイル
https://github.com/tsunk66/TD_mycobotController

#  ■参考
https://github.com/elephantrobotics/pymycobot
https://github.com/elephantrobotics/pymycobot/blob/main/docs/README.md
https://zenn.dev/karaage0703/books/3be6bad93b0c8e/viewer/ed17bf
https://note.com/npaka/n/n7f2dd658af7f