Unitree Go1 を使ってみよう！（その３ ソフト編）

今回はソフトウェア編です。

雑多なメモ状態ですが、参考になりましたら幸いです。（2022/3 に届いた Proモデルでの情報）

SDKの使ってみた様子/ROS/音声関係/環境などについて書いています。

（どうやらスピーカやらWiFi 5GHz -> 2.4GHz 変更などノウハウがまとまったドキュメントがUnitree からもらえるぽいので、購入された方はサポートに聞いてみましょう。私も取得しました。）

2024/5 追記

こちらの方の記事によりますと、2023年暮れ～以降の出荷バージョンから何やら対策がなされたとの事です。

https://qiita.com/Manyan3/items/cfab10bfacd1e646a171

私の記事は2022年のものなので、どこまで同じことができるかわかりませんが、参考のために残しておきます。

前回　その２　ハード編→

前々回　その１　スタート編→

追加

はじめに

ハード編にも書きましたが、内部のラズパイ・Jetson Nano にソフト追加したりするのには、USB-Ethernet 接続するのがシンプルに使えそうですが、 安易に apt update とかすると、最初の環境から変化していくので、標準ソフトへの影響が未知なのでよくなさそうです。

私は内部ボードのソフト構成はあまり変えず、追加のホストからLAN 経由でコントロールしています。

Unitree 公式リポジトリ

Unitree github リポジトリ

unitree_legged_sdk
UnitreeCameraSDK　（これはPro も公式対応）

unitree_ros
unitree_ros_to_real

ソフト構成

ラズパイもJetson Nano も、~/Unitree/autostart/ に各種バイナリがあり、~/Unitree/sdk/ にSDKが置いてある。
SDKは公式github にあるが、unitree_legged_sdk 等、ハードのversion によって構造体の宣言が異なって挙動が変わるものもあるよう。
また、faceLightSDK(LED) や ultraSoundSDK_Nano/ultraSoundSDK_Raspi(超音波距離センサ)など、まだ公式github に無いものもある。

公式 SDK

unitree_legged_sdk

各example のようす
Pro では、High-level モードの example_walk.cpp は動きました。(cmd.levelFlag = 0x00; を追加する)
また、IMU/モータ角度値などの状態値は取得できる模様。

トルク・速度・位置制御の Low-level モードの example_torque / velocity / position .cpp は動きませんでした。
Pro でも動かせた方がいたら是非お教えください。

cmd.levelFlag = 0x00～0xFF までインクリメントして試しましたが、やはり Lo-level モードは動かず。

（Pro / Edu でここら辺の挙動を変えてるのかな？あくまで推測だが、CRC チェックサム処理をPro / Edu で挙動を変えて制限してる？）

example_walk.cpp

最新(2022/4/5 時点) version 3.5.1 と、過去のもので、 include/comm.h の構造体の中身が一部異なるため、
古いバージョンは、実行するとデータが正しく取得できない。（実機のファームversion によってここらは変わりそう）

そういえば High-level モードで backflip モーション実行を試しましたが、さすがにこれは封印されてるのかな？バク転は実行されませんでしたｗA1 では出来るのと、Go1 の紹介ムービーでもしてたから微かに期待していたのですがｗ　Edu の方がモーター強そう？なので出来るのかどうかは不明
（バックフリップはできたので後述）

バックフリップ

バックフリップはアンロック操作が必要でした。

（バックフリップ（バク転）は負荷で脚が折れるよ～という海外の方のツイを見かけましたので、自己責任でやりたい方はDM下さい）

UnitreecameraSDK

各example のようす

gstremer の配信がexampleで実行されてるが、それをコマンドで再現したものもツイートに書いた。

使いやすいようexample 追加のプルリクを出しました！

実行前にカメラ関連プロセスをkill する必要があります。

カメラ

faceLightSDK

ultraSoundSDK_Nano/ultraSoundSDK_Raspi

ROS

Unitree Legged SDK の High level mode については Pro モデルでも動いたので、unitree_ros, unitree_ros_to_real とも実機で動かせました。

現在つくばチャレンジ2022 トライしており、これを利用しております。

MQTT コマンド　使用方法

MQTT モータ・各種コマンド
（基本的に、スマホアプリ・ブラウザで行えるコマンドです。）

