URDFから情報を抽出してROS 2パラメータにする [ヒューマノイド動歩行]
前回の記事↓ odome.hatenablog.com
開発リポジトリ↓ github.com
概要
ヒューマノイドロボットのモデルが記述されたURDFファイルを読み込み、制御に有用な情報をROS 2パラメータとして利用可能なYAMLファイルで出力するPythonスクリプトを作成しました。本記事では、このスクリプトの紹介を行います。
(後述しているように動作確認はしていますが、どのURDFモデルでも正常に動作する保証はいたしかねます。生成されたYAMLファイルが正常が正しいかを確認することを推奨します。)
3行で説明
URDFファイルを読んでROS 2パラメータのYAMLファイルを生成するスクリプト、model_parameter_generator.pyを作った(source_code)。
URDFモデルはヒューマノイドロボットを対象とし、動作試験はROBOTIS OP2のXacroをURDFに変換したもので行った(source_code & readme)。
生成されるYAMLファイルは、全関節の情報、四肢(正確にはツリー構造の枝ごと)ごとのリンク長や名前をまとめたもの、全関節名をまとめたもの、計3つ生成し、主に/joint_statesトピックや運動学計算での活用を想定している。
動機
スクリプトを作成するに至った動機を以下に示します。
運動学計算や/joint_statesトピックの作成には、ロボットの各リンク長や関節名を、ユーザ側でURDFファイルを直接読んでコードに反映する必要がある。
制御対象とするロボットモデルを変更する際に、再度URDFを読んでコードに反映するという作業は苦痛。加えて、書き換えるべきコードを理解しておく必要がある。
ROS 2パラメータでURDF内の必要な情報を扱えれば、コード理解と書き換え作業を削減できるのでは?
つまりは、制御対象を変更する作業を、より簡単で容易にできるようにしたい、これが動機であり、目的となります。
使用方法
ここでは、本スクリプトの使用方法について示します。ソースコードは以下となっています。
使用方法
スクリプトを実行する前に,対象とするURDF ファイルが robot_description/models/<ロボットの名前>/ディレクトリ内に置かれていること,プログラム内で定義されているURDFファイル名とロボットの名前が正しいかを確認する.
本スクリプトであるrobot_tools/robot_tools/model_parameter_generator.pyを実行する.(以下、例)
cd robot_tools/robot_tools/ # 移動 ./model_parameter_generator.py # 実行
- 四肢などの URDF 内に含まれるツリーが表示されるので,指示通りに各ツリーの名前を入力する.(以下、例)
[INFO] Robot name is robotis_op2 [WARNING] Already robot_description/config/robotis_op2 dir. [INFO] List of tree in URDF file. (with fixed joints) tree 0 : ['base_joint'] tree 1 : ['head_pan', 'head_tilt', 'head_cam'] tree 2 : ['l_sho_pitch', 'l_sho_roll', 'l_el'] tree 3 : ['r_sho_pitch', 'r_sho_roll', 'r_el'] tree 4 : ['l_hip_yaw', 'l_hip_roll', 'l_hip_pitch', 'l_knee', 'l_ank_pitch', 'l_ank_roll'] tree 5 : ['r_hip_yaw', 'r_hip_roll', 'r_hip_pitch', 'r_knee', 'r_ank_pitch', 'r_ank_roll'] [INFO] Please input free joint_tree_name. [INFO] If nothing is inputted, joint_tree_name is 'names_group_<number>'. [INFO] tree 0 name? > # ここに、tree 0の名前を入力
- 右脚と左脚のツリー名を指定する.(以下、例)
name's number | joint_group_name | joint_group_value --------------|------------------|------------------ 0 | names_group_0 | ['base_joint'] 1 | names_group_1 | ['head_pan', 'head_tilt', 'head_cam'] 2 | names_group_2 | ['l_sho_pitch', 'l_sho_roll', 'l_el'] 3 | names_group_3 | ['r_sho_pitch', 'r_sho_roll', 'r_el'] 4 | names_group_4 | ['l_hip_yaw', 'l_hip_roll', 'l_hip_pitch', 'l_knee', 'l_ank_pitch', 'l_ank_roll'] 5 | names_group_5 | ['r_hip_yaw', 'r_hip_roll', 'r_hip_pitch', 'r_knee', 'r_ank_pitch', 'r_ank_roll'] [INFO] What's left_leg's name? (please input name's number) > # ここに、左脚のname's numberを入力
出力結果
ここでは、本スクリプトが出力するYAMLファイルについて示します。
- param_<ロボットの名前>_joints.yaml
- 全関節の回転軸方向やリミットなどの情報を持つ。
- position_sensor_nameは、webotsで関節角度を取得する際に用いるセンサ名。以下同様。(以下、例)
/**: ros__parameters: robotis_op2_joints: base_joint: origin: xyz: - 0.0, 0.0, 0.0 rpy: - 0.0, 0.0, 0.0 head_pan: position_sensor_name: head_pan_sensor origin: xyz: 0 0 0.0205 rpy: 0 0 0 axis: 0 0 1 limit: effort: 2.8 lower: -2.6179939 upper: 2.6179939 velocity: 5.6548668 head_tilt: position_sensor_name: head_tilt_sensor origin: xyz: 0 0 0.03 rpy: 0 0.5759586531581288 0 axis: 0 -1 0 limit: effort: 2.8 lower: -2.6179939 upper: 2.6179939 velocity: 5.6548668 ...
- param_<ロボットの名前>_limb.yaml
- 四肢などのそれぞれの名前リストやリンク長のリストを持つ。
- 特に、運動学計算や/joint_statesトピックで有用であると思われる。
- 駆動しない関節(fixed)を含むリストと、含まないリスト両方を記録している。(以下、例)
/**: ros__parameters: robotis_op2_limb: limb_names: left_leg: l_leg right_leg: r_leg limb_without_fixed_joints: base: joint_names: null position_sensor_names: 'null' joint_numbers: null joint_unit_vector: null link_length: null joint_posture: null head: joint_names: - head_pan - head_tilt position_sensor_names: - head_pan_sensor - head_tilt_sensor joint_numbers: - 0 - 1 joint_unit_vector: - 0 0 1 - 0 -1 0 link_length: - 0 0 0.0205 - 0 0 0.03 joint_posture: - 0 0 0 - 0 0.5759586531581288 0 l_arm: joint_names: - l_sho_pitch - l_sho_roll - l_el position_sensor_names: - l_sho_pitch_sensor - l_sho_roll_sensor - l_el_sensor joint_numbers: - 2 - 3 - 4 joint_unit_vector: - 0 1 0 - -1 0 0 - 0 1 0 link_length: - 0 0.0575 0 - 0 0.0245 -0.016 - 0.016 0 -0.06 joint_posture: - 0 0 0 - 0.7853981633974483 0 0 - 0 -1.5707963267948966 0
- param_<ロボットの名前>_name_lists.yaml
- 全関節名と全position_sensor名を持つ。
- 特に、/joint_statesトピックで有用であると思われる。
- 駆動しない関節(fixed)を含むリストと、含まないリスト両方を記録している。(以下、例)
/**: ros__parameters: robotis_op2_name_lists: all_names_without_fixed_joints: all_joint_names: - head_pan - head_tilt - l_sho_pitch - l_sho_roll - l_el - r_sho_pitch - r_sho_roll - r_el - l_hip_yaw - l_hip_roll - l_hip_pitch - l_knee - l_ank_pitch - l_ank_roll - r_hip_yaw - r_hip_roll - r_hip_pitch - r_knee - r_ank_pitch - r_ank_roll all_position_sensor_names: - head_pan_sensor - head_tilt_sensor - l_sho_pitch_sensor - l_sho_roll_sensor - l_el_sensor - r_sho_pitch_sensor - r_sho_roll_sensor - r_el_sensor - l_hip_yaw_sensor - l_hip_roll_sensor - l_hip_pitch_sensor - l_knee_sensor - l_ank_pitch_sensor - l_ank_roll_sensor - r_hip_yaw_sensor - r_hip_roll_sensor - r_hip_pitch_sensor - r_knee_sensor - r_ank_pitch_sensor - r_ank_roll_sensor
そしてこれらROS 2パラメータファイルは、前々回の記事で紹介したParameter Serverに読み込まれ、各ノードからROS 2パラメータとして扱えるようになっています。
前々回の記事↓ odome.hatenablog.com
以上の実装によって、URDF内の情報をROS 2パラメータで扱えるようにしています。
最後に
今回は、URDFから情報を抽出してROS 2パラメータとして利用できるようにする、model_parameter_generator.pyの紹介を行いました。はじめに述べたように、どのURDFモデルでも正しいYAMLファイルを生成できることを保証することはできていませんが、スクリプトの利用によってロボットモデルを変更しやすくなったと思います。
今後の展望として、エラー処理の追加や、他URDFファイルでの実例を増やしていければ良いと考えています。
開発リポジトリ↓ github.com
ROS 2でプラグインを使う [ヒューマノイド動歩行]
前回の振り返り & 今回の概要
前回は、ROS 2の機能の1つであるROS 2パラメータとその利用に関して示しました。開発中の本ソフトウェア群での活用方法を示しました。
今回は、ROS 2のライブラリの1つであるPluginlibを用いたプラグイン機能とその活用方法に関して示します。
前回の記事↓ odome.hatenablog.com
開発リポジトリ↓ github.com
ROS 2におけるプラグイン
ROS 2は基本的に、1つのプロセスを1つのノードとし、ノードが相互に通信を行うことでシステムを構築します。そして、この1プロセス1ノードでシステムを分割することでシステムの拡張や一部変更が容易になるなど、様々な恩恵を受けることができます。
しかしこの手法でネック(そして心配)となってくるのが、通信を用いている点です。通信には当然ながら時間を要することに加え必要時間は一定ではないため、例えばある一定の短い周期で回したい処理内に通信があると、周期内に処理時間が収まらない状況が発生する可能性が考えられます。
ここで、この問題の解決をしたいとき、真っ先に思いつくのが“通信を使わずに全処理を1ノードに書く”ことだと思います。ただこれでは、複数ノードからなるシステムの利点であった拡張容易性と変更容易性が完全に死んでしまいます。この、“拡張容易性”と“変更容易性”(特に“変更容易性”)と“通信を使わない”の3点を併せ持つことができる仕組みの1つとして、今回の「プラグイン」機能があります。ROS 2でのプラグイン機能は、1ノードが1つ以上のプラグインを読み込むことで、1ノード内で行う処理の一部を読み込んだプラグインで行わせることができます。
何が良いのか?
このプラグイン機能の利点としては、以下が挙げられます。
- プラグインは差し替えが可能なので、ノードが読み込むプラグインを変更するだけで簡単に動作の変更が可能である。 ー> 変更容易性
- 差し替えが可能であることを利用し、拡張可能な箇所に予めプラグインを読み込む処理を加えておくといったことも可能である。 ー> (少しの)拡張容易性
- 通信を使用せずにノードによってライブラリとして読み込まれるため、通信のデメリットを含まない。
そして、ROS 2におけるプラグイン機能はPluginlibと呼ばれるC++用のROS 2ライブラリを利用することで実現できます。
Pluginlibによるプラグイン
Pluginlibは、オブジェクト指向プログラミング特有の、クラスを利用するプラグイン機能を提供します。具体的には、基本クラスをプラグインの雛形、基本クラスを継承した派生クラスを実際に処理を行うプラグインとして実装します。そのため、プラグインの差し替えは共通の基本クラスを持つプラグインである必要があります。しかし基本クラスと派生クラスは自ら実装するため、そこまでの問題にはならないでしょう。
どう活用するのか?
上記を踏まえて、開発の歩行制御ソフトウェア群でどのように活用するのかを考えます。
開発の本ソフトウェア群には、ヒューマノイドロボットの歩行動作を制御する歩行制御ソフトウェアが含まれています。この歩行制御ソフトウェアは、基本的に4つの要素からなり、4要素を順番に実行することでロボットの脚関節角度・関節回転速度が算出され、ロボットの脚を動かすことができます。これを周期的(今回は10[ms]周期)に繰り返し出力、適用することで、歩行が実現できます。
この歩行制御ソフトウェアを実装することを考えたとき、以下の3つの手法が考えられます。
- 4要素をそれぞれノードとして実装し、通信でつなぐ。
- 4要素を1つのノードとして実装する。
- 4要素を1つのノードにプラグインとして実装する。
前章までの話を踏まえて、変更容易性や処理時間の観点から第3の、4要素を1つのノードにプラグインとして実装する手法を採用しました。
実装
ここでは、ソフトウェア構成イメージとPluginlibを用いたプラグイン機能の実装(の一部)を示します。
プラグインの実装には、以下の5点が必要です。
- プラグインの雛形となる、基本クラス。
- プラグインである、雛形を継承した派生クラス。
- プラグインを説明する、XMLファイル。
- プラグインをビルドするための、CMakeLists.txtのコード。
- プラグインを読み込むノード。
ここでは実装例として、上図に含まれるWalking Pattern Generatorプラグインのコードへのリンクを示します。なお、ここでのWalking Pattern Generatorプラグインという名称は、あくまで基本構成を踏まえた仮のものであり、実装でのプラグインのクラス名は“walking_pattern_generator::WPG_LinearInvertedPendulumModel”です。
robot_manager/include/robot_manager/control_plugin_bases/PluginBase_WalkingPatternGenerator.hpp (基本クラス: source_code)
walking_pattern_generator/include/walking_pattern_generator/LinearInvertedPendulumModel.hpp (派生クラス、のhpp: source_code)
walking_pattern_generator/src/LinearInvertedPendulumModel.cpp (派生クラス、のcpp: source_code)
walking_pattern_generator/plugins.xml (XMLファイル: source_code)
walking_pattern_generator/CMakeLists.txt (ビルド: source_code)
Robot Manager (プラグインを読み込むノード、のcpp: source_code)
また、実装にあたってはROS 2の公式ドキュメントなどが参考になります。
最後に
今回は、ROS 2にパラメータ機能を提供するPluginlibの紹介と活用方法を示しました。歩行制御ソフトウェアとしての骨組みは組み上がってきていると感じているので、次の記事ぐらいで一旦、ソフトウェアの全体をまとめた記事を書こうと思います。
開発リポジトリ↓ github.com
ROS 2パラメータの利用 [ヒューマノイド動歩行]
前回の振り返り & 今回の概要
前回の記事では、ソフトウェアアーキテクチャの思案を行いました。 今回は、ROS 2の機能の一つであるパラメータ(以下、ROS 2パラメータ)を扱って、ソフトウェアアーキテクチャの中に組み込んでいきます。
前回の記事↓ odome.hatenablog.com
開発リポジトリ↓ github.com
ROS 2パラメータって何さ?
一言で言えば、ノードが持てる設定値、みたいなものです。起動時に値を受け取るなどすることでパラメータを持つことができ、他ノードのパラメータを取得、変更することもできます。以下のような特徴が挙げられます。
- ノードの起動時に、引数としてパラメータの設定(宣言&初期化)を与えることができる。引数には、ROS 2パラメータが記されたYAMLファイルも渡せる。
- ノード内で利用するには、パラメータの宣言と初期値の定義が必要である(NodeOptionsでの設定で無視することもできる)。
- 他ノードの特定パラメータの値を、Service通信を利用して取得することができる。また、変更することも可能である。
- コマンドラインから、全パラメータ名の取得や特定パラメータの値を取得、変更が行える。
何が良いのさ?
上記の特徴を持つROS 2パラメータの良さは、
- ノードの起動時に、引数としてパラメータの設定(宣言&初期化)を与えることができる。
これが挙げられます。これを利用することによって、ノード内で利用される値を起動時に容易に定めることができ、ハードコーディングを避けることによる利便性の向上を図ることができます。特にYAMLファイルを渡すことが可能なので、YAMLファイルの変更のみでノードの挙動を変えることも可能となります。
どう活用していくのか?
本ソフトウェア群でも、このROS 2パラメータの利点を有効に活用していきます。具体的には、以下を担わせます。
- 制御パラメータなどをYAMLファイルに記述しROS 2パラメータとして扱うことによる、制御パラメータの変更容易性向上
歩行制御において、歩行周期や片足支持期間などの制御パラメータは頻繁に変更することになります。このことから変更容易性は高いに越したことはありません。
ROS 2パラメータを含んだソフトウェアアーキテクチャの思案
以上を踏まえて、ROS 2パラメータを活用したソフトウェア構成イメージを以下に示します。
まず、制御パラメータなどをROS 2パラメータに対応したYAMLファイルに記述し、ノードであるParameter Serverで全YAMLファイルを読み込みます。そして、必要に応じて各ノードなどがParameter Serverから特定パラメータの値を取得する、という構成となっています。
この構成の検討箇所と取捨選択を示します。
- Parameter ServerでROS 2パラメータを一括管理することで、構成内の依存関係を単純化
- Parameter Serverが単一障害点となってしまうことに加えて各ノードが単独でパラメータを取得することも可能であるが、複数ノードで扱うパラメータ(歩行周期など)が同じ場合が存在することや依存関係の単純化による管理のしやすさを高く評価したため、この構成を採用した。
- ROS 2パラメータの定義はYAMLファイルにて実装
- 制御パラメータの数が多いことや今後も増えることを鑑みると、launchファイル内でParameter Serverノードに引数として直接パラメータの定義を渡すよりもYAMLファイルとして記述するほうが、利便性などの観点から良いと判断した。
この構成イメージをもとに、実装を行いました。
実装
まず、現時点(2024年1月31日)で実装しているYAMLファイルの一覧(リポジトリのディレクトリ構造も含めて)を示します。
- robot_bringup/: システムの起動および実行などを行うパッケージ
なお、robot_descriptionが持つYAMLファイルは、robot_tools/robot_tools/model_parameter_generator.py
によって、URDFファイルから生成されたものです。この生成スクリプトの詳細については、ここでは省きます。
次に、Parameter Serverのコードの一部を示します。
- NodeOptions
int main(int argc, char* argv[]) { rclcpp::init(argc, argv); rclcpp::NodeOptions opt; opt.automatically_declare_parameters_from_overrides(true); // ROS 2パラメータを定義しなくてもget_parameterできるようにする設定 rclcpp::spin(std::make_shared<ParameterServer>(opt)); rclcpp::shutdown(); return 0; }
- Get Parameters
// mode_switch on_or_offline_pattern_generate_ = get_parameter("mode_switch.on_or_offline_pattern_generate").as_bool(); // ROS 2パラメータの取得。.as_<>() で型を明示する必要がある。 debug_mode_ = get_parameter("mode_switch.debug_mode").as_bool(); using_simulator_ = get_parameter("mode_switch.using_simulator").as_bool();
ROS 2パラメータを通信により他ノードから取得する際は、Service通信とは記述が異なります。Server側で特に記述することはなく、Client側で以下のようにすることで取得することができます。
node_ptr_ = rclcpp::Node::make_shared("RobotManager"); client_param_ = std::make_shared<rclcpp::SyncParametersClient>(node_ptr_, "RobotParameterServer"); // 他ノードのROS 2パラメータを取得するために必要な変数。 // param: mode_switch ONLINE_OR_OFFLINE_GENERATE_ = client_param_->get_parameter<bool>("mode_switch.on_or_offline_pattern_generate"); // 他ノードのROS 2パラメータを取得 DEBUG_MODE_ = client_param_->get_parameter<bool>("mode_switch.debug_mode"); USING_SIMULATOR_ = client_param_->get_parameter<bool>("mode_switch.using_simulator");
最後に
今回は、ROS 2パラメータを利用して制御パラメータなどを容易に変更可能にする取り組みを行いました。まだROS 2パラメータ化していないものもあったりするので、今回示したものをアップデートしていきます。
開発リポジトリ↓ github.com
ソフトウェアアーキテクチャの思案 [ヒューマノイド動歩行]
前回の振り返り & 今回の概要
前回の記事では、動歩行動作を完成させました。前々回の実装における問題点を改善し、足を引きづらない動歩行動作を完成させました。
今回からは、理論の話よりも、歩行パターン生成器とその周辺ソフトウェアの開発について主に進めていきます。
今回は開発するソフトウェアの構成、ソフトウェアアーキテクチャの話をしていきます。
前回の記事↓
開発リポジトリ↓
何を開発するのか
一言でこれから開発していくものを示すと、汎用性・拡張性を考慮した歩行制御ソフトウェア、です。
前回まで開発してきたのは、歩行パターンを生成してシミュレータ上のロボットに適用するソフトウェアです。これを歩行パターン生成器として扱って来たわけですが、現状ではあまり整備されておらず、他の人に使ってもらえるような状態になっていません。加えて、現在の理想的な環境ではない場合に有効な歩行安定化制御系など、歩行パターンを生成する以外に必要なソフトウェアが含まれていない、あるいは不十分であったりします。
これら不足しているソフトウェアを含んだ全体を、歩行制御ソフトウェアとして開発していきます。
開発するソフトウェアアーキテクチャ
ソフトウェアを開発する場合、(特に私が思う)重要な要素として、そのソフトウェアの構成・構造が挙げられます。ここで、ソフトウェアの構成、構造、そして拡張性や再利用性などの性質を含めてソフトウェアのアーキテクチャと呼ぶことにします。
このソフトウェアアーキテクチャを決めるために、以下の要素に重点を置きます。
- シンプルに、わかりやすく
- 拡張性の確保
- 歩行パターン生成手法など、制御や実装に関する複数の要素をUser側で独自に実装できるようにする。
- 汎用性の確保
- 根幹や大幅な変更を加えることなく、User側はパラメータ調整とロボットモデルの提供のみで、様々なヒューマノイドロボットの歩行を実現できるようにする。
以上に重点をおいて考えていく中で、以下などのトレードオフ問題にあたり、今回の答えを設定しました。(このトレードオフから判断するのが一番苦労しました。答えがないので...)
問題:要素間の組み合わせに自由度をもたせる vs 要素間の流れを固定して通信のオーバーヘッドを減らす
問題:シミュレータ及び実機の搭載するソフトウェアにも汎用性をもたせる vs User側で用意してもらい此方側では制御ソフトウェアのみを提供する
- 答え:基本的にUser側で用意してもらうが、実装例をいくつか用意し、制御ソフトウェア側から提供するインターフェイスを明示する。
ここで”答え”と表現していますが、理論的に明確で普遍的な答えが用意されているわけではなく、あくまで私が今回選択したことです。ですので、より最適な答えも存在するかもしれません。
これらのことを踏まえて、以下のソフトウェアアーキテクチャを考案しました。
ソフトウェアアーキテクチャ
(少し雑ですが...)
(Pluginは、PluginlibというROS 2のライブラリを用いて開発しています。)
ここで、各要素=各Pluginの役割は、以下のとおりです。
- Foot Step Planner
- ロボットが移動する軌跡から着地位置を決定するPlugin。
- Walking Pattern Generator
- 着地位置を元に歩行パターン(重心軌道、ZMP軌道)を生成するPlugin。
- 前回まで開発していたものは、ココに当たる。
- Walking Stabilization Controller
- 安定した歩行を実現するための制御を行うPlugin。現在の姿勢から姿勢修正量を歩行パターンに反映させる。
- IK(path -> joint) == Convert To Joint States
- 歩行パターンから脚の全関節角度・角速度を生成するPlugin。
また記されていませんが、以下の要素もPluginとして提供することを考えています。
- Inverse Kinematics
- 逆運動学を解くPlugin。様々な解き方があるのでPlugin化したほうが拡張性・汎用性の向上に良いだろう。
- Forward Kinematics
- 順運動学を解くPlugin。あまりPlugin化する恩恵はないだろうが、あることに越したことはないだろう。
- Jacobian
- ヤコビアン(ヤコビ行列)を解くPlugin。これもいくつか解き方があるのでPlugin化したほうが良いだろう。
以上の様にこのソフトウェアアーキテクチャでは、1つのROS 2 Nodeに複数の制御Pluginを読み込ませて1つの制御ソフトウェアとし、ロボットに動作指令を送るNodeと通信を行う、というようにしました。また、Rviz2などの可視化ツールやLoggerを用いることで、開発やデバッグがしやすいようにしました。
このようなアーキテクチャにすることで、以下のような利点があると考えています。
- Plugin形式にすることで、User側で自由に独自Pluginを開発し、組み替えることができるようになる。
- Pluginとすることで入出力インターフェイスを固定し、独自Pluginを開発しようとするUser側で入出力を考える必要がなくなる。加えて各Pluginは独立しているため、1つのPluginを変えても他Pluginに直接的な影響が及ばない。
- 歩行制御における各要素を、Nodeごとの通信ではなくPluginとして1つのNodeに集約させているので、通信で発生するオーバーヘッドや情報の欠損などの、通信によって発生するリスクを抑えることができる。
- 歩行制御ソフトウェア(今回では、robot_manager Node)とロボットに動作指令をするソフトウェアを別Nodeとすることで、歩行制御ソフトウェア側はロボットのハ特定ハードウェア環境に依存させないことができる。
最後に
今回は、ソフトウェアアーキテクチャのことを考えました。もちろんまだ開発段階ですし私もほとんど経験がないことをしているので、今後また変わる可能性もあります。
これからは今回示したソフトウェアアーキテクチャを、実際に組んで実装していきます。
開発リポジトリ↓
動歩行の完成 [ヒューマノイド動歩行]
前回の振り返り & 今回の概要
(だいぶ期間が空きましたが、)前回の記事では、動歩行の実装をシミュレータ内のロボットに対して行いました。しかしその実装したプログラムが不完全で、うまく歩いてくれませんでした。
今回はそのバグを修正した結果、足を引きづらずに歩行できる動歩行動作を達成したことに関してまとめます。
今回は理論については余り触れず、前回との差異に関して主にまとめます。
前回の記事↓
開発リポジトリ↓
前回示した問題点
前回の時点では、動歩行の理論、つまり歩行パターンの生成まではできていました。しかし、以下の動画のようにまともな歩行ができていませんでした(前回示した動画とは異なりますが、おおよそ原因と前提は同じ(なはず)です)。
文でまとまると、以下の2つが挙げられます。
- 遊脚が目標足上げ高さの0.8[m]まで足を上げていない。
- 歩行途中で遊脚が床面を擦ってしまい、前に進めていない。
今回はこの問題を解決することを示します。
問題の改善
原因はどこにあるのか
前回うまく行かなかった原因は、歩行パターンを関節角度・角速度に変換するプログラムが不完全であったことと、歩行周期などのパラメータが不適切であったことが挙げられます。
この2点は、どうもスタンダードな設定方法があるわけではなく、主にロボットのハードウェア構造に大きく依存しているようです。ですので今回は、試行錯誤的にパラメータとプログラムの調整を行いました。遊脚軌道の適用方法
2足歩行の生物なりロボットは歩行時に、片脚を地面について胴体を進行方向に移動させ、もう片脚を持ち上げて次の着地位置まで足を移動させます。この持ち上げる脚を「遊脚」というのですが、ロボットの場合この遊脚の足を動かす軌道、いわゆる遊脚軌道を与えてやる必要があります。
今回、この遊脚軌道を正弦波として与えました。正弦波を遊脚軌道として与えた場合、用いるのは正弦波の「山」の部分です。ですので、正弦波の1/2周期が歩行時の片足支持期と同等となります。また、正弦波の振幅が遊脚の最大足上げ高さと同等となります。
以上を踏まえて正弦波を遊脚に適用してやる場合、プログラム的には、腰の高さ - 正弦波という形になります。適したパラメータ設定方法
歩行に関わるパラメータは、歩行の安定性に非常に大きく関わります。しかもハードによって適したパラメータがkとなるのですから、パラメータ調整は結構大変です。ここでは、ROBOTIS OP2をWebots内で歩行させた際のパラメータと、パラメータ調整手順を示します。
まず前提として、歩行に関わる主なパラメータは以下のようになっています。もちろん実装によって変動しますが、ある程度は共通していると思います。
|
・歩行パラメータ:時間に応じた、足の着地位置
・腰の高さ:歩行時の腰の高さ
・歩行周期:1歩に掛かる時間
・片脚支持期:歩行1周期中、片足しか床面に接していない時間
・両脚支持期:歩行1周期中、両脚が床面に接している時間
・足上げ高さ:遊脚が上げる足の最高高さ
|
次にパラメータの調整方法について、私は以下の手順に沿って行いました。
|
・1.歩行パラメータと腰の高さを決めて、着地位置と動作計画を明確にする。
・2.歩行周期と両脚支持期を決め打ちする。
・3.足踏みをさせてみて、足上げ高さを転倒の有無から決める。
・4.前進させてみる。
|
この手順は私が勝手に立てたものですが、不順にパラメータをいじってしまうとどの組み合わせを試したかがわからなくなるので、何かしらの手順に沿っておこなったほうが良いかもしれないです。その他の調整色々 ・腰の高さを、特異姿勢にならないように設定。
・遊脚において、転倒防止のために足上げ&着地開始瞬間に全関節角速度を最大。
改善した結果
上記のような改善をした結果、以下のパラメータで、以下の歩行動作を得ることができました。
パラメータ
- 歩行パラメータ
(基準を右足裏中心においている。適用時は基準を胴体中心に移動させている)
支持脚切替タイミング[s] | 0.0 | 0.8 | 1.6 | 2.4 | 3.2 | 4.0 | 4.8 | 5.6 |
---|---|---|---|---|---|---|---|---|
X軸方向[m] | 0.0 | 0.0 | 0.03 | 0.06 | 0.09 | 0.12 | 0.12 | 0.12 |
Y軸方向[m] | 0.037 | 0.074 | 0.0 | 0.074 | 0.0 | 0.074 | 0.037 | 0.037 |
- 制御周期:10[ms] (大抵、歩行制御は10[ms]ぐらいらしい)
- 歩行周期:0.8[s]
- 片足支持期:0.3[s]
- 両脚支持期:0.5[s]
- 腰の高さ:0.171856[m] (細かいことに意味は特にない)
改善後の歩行動作
改善前の動作と比べると、足を引きづらずに前進できていることがわかります。
最後に
今回は動歩行の実装を完成させました。
次回からは、この動歩行動作生成する歩行パターン生成器、及びその周辺ソフトウェアの開発に、主に取り組んでいきます。
開発リポジトリ
動歩行の実装 [ヒューマノイド動歩行]
前回の振り返り&今回の概要
前回の記事では、動歩行の実装に向けて、どのように進めていくかや進捗に関して示しました。今回は、動歩行の実装がほぼできたので、それについてまとめます。
前回の記事↓
開発リポジトリ
動歩行の実装
今回は、動歩行の理論の実装から、その理論をシミュレータ(Webots)上のヒューマノイドロボット(ROBOTIS OP2)に実装して実際に歩かせるところまでを行いました。具体的には、歩行パターンと各関節への動作命令を生成する歩行パターンジェネレータを開発し、ロボットに適用しました。
動歩行の理論
動歩行動作を実現するために、歩行パターンジェネレータは大きく分けて以下の2つの手順を含まなければなりません。
- 着地位置や進行方向などの直感的な情報から、重心の軌道やZMP位置などを生成。
- 生成された情報を元に、逆運動学を解くなどして各関節角度・角速度を取得。
ここで1番肝になってくるのが、手順1、つまり歩行パターンの生成です。この歩行パターンが、ロボットの歩行動作を決定づけます。
この歩行パターンを生成する理論は複数存在し、決定的な手法が定まっていません。ですので今回は、『ヒューマノイドロボット(改訂2版)』)にも示されており比較的単純である、ロボットを『3次元線形倒立振子』モデルに近似する手法を実装しました。
線形倒立振子モデル
この手法では、ロボットの重心から脚の足裏中心点を倒立振子で近似します。
まず、この線形倒立振子の考え方を述べます。重心にロボットの全質量が集中しているとして、重心を倒立振子の先端、重心から足裏中心点までを無質量のリンクで結び、足裏中心点を回転中心とします。また、リンクは伸縮できるものと考えます。足裏中心点を足の設置点として考えるため、足裏は床面と常に並行になると考えます。
ここで、片脚で全体重を支えて前進することを考えた時、重心は足裏中心点を回転中心として後方から前方へと移動します。この時、リンクが伸縮できることから重心の高さを一定に保つようにします。こうすることでZ軸方向の値が一定となるため、X,Y平面のみを考えるだけで良くなります。そして重心がある一定まで前進したら、脚を入れ替えます。入れ替えた結果、その直後の線形倒立振子は、重心が足裏中心点よりも後方にある状態になります。そしてまた、重心を足裏中心点を回転中心として後方から前方へと移動させます。
この繰り返しによって、線形倒立振子でモデル化されたロボットの歩行動作を考えていきます。
以上の説明を図にまとまると、以下のようになります。
次に、この線形倒立振子モデルを考えたときに生成するのは、重心軌道です。詳しい話は省略しますが、大まかな流れとして、
- 初めに歩行時に胴体を支える脚(支持脚)を置く位置(歩行パラメータともいう)を与え、
- 歩行パラメータを元に重心軌道を計算し、
- それと同時に重心軌道をより正確に実現するように歩行パラメータを修正
という手順を踏みます。
以上から得られた重心軌道と着地位置、加えて別で考える次の支持脚となる浮いている脚(遊脚)の軌道を元に逆運動学を解くことで、各脚の関節角度・角速度が得られます。
線形倒立振子モデルを用いた歩行パターンの生成結果
ここでは、前節で挙げた線形倒立振子モデルを用いた歩行パターン生成を、実際にプログラムに起こして計算した結果のグラフを示します。
ここで、想定するロボットはROBOTIS OP2(source: Webots Darwin-op.proto)とし、歩行パラメータは以下とします。なお、歩行パラメータの座標は初期右足裏中心点を基準(0, 0)とし、基準から見て左足方向をY軸の正方向、胴体から見て正面方向をX軸の正方向とします。また、腰の位置を両脚股関節Roll, Pitch軸位置の中心、高さを 0.171856[m] とします。
Time[s] | 0.0 | 0.8 | 1.6 | 2.4 | 3.2 | 4.0 | 4.8 | 5.6 |
---|---|---|---|---|---|---|---|---|
X Pos[m] | 0.0 | 0.0 | 0.05 | 0.10 | 0.15 | 0.2 | 0.2 | 0.2 |
Y Pos[m] | 0.037 | 0.074 | 0.0 | 0.074 | 0.0 | 0.074 | 0.037 | 0.037 |
以下に、生成した歩行パターンのグラフを示します。なお、座標の基準(0, 0)を初期右足裏中心点から初期胴体中心点に変更しています。また、制御周期は10[ms]としています。
このグラフから、Y方向では体を+ー方向に揺らし、X方向では着地位置に沿って前進できていることがわかります。
ヒューマノイドロボットを歩行させる
歩行パターンが生成できたので、次はそれを元にシミュレータ上のヒューマノイドロボットを歩かせます。実現のためには、前述したように逆運動学を解くことになりますが、特に今回の場合は、ここでもまたいくつか考えることがあります。
1つ目に、今回の単に直進する歩行パターンには以下の3つのステップが存在することです。
- 歩行開始時の支持脚と遊脚の動作
- 歩行中の支持脚と遊脚の動作
- 歩行終了時の支持脚と遊脚の動作
この3つは、全て動作が微妙に異なります。
- 初期重心位置のX座標が着地位置のX座標と同じであり初期速度が0。
- 初期重心位置が着地位置の後方にあり初期速度が≠0。
- 最終重心位置のX座標が着地位置のX座標と同じであり最終速度が0。
この違いを判定する必要があります。
2つ目に、遊脚の軌道を考えてやる必要があります。遊脚は前着地位置から次着地位置まで、足を浮かせて前に動かす必要があります。この遊脚の軌道も幾つか考えられますが、今回は正弦波を用いることにします。また、足を上げる瞬間と下げる瞬間においては、遊脚の各関節角速度を最大にしています。
以上の2点に注意しながら実装をします。
結果、以下の動作を得ることができました。なお、制御周期は10[ms]、遊脚の足上げ高さは0.08[m]としました。
(リンク先に動画があります。)
突っ込む点は幾つかありますが、動歩行動作ができていると言える動作ができています。
今後の改善点
今回得られている歩行動作から、以下の問題点が残っています。
- 遊脚が0.08[m]まで足を上げていない。
- 歩行途中で遊脚が床面を擦ってしまっているからか、向いている方向が変わってしまっている。
また、以下の発展が考えられます。
- 他の歩行パラメータを与えても歩行できる。
- エラーハンドリングを記述する。
- 歩行パターンジェネレータを他Nodeに分ける。
- etc...
最後に
今回は動歩行の理論から、実際にシミュレータのロボットへの実装までを行いました。ただ問題点や改善点など、まだまだ完璧とはいえないので、今後はそちらに取り組んでいきます、
開発リポジトリ
動歩行の実装へ向けて [ヒューマノイド動歩行]
前回の振り返り
前回までの記事では、ソフトウェアアーキテクチャの再考案に取り組んできました。今回からは、いよいよ動歩行の実装に取り組んでいきます。
前回の記事↓
今回の概要
今回は動歩行の実装に向けて、理論と現状の進捗に関して簡単に示します。
今回の前提
今回からおこなっていく動歩行の実装は、2足歩行ロボットやヒューマノイドロボットなどの研究に従事されている梶田秀司先生が編著された、『ヒューマノイドロボット(改訂2版)』に沿って進めていきます。ですので、この連載で扱っている理論の話は、主にこちらの書籍が元となっています。
こちらの書籍では、ヒューマノイドロボットの制御に関することが広くカバーされています。ヒューマノイドロボットに取り組むには、非常に素晴らしい書籍となっています。(皆さん、買いましょう。)
動歩行とは
はじめに、動歩行とはどのような動作かについてです。
動歩行とは、床面への重心投影点が支持多角形の外に出ることがある歩行動作のことを指します。逆に、支持多角形の外に出ない歩行動作は静歩行といい、(理論的に確認してはいませんが、)過去に実装しました。(記事はこちら:静歩行動作の実装)ここで、重心投影点は重心を床面に投影した点であり、支持多角形は床面に接触している足裏の外形をゴム紐で囲ったときにできる多角形です。
動歩行動作の具体的な例としては、人間のような歩き方が動歩行だといえます。より詳細にいえば、人間が行う通常の歩行動作は、動歩行に含まれます。実際のロボットだと、よくメディアに露出しているASIMOやAtlasといったヒューマノイドロボットの歩行動作は、ほとんどが動歩行だといえます。
どのように動歩行を実現するのか
次は、この動歩行はどのように実装できるのかについてです。
動歩行動作の理論としては、今まで幾つかの理論が示されてきました。単純に考えれば、これらの理論のうち1つをロボットに実装できれば動歩行は実現できるわけです。
しかし現実世界で歩かせる場合は、そう簡単には行きません。外乱、つまり不安定にさせる要素の影響によって、簡単に倒れてしまいます。特にヒューマノイドロボットは重心が比較的高い位置にあり、なおかつ狭い2つの足裏で体の全重量を支えるので、非常にバランスが悪いです。そんなヒューマノイドロボットが、未舗装の道や山道を、ランダムな風などの押される力に耐えながら歩行することは、超絶難しいわけです。
そんなヒューマノイドロボットを制御するためのソフトウェアの枠組みが、前提で紹介した『ヒューマノイドロボット(改訂2版)』には示されています。今回実装する形も、ここで示されている枠組みに沿っています。以下に、枠組みを構成するパーツを示します。
- 歩行パターンジェネレータ (今回作っていくところ!)
- 文字通り、歩行パターンを生成するソフトウェア。ここで歩行パターンは、歩行に用いる脚の各関節角度・角速度のこと。
- このソフトウェアがロボットの歩行動作を形作り、外乱が一切ない理想的な環境であれば、これだけでロボットが動歩行できる。
- 歩行安定化制御系
- これもまた文字通り、歩行を安定にするために制御をおこなうソフトウェア。ロボットからのフィードバックと歩行パターンジェネレータの出力などから計算を行ったりする。
- このソフトウェアで、どのように外乱に対策をおこなうかが決まる。ココもまた重要なソフトウェア。
以上の2つが、ロボット本体以外で必要となるソフトウェアである。
そして今回は、特に歩行パターンジェネレータに組み込むプログラムを開発することで、動歩行の実装を目指します。そして、外乱のないシミュレータを開発環境とすることで、歩行安定化制御系がなくとも歩行できる状況を考えます。
開発する歩行パターンジェネレータ
今回、開発する歩行パターンジェネレータで用いる理論は、ヒューマノイドロボットを粗視化して線形倒立振子として歩行パターンを生成する方法を用います。ここで考える線形倒立振子は、倒立振子の先端にロボットの全質量が集中している質点を重心として、重心と床面との接地点を結ぶ直動リンクの運動を蹴り力として考えます。ココの話は、また別の機会に改めてまとめたいと思います。
こちらの手法は『ヒューマノイドロボット(改訂2版)』でも詳しく示されているため、そちらを参考に進めていきます。
現在の開発進捗状況
最後に、現状の開発進捗を箇条書きで示します。
- プログラムを、webots_robot_handler内に関数として開発することに決定。通信を排除することで、考えることを少なくする意図がある。歩行パターンジェネレータの開発がうまくいったら、他Nodeに分けてソフトウェアアーキテクチャに組み込んでいく。
- 歩行パラメータを、その場で足踏みするように決定。つまり、X軸方向の位置と速度を0とした。これは、歩行パラメータの決定を後回しにした。
- 現状、実際に理論をプログラムに起こして理論通りになるように修正中。MATLABを用いて結果をグラフにプロットすることで、確認を行っている。2023/6/25現在、まだうまくいっていない。
動歩行の実装の進捗状況は、GitHubに挙げている開発リポジトリの以下のIssueで発信しています。理論通りにいっていないグラフとかがあります。
また、以下のGitHubのリポジトリに、開発中のソフトウェアを公開しています。