「Node-RED」タグアーカイブ

次期モデル Mini Pupper 2 ラズパイCM4で足座標指定

前回はラズパイ CM4で直接サーボ角度を指定してミニぷぱ動作を楽しみました。

次期モデル Mini Pupper 2 ラズパイCM4で足座標指定

 

ここではラズパイ CM4で足の座標を指定しての動作を楽しみました。

 

足座標指定

ここではMini Pupper2に搭載されたラズパイCM4からサーボを制御するESP32に足の座標をシリアルデータ送信して動作させます。

足の座標は各足の付け根を原点にして前後方方向のX [mm]とY方向へのロール角 [°]と高さZ [mm]を指定するようにいたしました。

 

更に座標移動には以前実験したスムージング機能を持たせて、シリアルデータにスムージングの効き具合も一緒に送るようにいたしました。

 

具体的には以下の13のデータを送って動作させます。
[スムージング[%],  前右x, θ. z, 後右x, θ. z, 前左x, θ. z, 後左x, θ. z, ]

スムージングは以下のように実施しています。

$$現在の座標 = 指示座標 * (100 -スムージング) [\%] + 前回の座標 * スムージング [\%] $$

 

動作

まずは各足の高さを指定して屈伸させてみました。
スムージングがない(0%)と動作が急峻になります。

 

スムージング機能によって動作が柔らかくなり振動も減っています。
移動座標と一緒にスムージング具合[%]も送っていることが今回のミソです。

歩行

足の座標を指定して歩行も試してみました。


こちらもスムージングによって動きが非常にスムーズになっています。

 

歩行動作は以下の4ステートをシリアルデータとしてCM4からESP32(シリアルサーボ)に送るだけで実現できました。

各ステートを100msec毎に送信。

 

スムージングによって各ステート間が良い感じで補完されてスムーズな歩行が実現されました。

次の動作座標を動的に動かしても同様にスムーズな動作が可能であると考えます。
足座標指定による動作は非常に有用な方法なのではないでしょうか。

おわりに

ここではラズパイ CM4で足先の座標を指定してのミニぷぱ動作を楽しみました。

スムージング具合も指定することで大まかな座標指定でも滑らかに動けることがわかりました。
CM4でモーション座標をサクサク流したり、もしくはセンシングで動的に座標を指定しても十分な動作が期待できそうで大変喜んでおります。

それではまた。

次期モデル Mini Pupper 2 ラズパイCM4でサーボ角度指定

前回はミニぷぱ2でディスプレイ表示とスピーカによる音声再生を楽しみました。

次期モデル Mini Pupper 2 CM4でメディアを堪能

 

また前々回はRaspberry Pi Compute Module 4 (CM4) からESP32に組み込まれた動作モードを起動しての動作を楽しみました。

次期モデル Mini Pupper 2 に Raspberry Pi Compute Module 4 を搭載

ここではCM4で直接サーボの角度を指定して動かす仕組みを構築して、ミニぷぱを楽しんでみました。

ミニぷぱ2構成

ミニぷぱ2の構成は大まかに以下の通りです (製品版は変更となる可能性がございます)。

前々回はCM4からシリアル通信で1文字モーションコード送って、ESP32に書き込んだ動作を起動させました。

ここではミニぷぱの12個のサーボの角度をCM4から直接指定して動作させてみました。

サーボ角度指定

CM4から12個分のサーボの角度をシリアル送信してESP32でサーボを指定の角度に動かします。

以下の動画のようにCM4のNode-REDでシリアルデータを送信しました。

伏せの状態を初期姿勢としすべてのサーボの角度を0°としています。
Node-REDでは以下のようにシリアル送信しています。

var a = new Int8Array(13);
a[0] = 0;
a[1] = 0;
a[2] = 0;
a[3] = 0;
a[4] = 0;
a[5] = 0;
a[6] = 0;
a[7] = 0;
a[8] = 0;
a[9] = 0;
a[10] = 0;
a[11] = 0;
a[12] = 0;
msg.payload = a + "\n";
return msg;

ミニぷぱのサーボは12個ありIDが1から12まで割り振られています。配列の0が扱いにくいのでa[13]としてa[1]~a[12]をサーボ角度として使用しています。

立ちの姿勢は以下の通り。

var a = new Int8Array(13);
a[0] = 10;
a[1] = 0;
a[2] = 45;
a[3] = 45;
a[4] = 0;
a[5] = -45;
a[6] = -45;
a[7] = 0;
a[8] = 45;
a[9] = 45;
a[10] = 0;
a[11] = -45;
a[12] = -45;
msg.payload = a + "\n";
return msg;

 

ミニぷぱ立つ

CM4から直接サーボの角度を指定できるようになったので、モーションの作りこみが楽になりました。
ESP32は書き込みに時間がかかりますので。。

早速モーションバリエーションを楽しみました。

なんと立ってしまいましたwww

詳細説明は省略しますがNode-REDのフローは以下のような感じ

 

サーボ角度をビシビシ指定しているので動きが急峻ですね。。
動作間のスムージングは必要そうです。

 

本当は立ち上がった後に前進歩行させたかったのだけど。。w
以下のように かかとかつま先のアタッチメントつければいいかもしれませんね。

おわりに

ここではCM4で直接サーボ角度を指定してミニぷぱ動作を楽しみました。

IMUはESP32に接続されているので、IMUと動作を絡める際にはどうすればよいかなどまだまだ検討の余地はあります。

CM4でサクサク モーション指定できるので色々な動作を検討したいです。
指定角度間のスムージングなども検討したいです。

それではまた。

追記

バク宙挑戦 (2022/11/15)

前機種でも試したバク宙。

 

今回もやってみた。CM4によってかなり軽量化されてるのでさてどうなるか。。

ダメでした。。後頭部殴打www

構造上後ろ足が後ろにまっすぐ伸びないのよ。。
前宙も試したけどダメだった。。

サーボのトルクどうこう以前に構造的に無理そうでした。

次の記事

次期モデル Mini Pupper 2 ラズパイCM4で足座標指定

次期モデル Mini Pupper 2 ラズパイCM4でメディアを堪能

前回はRaspberry Pi Compute Module 4 (CM4) をミニぷぱ基板に搭載してESP32との連動やAlexa連携を楽しみました。

次期モデル Mini Pupper 2 CM4でメディアを堪能

ここではミニぷぱ2基板上でCM4に接続されたディスプレイとスピーカの動作を楽しみました。

ここで使用している基板は製品版と異なる場合があります。何卒ご了承ください。

 

ディスプレイ

ディスプレイは前モデルと同様にST7789が使用されておりました。
以前と同じようにディスプレイ表示させてみました。バックライトのGPIOピンが異なるくらいの違いでした。

ロボット犬『Mini Pupperミニぷぱ』Pythonでモーション記述

動作

無事に表示。画像はST7789のpythonライブラリのサンプルを使用。
https://github.com/pimoroni/st7789-python

スピーカ

基板にはスピーカアンプ (NS4890B)も搭載されておりました。

アンプへの電源供給ON/OFFピンがGPIO22、モノラル入力がGPIO12でした。

アンプ出力に手持ちのスピーカを接続しました。
コネクタが持ってない型だったのではんだ付けしました。

動作

GPIO22をHighにしてアンプの電源をONにして、GPIO12からPWMを出力してみました。

