ロボット開発記録

ロボット開発の進捗や記録をするブログ

[第９週〜第１４週] 逆運動学を解いてロボットを制御する 設計・試作編 [ヒューマノイド動歩行]

ヒューマノイドロボット動歩行プロジェクト

先週の振り返り

　先週の記事では、逆運動学の数値解法の手順を示し、今後の予定やタスクを確認しました。

odome.hatenablog.com


今回の概要

　逆運動学を解くプログラムを試作し、アルゴリズムを考えていきます。また、開発を進めているソフトウェアの構成を設計していきます。
　タスクが比較的多いため、数週に分けて進めていきます。

進捗

2022/11/14

  1. 運動学を解くプログラムの試作（途中）
    ・ １番はじめの試作なので、WebotsやROS2を咬ませない、Python3オンリーのプログラムを組んでいる。（ソースコード）
    ・ 中で用いるパラメータは、第７週の記事で示した各関節の情報を元とする。
    ・ 今は骨組みを組んでいる段階だが、運動学の中身も徐々に書いていく。

odome.hatenablog.com

github.com


2. ソフトウェア構成の第１草案
・ 第１の案として、テキストベースではあるがソフトウェア構成を考えてみた。README.mdのConfiguration Plan (Draft)に示している。
・ ただ、同時進行で進めている運動学のプログラムを試作してる影響で、既に他案が朧気にあったりする。特に逆運動学を実際に解いて制御するといった時、逆運動学を解くNodeを１つ用意して左右の脚を順番に解くより、そのNodeを２つ用意して両方とも走らせて左右の脚を同時に解けば時短になるのではないか、と考えていたりする。つまりは、構成は今後大きく変わると考えられる。

github.com


2022/11/21

  1. 運動学を解くライブラリを調べてみる
    ・運動学を解くプログラムを試作中ではあるが、汎用性を意識するとライブラリを用いたほうが使いやすく適しているという助言を頂いた。ということで、ここでは２つのライブラリについて調べてみた。どちらも、運動学以外にも対応した多機能なライブラリとなっている。

    1.1. stack-of-tasks : pinocchio [GitHub, Document, Document(Doxygen)]
    ・ROSに対応しているライブラリ。今回用いているROS2-Foxy以外にも、ROS1-Noeticや最新のROS2-Humbleにも対応している。
    ・C++で記述されており、C++とPythonに対応している。
    ・ドキュメントが充実している。特にサンプルコードが豊富だと感じる。
    ・行列計算のC++ライブラリであるEigenと、衝突検出のC++ライブラリであるFCL（正式にはHPP-FCLが使われているらしい(HPP-FCLのREADME.md曰く)）をフレームワークに含んでいる。
    ・モデルは、URDFかSDFで記述。
    ・インストール方法は、Linuxの場合はaptも案内されている。Install Document

    1.2. RBDL : RBDL [GitHub, Document(Doxygen]
    ・ROSには対応していない（ROSの記述がない）ライブラリ。
    ・C++で記述されており、C++に対応している。Pythonに対応するためのWapperも存在する。
    ・ドキュメントが充実している。pinocchioには及ばないが。
    ・外部依存関係はないが、Eigenの最新バージョン（現時点では３）を使えば最適なパフォーマンスが得られる。
    ・モデルは、URDFかLuaで記述。
    ・インストール方法は、ソースをgit cloneしていくつかの手順を踏む必要がある。

    　どちらのライブラリも、著者: Roy Featherstone, 『Rigid Body Dynamics Algorithms』から影響を受けているとしている。ここで示したものと同様のメモをこちらに示している。
    　現在、運動学を解くプログラムを試作中ではあるが、これらのようなライブラリを用いることも検討中である。

  2. ロボットの全関節の角度０[rad]を調べた
    ・タイトル通り、全関節の角度を０[rad]にして、立たせてみた。結果、以下の画像のようになった。

全関節角度0[rad]

・脚は０[rad]で直立するが、腕は見た目から、肩X軸が45[deg]、肘Y軸が90[deg]曲がっている位置が０[rad]となっている。ここを制御時に注意する必要があることが確認できた。

  1. 順・逆運動学の再確認
    ・改めて、順・逆運動学について軽く復習した。とはいっても、技術書の該当箇所を軽く読んだ程度ではある。
    ・順運動学、逆運動学の解析的な解法は、著者: Richard.P.Paul, 訳: 吉川恒夫, 『ロボット・マニピュレータ』を参考とした。1984年出版の最初期の技術書ではあるが、非常に分かりやすくまとめられた良書であると思う。（前半ぐらいしか読んでないが...。）1984年出版であるにも関わらずまだ刷られていることにも驚き。
    　・復習の結果、逆運動学の解析的な解法は、非常に面倒くさいことが改めて確認できた。一度各関節角度の計算式を出してしまえばそこに値を当てはめてやればいいだけなのだが、マニピュレータの関節の並び方によって大きく変わるし、解が存在してもそれを求めるが非常に面倒くさかったりと、非常にクセがあって面倒くさい。このこともあって、数値解法を用いることにしている。
    ・逆運動学の数値解法は、現在調べ中である。来週の進捗ではまとめられるだろう。

2022/11/28

  1. 逆運動学の数値解法とライブラリの選択

・ 逆運動学の数値解法の問題として、関節角度修正量をどのように求めるかということがある。この解法としていくつもの提案が今までされているため、その中から適した方法１つを選んだ。
・ 結果、LMSR法(Levenberg Marquard, Singularity-Robust inverse)を選んだ。
・ LMSR法の理由として、特異点からの脱出が可能である点と、参考文献[1]で扱われている点が主に挙げられる。
・ 今回の選定では、主に参考文献[2]を参考にさせていただいた。様々な解法を比較していることに加え、ROBOTIS OP2の同型であるDARWIN-OPを検証に用いたデータが示されているため、非常に参考になった。

・ ライブラリに関しては先週調べたが、結果として、今回はpinocchio(GitHub))が適していると判断した。
・ 理由としては、ROS2 Foxyに正式対応している点と、ドキュメントが非常に充実している点が挙げられる。
・ 自作プログラムの試作と同時進行でpinocchioも使ってみたいと考えている。が、今後どうなるかはわからない。

・ 以上の検討に関するメモは、以下に記している。

github.com


  1. pinocchioを使ってみる

・ pinocchioが良いと判断したので、早速使ってみた。なお、pinocchioには超充実したドキュメントが存在しており、OverViewで示されているものだけでも十分に参考になる。

gepettoweb.laas.fr

・ 今後を想定し、ROBOTIS OP2のURDFをmodelとして読み込んでみた。以下にプログラムを示す。

""" 参考
・https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/index.html#OverviewComplex
"""

#include "pinocchio/parsers/urdf.hpp"

#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"

#include "iostream"
#include "ctime"

void datetime_now()
{
    time_t time_now = time(0);

    // std::time_t time = std::chrono::system_clock::to_time_t(time_now);
    char *datetime = ctime(&time_now);

    std::cout << "DateTime: " << datetime;
}

