calcTorque()のref_moment

[Naoki-Hiraoka]

この行の右辺第二項の計算はなぜこれでよいのでしょうか?

自分のソースの読み込みが足りていないのかもしれないのですが、

• マイナスでは無くプラスではないか
• localpをfoot_origin座標系に変換する必要があるのではないか

と思いました。

hrpsys-base/rtc/AutoBalancer/Stabilizer.cpp

Line 1085 in 5aab7f9

ft.segment(3,3) = rot * (ikp.ref_moment - ikp.localp.cross(ikp.ref_force));

[YutaKojio]

今見た感じそんな気がする．(?)

@kindsenior
この変更ってどこかで議論したり，どこかのブランチを参考に僕が持ってきたものでしたっけ？
それとも口頭のみで反映したのも僕のブランチのみだったでしょうか？
それか僕が間違っていると思って勝手に直しただけかもしれません．

[YutaKojio]

そもそもCalcTorqueで与える目標レンチは最終リンクのレンチという理解であってますでしょうか?（end-effectorやCOPではなく）

[kindsenior]

自分的には
このブランチのこのコミット
https://github.com/kindsenior/hrpsys-base/blob/643fc56d2d1a39b57f00d5e38368f90f25045266/rtc/AutoBalanceStabilizer/Stabilizer.cpp#L1473
のつもりです

符号について
プラスであってる気がします

座標系について以下であってると思ってるんだけど，自分も勘違いしてるかも．．．
（以下，m_robotのroot_linkに姿勢センサ計測値を反映したものを実測世界座標系と呼ぶ）

1. https://github.com/kindsenior/hrpsys-base/blob/643fc56d2d1a39b57f00d5e38368f90f25045266/rtc/AutoBalanceStabilizer/Stabilizer.cpp#L1472
   でikp.ref_force/momentをfoot_origin座標系から実測世界座標系にする
2. localpをm_robot->link(ikp.target_name)->Rで変換してるので，r x force を実測世界座標系で計算している
3. m_robotから計算するjacobianが実測世界座標系なので正しくトルクが計算できる

実装の経緯は@YutaKojio に指摘されて， @YutaKojio のブランチの変更をcherry-pickとかできないので理解，分解，再構築したんだったと思ってるので，同じことしたかったけど実装は別です

[Naoki-Hiraoka]

ありがとうございます。
https://github.com/kindsenior/hrpsys-base/blob/643fc56d2d1a39b57f00d5e38368f90f25045266/rtc/AutoBalanceStabilizer/Stabilizer.cpp#L1473 の、符号をマイナスではなくプラスに変更して、#39 に作成しました。

(メモ・補足)
ref_momentには、この行

hrpsys-base/rtc/AutoBalancer/Stabilizer.cpp

Line 703 in a6f1a9f

ikp.ref_moment = tmp_ref_moment[idx] + ((target->R * ikp.localCOPPos + target->p) - (target->R * ikp.localp + target->p)).cross(tmp_ref_force[idx]);

でCOPまわりのモーメントからend-effectorまわりのモーメントに変換したものが入っています。ref_momentの座標系は、

hrpsys-base/rtc/AutoBalancer/Stabilizer.cpp

Line 744 in a6f1a9f

ikp.ref_moment = foot_origin_rot.transpose() * ee_R * ikp.ref_moment;

でfoot_origin系に変換されています。

• calcJacobianで出力されるヤコビアンは、end-effectorの親リンクの変位がworld座標系で表現されています。
• openhrp3をソースから入れると、第二引数にlocalpを与えることができるようになり、直接end-effectorの変位をworld座標系で表現できるようになります。https://github.com/fkanehiro/openhrp3/blob/eac8c031f5675b13e7f70faf8798a89ab230efad/hrplib/hrpModel/JointPath.h#L66
• euslispの:calc-jacobian-from-link-listはend-effectorの変位がend-effector座標系で表現されています.