取得情報
（スマホアプリ・ブラウザから得られる情報（つまりMQTT コマンドからの情報）は、 unitree_legged_sdk で得られるものより精度が劣ります（float -> int 等））

参考ソース

mqtt python 例

# free use



import paho.mqtt.client as paho

import time
import struct

def on_connect(mqttc, obj, rc, a):
    mqttc.subscribe("$SYS/#", 0)
    print("rc: "+str(rc))

def on_message(mqttc, obj, msg):
    print("on_message: " + msg.topic+" "+str(msg.qos)+" "+str(msg.payload))

def on_publish(mqttc, obj, mid):
    print("mid: "+str(mid))

def on_log(mqttc, obj, level, string):
    print("on_log: " + string)




if __name__ == '__main__':
    mqttc = paho.Client()
    mqttc.on_message = on_message
    mqttc.on_connect = on_connect
    mqttc.on_publish = on_publish


    #mqttc.connect("192.168.12.1", 1883, 60)
    mqttc.connect("192.168.123.161", 1883, 60)



    #bytesarr = bytearray([0x03, 0xca]);
    #mqttc.publish("controller/current_action", bytesarr, 0)
    #time.sleep(1)



    #移動・回転
    #lx, rx, ry, ly
    
    #ヨコ移動
    #bytesarr = bytearray([0x26, 0x73, 0x2a, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);

    #移動＋回転
    #bytesarr = bytearray([0xcc, 0xcc, 0x4c, 0xbd, 0xcc, 0xcc, 0x4c, 0xbd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);

    #ry(からだ前後傾け) と移動の複合はできないぽい
    #bytesarr = bytearray([0x26, 0x73, 0x2a, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);


    #0.16回転
    #bytesarr = bytearray([0x00, 0x00, 0x00, 0x00, 0x26, 0x73, 0x2a, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);
    #bytesarr = bytearray([0x00, 0x00, 0x00, 0x00, 0x26, 0x73, 0x2a, 0xbe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);

    #0.3回転
    #bytesarr = bytearray([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);

    #bytesarr  = bytearray([0x00, 0x00, 0x00, 0x00, 0x99, 0x99, 0x99, 0xbe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) 
    #print(bytesarr)
    #mqttc.publish("controller/stick", bytesarr, 0)
    #time.sleep(2)


    #右移動
    bytesarr = bytearray([0xcc, 0xcc, 0x4c, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(0.3)

    #ストップ
    bytesarr  = bytearray([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) 
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(1)



    #左移動
    bytesarr = bytearray([0xcc, 0xcc, 0x4c, 0xbe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]);
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(0.3)

    #ストップ
    bytesarr  = bytearray([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) 
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(1)



    #前移動
    bytesarr = bytearray([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xcc, 0xcc, 0x4c, 0x3e]);
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(0.3)

    #ストップ
    bytesarr  = bytearray([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) 
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(1)



    #後ろ移動
    bytesarr = bytearray([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xcc, 0xcc, 0x4c, 0xbe]);
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(0.3)

    #ストップ
    bytesarr  = bytearray([0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) 
    mqttc.publish("controller/stick", bytesarr, 0)
    time.sleep(1)



    #LED(BLUE)
    bytesarr = bytearray([0x00, 0x00, 0xff]);
    mqttc.publish("face_light/color", bytesarr, 0)
    time.sleep(1)

    #LED(YELLOW)
    bytesarr = bytearray([0xff, 0xff, 0x00]);
    mqttc.publish("face_light/color", bytesarr, 0)
    time.sleep(1)


    '''
    #command
    #dance1, dance2, straightHand1, damping, standUp, standDown, recoverStand, stand, walk, run, climb  (from chunk-3caf6fe5.e09948eb.js) 

    #other ?
    #jump, backflip
    
    
    mqttc.publish("controller/action", "standUp", 0)
    time.sleep(0.3)

    mqttc.publish("controller/action", "standDown", 0)
    time.sleep(0.3)

    '''



    mqttc.loop_forever()

ほかに入っていたSDK

faceLightSDK_Nano

ultraSoundSDK_Raspi

外部にラズパイを接続してみましたが、そこからでもLED / Legged_SDK / CameraSDK は実行できました。ultraSoundSDK_Raspi は Jetson の方にも類似名で入っていますが、どうやら超音波センサはシリアル接続？センサがつながってる実機でないと実行時にエラーになりました。

スピーカ

（ちょっとここゴチャゴチャ書いてしまいましたが、このスピーカの項目の最後の方の、ネットワーク再生の設定のが一番シンプルに、他端末から再生できるようになりました。）

192.168.123.13 の頭部Nano にUSB接続と思われるスピーカがついています。マイクもあるはず

ps aux | grep wsaudio
sudo kill xxxx (wsaudio のプロセスをキル)

aplay ./testdog.wav
再生できないときがあったので下記

aplay -l
aplay --device=hw:2,0 ./testdog1.wav

USB audio の順番が変わる等か？

pulseaudio --kill
pulseaudio --start

alsamixer (F6 押すとUSB Audio Device の音量変更できる。これを変えると、スマホアプリでのPeripherals->Audio Device のところの機能の音量が変わった)

頭部Nano はUSB端子が外に出てないので、ファイルの転送には scp を使いましょう

scp ./testdog.wav unitree@192.168.123.13:~/

音量調整（これは設定間違えるとよくわからなくなったので注意）

pactl set-sink-volume @DEFAULT_SINK@ +5%
たぶんこっち？？？
pactl set-sink-volume 0 +5%
//pactl set-sink-volume 1 +5%
https://qiita.com/Hayao0819/items/1f01aa39db7326ff69ee
sink#1 の Volume front-left / front-right が音量に連動して、600% 以上とかまで上がりました。（のはたまたまバグ？）
さすがにそれで再生したらスピーカが壊れそうなノイジーな音になりましたので、ほどほどに。
（これは、デフォルトシンクを設定した上で、 aplay ./testdog.wav とオプション無しで実行した場合に、600%とかが適応されるみたい）

ネットワーク経由で別端末から再生するには、こちらを利用して、 /etc/pulse/default.pa を編集しました。

PULSE_SERVER=192.168.123.13 aplay ./testdog.wav

Mono / Stereo のファイルで再生出来たりできなかったり・・・謎

本体で再生出来ていても、ネットワーク経由のができなくなったり・・・

何か標準出力が変わる？

aplay -l ではなく aplay -L で見て、

aplay --device=plughw:2,0 ./testdog1.wav

なら、Mono / Stereo とも再生できた

ここを参考に、
頭部Nano の /etc/pulse/default.pa
最後尾に、

set-default-sink alsa_output.usb-C-Media_Electronics_Inc._USB_Audio_Device-00.analog-stereo
追加して、

pulseaudio --kill
pulseaudio --start
したらネットワーク再生もできた！

paplay -s 192.168.123.13 ./dog1.wav

でもいけた

別に用意したラズパイとかなら、AAC / *.m4a ファイルはこれで再生できた。mp3とかも
（deb パッケージをオフラインでコピーすれば、192.168.123.161 のラズパイでもインストールできるかもしれないけど、ライブラリが多かったのでせず。wave なら aplay/paplay で別ソフトインストール無しで再生できるので、wave に変換するのが吉）
（先に 192.168.123.13 の wsaudio は kill する必要あり）

sudo apt install mplayer

PULSE_SERVER=192.168.123.13 mplayer ./yobikomikun/yobikomikun.m4a

PointCloud2 出力ノード

192.168.123.161 ラズパイの~/Unitree/autostart/camerarosnode/cameraRosNode/startNode.sh　

ソースを見る限り、UnitreecameraSDK の StartCapture(false,true) で、圧縮なし、共有メモリONにして、rect image / Depth 取得を同時に行ってるぽい？

ビルド

ssh 192.168.123.14
cd
mkdir ./copy_camerarosnode
cp -r ~/Unitree/autostart/camerarosnode/* ./copy_camerarosnode/
cd ~/Unitree/autostart/camerarosnode/cameraRosnode/src/unitree_camera/
mkdir build
cd build
cmake ..
make
ls ./devel/lib/unitree_camera/

その他

Mujoco
https://twitter.com/devemin/status/1455419485737807873?s=20&t=AXIAgeMawIz5XEyBa5R1Qg

JetPack バージョン

192.168.123.13
The authenticity of host '192.168.123.13 (192.168.123.13)' can't be established.
ECDSA key fingerprint is SHA256:5d2GAwdV3M599daXFj6/UhO7ZzLA7DXRtRA5B7/iQ10.
Are you sure you want to continue connecting (yes/no)? yes
Warning: Permanently added '192.168.123.13' (ECDSA) to the list of known hosts.
unitree@192.168.123.13's password: 
Welcome to Ubuntu 18.04.5 LTS (GNU/Linux 4.9.140-tegra aarch64)

 * Documentation:  https://help.ubuntu.com
 * Management:     https://landscape.canonical.com
 * Support:        https://ubuntu.com/advantage
This system has been minimized by removing packages and content that are
not required on a system that users do not log into.

To restore this content, you can run the 'unminimize' command.

298 packages can be updated.
205 updates are security updates.

Last login: Mon Nov  8 15:06:09 2021 from 192.168.123.163

unitree@nano2gb:~$ cat /etc/nv_tegra_release 
# R32 (release), REVISION: 4.4, GCID: 23942405, BOARD: t210ref, EABI: aarch64, DATE: Fri Oct 16 19:44:43 UTC 2020

192.168.123.14

ubuntu@rpi4ubuntu2004:~$ ssh unitree@192.168.123.14
unitree@192.168.123.14's password: 
Welcome to Ubuntu 18.04.5 LTS (GNU/Linux 4.9.201-tegra aarch64)

 * Documentation:  https://help.ubuntu.com
 * Management:     https://landscape.canonical.com
 * Support:        https://ubuntu.com/advantage
This system has been minimized by removing packages and content that are
not required on a system that users do not log into.

To restore this content, you can run the 'unminimize' command.

262 packages can be updated.
162 updates are security updates.

Last login: Wed Dec 15 23:57:40 2021 from 192.168.123.163

unitree@unitree-desktop:~$ sudo cat /etc/nv_tegra_release 
[sudo] password for unitree: 
# R32 (release), REVISION: 5.0, GCID: 25531747, BOARD: t210ref, EABI: aarch64, DATE: Fri Jan 15 22:55:35 UTC 2021

192.168.123.15

unitree@192.168.123.15's password: 
Welcome to Ubuntu 18.04.5 LTS (GNU/Linux 4.9.201-tegra aarch64)

 * Documentation:  https://help.ubuntu.com
 * Management:     https://landscape.canonical.com
 * Support:        https://ubuntu.com/advantage
This system has been minimized by removing packages and content that are
not required on a system that users do not log into.

To restore this content, you can run the 'unminimize' command.

262 packages can be updated.
162 updates are security updates.

Last login: Wed Dec 15 19:02:28 2021 from 192.168.123.14

unitree@unitree-desktop:~$ cat /etc/nv_tegra_release 
# R32 (release), REVISION: 5.0, GCID: 25531747, BOARD: t210ref, EABI: aarch64, DATE: Fri Jan 15 22:55:35 UTC 2021

timezone

/etc/timezone 変えても意味なし
/etc/localtime のシンボリックリンクを ln -s で変える

Web サーバー

nginx のリバースプロキシ？でWebSocket, mqttプロトコルで内外データやり取りをしているようです。
192.168.123.161:8080 - firmware アップデート用
192.168.123.161:80 - 各種データやり取り用

nginx 設定
/mqtt
/upload
/map
/audio
/cam1 - cam5
/human

mqtt explorer が便利でした。

mqtt (mosquitto server) 192.168.123.161:1883
controller/current_action
controller/action (dance1, dance2, straightHand1, damping, standUp, standDown, recoverStand, stand, walk, run, climb (jump?), (backflip?) )
controller/stick
face_light/color

パケットハック

内蔵のラズパイ・Jetson Nano にパケット解析の WireShark ソフト追加するにあたって、オフラインで apt パッケージを移動して行いました。
また、USB-LAN モジュールなど使えば、直接ラズパイ・Jetson NanoをInternet に繋げられると思います。
（ただ、内部で動いてるソフトウェア・ROS関係の環境を壊さないように注意が必要かと思いますので、安易にソフト追加せず、必要なら外部ホストを用意したほうが良いかなと個人的に思ってます。）

WireShark を入れてパケットハック。
このソフト、Lua 言語でパケットデータをスクリプト処理するように拡張できて便利。

ポート調査

netstat -putona

で、各端末のポート接続状況が読み取れます。192.168.123.161 のラズパイ上で見ると全てつながってるぽい

アップデート

公式サイトにアップデートファイルがあります。 Nano, Raspi 両方のソフトが入っていますので、解凍すれば買う前にある程度ソフト構成が見られます。