top of page

Humanoid RTCs (Python)

　研究やホビーで使用される小型ヒューマノイドロボット(以下、ヒューマノイド)を動作させるRTCを作成する方法を解説します。

 

このページでは以下の知識を説明なく使用しますので、各解説ページで使用方法を学んでから読み進めてください。

1. はじめに

 

　今回は、以下のRTCを作成し、ヒューマノイドを動作させる方法を学びます。

ヒューマノイドには、HPIジャパン製のGR-001を用います。

 

作成するRTCは以下のようになります。

 

  • GR-001

GR-001を動作させます。

GR-001のローカル座標系に対して、位置・姿勢を指示して制御します。

 

  • TimedPose2DConsoleIn

GR-001の位置・姿勢をコンソールから指令します。

 

　

　本解説で想定するシステムは、以下の様になります。

シリアルケーブルを通して、PCとGR-001のコントローラ(RPU-11)を接続します。

5. おわりに

 

　小型ヒューマノイドを動作させるRTCを作成しました。

任意の位置に移動できるよう、逆運動学を解き、歩行パターンを生成しました。

このように、リアルタイムに歩行パターンを生成できれるようになればさまざまなシステムにヒューマノイドを組み込むことができます。

 

　今回示した手法はGR-001以外にも応用可能です。

ぜひ、自分のロボット用に応用してRTCを作ってみてください。

 

参考文献

  1. 「RPU-10取扱説明書」<http://www.futaba.co.jp/robot/download/manuals>(2014/04/22アクセス)

2. GR-001 RTC

 

　GR-001 RTCを作成します。

このRTCの構成や、アルゴリズムの細かい点についてはロボコンマガジン 2014年5月号をご覧ください。

 

　GR-001 RTCはTimedPose2D型のデータを受け取れるよう作成します。

TimedPose2Dは二次元平面の位置(x, y)と姿勢(heading)を格納できるデータ型です。

 

　指定された位置と姿勢に、以下のような手順で移動させます。

 

(a) 目標位置までの距離と角度を計算します。

(b) 目標方向に旋回します。

(c) 目標位置まで直進します。

(d) 目標姿勢に移動させます。

　それではソースコードを解説します。

 

  • 1行目

シリアル通信のためにPYSERIALモジュールをインポートします。

 

  • 2行目

数値計算のためにmathモジュールをインクルードします。

 

  • 21行目

シリアルオブジェクトをインスタンス化します。

 

  • 23-26行目

逆運動学の計算に必要なボディーパラメータを格納する変数を定義します。

 

self.shin_length： すねリンクの長さ mmを格納します。

self.thigh_length： ももリンクの長さ mmを格納します。

self.foot_length： 脚リンクの長さ(すねリンクの長さ + ももリンクの長さ) mmを格納します。

self.ready_z： 一般的なヒューマノイドロボットは膝を伸ばしたまま歩くことは出来ないので、適度に膝を曲げる必要があります。

　　　　　　       この値は足裏の高さ mmに相当し、両足を上げることで結果的に膝を曲げることができます。

 

  • 31-41行目

シリアルポートをオープンする関数です。

シリアル通信に関してはSerial Servo RTCsで解説していますので、そちらをご覧ください。

 

  • 41-43行目

シリアルポートをクローズする関数です。

シリアル通信に関してはSerial Servo RTCsで解説していますので、そちらをご覧ください。

 

  • 73-107行目

ロボットの動作を行う関数です。

 

move_robot(各シリアルサーボのデータを格納した二次元リスト)

 

複数のRSサーボにロングパケットモードを用いてGR-001のコントローラRPU-11を通して送信します。

RSシリーズのロングパケットモードは以下の様なパケットを送信します。

 

[Header] [ID] [Flags] [Address] [Length] [Count] [VID] [Data] [VID] [Data] ・・・ [Sum]

 

Header： パケットの先頭を表します。ここでは0xFA、0xAFとなります。

ID： 常に0x00を指定します。

Flags： 常に0x00を指定します。

Address： メモリーマップ上のアドレスを表します。Lengthに指定した長さのデータをメモリーマップに書き込みます。

Length： サーボ1つ分のデータの長さ(byte数)を指定します。Length = VID + Dataのバイト数

Count： サーボの数を表します。

Sum： チェックサムです。IDからDataまでの各バイトの排他的論理和(XOR)を計算した値を設定します。

 

