制御モジュールの解説：HAC入門編 Part.12

前回までの記事で，KXR_HAC_Softwareの各モジュールの解説を行ってきました．行動計画モジュール，タスク実行モジュール，画像認識モジュールと解説を進めて来ましたが，最後に，実際にロボットを動かす制御モジュールを解説します．といっても，歩行制御などの細かな部分はKXR-L2に搭載されたコントロールボードRCB-4HV miniがすでに担ってくれています．そのためここでは，RCB-4HVとpythonモジュールをどのように連携してロボットを思い通りに動かすか，をメインに解説していきます．

制御モジュール

Rcb4baseLibを使ってRCB-4HVと連携

Pythonモジュールをコントロールボードと連携させること自体も，製造元の近藤科学様によるライブラリがすでに用意されているのでそこまで難しくありません．以前に投稿した開発環境構築の記事に従ってUSBアダプターの接続設定ができていれば，Raspberry PiのUSBポートとRCB-4HVを繋ぐことで通信が行えるはずです．

class MotionLibrary:  
    def __init__(self):
        print("\n[Initializing RCB-4...]")
        # sys.path.append('../Rcb4Lib') #Rcb4Libの検索パスを追加
        self.rcb4 = Rcb4BaseLib() #Rcb4BaseLibのインスタンスを生成
        self.rcb4.open('/dev/ttyUSB0',115200,1.3) #RCB4とのシリアル通信ポートをオープン

MotionLibararyクラスに上記のようにあるとおり，Rcb4BaseLibをインポートしてインスタンス生成，Dual USB アダプターが接続されているttyUSB0ポート*でシリアル通信を開始すれば，Pythonモジュールからモーションの再生が可能になります．

（*環境によって違う可能性があります．先ほどの記事の”近藤科学のUSBアダプタを認識できるようにする”項を参考に適宜変更してください）

Pythonモジュールからモーションを再生する二通りの方法

RCB-4HV内に保存されたモーションをPythonモジュールから再生する際に，二つの方法をとることができます．詳しくは，近藤科学様によるこの記事で解説されていますが，一方（MotionPlay）はモーションの一回きりの再生に適しており，もう一方（SetButton）はモーションの繰り返し再生に適しているという特徴があります．

モーション再生の二通りの方法（https://kondo-robot.com/faq/rcb4_motion_play_ex）

キックや起き上がりなどの単純なモーションはMotionPlayによる再生で問題ありませんが，歩行動作は繰り返し再生しなければ1歩毎に一時停止してしまいます．SetButtonによるモーション再生であれば，仮想的なコントローラボタンを押下する指令を送ることで，次にボタンを離す指令を送るまではモーションが連続して再生されます．そこで，KXR_HAC_Softwareの制御モジュールでは，上記二通りのモーション再生の方法を使い分けています．

なお，RCB-4HV側に書き込むプロジェクトについては，KXR_HAC_Softwareに存在するHeartToHeart_projectフォルダを参照してください．

制御モジュールの実装

では，motion_control_library.pyにおける実装について，各要素ごとに解説を行っていきます．全体に関して，基本的には歩数と機体姿勢によるデッドレコニングを前提としたものとなっています．歩行や旋回に関して，一歩あたり（一部単位時間あたり）の移動量の実測値がparameterfile.pyに記述されており，指示された移動量に対し適当な歩数分モーションが再生される構造です．また，現在は使用していませんが，歩数ベースで大まかな自己位置を推定する機能も有しています．

class MotionLibrary

本クラスに，モーション再生を行う関数が動作ごとに実装されています．これらがタスク実行モジュールから逐次呼び出され，行動計画に基づいてロボットが動きます．

init(self)

クラスの初期化関数です．Rcb4BaseLibのインスタンスを生成し，RCB-4HVとの通信を確立します．IMUの読み取り用クラスのインスタンス生成と，自己位置，姿勢などの各変数の初期化も行っています．

calibrate_IMU(self)

現在の機体姿勢が原点となるように，IMUのキャリブレーションを行う関数です．

get_body_angle(self)

IMUから機体姿勢を取得する関数です．

check_button_pressed_state(self)

モーション再生用の仮想ボタンの状態を返す関数です．

walk_forward_continue(self)

仮想ボタンを押下し，ロボットを連続して前に歩行させる関数です．後述するstop_motion(self)関数が呼び出されるまで歩行し続けます．

walk_forward_timed(self)

仮想ボタンを押下し，ロボットを連続して前に歩行させる関数です．渡された移動距離分歩行します．

stop_motion(self)

仮想ボタンを解放する関数です．rcb4.SetButton関数で再生されていたモーションが停止します．

touch_ball(self)

ボールを触るモーションを一回再生する関数です．モーション再生が終わるまでPython側の処理は待機します．

stand_up(self)

寝た状態から起き上がるモーションを一回再生する関数です．モーション再生が終わるまでPython側の処理は待機します．

walk_forward(self)

ロボットを指定距離前進させる関数です．一歩ごとに一時停止があります．

walk_sideway(self)

ロボットを指定距離横に移動させる関数です．指定距離が負であれば左，正であれば右に移動します．

turn(self)

ロボットを指定角度旋回させる関数です．指定角度が負であれば左，正であれば右に旋回します．

wait_for_motion_completion(self)

モーション再生が終わるまで待機する関数です．

calculate_field_coordinate(self)

歩数と機体姿勢をベースに，自己位置を積算する関数です．各移動用関数から呼び出されて逐一計算されます．

get_field_absolute_coordinate(self)

calculate_field_coordinate(self)で積算された自己位置座標を返す関数です．

enable_plot(self)

自己位置をグラフにプロットするための設定を行う関数です．

plot_graph(self)

グラフをプロットする関数です．Raspberry Piのデスクトップに表示されます．

おわりに

KXR_HAC_Softwareの制御モジュールでは，Rcb4BaseLibを利用してPythonモジュールとRCB-4HVを連携することで，行動目標で指定されたモーションを再生しています．KXRにもともと付属するサンプルモーションが優秀なことで，ロボットの細かい動きを新たに作ることなく，HAC競技を完遂できる運動性能を持たせることができています．

ボールに触るHAC-KXR

さて，以上で，KXR_HAC_Softwareの解説は全て完了しました．最初の記事から2年近く経ってしまった”HAC入門編”連載も，これにて一旦の区切りとなります．これまで読んでくださった方々，長い間お付き合いいただき，ありがとうございました．Humanoid Autonomous Challengeの魅力を十分伝えることはできたでしょうか．

この連載が，皆様のHAC機体，あるいはそれ以外の自律ロボット製作に少しでも役立てば幸甚です．筆者も，これからはプライベートでHAC競技にチャレンジしていく所存です．引き続き，よろしくお願いいたします．