Open mttamtam opened 7 years ago
2つめに関してです.
cp_check_margin
関連は当時矩形を前提として書いてしまったので,
矩形でしかうまく機能しないと思ます.
また,cp_check_margin
は転倒判定のために使っていて,
leg_xxx_margin
で決められた足の縁からのマージンをセットするようになっています.
なので0が入ることもあります.
なるほど、理解しました(そうすると転倒判定の範囲はけっこう狭いですね)。
ZMPDistributor.hのis_inside_support_polygonをみてみると、判定をすべてエンドエフェクタの4辺のマージンに対して行っていて、各エンドエフェクタの支持多角形を用いていない(1つめとかいたほうのやつ)ので、転倒判定がうまく動作していても支持多角形計算がどこかで正常に動作していることにはならない。
ただし、エンドエフェクタの4つのleg_xxx_margin
は撮れていて、
ゼロがひとつだけずつ入っているのはonInitializeの中での動作なので、
szd->set_vertices_from_margin_params();
がうまく動いてないんでしょうか。
https://github.com/start-jsk/rtmros_tutorials/issues/524#issue-244911669 の一つめに関して
eefm_support_polygon_vertices_sequence
に値が入っていなかったのは,
JAXONのconfig.pyでセットされていなかったからで,
https://github.com/start-jsk/rtmros_tutorials/pull/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は使っていなかったはずなので問題はなかったと思います.
2つめ is_inside_support_polygon関数はFootSupportPolygonクラスのものを使うようにすれば矩形以外でも対応できるようになると思います. (矩形を想定するにしてもコードが汚いのでいずれ書き直したいとは思ってました)
そもそも,@mttamtam さんでは,何をするために何が問題になっているか教えて頂ける助かります.
支持多角形を限定した状態でSTを入れたいので、現状のEEFMQPCOPではなくEEFMQPを用いて動作させようとしています。(こちらでもleg_xxx_marginだけつかってるんでしょうか?) ちなみに、2つめは、現在は転倒判定はあまり気にしていないのですが、コードを見ていておやとおもって質問しました。
eefm_support_polygon_vertices_sequence
が空の状態でも、要素数がエンドエフェクタの数に一致してしまっていたためにverticesがセットされてしまいゼロが入っていたので、 二回目以降にleg_xxx_marginを設定するとリセットされてしまう気がします。支持多角形を限定した状態でSTを入れたいので、現状のEEFMQPCOPではなくEEFMQPを用いて動作させようとしています。(こちらでもleg_xxx_marginだけつかってるんでしょうか?)
これは,https://github.com/fkanehiro/hrpsys-base/pull/1158 を使うということでしょうか?
であればis_inside_support_polygon
(leg_xxx_marginのみ)を使っていて,
もし矩形以外や手足を使う場合は,is_inside_support_polygon
を修正する必要があります.
必要であれば僕がやります.
525 の修正ですが、今までeefm_support_polygon_vertices_sequenceが空の状態でも、要素数がエンドエフェクタの数に一致してしまっていたためにverticesがセットされてしまいゼロが入っていたので、 二回目以降にleg_xxx_marginを設定するとリセットされてしまう気がします。
まず,https://github.com/start-jsk/rtmros_tutorials/issues/524#issue-244911669 で報告してもらったときにすべて0だった理由ですが,onInitialize内で https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/Stabilizer/Stabilizer.cpp#L486-L490 のようにlimbs数分0で初期化されていて, パラメータをセットするときにはget -> setが行われるので,https://github.com/start-jsk/rtmros_tutorials/pull/525 がない状態では0が入ってました.
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で送るしか現状ないと思います.
1つめ STでeefm_support_polygon_vertices_sequenceというのがありますが、機能していないように見えます。 多分両足両手についてのリストが、それぞれの支持多角形を表す点列がリストとして出てくるのだと思いますが、すべてゼロ点が1つずつ入っているという状態です。 このコードで確認できます。
直接指示多角形を与えなくてもleg_xxx_marginはurata_hrpsys_configで与えているので起動時に適切に設定されているはずだと思うのですが、この挙動は仕様で私の取得の仕方がおかしいのか、それともバグなのでしょうか。
2つめ cp_check_marginというのを渡していると思いますが、 urata_hrpsys_config.pyではJAXONはすべて
となっていて、(足裏形状に応じて変えるべきだというのはさておき)0があるのは不思議なのと(他のロボットや、STの初期値はそうなってないように見える)、idlをみると
となっていて、数値の順番もおかしい気がします。
あと、STで使っている足のsupport polygonは、矩形であることを前提としたコードになっている気がしますが違う形を入れても問題ないですか? @snozawa san