また、RPU-11を通してシリアルサーボに命令を送る場合は、以下のようにHeaderとLengthを頭に付けます。

 

[RPU header] [Length] [RSシリーズの通常パケット]

 

RPU header： RS485通信のためのパケットを表します。送信のみの場合は0x53Hを使用します。

Length： RSシリーズの通常のパケットの長さを指定します。

 

  • 48行目

RPU-11用のパケットは後で追加します。

まずは通常のロングパケットモードのパケットを作り、リストに格納します。

 

  • 49-54行目

各シリアルサーボのIDや位置、移動時間を順番にリストに追加します。

 

  • 56-59行目

チェックサムを計算し、リストに追加します。

 

  • 61行目

RPU-11用のパケットを格納したリストを作り、これまで作成したパケットが格納されたリストと結合します。

 

  • 63行目

コマンドを出力します。

 

  • 65-66行目

余弦定理を用いて、3つの辺の長さから内角の値を返します。

 

内角の大きさ calculation_law_cosines(l_1 mm, l_2 mm, l_3 mm)

 

各辺と求める角度の関係は以下の様になっています。

l_3で指定した辺の対角αの値が戻り値として返されます。

 

4. 動作確認

 

　作成したRTCを実行して下さい。

GR-001 RTCのポート名は必ず変更し、各環境に合うよう設定してください。

その後、各RTCを接続し、Activateします。

Activate後にTimedPose2DConsoleIn RTCから姿勢と位置を入力します。

するとGR-001が移動するのが確認できます。

 

　なお、GR-001 RTCの実行時に「Permission denied」と表示された場合はsudoを付けて実行してください。

もしくは、以下のコマンドを入力して、dialoutにユーザーを追加してください。

ロボコンマガジン
3. TimedPose2DConsoleIn RTC

 

　TimedPose2DConsoleIn RTCを作成します。

　テンプレートを以下に従って設定てください。

 

  • モジュール名 

TimedPose2DConsoleIn

  • ベンダ名

あなたの名前

  • モジュールカテゴリ

Console

  • 実行周期

1000

  • アクションコールバック

onInitialize

onExecute

  • データポート

・pose2dポート

ポート名(OutPort) pose2d
データ型 RTC::TimedPose2D
変数名 pose2d
表示位置 RIGHT

  •   言語 

Python

 

　プロジェクトの設定が終わったら、TimedPose2DConsoleIn.pyに以下のコードを記述します。

以下のコードは抜粋となります。生成したテンプレートに当てはめて下さい。

　コードの解説をします。

 

  • 4-5行目

OpenRTM-aist Python版では、データ型の初期化が間違っていることがあります。

TimedPose2D型も初期化の引数が間違っていますので、修正してください。

 

  • 11-14行目

TimedPose2D構造体のメンバを、順にユーザに入力させます。

 

  • 16行目

タイムスタンプをセットします。

 

  • 18行目

ポートから出力します。

 

　以上で、TimedPose2DConsoleInのコーディングは終了です。

　ここで説明するRTCは以下の様な構成で動作を確認しています。

 

  • ヒューマノイドロボット

HPIジャパン製 GR-001 RS301,302サーボ仕様

　テンプレートを以下に従って設定てください。

 

  • モジュール名 

GR001

  • ベンダ名

あなたの名前

  • モジュールカテゴリ

Humanoid

  • 実行周期

1000

  • アクションコールバック

onInitialize
onActivated

onDeactivated

onAborting
onExecute

  • データポート

・pose2dポート

ポート名(InPort) pose2d
データ型 RTC::TimedPose2D
変数名 pose2d
表示位置 LEFT

  • コンフィギュレーション

・portコンフィギュレーション： デバイスのポート番号

名称 conf_port
データ型 string

デフォルト値 COM1
変数名 conf_port

・baudrateコンフィギュレーション： 通信のボーレート bps

名称 conf_baudrate
データ型 long

デフォルト値 115200

変数名 conf_baudrate

・walk durationコンフィギュレーション： 歩行時のシリアルサーボの移動時間 10ms

名称 conf_walk_duration
データ型 long

デフォルト値 50
変数名 conf_walk_duration

・step xコンフィギュレーション： 1ステップの移動量 mm

名称 conf_step_x
データ型 double

デフォルト値 40

変数名 conf_step_x

・step thetaコンフィギュレーション： 1ステップの旋回量 deg

名称 conf_step_theta
データ型 double

デフォルト値 30