ブザーのような音が出力されています。

 

CM4には通常のラズパイのようにヘッドホンジャックがないので、以下をconfig.txt に追加します。

dtoverlay=audremap,pins_12_13 

これによって、音声再生出力がGPIO12, 13からされるようになります。
ミニぷぱ2基板はモノラル出力なのでGPIO12のみ使用します。

CM4でYoutubeを再生しスピーカから音声が再生されることを確認しました。

参考

おわりに

ここではミニぷぱ2でディスプレイ表示とスピーカによる音声再生を楽しみました。

やはりラズパイはメディアに強いなという実感を強く受けました。
ミニぷぱの動作にメディアを絡めることでさらに表現が増すのではないでしょうか。

それではまたお会いしましょう。

追記

移動するメディアプレイヤー (2022/11/17)

CM4でYoutube再生しつつディスプレイにはサムネイルを表示。
ただそれだけww

次の記事

次期モデル Mini Pupper 2 に Raspberry Pi Compute Module 4 を搭載

次期モデル Mini Pupper 2 に Raspberry Pi Compute Module 4 を搭載

これまでは次期モデル Mini Pupper 2 をESP32のみで楽しんでまいりました。
前回はより製品版に近いとされる基板の動作を確認しました。

次期モデル Mini Pupper 2 新規基板を味見

 

Mini Pupper 2 の基板にはRaspberry Pi Compute Module 4 (CM4) も搭載可能です。

 

この度、CM4を提供いただきました。MangDang社様本当にいつもありがとうございます!!

ここからはCM4とESP32によるミニぷぱ動作を検証します。

ここで使用している基板は製品版と異なる場合があります。何卒ご了承ください。

 

CM4設定

提供いただいたCM4モジュールはeMMCのないモデルでした。
小さくてビックリ!!

 

早速搭載。

 

Mini Pupper 2基板にはSDカードスロットとmicro-HDMIコネクタが搭載されています。

 

Raspi OS(32bit)を書き込んだSDカードを入れて起動を試みたのですが、すぐにリセットしてしまう謎の症状に苦しみました。。

調査したところ以下でも同様の症状が報告されており、解決策として書き込みのあった
config.txt を修正 ( “dtoverlay=vc4-kms-v3d” -> “dtoverlay=vc4-fkms-v3d”) する方法で改善し無事に起動できました。

https://forums.raspberrypi.com/viewtopic.php?t=323920

CM4 – ESP32間通信確立

CM4の起動が無事できたので更にVNCとNode-REDの設定も実施し、ここではESP32-S3との通信を目指します。

基板上ではESP32-S3のUART1とCM4のUART3が接続されていましたので、以下を参考にCM4のUART3を有効化しました。
https://toki-blog.com/pi-serial/#UART3UART4UART5

無事にシリアル通信が可能になりました。

ミニぷぱ動作

CM4とESP32-S3 間の通信ができるようになったので、CM4から文字を送ってESP32に仕込んだ動作を起動してみました。

CM4上のNode-REDから文字をシリアル送信してESP32で文字に対応した動作でサーボを制御しています。

 

Alexa連携

ミニぷぱがラズパイOS (Node-RED) で使えるようになりましたのでAlexaとの連携も容易となります。

以下と同じ方法で連携してみました。

ラズパイで気圧測定

 

Node-RED Alexa Home Skill Bridgeでミニぷぱ動作用のデバイスを用意してAlexaアプリの定型アクションで起動発話を設定します。

 

Node-REDで動作用デバイスとモーションコマンドを連結させます。

動作

簡単にミニぷぱとAlexaを連携できました♪

おわりに

ここでは提供いただいたCM4モジュールをミニぷぱ基板に搭載してESP32との連動やAlexa連携を楽しみました。

これまでにCM4なしでESP32のみでミニぷぱ動作を実現していましたので、ここではCM4はESP32に組み込まれた動作を指定するのみの仕事しかしておりません。
しかしESP32で制御用のブラウザページを作りこむのはかなり大変だったので、ラズパイ(CM4)でかなり楽になりそうです。

ミニぷぱ動作においてCM4とESP32の役割の振り分けにはいろいろな方法が考えられそうです。
思いついたモノを上げると

  1. ESP32で各動作を組み込んでおいてCM4で起動 (今回の手法)
  2. CM4から足の座標をESP32に指定
  3. CM4から各サーボの角度をESP32に指定

ESP32のプログラム書き込みが面倒なので上の手法の2か3が良いのかなと現時点では思っています。
製品版の手法もウォッチしつつ検討すすめます。

 

次回はディスプレイやスピーカの動作確認を実施したいです。

次の記事

次期モデル Mini Pupper 2 に Raspberry Pi Compute Module 4 を搭載

ロボット犬『Mini Pupperミニぷぱ』で 書道

これまでミニぷパに色々な動作や機能を実装し楽しんでまいりましたが、
ついに字を書けるようになりました!

 

動作

早速 凛々しい書道スタイルをご覧ください。

 

足に筆ペンを固定していますw

 

機構

基本的には以前ロボットアームで実施した機構と同じです。

Processing で制御 ーロボットアーム自作への道3ー

 

Processingのウィンドウ上の座標をミニぷパに送信して足の平面座標(x, y)として制御します。

マウスをクリックすると足の高さを下げて筆ペンを地面に接地します。
ドラッグで線や絵を描けます。マウスがクリックされていない時は足を20mm上げてペン先を浮かせています。

 

ProcessingからのUSBシリアルはATOM Liteで直接うまく受信できなかったので
前回のAIカメラ HuskyLensの時と同様に一旦Seeeduino XIAOで受信してATOMに座標情報を送りました。

[amazonjs asin=”B09MVHQ85B” locale=”JP” title=”ATOM Lite”]

[amazonjs asin=”B09HMY9X6D” locale=”JP” title=”SeeedStudio Seeeduino XIAO(ピンヘッダー実装済み) 102010388″]

次の記事

次期モデル Mini Pupper 2 の初期サンプルを味わう

ロボット犬『Mini Pupperミニぷぱ』を AIカメラ HuskyLens で堪能

前回は9軸 IMUセンサ BNO055を用いてミニぷぱ制御を楽しみました。

ダイソー版 プラレール を改造したよ♪

 

ここでは更に AIカメラ HuskyLensを追加して ミニぷぱを賢くしちゃいます。

 

 

AIカメラ HuskyLens

HuskyLensは顔認識、物体追跡、物体認識、ライン追跡、色認識、タグ認識などの機能を有するAIカメラです。

[amazonjs asin=”B085NL4XCB” locale=”JP” title=”Gravity: HuskyLens AI ビジョンセンサー | シリカゲル保護カバー付き | 顔識別、色識別、ラベル(QRコード)識別、オブジェクト識別、ライン追跡、オブジェクト追跡機能 | Arduino、Raspberry Pi、micro:bitと互換性がある”]

 

 

ここではHuskyLensの設定で色認識データをI2C出力にして使用しました。

 

認識した色の座標位置でミニぷぱを制御します。

 

 

HuskyLensをミニぷぱに接続

前回製作したATOM Liteをコントローラに採用したミニぷぱにHuskyLensを接続します。

注意点

HuskyLensのI2C出力をATOM Liteに接続して素直に行きたかったのですが。。
毎度恒例の問題発生。