int main(int argc, char **argv)
{
    datetime_now();

    const std::string urdf_filename = std::string("<ここに、URDFファイルのパスを記述>");

    pinocchio::Model model;
    pinocchio::urdf::buildModel(urdf_filename, model);
    std::cout << "model name: " << model.name << std::endl;

    for(pinocchio::JointIndex joint_id = 0; joint_id < (pinocchio::JointIndex)model.njoints; ++joint_id){
        std::cout << "joint: " << model.names[joint_id] << std::endl;
    }

    pinocchio::Data data(model);

    std::cout << std::endl;
}

・ ここで使用しているURDFは公式が公開しているxacroファイルから自動生成したものを使っている。生成するコマンドは以下。

xacro robotis_op2.urdf.xacro(=変換元xacroファイル名) >  robotis_op2.urdf(=変換後urdfファイル名)

・ 以上のことから、ヒューマノイドのモデルのジョイントがすべて読み込めていることがわかった。
・ しかし、歩行制御に必要なのは両脚であり、運動学で解く対象も脚だけである。そのため、脚だけのモデルを別途作る必要があると考えられ、それがまだできていない。ここが解決すれば実装のほうにすぐ移れるので、早急に解決したい。

  1. 運動学を解くプログラムの試作（続）

・自作プログラムの試作も進めている。現時点では、各関節の同次変換行列を定義し、回転角度を指定すればその角度分回転した結果の同次変換行列を得られるところまで進んでいる。
・ ここまで進めば順運動学は目前なので、試作して実際に正しい値が得られるか確認したく思っている。
・ 試作のプログラムは、以下で更新を続けている。

github.com


2022/12/5

  1. 続・自作プログラムの試作

・進捗：順運動学を解くことができた。また、腕の逆運動学を解析的に解くプログラムを試作中。ソースコード：GitHub
・今後：腕の逆運動学を解析的に解くプログラムを完成させ、Webots_ROS2に実装する。


  1. 続・pinocchioを用いたプログラムの試作

・進捗：脚の逆運動学を、LMSR法を用いた数値解法で解くプログラムを試作中。苦戦中。特にJacobian。関数は充実しているから、数値解法の理論を把握していればいけそうな雰囲気ではある。ソースコード：非公開（後々、公開予定）
・数値解法は解析的解法よりも自由度に左右されないから、腕か脚かは関係ないと思う。
・今後：プログラムができ次第、Webots_ROS2に実装。自作プログラムよりもこっちを優先して実装したい。


・来週には、何かしらを実装してロボットを動かしたいと思う。

2022/12/29

  1. 続・自作プログラムの試作
    ・腕の逆運動学を解析的に解こうとしていたが、腕にオフセットがあることから上手く導出することができなかった。そのため、脚の逆運動学を解くプログラムを自作した。
    　・詳細は以下の記事でまとめている。

  2. 続・pinocchioを用いたプログラムの試作
    ・進捗なし。脚を解析的に逆運動学を解けたので、それが実装できたら、pinocchioに再チャレンジする予定。

--- Close ---

参考文献

・ [1], オーム社, 梶田秀司, "ヒューマノイドロボット（改訂２版）", 2020
　・ ↑理論を考える上で、主に参考にさせていただいている技術書
・ [2], 林原靖男・高橋直樹, "ヒューマノイドロボットを対象とした逆運動学の性能比較", 2020

[第８週] 逆運動学を解いてロボットを制御する 準備編 [ヒューマノイド動歩行]

ヒューマノイドロボット動歩行プロジェクト

先週の振り返り

　先週の記事では、逆運動学を解く準備の準備として、ロボットの関節に関する情報をまとめました。

odome.hatenablog.com


今週の概要

　今後進めていく逆運動学の解法の手順を、理論を省きながら示します。（今週の進捗は少ない、というかほぼ無いです。）
　理論をまとめるとなると結構な量になってしまうので省略します（進捗記録ですし）。
　今回以降の話は、主な参考文献として、『ヒューマノイドロボット 改訂２版』(編著：梶田 秀司)(参考文献[1]としている)を参考に進めていきます。必要な理論も一通り網羅されているので、ヒューマノイドを勉強するうえで重宝しています。

今週の進捗

逆運動学を解く手順

　今回考えている逆運動学を数値的に解く手順を、以下に示す。なお、参考文献[1]に記されている方法と同様であり、それに沿って進めていこうと思う。

1. 注目するリンクの、目標とするリンクの位置と姿勢を用意する。
2. 胴から注目リンクに存在する関節の角度を得、これを並べてベクトルとする。
3. 順運動学を解き、胴に設定されている座標系からの注目リンクの位置と姿勢を得る。
4. 現注目リンクと目標の位置と姿勢の誤差を得る。
5. 誤差の大きさを判定し、十分に小さければ終了。そうでなければ、これを小さくする関節角度修正量を計算して得る。
6. 得た関節角度修正量を、2. で得た関節角度を並べたベクトルに加え、3. の順運動学計算から繰り返す。

　数値解法の逆運動学を考える上での問題として、関節角度修正量をどのように求めるのかというものがある。この問題に対していくつかの方法がある（らしい）が、今回は前述したように参考文献[1]に倣って進めるので、Newton-Raphson法で計算を行い、求めることにする。]

最後に・今後の予定

　今週の進捗はこれだけになります（ごめんなさい）。今回の進捗は、今後の予定を少し明確にしました。
　先週で各関節間の位置関係と各関節の情報を得ることができているので、理論的な計算に入れる状態ではあります。
　先週示した予定を、現在の進捗を繁栄して示します。

　・ロボットが持つ各リンクを繋ぐ関節の位置座標を得る。＜第７週で達成＞
　・運動学を解くのに必要な座標変換を解く。ヤコビアン（Jacobian）を得る。
　・理論に基づいて、数値解法による逆運動学を解く。
　・解いた結果を元に、ロボットの姿勢を制御してみる。
　・解いた逆運動学の解を元に、その場足踏みを達成する。

　これを踏まえて、来週には少しでも理論に沿った進捗が示せればと思います（他の事で忙しいので来週も薄味になりそうですが）。

参考文献

・[1], オーム社, 梶田秀司, "ヒューマノイドロボット（改訂２版）", 2020

[第７週] 逆運動学を解く 準備の準備編 [ヒューマノイド動歩行]

ヒューマノイドロボット動歩行プロジェクト

先週の振り返り

　先週の記事では、ROS2とWebotsの連携を、実際にプログラムを組むことで確認しました。
　これによって、ようやく開発環境が明確になりました。

今週からの概要

　今週からいよいよ、理論の話に入っていきます。最終目標である動歩行制御を達成するためのアプローチは様々ありますが、まずは運動学を解き、逆運動学に基づいてロボットの姿勢制御を行っていきます。
　現在の予定では、以下の流れで進めていきます。

　 　・ロボットが持つ各リンクを繋ぐ関節の位置座標を得る。
　・運動学を解くのに必要な座標変換を解く。ヤコビアン（Jacobian）を得る。
　・理論に基づいて、数値解法による逆運動学を解く。
　・解いた結果を元に、ロボットの姿勢を制御してみる。
　・解いた逆運動学の解を元に、その場足踏みを達成する。