変数名 conf_step_theta

・step zコンフィギュレーション： 歩行時の足上げ量 mm

名称 conf_step_z
データ型 double

デフォルト値 20

変数名 conf_step_z

・ready zコンフィギュレーション： 初期姿勢 mm(足の高さから算出)

名称 conf_ready_z
データ型 double

デフォルト値 5

変数名 conf_ready_z

・ready durationコンフィギュレーション： 初期姿勢移動時の移動時間 10ms

名称 conf_ready_duration
データ型 short

デフォルト値 200

変数名 conf_ready_duration

・shin lengthコンフィギュレーション： すねリンクの長さ mm

名称 conf_shin_length
データ型 double

デフォルト値 60

変数名 conf_shin_length

・thigh lengthコンフィギュレーション： ももリンクの長さ mm

名称 conf_thigh_length
データ型 double

デフォルト値 50

変数名 conf_thigh_length

・rollコンフィギュレーション： 歩行時の重心移動量 0.1deg

名称 conf_roll
データ型 short

デフォルト値 150

変数名 conf_roll

  •   言語 

Python

 

　プロジェクトの設定が終わったら、GR001.pyに以下のコードを記述します。

以下のコードは抜粋となります。生成したテンプレートに当てはめて下さい。

  • 68-87行目

足先の逆運動学を計算します。

 

足関節の角度 calculation_ik(足先の踏み出し量 mm, 足先の高さ mm)

 

各リンクや角度の表記は以下のようになっています。

右脚と左脚の動作にはmove_right_foot関数と、move_left_foot関数を使用します。

 

  • 175-187行目

ロボットの歩行を制御する関数です。

指定された移動量とステップの幅から歩行回数を決定し、最後に初期姿勢をとるように帳尻をあわせます。

 

walk(移動量 mm, 1ステップの幅 mm, 足上げ量 mm, ロール軸の移動量 0.1deg, 移動時間 10ms)

 

  • 177-178行目

歩行回数を移動量と1ステップの幅から求めます。

また、ステップ幅以下のあまり量を求めます。

 

  • 180-181行目

求めた歩行回数分歩行させます。

 

  • 183-184行目

あまり量がある場合は、あまり分だけ歩行させます。

 

  • 186-187行目

初期姿勢に移動するよう、最後に帳尻をあわせます。

 

  • 189-206行目

ロボットを右旋回させる関数です。

 

step_right_turn(1ステップの回転角 deg , 足上げ量 mm, ロール軸の移動量 0.1deg, 移動時間 10ms)

 

脚を上げている間に、遊脚している脚のヨー軸を回転させることで旋回します。

 

  • 192行目

右脚を遊脚させます。

 

  • 194-196行目

右脚ヨー軸を指定された1ステップの旋回角度分だけ移動させ、その間スリープさせます。

 

  • 198行目

右脚を着地させます。

 

  • 201行目

左脚を遊脚させます。

 

  • 203-205行目

右脚ヨー軸を初期位置に移動させ、その間スリープします。

 

  • 207行目

左脚を着地させます。

 

  • 209-227行目

ロボットを左旋回させる関数です。

step_left_turn関数と左右が入れ替わっただけで、あとは同様の処理を行います。

 

  • 229-254行目

ロボットの旋回を制御する関数です。

指定された旋回角度 degだけ、1ステップの回転角から旋回回数を決定し、最後に初期姿勢をとるように帳尻をあわせます。

 

  • 231-232行目

旋回回数を旋回角度と1ステップの回転角から求めます。

また、ステップ幅以下のあまり量を求めます。

 

  • 234-237行目

旋回時に腕と脚が干渉するため、左右の脇を開いて干渉を回避します。

脇を適当な角度開くよう、ロボットを動作させます。

 

  • 239-243行目

旋回角が負の場合は右旋回を行い、0より大きい場合は左旋回を行います。

 

  • 245-249行目

あまりがある場合は、あまり分だけ旋回させます。

 

  • 251-254行目

開いた脇を元に戻します。

 

  • 256-270行目

Activate時にシリアルポートのオープンや、コンフィギュレーションの読み込みを行います。

 

  • 257行目

コンフィギュレーションに設定されたポートと、ボーレートでシリアルポートをオープンします。

 

  • 259-262行目

脛リンク、腿リンクの長さと、初期位置をコンフィギュレーションから読み込みます。

また、コンフィギュレーションの値から脚リンクの長さを計算します。

 

  • 264行目