以下のHuskyLens Arduinoライブラリを使用してI2C通信したいのですが、ESP32には使用できませんでした。
 https://github.com/HuskyLens/HUSKYLENSArduino/tree/master/HUSKYLENS/examples/HUSKYLENS_I2C

かなりライブラリを修正しないといけない感じだったので直結は潔くあきらめて、Seeeduino XIAOで中継することにいたしました。

 

構成

 

HuskyLensとATOM Lite間にSeeeduino XIAOを挿入してI2C-Uart変換を実施しています。

 

 

部品

  • ATOM Lite
    [amazonjs asin=”B09MVHQ85B” locale=”JP” title=”ATOM Lite”]
     
  • HuskyLens
    [amazonjs asin=”B085NL4XCB” locale=”JP” title=”Gravity: HuskyLens AI ビジョンセンサー | シリカゲル保護カバー付き | 顔識別、色識別、ラベル(QRコード)識別、オブジェクト識別、ライン追跡、オブジェクト追跡機能 | Arduino、Raspberry Pi、micro:bitと互換性がある”]
     
  • Seeeduino XIAO
    [amazonjs asin=”B09HMY9X6D” locale=”JP” title=”SeeedStudio Seeeduino XIAO(ピンヘッダー実装済み) 102010388″]
     
  • BNO055
    [bc url=”https://www.switch-science.com/catalog/5511/”]
     
  • PCA9685モジュール
    [amazonjs asin=”B07SLRG5J1″ locale=”JP” title=”VKLSVAN 2個 PCA9685 16チャンネル 12-ビット PWM Servo モーター ドライバー IIC モジュール Arduinoと互換”] 

  • 5V 降圧DCDC
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13536/”]

 

動作

赤色の横方向座標で旋回移動させてみました。

XIAOからのHuskyLensデータをATOM Liteを2コアにしてCore0で常時シリアル受信するようにして、サーボ制御との並行動作を実現しています。

 

これまで仕込んだ歩行動作なども問題なく実行できます。

 

おわりに

AIカメラ HuskyLensの追加によって ミニぷぱ がとても賢くなりました!

今回は色認識で制御していますが顔認識やライントレースなどもできそうです。

どんどん賢く 高機能になる 我が家のミニぷぱ

 

そのうちしゃべりだして立ち上がって二足歩行するかもしれませんね。

次の記事

Clock Arm 時計を自作

ロボット犬『Mini Pupperミニぷぱ』を BNO055で姿勢制御

前回まではラズパイ4でミニぷぱを楽しみました。

ロボット犬『Mini Pupperミニぷぱ』がやってきた

ここではまたマイコンによる制御に立ち戻って新たなIMUセンサを楽しみます。

 

 

BNO055

以前はコントロールにATOM Matrixを用いて、内蔵のIMUセンサ MPU6886を用いて姿勢制御も楽しみました。

重力方向軸の回転角度 Yaw角を検出して床を回転させても常に同じ方向に旋回する制御も実施したかったのですが、MPU6886ではYaw角のドリフトがかなり大きく断念しました。

ここでは違うIMUセンサを採用してYaw角旋回制御を目指します。

6軸IMUのMPU6886がYaw角がとれないことはしょうがないのですが、同じ6軸でもMPU6050ではオフセット校正を施すと割とドリフトの小さいYaw角を得ることができます。

MPU6050を使用しても良かったのですがここでは9軸IMUセンサ BNO055を使ってみようと思います。

BNO055は正直内部でどういった処理をしているのか不明なのですが、正確な姿勢角を得ることができます。

以下のサンプルコードで校正や補正なしですぐに姿勢角を得ることができます。
 https://github.com/adafruit/Adafruit_BNO055/blob/master/examples/rawdata/rawdata.ino

 

[bc url=”https://www.switch-science.com/catalog/5511/”]

 

以下でIMUセンサを各種比較してますのでよろしければどうぞ

スマート靴占い 『 IしたoTんきになぁ〜れ 』

 

BNO055をミニぷぱに導入

以前のATOM Matrixの機構にBNO055を追加しても良かったのですが、MPU6886を搭載したATOM Matrixに更にIMUを追加するのが冗長でどうしても許せなかったのでコントローラにはATOM Liteを採用しました。

[amazonjs asin=”B09MVHQ85B” locale=”JP” title=”ATOM Lite”]

 

注意点

BNO055とはI2Cで通信するのですが、M5Stack ArduinoのM5Atom.hではI2Cピンを指定してAdafruit_BNO055ライブラリでのBNO055との通信ができませんでした。

 

そこでここではM5Stack Arduinoライブラリは使用せずESP32 ArduinoライブラリでATOM Liteを使用することにしました。ESP32 ArduinoライブラリではI2Cピンを自由に指定してもBNO055と通信が問題なくできました。

ここではBNO055との通信はWire.begin(25, 21)で実施し、
サーボドライバ PCA9685とはWire1 .begin(22, 19)で実施しました。
これで問題なく動作を確認できました。

 

宴会芸

BNO055導入によって、正確な姿勢角を得ることが可能となりました。

ここではカルマンフィルタなど使用せず姿勢角の生データをそのまま使用しました。
BNO055が優秀で非常に楽です。

 

Yaw角を検出して旋回動作させて一方方向を保持する動作をさせてみました。

いい感じ!!

 

以前仕込んだ動作も問題なくできています。

床を傾けた時の足の高さでの平行制御もBNO055の姿勢角のみのP制御ですがスムーズに実現できています。

 

おわりに

ここでは9軸 IMUセンサ BNO055を用いてミニぷぱ制御を楽しみました。

BNO055が有能すぎて夢だったミニぷぱ宴会芸も実現できました!!

ミニぷぱがドンドン賢くなってくので大変嬉しいです!
引き続き勉強進めます!

次の記事

ロボット犬『Mini Pupperミニぷぱ』で 書道

ロボット犬『Mini Pupperミニぷぱ』Pythonでモーション記述

前回はミニぷぱ正規の構成でラズパイ4にRaspi OSとNode-REDを導入して動作を確認いたしました。

ロボット犬『Mini Pupperミニぷぱ』がやってきた

 

ここでは、動作モーションを増やしてのコントロールを検討します。

 

 

モーション制御検討

Python

前回はNode-REDで動作を仕込みましたが、複数のモーションになると煩雑になるためPythonを採用することにしました。

サーボドライバ PCA9685用のPythonライブラリは以下を使用しました。
 https://github.com/adafruit/Adafruit_Python_PCA9685

ちなみにこのライブラリでも専用基板のサーボナンバー 1~12がPCA9685ノードの15~4に対応していました。基板上そういった接続なっているのかもしれません。

Node-RED Dashboard

ミニぷぱコントロールUIにはNode-RED-Dashboardを使用することにしました。
 https://flows.nodered.org/node/node-red-dashboard

これによってブラウザ上のボタンなどで制御可能となります。

動作

Node-RED-Dashboardのボタンで足の高さを指定しています。
ブラウザで動くのでスマホでもコントロールできます。

 

ミニぷぱモーション制御

Dashboard

Node-RED Dashboardのnumericノードで足の高さや上げ幅、歩幅などを指定します。
buttonノードで各種モーションを起動します。

モーションは初期姿勢、足踏み、ジャンプ、ランニングマンを用意しました。

Node-RED

Node-REDの構成は以下のとおり

 

動作指定ボタンを受けて各モーションのPythonコードを起動しています。

Python起動時にnumericノードで指定したパラメータをmsg.payloadで引数として渡しています。

Pythonコード

足踏み動作は以下のような感じで

# Simple demo of of the PCA9685 PWM servo/LED controller library.
# This will move channel 0 from min to max position repeatedly.
# Author: Tony DiCola
# License: Public Domain
from __future__ import division
import time
import math
import sys

# Import the PCA9685 module.
import Adafruit_PCA9685

args = sys.argv


# Uncomment to enable debug output.
#import logging
#logging.basicConfig(level=logging.DEBUG)

# Initialise the PCA9685 using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685()

# Alternatively specify a different address and/or bus:
#pwm = Adafruit_PCA9685.PCA9685(address=0x41, busnum=2)

# Configure min and max servo pulse lengths
servo_min = 130  # Min pulse length out of 4096
servo_max = 640  # Max pulse length out of 4096

offset = [0, 78, 117, 36, 83, 36, 97, 78, 119, 35, 100, 69, 125]

L1=50
L2=56

#rRz = int(args[1])
height = int(args[1])
walkHeight = int(args[2])
stride = int(args[3])
tilt = int(args[4])
period = int(args[5])


# Set frequency to 60hz, good for servos.
pwm.set_pwm_freq(60)

#servo control
def servo_write(ch, ang):
    ang = int((servo_max - servo_min) / 180.0 * ang + servo_min)
    pwm.set_pwm(16 - ch, 0, ang)


def fRIK(x, th0, z):
    zd = z / math.cos(th0/180.0 * math.pi)
    ld = math.sqrt(x*x + zd*zd)
    phi = math.atan2(x, zd)
    
    th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
    th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1

    servo_write(1, th0 + offset[1]);
    servo_write(2, th1 * 180.0 / math.pi + offset[2]);
    servo_write(3, th2 * 180.0 / math.pi + offset[3]);
    
def rRIK(x, th0, z):
    zd = z / math.cos(th0/180.0 * math.pi)
    ld = math.sqrt(x*x + zd*zd)
    phi = math.atan2(x, zd)
    
    th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
    th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1

    servo_write(7, th0 + offset[7]);
    servo_write(8, th1 * 180.0 / math.pi + offset[8]);
    servo_write(9, th2 * 180.0 / math.pi + offset[9]);
    
def fLIK(x, th0, z):
    zd = z / math.cos(th0/180.0 * math.pi)
    ld = math.sqrt(x*x + zd*zd)
    phi = math.atan2(x, zd)
    
    th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
    th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1

    servo_write(4, th0 + offset[4]);
    servo_write(5, -(th1 * 180.0 / math.pi) + offset[5]);
    servo_write(6, -(th2 * 180.0 / math.pi) + offset[6]);

def rLIK(x, th0, z):
    zd = z / math.cos(th0/180.0 * math.pi)
    ld = math.sqrt(x*x + zd*zd)
    phi = math.atan2(x, zd)
    
    th1 = phi - math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld))
    th2 = math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1

    servo_write(10, th0 + offset[10]);
    servo_write(11, -(th1 * 180.0 / math.pi) + offset[11]);
    servo_write(12, -(th2 * 180.0 / math.pi) + offset[12]);
 