　逆運動学の最終目標である『その場足踏み』は、今後の動歩行に向けたタスクとなっています。
　なお、今後進めていくにあたって、主に『ヒューマノイドロボット 改訂２版』(編著：梶田 秀司)を参考にさせていただき、進めていきます。
　また理論のまとめ方に関して、このブログは活動を記録することを主としているので具体的にまとめることはせず、必要最低限のメモに近い書き方で記していきます。

今週の進捗

各リンクを繋ぐ関節の位置

　ロボットがもつ関節の位置座標を、以下にまとめる。
　今回のProjectで用いているロボット、『ROBOTIS OP2』。このロボットはオープンソースとなっているが、関節間の距離などの詳細な寸法は記録されていなかった。
　そこで、Webotsのソースコード内に存在するDARwIn-OP（ROBOTIS OPと同型のロボット）の.protoファイル（ソースコード: GitHubリンク）（シミュレーションモデルを記述したテキストファイル）に記載されている関節（HingeJoint）の位置座標を参考にする。

　以下に、ロボットの各関節のステータスを示す。
　なお、各関節の親子関係を元に階層的に記している。例えば、PelvYLはPlevLの親であり、LegUpperLはPlevLの子である。

<記載情報説明＿関節名: DEF名, 座標系(正面-右-上), 位置座標(親を座標中心とした位置座標), ソースコードでの行番目>
Robot
- Neck: 19: DEF DNeck, x-y-z, axis (x0, y0, z1), anchor (x0, y-0, z0.051), line-213
- - Head: 20: DEF DHead, x-y-z, axis (x-1, y0, z0), anchor ( = Neck), line-261
- PelvYL: 8: DEF DPelvYL, x-y-z, axis (x0, y0, z-1), anchor (x-0.005, y0.037, z-0.1222), line-454
- - PlevL: 10: DEF DPelvL, z-x-y, axis (x0, y0, z-1), anchor ( = PelvYL), line-500
- - - LegUpperL: 12: DEF DLegUpperL, z-x-y, axis (x-1, y0, z0), anchor ( = PelvYL), line-548
- - - - LegLowerL: 14: DEF DLegLowerL, z-x-y, axis (x-1, y0, z0), anchor (x-0, y-0.093, z0), line-606
- - - - - AnkleL: 16: DEF DAnkleL, z-x-y, axis (x1, y0, z0), anchor (x0, y-0.093, z0), line-652
- - - - - - FootL: 18: DEF DFootL, z-x-y, axis (x0, y0, z1), anchor ( = AnkleL), line-698
- PelvYR: 7: DEF DPelvYR, x-y-z, axis (x0, y0, z-1), anchor (x-0.005, y-0.037, z-0.1222), line-899
- - PelvR: 9: DEF DPelvR, z-x-y, axis (x0, y0, z-1), anchor ( = PelvYR) line-945
- - - LegUpperR: 11: DEF DLegUpperL, z-x-y, axis (x1, y0, z0), anchor ( = PelvYR), line-989
- - - - LegLowerR: 13: DEF DLegLowerR, z-x-y, axis (x1, y0, z0), anchor (x-0, y-0.093, z0), line-1047
- - - - - AnkleR: 15: DEF DAnkleR, z-x-y, axis (x-1, y0, z0), anchor (x0, y-0.093, z0), line-1093
- - - - - - FootR: 17: DEF DFootR, z-x-y, axis (x0, y0, z1), anchor ( = AnkleR), line-1139
- ShoulderL: 2: DEF DShoulderL, x-y-z, axis (x0, y1, z0), anchor (x-0, y0.082, z0), line-1340
- - ArmUpperL: 4: DEF DArmUpperL, z-x-y, axis (x0, y0, z-1), anchor (x0, y-0.016, z0), line-1386
- - - ArmLowerL: 6: DEF DArmLowerL, z-x-y, axis (x1, y0, z0), anchor (x0, y-0.06, z0.016), line-1446
- ShoulderR: 1: DEF DShoulderL, x-y-z, axis (x0, y-1, z0), anchor (x0, y-0.082, z-0), line-1586
- - ArmUpperR: 3: DEF DArmUpperR, z-x-y, axis (x0, y0, z-1), anchor (x0, y-0.016, z0), line-1632
- - - ArmLowerR: 5: DEF DArmLowerR, z-x-y, axis (x-1, y0, z0), anchor (x0, y-0.06, z0.016), line-1692

　ロボットに搭載されている各関節の名前と位置は、公式ドキュメントに記載されている。ここから引用すると、ロボットの関節の位置は、以下のようになっている。

https://raw.githubusercontent.com/cyberbotics/webots/released/docs/guide/images/robots/robotis-op2/robotis_op2_servo_map.png
title: Position of the Motors

　また、関節と同位置に関節の角度を取得する、PositionSensorが搭載されている。運動学には直接は関係ないが、このステータスも、以下に示す。

maxForce = maxTorque: 2.5 [N*m]=[kgf*m]=[kg*(m/(s^2))*m]
acceleration: 55 [rad/(s^2)]
maxVelocity: 12.26 [rad/s]
dampingConstant: 0.002
staticFriction: 0.025 [N*m]

Position: ~ [rad]

- Neck: 19: minPosition -1.81, maxPosition 1.81, line-213
- - Head: 20: minPosition -0.36, maxPosition 0.94, line-261
- PelvYL: 8: minPosition -0.69, maxPosition 2.5, line-454
- - PlevL: 10: minPosition -1, maxPosition 0.93, line-500
- - - LegUpperL: 12: minPosition -0.5, maxPosition 1.68, line-548
- - - - LegLowerL: 14: minPosition -2.25, maxPosition 0.03, line-606
- - - - - AnkleL: 16: minPosition -1.39, maxPosition 1.22, line-652
- - - - - - FootL: 18: minPosition -1.02, maxPosition 0.6, line-698
- PelvYR: 7: minPosition -2.42, maxPosition 0.66, line-899
- - PelvR: 9: minPosition -1.01, maxPosition 1.01, line-945
- - - LegUpperR: 11: minPosition -1.77, maxPosition 0.45, line-989
- - - - LegLowerR: 13: minPosition -0.02, maxPosition 2.25, line-1047
- - - - - AnkleR: 15: minPosition -1.24, maxPosition 1.38, line-1093
- - - - - - FootR: 17: minPosition -0.68, maxPosition 1.04, line-1139
- ShoulderL: 2: minPosition -3.14, maxPosition 2.85, line-1340
- - ArmUpperL: 4: minPosition -2.25, maxPosition 0.77, line-1386
- - - ArmLowerL: 6: minPosition -1.18, maxPosition 1.63, line-1446
- ShoulderR: 1: minPosition -3.14, maxPosition 3.14, line-1586
- - ArmUpperR: 3: minPosition -0.68, maxPosition 2.3, line-1632
- - - ArmLowerR: 5: minPosition -1.65, maxPosition 2.5, line-1692