ポートがオープンするまで十分な時間スリープします。

 

  • 266行目

ロボットを初期姿勢に移動させます。

 

  • 273行目

ポートをクローズします。

 

  • 277-320行目

onExecute時には、pose2dポートから入力があった場合、指定の位置・姿勢に移動します。

 

  • 279-284行目

pose2dの値を読み込み、読み込んだ値を変数に格納します。

 

  • 288行目

目標位置(x,y)から、移動距離lを求めます。

 

  • 290-302行目

旋回角thetaを求めます。

 

  • 291-302行目

xが0ではないとき旋回角thetaをmath.atan2関数で求めます。

 

  • 294-302行目

もしthetaが0でないとき、thetaだけ旋回させます。

thetaはラジアンですので、度数に変換してturn関数に与えます。

turn関数のその他の引数には、コンフィギュレーションで指定した値を渡します。

最後に、適度にスリープします。

 

  • 304-312行目

もしlが0より大きい場合は、walk関数を使って歩行させます。

walk関数のその他の引数には、コンフィギュレーションで指定した値を渡します。

最後に、適度にスリープします。

 

  • 314-320行目

もしheadingが0ではない場合は、先ほどと同様にturn関数を使って旋回させます。

 

  • 324-325行目

エラー時にポートをクローズします。

 

　以上で、GR-001のコーディングは終了です。

 

今回はコードが長く、大変でしょうが頑張って最後までコーディングしてみて下さい。

GR-001以外のヒューマノイドでも、同様の構造でRTC化できると思います。

ぜひ、お手元のヒューマノイドでお試し下さい。

  • 69-78行目

脚のリンクの長さと、角度を計算します。

 

  • 80-84行目

角度のphiの値を求めます。

zが0出ないとき、math.atan2関数を使ってphiの値を求めます。

 

  • 86-90行目

th_1、th_2、th_3の値を求めます。

これらの値は各脚のシリアルサーボの位置(角度)となります。

求めた値を各角度を返します。

 

  • 92-93行目

RSシリーズのサーボは0.1 deg単位で指示するので、角度を10倍して返します。

 

角度 0.1deg convert_rs(角度 deg)

 

  • 95-96行目

度数を、ラジアンに変換して返します。

 

角度 rad to_degree(角度 deg)

 

 

  • 98-118行目

ロボットを初期姿勢に移動させます。

 

set_ready_position(初期高さ mm, 移動時間 10ms)

 

初期高さ mmは初期姿勢を計算するための足先の高さです。

ここではready_zを指定することを想定しています。

 

  • 99-100行目

各関節の角度の計算は腰の座標系で行うので、足先の座標系から腰の座標系に変換します。

 

  • 102-104行目

初期姿勢時の各関節の角度を計算し、RS用に単位を変換したあとタプルに格納します。

 

  • 106-115行目

複数のシリアルサーボのデータを二次元のリストに格納します。

 

  • 117-118行目

シリアルサーボのデータをmove_robot関数に与えて、GR-001を動作させます。

その後、動作時間の間スリープさせます。

 

  • 120-141行目

ロボットの右脚を動作させる関数です。

 

move_right_foot(足の踏み出し量 mm, 足のあげ量 mm, ロール軸の移動量 0.1deg, 移動時間 10ms)

 

足の踏み出し量と足のあげ量は、足先の目標位置(x, z)です。

ロール軸の移動量は、重心を移動するための股と足首関節の移動量です。

ロボットが脚を上げるには、反対の脚に重心を移動する必要があり、そのときのにロール軸を回転させて移動させます。

各ロール軸に体重移動量を指定する以外は、set_ready_position関数と同様の処理を行います。

 

  • 143-164行目

ロボットの左脚を動作させる関数です。

move_right_foot関数と左右が入れ替わっただけで、あとは同様の処理を行います。

 

  • 166-173行目

ロボットを歩行させる関数です。

 

step_forward(1ステップの幅 mm, 足あげ量 mm, ロール軸の移動量 0.1deg, 移動時間 10ms)

 

ステップは1回の移動量です。右脚と左脚はstep / 2ずつ移動します。

1ステップの幅と足のあげ量の関係は下図の用になります。

今回はstep / 4の位置で脚をあげることで、三角形の足先軌道を描きます。

動作の様子は、以下の動画をご覧ください。

$ sudo gpasswd –a <ユーザ名> dialout

bottom of page