while True:
    time_mSt = time.time() * 1000
    tim = 0
    while tim < period:
        tim = time.time()  * 1000 - time_mSt
        tt = tim * math.pi / 2.0 / period
        fRIK(0, 0, height - walkHeight * math.sin(tt))
        rLIK(0, 0, height - walkHeight * math.sin(tt))

    time_mSt = time.time() * 1000
    tim = 0
    while tim < period:
        tim = time.time() * 1000 - time_mSt
        tt = tim * math.pi / 2.0 / period
        fRIK(0, 0, height - walkHeight * math.cos(tt))
        rLIK(0, 0, height - walkHeight * math.cos(tt))
    
    time_mSt = time.time() * 1000
    tim = 0
    while tim < period:
        tim = time.time()  * 1000 - time_mSt
        tt = tim * math.pi / 2.0 / period
        rRIK(0, 0, height - walkHeight * math.sin(tt))
        fLIK(0, 0, height - walkHeight * math.sin(tt))

    time_mSt = time.time() * 1000
    tim = 0
    while tim < period:
        tim = time.time() * 1000 - time_mSt
        tt = tim * math.pi / 2.0 / period
        rRIK(0, 0, height - walkHeight * math.cos(tt))
        fLIK(0, 0, height - walkHeight * math.cos(tt))

 

参考

動作

 

前後左右への歩行を仕込もうと思ったのですがdashboardにはジョイスティックがなくボタンやスライダじゃきついなぁと思った時に
Blynkはラズパイもいけたことに気づきました。。。

次はBlynkでの動作検証します。

おわりに

ここではNode-RED DashboardとPythonを用いてモーションコントロールを楽しみました。

しかしNode-RED DashboardのUIではリアルタイムラジコンを制御するには少し物足りない印象でした。

Blynkがラズパイ対応していたことに今さら気づいたので次回検証進めます!

 

追記 (ディスプレイ表示)

どうもラズパイ4はBlynkに対応されてないようなので、ラズパイを用いた本構成の検討はコレで一旦休止しようと思います。

最後にせっかくなのでディスプレイ表示だけ確認しました。

ディスプレイには320×240ピクセルのST7789が使用されていることが以下で解ります。
SPIで制御されておりピン配もPythonコードから読み取ることができます。
 https://github.com/mangdangroboticsclub/QuadrupedRobot/tree/MiniPupper_V2/Mangdang/LCD

ST7789制御

以下を参考にST7789用Python環境を構築しました。

 

またRaspiコンフィグでSPIを有効にします。

Pythonコード

上記を参考にして320×240のJPEG画像を表示するPythonコードをこしらえました。

import ST7789
from PIL import Image

# Create a display instance
disp = ST7789.ST7789(port=0, cs=0, rst=27, dc=24, backlight=26, width=320, height=240, rotation=180, spi_speed_hz=40 * 1000 * 1000)

# Added: Change to SPI MODE 3
disp._spi.mode = 3
disp.reset()
disp._init()

# Open image file
image = Image.open("L.jpg")

# Resize to screen size
image = image.resize((320, 240))

# Show it on display
disp.display(image)

動作

 

表示できました!
思えばラズパイにLCDつないでSPIで制御したの初めてかも
勉強になりました!

次の記事

ロボット犬『Mini Pupperミニぷぱ』を BNO055で姿勢制御

ロボット犬『Mini Pupperミニぷぱ』ラズパイ4 Node-REDで味見

前回はATOM Matrixを用いて ミニぷぱの動作を楽しみました。

ロボット犬『Mini Pupperミニぷぱ』がやってきた

ここでは正規のラズパイ4と専用基板を用いて ミニぷぱを動かせるようにします。

 

 

ミニぷぱ専用基板

以前 ミニぷぱに同梱のラズパイ4にRaspi OSを入れて専用基板でのサーボモータ制御を試みたのですが、サーボに電源が供給されず断念しました。
(2022/2/16現在 専用基板の回路図は未公開)

