STの支持多角形の扱い

[mttamtam]

１つめ
STでeefm_support_polygon_vertices_sequenceというのがありますが、機能していないように見えます。
多分両足両手についてのリストが、それぞれの支持多角形を表す点列がリストとして出てくるのだと思いますが、すべてゼロ点が１つずつ入っているという状態です。
このコードで確認できます。

(require "package://hrpsys_ros_bridge_tutorials/euslisp/jaxon_red-interface.l")
(jaxon_red-init)
(pprint (send (send *ri* :get-st-param) :slots))
(warn "~%(setq *hoge* (send (send *ri* :get-st-param) :eefm_support_polygon_vertices_sequence))~%")
(warn "(mapcar #'(lambda(lv) (mapcar #'(lambda(v) (send v :pos)) (send lv :vertices))) *hoge*)~%")

直接指示多角形を与えなくてもleg_xxx_marginはurata_hrpsys_configで与えているので起動時に適切に設定されているはずだと思うのですが、この挙動は仕様で私の取得の仕方がおかしいのか、それともバグなのでしょうか。

２つめ
cp_check_marginというのを渡していると思いますが、
urata_hrpsys_config.pyではJAXONはすべて

    stp.cp_check_margin=[0.05, 0.045, 0, 0.095]

となっていて、（足裏形状に応じて変えるべきだというのはさておき）0があるのは不思議なのと（他のロボットや、STの初期値はそうなってないように見える）、idlをみると

      /// CP margin [m] (front, rear, inside, outside)
      DblArray4 cp_check_margin;

となっていて、数値の順番もおかしい気がします。

あと、STで使っている足のsupport polygonは、矩形であることを前提としたコードになっている気がしますが違う形を入れても問題ないですか？ @snozawa san

[YutaKojio]

2つめに関してです．
cp_check_margin関連は当時矩形を前提として書いてしまったので，
矩形でしかうまく機能しないと思ます．
また，cp_check_marginは転倒判定のために使っていて，
leg_xxx_marginで決められた足の縁からのマージンをセットするようになっています．
なので0が入ることもあります．

[mttamtam]

なるほど、理解しました（そうすると転倒判定の範囲はけっこう狭いですね）。

[mttamtam]

ZMPDistributor.hのis_inside_support_polygonをみてみると、判定をすべてエンドエフェクタの４辺のマージンに対して行っていて、各エンドエフェクタの支持多角形を用いていない（１つめとかいたほうのやつ）ので、転倒判定がうまく動作していても支持多角形計算がどこかで正常に動作していることにはならない。
ただし、エンドエフェクタの４つのleg_xxx_marginは撮れていて、
ゼロがひとつだけずつ入っているのはonInitializeの中での動作なので、
szd->set_vertices_from_margin_params();がうまく動いてないんでしょうか。

[YutaKojio]

#524 (comment) の一つめに関して
eefm_support_polygon_vertices_sequenceに値が入っていなかったのは，
JAXONのconfig.pyでセットされていなかったからで，
#525 で修正しました．
ちなみに，hrpsys内の https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/Stabilizer/Stabilizer.cpp#L2156 で
leg_xxx_margin からverticesをセットするというおなじようなコードがありますが，
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/Stabilizer/Stabilizer.cpp#L2158-L2173 が下にあるので，eefm_support_polygon_vertices_sequenceのサイズおよび値が優先されるはずです．
JAXONでも今までもleg_xxx_marginは正しくセットされていて，現状の制御で使われていたものはleg_xxx_marginだけでverticesは使っていなかったはずなので問題はなかったと思います．

２つめ
is_inside_support_polygon関数はFootSupportPolygonクラスのものを使うようにすれば矩形以外でも対応できるようになると思います．
（矩形を想定するにしてもコードが汚いのでいずれ書き直したいとは思ってました）

そもそも，@mttamtam さんでは，何をするために何が問題になっているか教えて頂ける助かります．

[mttamtam]

支持多角形を限定した状態でSTを入れたいので、現状のEEFMQPCOPではなくEEFMQPを用いて動作させようとしています。（こちらでもleg_xxx_marginだけつかってるんでしょうか？）
ちなみに、２つめは、現在は転倒判定はあまり気にしていないのですが、コードを見ていておやとおもって質問しました。

#525 の修正ですが、今までeefm_support_polygon_vertices_sequenceが空の状態でも、要素数がエンドエフェクタの数に一致してしまっていたためにverticesがセットされてしまいゼロが入っていたので、 二回目以降にleg_xxx_marginを設定するとリセットされてしまう気がします。

[YutaKojio]

支持多角形を限定した状態でSTを入れたいので、現状のEEFMQPCOPではなくEEFMQPを用いて動作させようとしています。（こちらでもleg_xxx_marginだけつかってるんでしょうか？）

これは，fkanehiro/hrpsys-base#1158 を使うということでしょうか？
であればis_inside_support_polygon(leg_xxx_marginのみ)を使っていて，
もし矩形以外や手足を使う場合は，is_inside_support_polygonを修正する必要があります．
必要であれば僕がやります．

#525 の修正ですが、今までeefm_support_polygon_vertices_sequenceが空の状態でも、要素数がエンドエフェクタの数に一致してしまっていたためにverticesがセットされてしまいゼロが入っていたので、 二回目以降にleg_xxx_marginを設定するとリセットされてしまう気がします。

まず，#524 (comment) で報告してもらったときにすべて0だった理由ですが，onInitialize内で https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/Stabilizer/Stabilizer.cpp#L486-L490 のようにlimbs数分0で初期化されていて，
パラメータをセットするときにはget -> setが行われるので，#525 がない状態では０が入ってました．

set_vertices_from_margin_paramsがどういう経緯でできたのかわかりませんが，
i_stp.eefm_support_polygon_vertices_sequenceのサイズがlimbs数にあっているときは，leg_xxx_marginの値は上書かれてしまいます．
なので，leg_xxx_marginを変えることによりverticesを変えたいときには，
i_stp.eefm_support_polygon_vertices_sequence にもそれに対応するようにセットするか，
あえてi_stp.eefm_support_polygon_vertices_sequenceを間違ったlengthで送るしか現状ないと思います．