逆動力学計算する関数について

Japanese (日本語)
1

コミュニティの皆様、

当方はInverseDynamicsの計算しようとしているが、ソースコード読んでたら外力の計算が抜けている気がしますので、質問させていただきます。

この行の関節トルクを計算する前に、
f -= link → F_ext();
で外力の影響も計算に入れるべきではないでしょうか？
(梶田さんの教科書のサンプルコードにも抜けています)
よろしくお願いします。

2

ご指摘ありがとうございます。
確かに外力F_extについては反映されていないようですね。

ちなみに f+= ではなくて、f-=となるのですね。
（すみません、この部分かなり昔にコーディングしたのでほとんど忘れておりまして・・・。）

@s_kajita さん、もしよろしければこれでよいかコメントをお願いします。

3

梶田です。
オーム社「ヒューマノイドロボット」のp.210の(6.36)式と図6.18のコードですね。F_ext()がリンク重心に作用する外力の並進力とモーメントであれば、それで良い筈です

ただし、根幹に関わるので安易にコードを変更すべきではないと思います。
(1) この問題を確認できるテストコードを作成し、現在のままでは間違った答えが得られる事を示す
(2) 問題の修正を行う事で、正しい答えが得られる事を示す

wukunさんに上記を行っていただき、テストコードを提供していただくのはどうでしょうか？ ＞中岡様

4

梶田さんコメントありがとうございます。

wukunさんに上記を行っていただき、テストコードを提供していただくのはどうでしょうか？ ＞中岡様

それは大変有難いです。
wukunさん、いかがでしょうか？
（既にこの件でテストを行われているかもしれませんね。もしそうでしたら、その旨お知らせください。）

6

はい、私も作りたいと思います。
ですが、いくつか聞きたいことがあります

  1. choreonoidいままでテストはどう行ってきたでしょうか？
    リポジトリ探してみたらテストコードらしいものが見つからないです。もしございましたら教えて頂けませんでしょうか。自分のテストコードを追加してpull request出したいと思います。

  2. テストの方針ですが、いまローカルで以下の手順でテストやっています

    1. 適当なロボットモデルを使用し(例えばPA10)、BodyLoaderでbodyポインタ作成
    2. 重力の働きでロボットが落ちるようなな姿勢q設定し、dq、ddqを0と設定
    3. calcForwardKinematics(true, true)
    4. 各リンクの重心に重力加える addExternalForce(g * m_robot->joint(i)->m(), m_robot->joint(i)->c());
    5. calcInverseDynamics(m_robot->rootLink());で静止状態に保つための各軸のトルクuを計算。（現況だと外力が反映されずすべて0と計算される。）
    6. この静止状態を保つための各軸トルクを別途で計算し、uと比較。差が一定以下(1e-5とか？)ならテスト成功と判断

でいかがでしょうか。
よろしくお願いします。

7

もう一つ質問させていただきます。linkに外力を加える関数
addExternalForce(const Vector3& f_global, const Vector3& p_local)
この関数は外力の並行力成分を加えると思いますが、外力のトルク成分を印加する場合にどうすればよいのか教えて頂けませんでしょうか。

また、同じ並行力の外力を作用しても、原点周りのトルクはその時の位置姿勢T_に依存するので、
リンクが移動すると tau_ext()は再計算しないといけないと思います。現在の実装でtau_ext()の再計算は可能でしょうか？
addExternalForceをもう一回呼ぶと過去の外力を含めて外力が加算されるので所望の結果にならないです…

よろしくお願いします。

8

choreonoidいままでテストはどう行ってきたでしょうか？

すみません、今のところユニットテストのようなものはありません。

テストの方針ですが、いまローカルで以下の手順でテストやっています

この方針でよろしいのではないかと思います。
(ちなみにこの件は、元々wukunさんがやろうとされていたことがあって、そこでInverseDynamicsの改良が必要になったということだと思うので、その元々やろうとされていたことがそのままテストになるのであれば、それでよろしいのではないかと思います。）

お手数をおかけしますが、よろしくお願いいたします。

9

トルク成分については、トルクのグローバル座標での値をそのままtau_extに入れてもらえばＯＫのはずです。

また、同じ並行力の外力を作用しても、原点周りのトルクはその時の位置姿勢T_に依存するので、
リンクが移動すると tau_ext()は再計算しないといけないと思います。現在の実装でtau_ext()の再計算は可能でしょうか？

その通りです。そして、tau_extは自前で更新する必要があります。

確かにトルクがグローバル原点周りというのは使う側からすると分かりづらいですよね。ただこのやり方はロボティクスにおいては標準的なのかもしれません。
そのあたりの見解について、度々すみませんが、 @s_kajita さん、何かご存知でしたら、コメントいただけるとうれしいです。（いちおう梶田さんのコードや教科書に則っていますので・・・。）

10

ご教授あるがとうございました。リンクが移動するたびに以下のコードで原点周りの外力(重力)を更新できました。

 for (int i=0; i<m_dof; i++) {
      m_robot -> joint(i) -> tau_ext() = cnoid::Vector3d::Zero();
      m_robot -> joint(i) -> f_ext() = cnoid::Vector3d::Zero();
      m_robot -> joint(i) -> addExternalForce(g * m_robot->joint(i)->m(), m_robot->joint(i)->c());
    }

騒がしてすみませんでした。
テストコードはまた後日にお渡しします。よろしくお願いします。

11

はい、それで正しいです。
現在の動力学演算の実装では、すべての外力はそれに伴って発生する原点周りの外力トルクを計算しなくてはいけません。

tau_ext = (力の作用点の絶対位置）×（外力ベクトル）

12

お待たせしました。テストを作ってみました。

参考になると幸いです。
よろしくお願いします。

13

ありがとうございます。
すみません、いまちょっと会社の業務が立て込んでおりまして、この件の取り込みについてしばらくお待ちいただけますでしょうか。
よろしくお願いします。

14

テストプログラムありがとうございます。
こちらでも試してみました。
なるほど、このようにしてテストができるのですね。
大変参考になります。

逆運動学のコードの方も修正してコミットしておきました。

また、開発版ではテストプログラムのCMakeに失敗していたのですが、そちらも修正しました。

開発版ではQt4のサポートをやめてQt5専用としたのですが、このファイルに関してはQt4の記述が残っていて、それでQt5の方が検出できなくなっていました。

この度はバグと修正方法のご指摘誠にありがとうございました。