そうしましたところ、ミニぷぱの開発元様よりサーボ電源のイネーブルピンの存在をご教示いただきました。

ラズパイのGPIO21、GPIO25がサーボ電源供給のENピンとのことです。
それぞれサーボモータドライバのPCA9685の16個分のサーボの半分ずつの電源スイッチとなっているようです。

PCA9685動作

サーボドライバ PCA9685 はI2Cがインターフェースとなっております。

 

Raspi OSでI2C通信を有効にします。

 

I2C有効後に専用基板を載せてi2cdetect で接続されているI2Cデバイスを調べてみました。

0x40がPCA9685です。0x70はなんだろ? 🙄 
https://github.com/mangdangroboticsclub/QuadrupedRobot/tree/MiniPupper_V2/Mangdang によると
専用基板にはI2Cは3系統あって正規のI2Cピン(i2c-1)にはPCA9685しか つながってないはずだけど?

i2c-1 : for pwm controller PCA9685
i2c-3 : for EEPROM
i2c-4 : for fuel gauge max17205

サーボ制御

ラズパイ4にNode-REDを導入して専用基板でサーボを動かしてみました。
以下のPCA9685用のノードで制御しています。
 https://flows.nodered.org/node/node-red-contrib-pca9685

ちょっとハマったのが専用基板のサーボ1を動かすためにはPCA9685ノードで15を設定する必要がありました。
専用基板のサーボナンバー 1~12がPCA9685ノードの15~4に対応していました。
PCA9685ノードがおかしいのか専用基板の配線がそのようになっているのかは不明です。

rpi-gpio outノードでGPIO21、GPIO25をHighにしてサーボに電源供給しています。

 

無事に専用基板でサーボを動かせるようになったので、いよいよミニぷぱ制御に挑戦です!

 

ミニぷぱ 足制御

ESP32やATOM Matrixで行っていた時と同様に逆運動学でミニぷぱの足を制御します。

とりあえず1本動かしてみた。

 

Node-REDで足の高さを指定して制御しています。

IK 右前足ファンクションで足の高さをうけて各サーボの角度を導出しています。

L1=50
L2=56;

x = 0
zd = msg.payload / Math.cos(0/180.0 * Math.PI);

ld = Math.sqrt(x * x + zd * zd)
phi = Math.atan2(x, zd)

msg.payload1 = 0;
msg.payload2 = phi - Math.acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld)) * 180.0 / Math.PI
msg.payload3 = Math.acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) * 180.0 / Math.PI + msg.payload2
return msg;

 

足踏み

足1本を制御出来ればこっちのもんです。同じように残り3本も制御できるようにして足踏み動作してみました。

足踏み動作実現!
久しぶりに外装も全て取り付けて可愛さが復活しました。
ディスプレイも表示させたいな 🙄 

 

しかしNode-REDで動作シーケンス組むのは結構大変だったな。。。

歩行動作とかも仕込みたいので実装手法は検討が必要そうです。

 

おわりに

ついにラズパイ4と専用基板でミニぷぱを動かせるようになりました。

引き続きもろもろ検証して自由自在に動かせるようにしたいと思います。

次の記事

ロボット犬『Mini Pupperミニぷぱ』を BNO055で姿勢制御

ロボット犬『Mini Pupperミニぷぱ』ATOM Matrix で動作再検討

前回はコントローラにATOM Matrixを採用してIMUセンサによるバランス制御を実現しました。

ロボット犬『Mini Pupperミニぷぱ』がやってきた

ここではさらに動作の検証を進めましたので報告いたします。

 

バク転

↑このロボットのようにバク転できたらいいなと思い、試してみました。

 

できるわけねーか。。

次だ 次

 

歩行改善

他の4足歩行に比べて、歩行がぎこちないのでロボット犬との距離が以前に比べ縮まった現在、
改めて歩行動作を検討してみました。

これまでは対角線の2本を上げている間に接地している2本を動かして歩行しており、足上げと前後運動が分断し かつ足上げの時間が長く前後運動も等速ではありませんでした。

足上げの時間を短くし、足の前後運動を等速したところ非常にスムーズな歩行が実現されました。
機体のブレも小さく移動もスムーズです。これが正解の歩行方法のようですね。
生き物らしさは失われた気もしますが 🙄 

ランニングマン

4足歩行ロボットといえばSPOTのこの動作ですよね。
そうランニングマン!

 

ミニぷぱでも実現したく以下動画で勉強しつつ。。

 

ランニングマン動作を作りこんでみましたが。。

う~ん。。。イマイチ。。
よくよく見るとランニングマンは全足が一瞬浮く時間があって、その際に足を引いてるのですよね。。

ちょっとミニぷぱでは厳しいのかな?
引継ぎ勉強いたします。

おわりに

ここではATOM Matrixを用いて ミニぷぱの動作を再検証いたしました。

 

だいぶ4足歩行ロボットの理解が深まりましたので、次はミニぷぱ正規の専用回路の動作を検証しラズパイ4での制御に挑戦したいです。

次の記事

ロボット犬『Mini Pupperミニぷぱ』ラズパイ4 Node-REDで味見

ロボット犬『Mini Pupperミニぷぱ』を ATOM Matrix で堪能

前回はミニぷぱのコントローラにESP32を採用し、足を自由自在に制御できるようになり4足歩行についての理解を深めました。

ロボット犬『Mini Pupperミニぷぱ』がやってきた

ここではコントローラにIMUを内蔵したATOM Matrixを使用してみましたので報告いたします。

 

 

ATOM Matrix

4足歩行について理解が深まりましたので、次は以下を再現してみたいと考えました。

恐らくIMUセンサで姿勢を検出して足の高さを制御して、機体を平行に保っているものと思われます。

 

これまでのESP32を使用した機構にIMUセンサを追加しようとも思ったのですが、
ESP32-PICOとIMUセンサ MPU6886がコンパクトに内蔵されたATOM Matrixを使用することにしました。

[amazonjs asin=”B09MVK6SXN” locale=”JP” title=”ATOM Matrix”]

コントロール基板

SHISEIGYO-1 専用基板PCA9685モジュールを用いてコントローラを構築しました。

 

注意点

ATOM MatrixとPCA9685モジュール間のI2C通信として
Wire.begin(22, 19); //SDA, SCL
を使用しました。