　以上のように、ロボットが持つ関節の位置を得ることができた。
　ここで示したステータスは、開発リポジトリのこの.txtファイルにの同様に記している。

最後に・今後の予定

・今回で、関節の位置座標を得ることができました。
・次は予定通り、座標変換やヤコビアンなど、徐々に運動学に入っていきたいと思います。

[第４〜６週] ROS2とWebotsの連携 実装編 [ヒューマノイド動歩行]

ヒューマノイドロボット動歩行プロジェクト

先週の振り返り

　先週の記事までは、ROS2とWebotsの連携についての調査を一通り行いました。

今週以降の概要

　調査した結果を元に、WebotsとROS2の連携を実装を進める。

連携の実装

　今週から、WebotsとROS2の連携を実装していきます。これには数週間かかるため、この記事を毎週更新することで実装の記録をつけていきます。
　先週まで調査してきたことを元に実装していき、そこで判明したことがあれば一緒に記載していきます。まとめ方としては、毎週ごとに進捗記録を列挙する形になります。
　以下に、開発リポジトリを示します。

github.com

　参考サイトは以下の２つとなっています。

github.com

github.com


実装進捗

2022/10/10

　・Wikiに記載されているコードや公式demoを参考に、以下のファイルを作成。
　　・walking_pattern_generator/include/walking_pattern_generator/WebotsRobotController.hpp
　　・walking_pattern_generator/launch/robot_launch.py
　　・walking_pattern_generator/resource/webots_robotis_op2_description.urdf
　　・walking_pattern_generator/src/WebotsRobotController.cpp
　　・walking_pattern_generator/CMakeList.txt
　　・walking_pattern_generator/package.xml
　　・walking_pattern_generator/walking_pattern_generator.xml

