先週の振り返り

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

odome.hatenablog.com

今回の概要

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

進捗

2022/11/14

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

odome.hatenablog.com

github.com

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

github.com

2022/11/21

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

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』から影響を受けているとしている。ここで示したものと同様のメモをこちらに示している。
　現在、運動学を解くプログラムを試作中ではあるが、これらのようなライブラリを用いることも検討中である。
ロボットの全関節の角度０[rad]を調べた
・タイトル通り、全関節の角度を０[rad]にして、立たせてみた。結果、以下の画像のようになった。

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

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

2022/11/28

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

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

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

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

github.com

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ファイル名)

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

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

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

github.com

2022/12/5

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

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

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

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

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

2022/12/29

続・自作プログラムの試作
・腕の逆運動学を解析的に解こうとしていたが、腕にオフセットがあることから上手く導出することができなかった。そのため、脚の逆運動学を解くプログラムを自作した。
　・詳細は以下の記事でまとめている。
続・pinocchioを用いたプログラムの試作
・進捗なし。脚を解析的に逆運動学を解けたので、それが実装できたら、pinocchioに再チャレンジする予定。

--- Close ---

参考文献

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

ロボット開発記録

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

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

先週の振り返り

今回の概要

進捗

2022/11/14

2022/11/21

2022/11/28

2022/12/5

2022/12/29

参考文献