ATOM内蔵の MPU6886は
Wire1.begin(25,21);  (https://github.com/m5stack/M5Atom/blob/master/src/utility/MPU6886.cpp)
でI2C通信しているのでPCA9685モジュールとの干渉は心配ありません。

但し、M5Atomライブラリを用いて
M5.begin(true, true, true);   (Serial Enable, I2C Enable, Display Enable)
でI2Cを初期化してしまうと
Wire.begin(25, 21, 100000UL);
が実行されPCA9685モジュールと干渉してしまうので
M5.begin(true, false, true);
とする必要があります。

アドレスが違うので同じI2Cピンを用いても良いのですがMPU6886とPCA9685モジュールの既存のライブラリを使用したく、そのライブラリでWre設定がなされてるので通信ピンを分けました。
今後 高速化のためにコアを分けることも見据えております。

動作

ATOM Matrixでも問題なく歩行動作を確認できました。

 

平行制御

いよいよIMUセンサをもちいて機体を平行に保つ制御を実現させます。

ATOM MatrixのIMUの座標軸は以下のようになっています。

X軸とY軸の姿勢角をセンシングして足の高さを制御します。
それぞれの軸の姿勢角と角速度にはカルマン・フィルタを用いました。
 https://github.com/TKJElectronics/KalmanFilter

1軸制御

まずは1軸 (Y軸)での制御を実現させます。
なかなか制御がうまくいかず。。

 

いろいろ制御方法を試行錯誤してる際にサーボが暴走しPCA9685モジュールの逆起電力防止PMOS燃えて動かなくなりました。。煙出たから。。。

しょうがないのでPMOS外してショットキーダイオードをぶち込みましたww
あくまで再購入品が届くまでの応急処置です(しかし こういったイレギュラーな状況下での対応にエンジニアの差というものが出ます)。

 

P制御

センシングしたY軸の角度$θ_Y$から機体を平行にするための足の移動量$h_Y$を以下で算出し制御してみます。

$$h_Y = h_{Y-1} + {Kp}_Y × \sin{θ_Y} $$

$$   但し、h_{Y-1}:直前に算出した移動量、 {Kp}_Y:係数$$

姿勢角から高さ変化量を導出して随時足して目標値に収束させます。
sinをとる必要はなさそうですが何となくそうした。

 

地面の傾きに追従して機体を平行に保とうとしますが、まだぎこちないです。

 

PD制御

さらにY軸の角速度$ω_Y$もフィードバックさせてみます。

$$h_Y =h_{Y-1} + {Kp}_Y × \sin{θ_Y + {Kd}_Y ×ω_Y / 100.0}   但し、{Kd}_Y:係数$$

係数${Kd}_Y$が大きい値にならないように100.0で割っています。

 

かなりヌルヌルと追従できるようになしました!

2軸制御

同様にX軸についてもセンシングして足の高さを制御し、2軸での平行追従制御を実現させました。

 

おわりに

IMUを内蔵したATOM Matrixをコントローラに採用することで、やってみたかったバランス制御が実現できました。

IMUのYaw角センスで旋回制御して常に同じ方向を向くのもやってみたかったのですが、MPU6886のドリフトが大きく実現できませんでした。
違うセンサでいつか実施したいですね。

次の記事

ロボット犬『Mini Pupperミニぷぱ』ラズパイ4 Node-REDで味見

ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討2

前回はESP32で4本の足の制御を実現し4足歩行を学びました。
ここでは更に深堀します。

ロボット犬『Mini Pupperミニぷぱ』がやってきた

 

 

電源コードレス化

前回は5V ACアダプタから電源供給していましたが、ミニぷぱのバッテリを使用してコードレス化を目指しました。

ミニぷぱには2セルの7.4V LiPoバッテリが搭載されています。

構成

LiPoバッテリからPCA9685モジュールのサーボ電源 (V+) と5V DCDCコンバータに給電し、
PCA9685モジュールのロジック電源 (VCC) とESP32にはDCDCの5V出力で給電します。

相変わらずのブレッドボード。。

部品

  • ESP32 開発ボード
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13628/”]
  • PCA9685モジュール
    [amazonjs asin=”B07SLRG5J1″ locale=”JP” title=”VKLSVAN 2個 PCA9685 16チャンネル 12-ビット PWM Servo モーター ドライバー IIC モジュール Arduinoと互換”]
  • 5V DCDCコンバータ
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13536/”]

動作

電源コードがなくなったので、これでどこにでも行けます。

 

本当に散歩してきました。かわいい。
毎日したい

 

歩行検討

これまでは足の移動を座標指定しサーボが最速で動ける速度で直線的に動かして
遅延時間を設けて動作周期を変えていました。

ここでは足を細かく時間分割して正弦波形で動かすようにしてみました。
正弦波で足を動かすことによって足の方向転換時の加速度を低減を狙っています。

足踏み

足の高さを正弦波形で変えます。

線型的に足を移動して遅延で周期を設けた場合と正弦波形での移動では
正弦波のほうが機体のブレが小さくなりました。

歩行

方向時の足の位置やロール角も正弦波形で動かしてみました。

動作周期が短い場合にはさほど効果を感じませんでしたが
周期を長くするとマイルドな足移動が効いてきました。

正弦波形で動かしているというよりは時間分割で細かく座標していることが大きく効いたようです。
しかし、無駄な加速度は機体のブレに繋がるので正弦波でオールオッケーということにいたします。

 

Arduinoコード

今回作成したESP32用のArduinoコードを以下で公開いたします。
 https://gist.github.com/homemadegarbage/c31722700b58e631aec13b39db416e0b

ESP32によるミニぷぱの制御は当然 オフィシャルなものではありませんので
実施の際には自身でコードを理解・修正し自己責任で宜しくお願い致します。
過度なサポート要求もお断りさせていただきます。

コードの概要は以下の通りです。

Blynk

スマホのBlynkアプリを用いてBLE通信で歩行動作指示を送ります。

以下のESP32のBLE用サンプルコードを参考にプログラミングしました。
 https://github.com/blynkkk/blynk-library/blob/master/examples/Boards_Bluetooth/ESP32_BLE/ESP32_BLE.ino

 

以下はミニぷば用Blynkスマホアプリ画面

歩行時に上げる足の高さ(WALK HEIGHT)や歩幅(STRIDE)、旋回時のロール角度(TILT)などを指定しています。

ボタンで足踏み(STEP)やジャンプ(JUMP)動作を起動

ジョイスティックで前後(walkAd, walkBack)や旋回(walkL, walkR) 歩行します(ヴァーチャルピン V20使用)。

PCA9685

PCA9685モジュールで12個のサーボを制御します。
ESP32からI2CでPCA9685モジュールにサーボの角度指示をします。

PCA9685用のArduinoライブラリは以下の秋月電子のサイトで公開されているものを使用しました。
 https://akizukidenshi.com/catalog/g/gK-10350/

 
サンプルコードを参考に以下のように12個のサーボを制御しました。

PCA9685 pwm = PCA9685(0x40);    //PCA9685のアドレス指定(アドレスジャンパ未接続時)

#define SERVOMIN 130            //最小パルス幅 (標準的なサーボパルスに設定)
#define SERVOMAX 640            //最大パルス幅 (標準的なサーボパルスに設定)

void setup() {
 pwm.begin();                   //初期設定 (アドレス0x40用)
 pwm.setPWMFreq(60);            //PWM周期を60Hzに設定 (アドレス0x40用)
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度(0~180)をPWMのパルス幅(130~640)に変換
  pwm.setPWM(ch, 0, ang);
}

逆運動学

足の移動は以前検証した通り、足のロール角と足の前後位置と上下位置を指定して、
逆運動学によってロール角以外のサーボの角度を算出します。

得られたサーボ角をPCA9685用のサーボ制御関数 [servo_write()]に代入してそれぞれのサーボを動かします。

足の座標とサーボ角度算出の式は以下の通り

 

腿の長さ$L_1$は50mm、下肢の長さ$L_2$は56mmでした。
足のロール角を$θ_0$、腿の角度を$θ_1$、下肢を動かすサーボの角度を$θ_2$とします。

 

ここでは腿のつけ根のロール角$θ_0$と座標x, zを指定して足を制御することにしました。