　・公式tutorialを参考に、worldファイル（webots_simple_world.wbt）を作成。
　　・公式tutorialの”Create the simulation world in Webots”を完了後、既存のロボットモデルを追加。今回はROBOTIS社のROBOTIS-OP2を使用。Addボタンを押した後に出るポップアップウィンドウから以下のようにして追加すれば良い。(Webots Manual, ROBOTIS' Robotis OP2)
　　　

Robotis OP2 の適用
　　・追加したnodeの設定を少しいじり、結果として以下のようなシミュレーション環境を作成。 　　　
simulation環境の様子
　・コンパイルは通ったが、CMakeにLaunchファイルのパスを書いてなかったり、ROS2の初期化コード（〜::init(...)）をどうするかいまいち分かってなかったりと、まだまだ不完全。

　・今後は、各コードの意味をより掘っていきたい。

2022/10/17

　・以下の項目を達成。
　　・ロボットのモータに、関節角度を命令し、操作。
　　・関節に搭載されているPositionSensor(関節角度を観測するセンサ)から値を取得、観測。
　　・他nodeへのPublish。

　・方法
　　・モータの操作
　　　・まず、ヘッダファイルをincludeする必要がある。#include <webots/Motor.hpp>
　　　・initの引数であるnodeからWebotsのAPIのRobotにアクセス。この返り値はモータに限らず、ロボットに搭載されている全てのモジュールにアクセスするために用いる。（型: webots::Robot）
　　　・robot->getMotor("<モータ名>")で特定のモータにアクセス。この返り値を得る。ここから、モータの回転角度や回転速度などを命令する。（型: webots::Motor）
　　　・今回はモータの回転角度を操作したいので、上記の返り値を持つ変数の要素の１つにアクセスし、回転角度[rad]を命令。<変数>->webots::Motor::setPosition(<回転角度[rad]>)。
　　　・以下、今回作成したプログラムからモータ操作に関するコードを抜粋。

// 元プログラム名: WalkingPatternGenerator.hpp
~
            webots::Robot *robot;
            webots::Motor *motor[20];
~

// 元プログラム名: WalkingPatternGenerator.cpp
~
#include  <webots/Motor.hpp>
~
        robot = node->robot();
        motor[0] = robot->getMotor("AnkleL");  // 左足首
~
        motor[0]->webots::Motor::setPosition(1);  // 1[rad]
~

　　・関節角度値の取得
　　　・今回のロボットの場合、関節の角度を測るPositionSensorはモータと同じ場所に同数取り付けられている。得られる値もモータ同様[rad]となっている。
　　　・取得するまでの手順はモータの操作とほぼ同様。値は返り値として得られるため、getValue()で値を得る際は変数に返り値を渡す必要がある。（返り値の型: double）
　　　・また、値を得る前に、値取得を有効（enable）にする必要があり、その際に値取得のサンプリング周期（単位: [ms]）を定める。
　 　　　　・注意すべきこととして、有効にして１周期後に初めて値が得られることが挙げられる。有効にしてすぐに値が得られるわけではない。
　　　・以下、関するコードを抜粋。

// 元プログラム名: WalkingPatternGenerator.hpp
~
            webots::Robot *robot;
            webots::PositionSensor *positionSensor[20];
~

// 元プログラム名: WalkingPatternGenerator.cpp
~
#include  <webots/PositionSensor.hpp>
~
        robot = node->robot();
~
        positionSensor[0] = robot->getPositionSensor("AnkleLS");  // 左足首
~
        positionSensor[0]->webots::PositionSensor::enable(100);  // sampling period: 100[ms]
~
        double hage = positionSensor[0]->webots::PositionSensor::getValue();  // 値取得
~

　　・他nodeへのPublish
　　　・他nodeへのPublishは、通常のROS2で作成するPublisherのコードを記載することで実現できる。
　　　・新しいnodeを宣言、定義したり、ROS2を初期化するコードを記述する必要はない。（Pluginだから？）
　　　　・Pythonで書かれた公式のdemoでは、nodeを新しく再生し、initで初期化もしていた。（C++でもできるのか？）
　　　　・恐らく、同様の方法でSubscribeも可能だと思われる。
　　　・今回は実験として、String型のメッセージを"test"という名前のTopicでPublishし、Subscribeしたnodeの方でそのString型のメッセージをscreenに出力した。
　　　　・Subscribeするnodeの名前は、”Test1_Sub”とした。
　　　・以下、Publishに関するコードを抜粋。Subscribeするnodeは、test1_sub.という名前を持つプログラムで定義されており、開発リポジトリで確認できる。

// 元プログラム名: WalkingPatternGenerator.hpp
~
            rclcpp::Publisher<std_msgs::msg::String>::SharedPtr __pub;
~

// 元プログラム名: WalkingPatternGenerator.cpp
~
#include  <webots/PositionSensor.hpp>
~
// (void WalkingPatternGenerator::init()内)
        __pub = node->create_publisher<std_msgs::msg::String>("test", rclcpp::QoS(10));
~
// (void WalkingPatternGenerator::step()内)
        auto datamsg = std::make_shared<std_msgs::msg::String>();
        static int count = 0;
        datamsg->data = "Hello: " + std::to_string(count++);
        __pub->publish(*datamsg);
~


　・非常に有意義な進捗結果が得られ、実装に関する知識がより明確に得られた。
　・まだいくつかやり残していることがあるため、それをこの１週間で済まし、来週には調査記録の確認を完了したい。

2022/10/24

　・以下の項目を達成
　　・ロボットに搭載されている角加速度センサ、角速度センサの値を取得、観測。
　　・webotsのSupervisorから、ロボットの位置や傾きなどを取得、観測。
　　・webotsのバージョンを2022bに、webots_ros2パッケージをv2022.1.2に更新

　・Versionの更新
　　・webotsと、ROS2連携に必要なwebots_ros2のバージョンを最新に更新した。開発中のプログラムの変更点は、worldファイル以外は特にない。worldファイルも、バージョンアップ前と生成環境は変わっていない。

　・センサから値を取得
　　・加速度センサ（Accelerometer）
　　　・ロボットの加速度[m/s2]を得る。ROBOTIS_OP2の場合、加速度を、-3g[m/s2] ~ +3g[m/s2]の範囲を0 ~ 1024の数値（const double）で表現し、それが返り値として得られる。
　　　　・ここで g は、重力加速度9.81[m/s2]を表している。
　　　　・加速度センサの返り値には、シミュレーション環境に対して垂直方向に働いている重力1g[m/s2]が含まれている。
　　・角速度センサ（Gyro）
　　　・ロボットの角速度[rad/s]を得る。ROBOTIS_OP2の場合、角速度を、-1600[deg/sec] ~ +1600[deg/sec]の範囲を0 ~ 1024の数値（const double）で表現し、それが返り値として得られる。
　　　・以下、加速度センサと角速度センサのコード例。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void ~::step()内
~
        accelerometerValue = accelerometer->webots::Accelerometer::getValues();  // 加速度センサの返り値取得。３要素の配列[x, y, z]
        gyroValue = gyro->webots::Gyro::getValues();  // 角速度センサの返り値取得。３要素の配列[x, y, z]
~


　・Supervisorの詳細
　　・Supervisorを使う事前準備
　　　・Supervisorとは、現実世界で人間がおこなうような行動、例えばロボットの位置や動作のリスタートなどをWebots上でおこなうために用いるAPIとなっている。ドキュメントはこちらとなっており、詳細はそこに記されている。
　　　・モータの操作の時と同じように、initの引数であるnodeからWebotsのAPIのRobotにアクセス。それを、webots::Supervisorの型を持つ変数（supervisor）に入れる。（モータの操作の時は、webots::Robotであった。）
　　　・supervisorからgetFromDef()関数を呼び出し、対象のロボット（正確にはノード）のDEF名を引数として渡し、返り値をwebots::Node型の変数（SupervisorNode）で受け取る。この変数から様々なsupervisorを呼び出すことになる。
　　　　・なお、DEF名は（恐らく）事前に設定する必要がある。今回はworldファイル内の一部を、以下のように記述して対応した。（なお、DEF名が存在する意味は分かっていない）

// 元プログラム名: webots_simple_world.wbt
~
DEF ROBOTIS_OP2 RobotisOp2 {  // ここで、DEF名をROBOTIS_OP2に設定

~


　　・以下、事前準備のコード。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void WalkingPattern Generator::init()内
        supervisor = Node->robot();  // Node = node
        SupervisorNode = supervisor->getFromDef("ROBOTIS_OP2");
        if (SupervisorNode == NULL) {  // ちゃんとDEF名が存在するかのチェック
            std::cout << "ERROR!: getFromDef == NULL" << std::endl;
            while(true){  // 存在しなかったら、エラーを吐いて無限ループ
            }
        }
~

　　・ロボットの位置と傾きの取得（Fieldから）
　　　・Fieldから、ロボットの位置と傾きを得る。
　　　・このFieldはロボットに固有に設定されたものであり、ロボット自身が持つ自身の情報を得ることができる。ROBOTIS_OP2のFieldはこのドキュメントに記されている。
　　　・Fieldより、ロボットの位置はtranslation、傾きはrotationから得ることができる。
　　　　・Fieldの活用は、はじめに得たいFieldを関数から指定し、その際に得られた返り値を通じて値を読み書きする。
　　　・以下、コード例。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void ~::init()内
~
        field_translation = SupervisorNode->getField("translation");   // 変数の型はwebots::Field
        field_rotation = SupervisorNode->getField("rotation");  // 引数はField名
~
// void::step()内
~
        translation = field_translation->getSFVec3f();  // グローバル座標系からロボットの位置ベクトル。3要素の行列[x, y, z]。単位[m]
        rotation = field_rotation->getSFRotation;  // 回転角度。3要素の行列[x, y, z]。単位[rad]


　　・ロボットの位置、回転行列、同次変換行列、重心位置、速度、加速度の取得
　　　・グローバル座標系からロボットのローカル座標系への位置ベクトル、回転行列、同次変換行列、重心位置、そしてロボット自身が持つ速度と角速度を得る。
　　　・この場合の位置ベクトルは、Fieldから得たロボットの位置ベクトルと同値であった。
　　　・同次変換行列は、位置ベクトルと回転行列を1つの行列として表したものであり、その要素は別で得られる位置ベクトルや回転行列と同値であった。つまり、同時変換行列の特定の要素を抜き出せば、位置ベクトルと回転行列が得られる。
　　　・以下、コード例。

// 元プログラム: WalkingPatternGenerator.cpp
~
// void ~::step()内
~
        position = SupervisorNode->getPosition();  // 位置ベクトル[x[m], y[m], z[m]]。translationと同値
        orientation = SupervisorNode->getOrientation();  // 回転行列[[x,y,z], [x,y,z], [x,y,z]]
        pose = SupervisorNode->getPose();  // 同次変換行列[[回転行列(3*3), 位置ベクトル(3*1)], [0, 0, 0, 1]]
        centerOfMass = SupervisorNode->getCenterOfMass();  // ロボットの重心位置[x, y, z]。単位[m]
        velocity = SupervisorNode->getVelocity();  // ロボットの速度[m/s]、角速度[rad/s]。各3要素、1行6要素の配列。
~


　・以上で、制御に活用できる今考えられる情報を、ほとんど得ることができた。
　　・接触点を返すgetContactPoint()も利用したかったが、まだうまく使えていない。制御に必須でもない。
　・これで、今できる連携方法の確認は完了したと判断する。

最後に・今後の予定

・３週に渡って、WebotsとROS2の連携を、実際にプログラムを組むことで確認することができました。
・来週からはようやく、本質である制御の理論に挑戦していきたいと思います。
・開発のソースコードは、こちらから確認できます。

参考サイト

・Open Robotics, ROS 2 Documentation: Foxy
・cyberbotics, Webots公式サイト
・cyberbotics, Webots Reference Manual
・cyberbotics, Webots User Guide
・cyberbotics, Webots, GitHub
・cyberbotics, webots_ros2, GitHub
・cyberbotics, webots_ros2 Wiki, GitHub
・@Nek, ROS2導入＆レクチャー, Qiita
　↑ 特にコードの記述の参考にさせていただいたサイト様
・開発リポジトリ, open-rdc, ROS2_Walking_Pattern_Generator, GitHub

[第３週] ROS2とWebotsの連携 調査編２ [ヒューマノイド動歩行]

ヒューマノイドロボット動歩行プロジェクト

先週の振り返り

　先週の記事では、WebotsとROS2の連携方法と、その連携に関する調査を開始したところでした。

今週の概要

　先週に引き続き、WebotsとROS2に関する調査の報告
＜追記＞ ・2022/10/6: URDFの記述に関する説明の修正。

連携に関する調査

　先週に引き続き、WebotsとROS2の連携に関して調査を進めていきます。この２つの連携は、Webots公式から配布されているパッケージを使うことで実現できます。demoもあるため実装後すぐに連携を体験できます。
　しかし自分自身でWebotsと連携するROS2パッケージを作成するとなると、その連携の仕組みを知る必要があります。そのために必要な情報を調査し、まとめていくことを先週から行っています。今回はその現状報告となります。
　なお今回の情報源は、webots_ros2のWikiとWebots User Guide、そしてWebots Reference Manualです。ここを参照すれば十分なのですが、ここでは今回の開発で必要な箇所のみに絞って調査しています。（調査の備忘録）

　調査対象は、大きく分けて以下の３つです。
　・Webots上に既に存在するロボットをROS2で認識する方法
　・Webots上のロボットをROS2から制御する方法
　・上記２つのことをするために必要なヘッダと関数（C++）

注意！：この記録が正しいとは限りません。間違いを見つけ次第できる限り 修正していきます。

1.WebotsにあるロボットをROS2で認識する方法

・Webotsに既にあるロボットをROS2で認識する場合、URDFに説明を記述する必要があります。以下、公式のdemoから引用です。

webots_ros2/webots_ros2_universal_robot/resource/webots_abb_description.urdf

<?xml version="1.0" ?>
<robot name="ABB Webots">
    <webots>
        <plugin type="webots_ros2_control::Ros2Control" />
    </webots>

    <ros2_control name="WebotsControl" type="system">
        <hardware>
            <plugin>webots_ros2_control::Ros2ControlSystem</plugin>
        </hardware>

        <!-- ABB -->
        <joint name="A motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="B motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="C motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="D motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="E motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="F motor">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>

        <!-- ROBOTIQ 3F Gripper -->
        <joint name="palm_finger_1_joint">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_1_joint_1">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_1_joint_2">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_1_joint_3">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="palm_finger_2_joint">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_2_joint_1">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_2_joint_2">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_2_joint_3">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_middle_joint_1">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_middle_joint_2">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>
        <joint name="finger_middle_joint_3">
            <state_interface name="position"/>
            <command_interface name="position"/>
        </joint>

    </ros2_control>
</robot>

引用元：GitHub, cyberbotics, webots_ros2/webots_ros2_universal_robot/resource/webots_abb_description.urdf (観覧日: 2022/10/4)

github.com

　・~はじめ２つのpluginは、Webotsと連携させるために必要なものとなっています。~
　　・webots_ros2_control::Ros2Control：ros2_controlの実装。タグ内に記述（追記2022/10/9: コレがなくても、対応するような独自プラグインを作成すれば連携は可能。公式demo,mavicを参照）
　　　・タグ：webots関連の構成全体がこのタグ下で宣言される。
　　・webots_ros2_control::Ros2ControlSystem：ros2_controlの使用に必要
　　　・joint_nameは元のロボットモデルに合わせる必要がある。モデルのjoint名は、既存のものであれば公式からドキュメントが公開されているため、そちらを見れば良いだろう。以下、例で用いているロボットのドキュメントのリンク。

cyberbotics.com

cyberbotics.com

　・なお、Webots上のロボットに搭載されているセンサもURDFに説明を記述してやることで扱えるようになります。以下は、URDFの記述方法を記した公式Wikiのページと、例として参考になる公式demoのコードへのリンクです。

github.com

github.com

　・上記以外にも、独自のプラグインを適用することもできます。このプラグインはROS2からの制御に使うことができます。以下、例として公式demoからの引用です。以下のようにプラグイン名（プラグイン記述プログラムとしてはclass名）を記述する必要があります。

webots_ros2/webots_ros2_tesla/resource/tesla_webots.urdf

<?xml version="1.0" ?>
<robot name="Tesla Webots">
    <webots>
        <plugin type="webots_ros2_tesla.tesla_driver.TeslaDriver" />
    </webots>
</robot>

このプラグインのソースコード：webots_ros2_tesla/tesla_driver.py 引用元：GitHub, cyberbotics, webots_ros2/webots_ros2_tesla/resource/tesla_webots.urdf (観覧日: 2022/10/4)

github.com


2.WebotsにあるロボットをROS2から制御する方法

・制御方法の１つとして、独自の制御プラグインを作成して使うことが考えられます。プラグインを適用するには、上記で示した独自プラグインの適用方法に沿う必要があります。
　・このプラグインはROS2のnode作成に加え、WebotsのAPIを使うためのclassを継承する必要があります。以下、公式demoから、プラグインとURDFのコードの引用です。

webots_ros2/webots_ros2_mavic/webots_ros2_mavic/mavic_driver.py

# Copyright 1996-2021 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""ROS2 Mavic 2 Pro driver."""

import math
import rclpy
from geometry_msgs.msg import Twist


K_VERTICAL_THRUST = 68.5    # with this thrust, the drone lifts.
K_VERTICAL_P = 3.0          # P constant of the vertical PID.
K_ROLL_P = 50.0             # P constant of the roll PID.
K_PITCH_P = 30.0            # P constant of the pitch PID.
K_YAW_P = 2.0
K_X_VELOCITY_P = 1
K_Y_VELOCITY_P = 1
K_X_VELOCITY_I = 0.01
K_Y_VELOCITY_I = 0.01
LIFT_HEIGHT = 1


def clamp(value, value_min, value_max):
    return min(max(value, value_min), value_max)


class MavicDriver:
    def init(self, webots_node, properties):
        self.__robot = webots_node.robot
        self.__timestep = int(self.__robot.getBasicTimeStep())

        # Sensors
        self.__gps = self.__robot.getDevice('gps')
        self.__gyro = self.__robot.getDevice('gyro')
        self.__imu = self.__robot.getDevice('inertial unit')

        # Propellers
        self.__propellers = [
            self.__robot.getDevice('front right propeller'),
            self.__robot.getDevice('front left propeller'),
            self.__robot.getDevice('rear right propeller'),
            self.__robot.getDevice('rear left propeller')
        ]
        for propeller in self.__propellers:
            propeller.setPosition(float('inf'))
            propeller.setVelocity(0)

        # State
        self.__target_twist = Twist()
        self.__vertical_ref = LIFT_HEIGHT
        self.__linear_x_integral = 0
        self.__linear_y_integral = 0

        # ROS interface
        rclpy.init(args=None)
        self.__node = rclpy.create_node('mavic_driver')
        self.__node.create_subscription(Twist, 'cmd_vel', self.__cmd_vel_callback, 1)

    def __cmd_vel_callback(self, twist):
        self.__target_twist = twist

    def step(self):
        rclpy.spin_once(self.__node, timeout_sec=0)

        roll_ref = 0
        pitch_ref = 0

        # Read sensors
        roll, pitch, _ = self.__imu.getRollPitchYaw()
        _, _, vertical = self.__gps.getValues()
        roll_acceleration, pitch_acceleration, twist_yaw = self.__gyro.getValues()
        velocity = self.__gps.getSpeed()
        if math.isnan(velocity):
            return

        # Allow high level control once the drone is lifted
        if vertical > 0.2:
            # Calculate velocity
            velocity_x = (pitch / (abs(roll) + abs(pitch))) * velocity
            velocity_y = - (roll / (abs(roll) + abs(pitch))) * velocity

            # High level controller (linear velocity)
            linear_y_error = self.__target_twist.linear.y - velocity_y
            linear_x_error = self.__target_twist.linear.x - velocity_x
            self.__linear_x_integral += linear_x_error
            self.__linear_y_integral += linear_y_error
            roll_ref = K_Y_VELOCITY_P * linear_y_error + K_Y_VELOCITY_I * self.__linear_y_integral
            pitch_ref = - K_X_VELOCITY_P * linear_x_error - K_X_VELOCITY_I * self.__linear_x_integral
            self.__vertical_ref = clamp(
                self.__vertical_ref + self.__target_twist.linear.z * (self.__timestep / 1000),
                max(vertical - 0.5, LIFT_HEIGHT),
                vertical + 0.5
            )
        vertical_input = K_VERTICAL_P * (self.__vertical_ref - vertical)

        # Low level controller (roll, pitch, yaw)
        yaw_ref = self.__target_twist.angular.z

        roll_input = K_ROLL_P * clamp(roll, -1, 1) + roll_acceleration + roll_ref
        pitch_input = K_PITCH_P * clamp(pitch, -1, 1) + pitch_acceleration + pitch_ref
        yaw_input = K_YAW_P * (yaw_ref - twist_yaw)

        m1 = K_VERTICAL_THRUST + vertical_input + yaw_input + pitch_input + roll_input
        m2 = K_VERTICAL_THRUST + vertical_input - yaw_input + pitch_input - roll_input
        m3 = K_VERTICAL_THRUST + vertical_input - yaw_input - pitch_input + roll_input
        m4 = K_VERTICAL_THRUST + vertical_input + yaw_input - pitch_input - roll_input

        # Apply control
        self.__propellers[0].setVelocity(-m1)
        self.__propellers[1].setVelocity(m2)
        self.__propellers[2].setVelocity(m3)
        self.__propellers[3].setVelocity(-m4)

引用元：GitHub, cyberbotics, webots_ros2/webots_ros2_mavic/webots_ros2_mavic/mavic_driver.py (観覧日: 2022/10/4)

webots_ros2/webots_ros2_mavic/resource/mavic_webots.urdf

<?xml version="1.0" ?>
<robot name="Mavic Webots">
    <webots>
        <device reference="gps" type="GPS">
            <ros>
                <enabled>true</enabled>
                <alwaysOn>true</alwaysOn>
            </ros>
        </device>

        <plugin type="webots_ros2_driver::Ros2IMU">
            <enabled>true</enabled>
            <topicName>/imu</topicName>
            <alwaysOn>true</alwaysOn>
            <frameName>imu_link</frameName>
            <inertialUnitName>inertial unit</inertialUnitName>
            <gyroName>gyro</gyroName>
        </plugin>

        <plugin type="webots_ros2_mavic.mavic_driver.MavicDriver" />
    </webots>
</robot>

引用元：GitHub, cyberbotics, webots_ros2/webots_ros2_mavic/resource/mavic_webots.urdf (観覧日: 2022/10/4)

github.com

　・このプログラムは、ドローンのDJI' Mavic 2 PROWebotsドキュメントを制御するプログラムである。classのMavicDriverがプラグインとしてURDFに登録する必要がある。

　・結構長く書かれているが、重要なのはinit関数とstep関数、そしてこのclassが継承をしているということである。
　　・classの継承：WebotsのAPIを使うために、WebotsNodeを継承する必要がある。これはwebots_ros2/webots_ros2_driverから提供されており、継承元classのコードもそこから参照できる。
　　・init関数：WebotsNodeで定義されている関数をオーバーライド。ROS2のnodeの定義やWebotsのAPIを使うための処理、各変数の宣言や定義がされている。webots_node.robotから、Webots上のロボットにアクセスできる。これはWebotsのAPIの１つであるRobot（公式ドキュメント）に対応している。ここを通じてロボットのセンサデータを得たりデバイスの操作が可能となる。そのデバイスもURDFで記述しておく必要がある。
　　・step関数：WebotsNodeで定義されている関数をオーバーライド。この関数内に書かれたコードを繰り返し実行する。

　・１つの疑問として、プラグインには、nodeを直接実行するコード（main関数）は記述されていない。そのため、プラグインが実行されるタイミングがわからない。ただ予想として、Launchファイルが実行されてWebotsが完全に立ち上がった最後に実行されていることが考えられる。

　以上のような構成となっています。この他にもLaunchファイルやPythonであればsetup.pyを記述する必要があります。
　以上のこれらのことは公式Wikiにも書かれています。以下、そのドキュメントです。

github.com

github.com


3.必要なヘッダと関数（C++）

・以上まで、制御するために必要なものを特定しました。しかし公式demoはPythonで書かれており、今回使うC++とは大きく記述が異なります。ここでは、C++で記述する場合のコードを調査します。

　・C++の場合であっても必要なことや処理の流れは変わらず、WebotsNodeクラスを継承する必要がある。異なるのは記述と、ビルドの設定を記述するファイルがsetup.pyではなくCMakeFileであること、そしてプラグインの記述に、classを宣言するプログラム（ヘッダファイル：.hpp）とclassを定義するプログラム（.cpp）が必要であること。
　　・このclassの書き方はROS2で行われるものと同様。ヘッダファイルで宣言されるclassが継承するclassを、webots_ros2_driver::PluginInterfaceとすれば良い。このPluginInterfaceはWebotsNodeも含んでいるため、これを継承することでWebotsのAPIを扱うことができる。

　C++の記述のテンプレートやCMakeFileの記述例は、公式Wikiの方に書かれています。そちらを参考にすれば良いと考えています。

github.com


　今回の調査では、以上のことまで把握することができました。これらすべては、Webots公式の方から公開されているドキュメントを参考にしています。そちらも参照してください。（リンクは参考文献に記載）


終わりに・今後の予定

　前回から今回に渡って、ROS2とWebotsの連携について、ドキュメントを元に調査をしてきました。次回からはこの調査を元に、実際にプログラムを書きながらロボットを動かすことに挑戦しようと考えています。

　ここまで読んでいただき、ありがとうございました。また来週も、よろしくお願いします。

参考文献

・Webots User Guide
・Webots Reference Manual
・Home · cyberbotics/webots_ros2 Wiki · GitHub （最終観覧日：2022/10/4）

[第２週] ROS2とWebotsの連携 調査編１ [ヒューマノイドロボット動歩行]

ヒューマノイドロボット動歩行プロジェクト

先週の振り返り

　先週の記事では、環境の構築を行いました。Ubuntu20.04上でROS2とWebotsを使っていくことを示しました。

今週の概要

　・ROS2とWebotsの連携に関する調査

ROS2とWebotsの連携


連携について

　シミュレータであるWebotsは、公式にROS2との連携がサポートされています。Webotsに適用されたセンサを含んだロボットをROS2の通信から制御できたり、ROS2からURDFを用いてロボットを適用することもできます。しかしROS2との連携を主としたシミュレータではないため、Gazeboと比較して少し手間があり勝手は違います。公式から充実したチュートリアルやドキュメントが提供されており、そこを理解できれば十分に連携させることができるでしょう。（Webots公式ドキュメント, ROS2ドキュメント）今回のプロジェクトでも、この公式のドキュメントを主に扱っていきます。

連携方法

　Webotsの公式GitHubから、ROS2と連携させるためのパッケージが提供されています。これをcloneして使用しても良いのですが、今回は公式でも示されている、aptを用いたインストールを行います。以下のコマンドでインストールすることができます。こことは異なりますが、ROS2のドキュメントではdriverのみをインストールしています。

sudo apt install ros-foxy-webots-ros2

　動作確認には、付属のサンプルを実行することが有効です。サンプルはいくつかありますが、以下の例では、２つのマニピュレータロボットが動作する様子が確認できます。

ros2 launch webots_ros2_universal_robot multirobot_launch.py

実行結果 (universal_robot multirobot_launch.py sample)

今後に向けての調査

　以上までで連携の環境を構築してきました。しかしながら今後実際にプログラムを書いていく上で、どのようにWebotsとROS2は連携しているのかという仕組みと、C++でプログラミングしていく上で連携のために用いるライブラリなどはどうなっているのかを把握する必要があります。
　ここから数週に渡って、そのような必要知識を得るための調査を行います。具体的には、既に示したWebots公式ドキュメントや連携のパッケージに含まれるサンプルを紐解いて、必要な知識を得ていこうというものです。以下、現在までに得られた調査の結果を簡単に示します。ただこれは調査途中のものであり、勘違いで間違ったことを記載している可能性が十分にあります。ご了承ください。

・ロボットのモデルについて
　　Webotsに適用するロボットのモデルはprotoファイルで定義されており、protoファイルにはパラメータなどロボットが持つ要素がテキストで記載されています。ROS2との連携においてWebotsに既に適用されているロボットを用いる場合、URDFファイルには対象のロボットの形状やパラメータなどは記載せず、protoファイルを参照し、URDFにプラグインを適用後、jointやsensorをprotoファイルと対応付ければ良さそうです。（参考コード）ただ、他のファイルが多く存在しもう一つのロボット (ur5e) では記述が異なるため、要調査です。

・C++でのライブラリについて
　これは公式ドキュメントのこちらを見れば把握できると考えられます。まだちゃんと読んでいないので、来週までには理解したいところです。

　今週の調査結果はここまでになります。まだ公式ドキュメントを全ては読めていないので、来週までには済ませておきたいところです。

終わりに・今後の予定

　今回は、ROS2とWebotsを連携させる方法と、連携に関する多少の調査結果を示しました。
　来週以降は連携に関する調査を継続し、より深く理解していきたいと考えています。まだまだ勉強を始めたばかりですので、もし知見を持っている方がいらっしゃったらアドバイスをいただけると幸いです。現状、開発はROS2とWebotsの連携を考えて進めていますが、調査の状況や自身の技術力のよっては、変更する可能性があります。このことを最後に示して、今回の記事を終わりたいと思います。

　最後まで読んでいただき、ありがとうございました。また次週も読んでいただけると幸いです。

参考サイトリンク
・Open Robotics, ROS 2 Documentation: foxy, "Setting up a robot simulation (Webots)"
・cyberrobotics, GitHub, "webots_ros2"
・上記のWiki (記事中で公式ドキュメントとして紹介)

[第１週] 開発環境を構築する [ヒューマノイドロボット動歩行]

ヒューマノイドロボット動歩行プロジェクト

先週の振り返り

　先週は、どのようなことをやっていくかを示しました。概要としては、ヒューマノイドロボットをシミュレータ上で安定した動歩行をさせたい、それを実現する制御器を開発していきます。

今週の概要

・環境構築
　　開発していく環境と、その構築の方法を示します。

環境構築

環境の説明

　今回の開発は、ROS2と呼ばれる、ロボット開発によく用いられるOSSのフレームワークを使っていきます。そして開発したソフトウェアの効果を評価するために、Webotsと呼ばれるシミュレータをROS2と連携させて使います。 　以上のことを踏まえて、以下の環境で開発を進めていきます。
　　・Ubuntu 20.04.5 LTS 64bit (desktop)
　　・ROS2 Foxy (desktop)
　　・Webots 2022b (debパッケージでinstall)
　　　（10/31更新: 2022a -> 2022b）

環境構築方法

　基本的に公式サイトのドキュメントに方法は記載されているので、ここではそこへのリンクと全体の流れをサラッと示します。

１．Ubuntu 20.04.5 LTS環境の構築
　現在（2022/9/19）では１つ古いバージョンの20.04ですが、まだ公式サイトのこちらで配布されています。複数の形式で配布されていますが、server版ではなくdesktop版を選びます。intelやamd製の64bitCPUを積んでいるPCであれば、ubuntu-20.04.5-desktop-amd64.isoを選べば問題ないと思われます。このisoファイルでliveUSBやliveDVDを作り、起動し、インストール...という手順になります。

２．ROS2 Foxy環境の構築
　こちらのドキュメントのInstallationに沿えばインストールできます。こちらもserver版とdesktop版がありますが、私はdesktop版を入れました。

３．Webots 2022b環境の構築
　こちらの公式からwebots_2022b_amd64.debをダウンロードし、公式ドキュメントのこちらに沿ってインストールすればOKです。ドキュメントは現在（2022/9/19）最新バージョンの2022bに沿って記されていますが、同様の方法でいけます。一応、コマンドも以下に示します。

sudo apt install ./webots_2022b_amd64.deb


４．Webots ROS2連携パッケージのインストール 　WebotsとROS2の連携は、公式にサポートされており、どちらからもチュートリアルが公開されています。この連携にはWebotsの公式が公開しているパッケージが必要になるので、それをインストールします。これもこちらの公式Wiki通りに進めれば大丈夫です。インストールのコマンド例は以下のようになります。

sudo apt install ros-foxy-webots-ros2


終わりに・今後の予定

　以上で、開発の環境が整いました。
　来週からはサンプルコードなどからWebotsとROS2の連携方法を具体的に調査し、まとめていこうと思います。実は既に進めていまして、こちらの備忘録のほうに軽く記録しています。できる限り早く調査を終えて、プログラムを書き始めたいところです。

　今週は以上です。ありがとうございました。

・参考サイトリンク
・Ubuntu公式
・Ubuntu release公式(old packageの配布)
・公式ROS2 Foxy Documentation
・Webots公式
・Webots_ros2(公式連携package)