足を上げて(Z軸)、前後(X軸)するためのサーボ角度は以下で導出されます。

腿のサーボの角度$θ_1$は余弦定理を用いて導出します。

${L_2}^2 = {L_1}^2 + {l_d}^2 – 2{L_1}{l_d}\cos{θ_1}’  $

${θ_1}’ = \cos^{-1} \left(\frac{{L_1}^2+{l_d}^2 – {L_2}^2}{2{L_1}{l_d}} \right) $

$$θ_1 = φ – {θ_1}’  = φ -\cos^{-1} \left(\frac{{L_1}^2+{l_d}^2 – {L_2}^2}{2{L_1}{l_d}} \right) $$

$$      但し、φ = \tan^{-1} \left(\frac{x}{z’} \right)$$

$$          l_d = \sqrt{x^2 + {z’}^2}$$

$$  高さを足のロール角で補正 z’ = \frac{z}{\cos{θ_0}}$$

 

下肢の角度はサーボの角度$θ_2$と平行になるように$θ_1$とも連動しています。
こういうの平行リンクっていうんでしょうか。
同様に余弦定理を用いて導出します。

${l_d}^2 = {L_1}^2 + {L_2}^2 – 2{L_1}{L_2}\cos\left({\pi-θ_2+θ_1}\right) = {L_1}^2 + {L_2}^2 + 2{L_1}{L_2}\cos\left({θ_2-θ_1}\right) $

$$θ_2 = \cos^{-1} \left(\frac{{l_d}^2-{L_1}^2 – {L_2}^2}{2{L_1}{L_2}} \right) + θ_1$$

以上より右前足の逆運動学の計算関数は以下のようになります。

void fRIK(float x,float th0,float z){
  float phi,ld, th1, th2;
  float zd = z / cos(th0/180.0 * PI);
  
  ld=sqrt(x*x + zd*zd);
  phi=atan2(x, zd);
  
  th1 = phi - acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1;

  servo_write(1, th0 + offset[1]);
  servo_write(2, th1 * 180.0 / PI + offset[2]);
  servo_write(3, th2 * 180.0 / PI + offset[3]);
}

サーボ角度指定の際に初期位置を補正するoffset[]も加算しています。

残り3本の足についても同様に関数を用意して制御します。

足移動

ここでは足移動の際に正弦波形で細かく座標移動を行いました。

時間分割による座標変化のコードは以下の書籍のサンプルコードを参考にしました。

[amazonjs asin=”B07CXW1LVD” locale=”JP” title=”ROBO-ONEにチャレンジ! 二足歩行ロボット自作ガイド”]

 
書籍のサンプルコードは以下でダウンロードできます。
_710_humanoide_BNO055_KO_mega_walking.inoを参考にしました。
 https://www.ohmsha.co.jp/book/9784274222115/

例えば、足踏みの場合は以下のようになります。

if(Step){ //足踏み
  float tim,tt;
  unsigned long time_mSt= millis();

  tim=0;    
  while(tim<period){
    tim=millis()-time_mSt;
    tt=float(tim * PI / 2 / period);
    fRIK(fRx, fRthd0, fRz - walkHeight * sin(tt));
    rLIK(rLx, rLthd0, rLz - walkHeight * sin(tt));
  }

  time_mSt= millis();
  tim=0;    
  while(tim<period){
    tim=millis()-time_mSt;
    tt=float(tim * PI / 2 / period);
    fRIK(fRx, fRthd0, fRz - walkHeight * cos(tt));
    rLIK(rLx, rLthd0, rLz - walkHeight * cos(tt));
  }

  time_mSt= millis();
  tim=0;    
  while(tim<period){
    tim=millis()-time_mSt;
    tt=float(tim * PI / 2 / period);
    rRIK(rRx, rRthd0, rRz - walkHeight * sin(tt));
    fLIK(fLx, fLthd0, fLz - walkHeight * sin(tt));
  }

  time_mSt= millis();
  tim=0;    
  while(tim<period){
    tim=millis()-time_mSt;
    tt=float(tim * PI / 2 / period);
    rRIK(rRx, rRthd0, rRz - walkHeight * cos(tt));
    fLIK(fLx, fLthd0, fLz - walkHeight * cos(tt));
  }
}

periodで周期の1/4時間を指定して足の高さを正弦波で上下しています。

それぞれの動作も同様に正弦波で移動させています。

 

おわりに

コントローラにESP32を採用し、足を自由自在に制御できるようになったため
4足歩行についての理解が非常に深まりました。

本当に素晴らしいキットをいただいたことを幸せに思います。

ラズパイによる正規の制御の学習進めたいと思います!

あー楽しいね!

次の記事

ロボット犬『Mini Pupperミニぷぱ』を ATOM Matrix で堪能

ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討1

前回はラズパイ4とミニぷぱ専用基板での制御を一旦諦め、邪道ですがESP32での逆運動学による足制御を実施しました。

ロボット犬『Mini Pupperミニぷぱ』がやってきた

前回は足1本のみで動作確認しましたが、ここでは足4本を動かして歩行動作を確認しましたので報告いたします。

 

 

ESP32で4本足制御

12個のサーボをPCA9685モジュールに接続してESP32で制御します。

 

相変わらずのブレッドボードww
電源は5V ACアダプタで供給

部品

  • ESP32 開発ボード
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13628/”]
  • PCA9685モジュール
    [amazonjs asin=”B07SLRG5J1″ locale=”JP” title=”VKLSVAN 2個 PCA9685 16チャンネル 12-ビット PWM Servo モーター ドライバー IIC モジュール Arduinoと互換”]

動作

前回同様にBlynkアプリで足の位置をスマホで指定してBLE通信でサーボを制御しています。

 

これで遂に4本の足を自由自在に動かすことができるようになりました。

 

歩行動作検討

正規のミニぷぱの歩行動作を参考にしつつ4足歩行を実現させます。

足踏み

足踏みは対角の足を交互に上げ下げすることで実現できました。

前進後進

足踏みを応用して、対角の足2本を上げている間に接地している足を後ろ(もしくは前方)に動かすことで歩行できました。

横移動

左右への横移動は 対角の足2本を上げている間に接地している足のロール角を一方に傾けることで実現できました。

ジャンプ

機体を下げて、急峻に足を伸縮させてジャンプ動作を確認することができました。
カスタムサーボのトルクの高さをうかがうことができます。

 

おわりに

ついに4足歩行を自身の手で実現することができました。

これまで4足歩行がどのように実現されるか分かっておりませんでしたが、動画でロボット犬の歩行の様子を凝視したり 実機で色々試したりすることで大変理解が深まりました。

さらに学習をすすめ ぷぱっていこうと思います。

次の記事

ロボット犬『Mini Pupperミニぷぱ』を ATOM Matrix で堪能

ロボット犬『Mini Pupperミニぷぱ』逆運動学で足制御

前回は足を自由に動かすための準備として、ラズパイ4のOS吟味等実施しました。

ロボット犬『Mini Pupperミニぷぱ』カスタマイズ下準備

ここではさらにカスタマイズを進めて足を逆運動学で自由自在に制御できるようになりましたので報告いたします。

 

 

PCA9685検証

専用基板にはサーボドライバPCA9685が載っており、ラズパイからI2Cでサーボを制御していることが前回わかりました。

 

そこでラズパイ4に専用基板を載せて、Node-REDからPCA9685を制御してサーボを動かしてみようと考えました。

PCA9685用Node-REDノードは以下を使用
 https://flows.nodered.org/node/node-red-contrib-pca9685

しかし、専用基板からバッテリを接続して起動しましたが、サーボの電源には電圧がフルで共有されず動かすことができませんでした。。。

恐らく、専用基板にロードスイッチかDCDCのイネーブルピンなどの起動制御があるのだと思いますが、
現状 回路図が未公開なのでラズパイによるサーボ制御は一旦保留とします。

 

ESP32とPCA9685

こうなったら一旦ラズパイ4は忘れて、慣れ親しんだESP32でPCA9685を制御しちゃいましょう。

 

ESP32開発ボードと以下のPCA9685モジュールを使用しました。

[amazonjs asin=”B07SLRG5J1″ locale=”JP” title=”VKLSVAN 2個 PCA9685 16チャンネル 12-ビット PWM Servo モーター ドライバー IIC モジュール Arduinoと互換”]

 

いつもの組み合わせなので簡単にサーボを制御できます。
BlynkをつかってBLEでスマホから制御しています。

 

ミニぷぱの右前足のサーボ3個を繋げてみました。

それぞれのサーボの角度を指定して自由に動かすことができるようになりました。

 

逆運動学で足を制御

右前足を位置座標を指定して動かせるようにします。

座標は腿のつけ根を原点として以下で定義します。

 

腿の長さ$L_1$は50mm、下肢の長さ$L_2$は56mmでした。
足のロール角を$θ_0$、腿の角度を$θ_1$、下肢を動かすサーボの角度を$θ_2$とします。

 

ここでは腿のつけ根のロール角$θ_0$と座標x, zを指定して足を制御することにしました。

足を上げて(Z軸)、前後(X軸)するためのサーボ角度は以下で導出されます。

腿のサーボの角度$θ_1$は余弦定理を用いて導出します。

${L_2}^2 = {L_1}^2 + {l_d}^2 – 2{L_1}{l_d}\cos{θ_1}’  $

${θ_1}’ = \cos^{-1} \left(\frac{{L_1}^2+{l_d}^2 – {L_2}^2}{2{L_1}{l_d}} \right) $

$$θ_1 = φ – {θ_1}’  = φ -\cos^{-1} \left(\frac{{L_1}^2+{l_d}^2 – {L_2}^2}{2{L_1}{l_d}} \right) $$

$$      但し、φ = \tan^{-1} \left(\frac{x}{z’} \right)$$

$$          l_d = \sqrt{x^2 + {z’}^2}$$

$$  高さを足のロール角で補正 z’ = \frac{z}{\cos{θ_0}}$$

 

下肢の角度はサーボの角度$θ_2$と平行になるように$θ_1$とも連動しています。
こういうの平行リンクっていうんでしょうか。
同様に余弦定理を用いて導出します。

${l_d}^2 = {L_1}^2 + {L_2}^2 – 2{L_1}{L_2}\cos\left({\pi-θ_2+θ_1}\right) = {L_1}^2 + {L_2}^2 + 2{L_1}{L_2}\cos\left({θ_2-θ_1}\right) $

$$θ_2 = \cos^{-1} \left(\frac{{l_d}^2-{L_1}^2 – {L_2}^2}{2{L_1}{L_2}} \right) + θ_1$$

 

以上で 足のロール角$θ_0$と足の前後(X軸)位置と上下(Z軸)位置の指定で足のサーボを制御できるようになりました。

動作

実際導出した式で足を動かしてみます。

スマホから前後(X軸)位置と上下(Z軸)位置を指定し、サーボ角を導出して制御します。

 

ロール角を動かしても前後(X軸)位置と上下(Z軸)位置は保たれています。

 

おわりに

ラズパイ4と専用基板での制御を一旦諦め、邪道ですがESP32での足制御を検討しました。

逆運動学でやっと自由に動かせるようになったので、コレを4本に展開して
歩行の学習に入りたいと思います。

実に楽しみです!

次の記事

ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討1

ロボット犬『Mini Pupperミニぷぱ』カスタマイズ下準備

前回は提供いただいたロボット犬『Mini Pupperミニぷぱ』のネットワーク環境の調整などを行いました。

ロボット犬『Mini Pupperミニぷぱ』もろもろ調整

 
基本動作は確認することができたので、4足歩行を勉強して動作のカスタマイズをしたくなってました。

カスタマイズに向けていろいろ調べましたので報告します。

 

 

画像カスタマイズ

以下を参考にディスプレイ画像を変えてみました。
 https://docs.google.com/document/d/1F9AL1InhVXOyn0aPAk4QtorclZ2lEJLl/edit

320×240のpng画像を準備して同名ファイルと入れ替えるだけでできました。

 

下準備

次にいよいよ ミニぷぱの構成や制御方法を勉強して4足歩行方法の理解やオリジナル動作追加など実施したいと思いましたが。。。

ミニぷぱプログラム

制御プログラムは以下あたりをまさぐればよいのでしょうが、正直よくわかりませんでした。

https://github.com/mangdangroboticsclub/QuadrupedRobot/tree/MiniPupper_V2/Mangdang

https://github.com/mangdangroboticsclub/QuadrupedRobot/tree/MiniPupper_V2/StanfordQuadruped/src

ミニぷぱ回路構成

専用基板の回路図はまだ公開されていないようで、
どのようにサーボを制御しているのか明確にすることができませんでした。

https://mangdang-minipupperdocs.readthedocs-hosted.com/en/latest/reference/PCB.html

基板を観るとサーボドライバICのPCA9685が載っていたので、ラズパイ4からI2CでPCA9685に信号を送ってサーボを制御していることが分かります。

 

ラズパイ4と戯れる

ミニぷぱをソフトウェアや回路から追いかけて理解するのは現状私には難しいことが分かりましたので、
まずはラズパイ4との距離をつめることにします。

Ubuntu 20.04

最新キットはUbuntu 21.10で動作しておりますが、前回 VNCのパスワードが消滅するなどの不具合がありましたので、Ubuntu 20.04の動作を観てみようと思いました。

Ubuntu20.04 server をRaspberry Pi ImagerでWiFi 設定済みでイメージ焼いて
GUIも無事インスト―ルできましたが なぜかGUIではWiFiが使えず。。

一旦保留にします。

Raspi OS

ラズパイ4は今回初めて触るので普通にRaspi OSを入れて楽しむことにしました。

慣れ親しんだNode-Redをインストール。
 https://nodered.jp/docs/getting-started/raspberrypi

Node-RedからラズパイのGPIOをPWM制御してサーボを動かしてみました。

 

おわりに

足を自由自在に動かせるようになれば、四足歩行を体験的に学習できると考えましたが
現状 専用基板の回路図も公開されておらず、コードも複雑でどこに手を付けてよいのかわかりません。

引き続きラズパイ4をいじって自分なりにサーボを動かせるようになって足の制御を目指そうと考えておりますが、
ESP32などに浮気するかなどと よからぬ雑念がよぎっていることも確かです。

公式の情報公開も追いかけつつ勉強進めます!

次の記事

ロボット犬『Mini Pupperミニぷぱ』逆運動学で足制御