CollisionDetectorで干渉から抜ける時に両足裏がずれる

[mmurooka]

CollisionDetectorで
例えば腕と腰の間で干渉する姿勢が送られて動作が止まっている最中に
干渉しない姿勢が送られてきて動く時に，
両足裏が同一平面上になくなるように補間されてしまい，
転んでしまいます．

[k-okada]

• 腕と腰，という例はなにか意味が有るものでしょうか？右上と左手が干渉する，，，となっていても同じ現象でしょうか
• 動作が止まっている姿勢はどういうもので，干渉しない姿勢はどういうものでしょうか？ 人間の写真でもいいので．

◉ Kei Okada

2015-05-13 11:43 GMT+09:00 Masaki Murooka notifications@github.com:

CollisionDetectorで
例えば腕と腰の間で干渉する姿勢が送られて動作が止まっている最中に
干渉しない姿勢が送られてきて動く時に，
両足裏が同一平面上になくなるように補間されてしまい，
転んでしまいます．

—
Reply to this email directly or view it on GitHub
#626.

[mmurooka]

• 腕と腰，という例はなにか意味が有るものでしょうか？右上と左手が干渉する，，，となっていても同じ現象でしょうか

腕と腰の例でしか実際に起きたことはないですが，
どこでも（右手と左手でも）同じ現象が起きる気がします．
腕と腰の例はHRP2とJAXONの両方で起きています．

• 動作が止まっている姿勢はどういうもので，干渉しない姿勢はどういうものでしょうか？ 人間の写真でもいいので．

添付画像のような雰囲気の2つの姿勢（一方はcollisionあり，もう一方はcollisionなし）の間の遷移でおきます．

(send *robot* :angle-vector #f(0.062043 5.70596 -23.1129 50.2447 -27.1318 -5.70586 0.0 0.062075 5.67029 -21.516 47.0273 -25.5114 -5.67029 0.0 0.0 0.0 0.0 40.0 30.9668 -9.3616 38.7669 -117.817 16.7411 -34.9168 -0.463209 60.0 50.0 30.0 10.0 -120.0 25.0 5.0 -20.0 -60.0))

(send *robot* :angle-vector #f(0.0 0.0 -26.0 50.0 -24.0 0.0 0.0 0.0 0.0 -26.0 50.0 -24.0 0.0 0.0 0.0 0.0 0.0 0.0 10.0 -10.0 0.0 -25.0 0.0 0.0 -10.0 15.0 10.0 10.0 0.0 -25.0 0.0 0.0 -10.0 -15.0)) ;; reset-manip-pose

[k-okada]

足は絶対にcollisionしない，というasumptionは危険かな．
つまりどのような状態でも足は動かす．と．
collisionしてもどうしようもない，という話もあるので．

後は，リカバーの時に，関節角度空間補間ではなくて，手先位置姿勢空間補間する，というのも有る

◉ Kei Okada

2015-05-13 13:16 GMT+09:00 Masaki Murooka notifications@github.com:

• 腕と腰，という例はなにか意味が有るものでしょうか？右上と左手が干渉する，，，となっていても同じ現象でしょうか

  腕と腰の例でしか実際に起きたことはないですが，
  どこでも（右手と左手でも）同じ現象が起きる気がします．
  腕と腰の例はHRP2とJAXONの両方で起きています．

• 動作が止まっている姿勢はどういうもので，干渉しない姿勢はどういうものでしょうか？ 人間の写真でもいいので．

  添付画像のような雰囲気の2つの姿勢（一方はcollisionあり，もう一方はcollisionなし）の間の遷移でおきます．

(send robot :angle-vector #f(0.062043 5.70596 -23.1129 50.2447 -27.1318 -5.70586 0.0 0.062075 5.67029 -21.516 47.0273 -25.5114 -5.67029 0.0 0.0 0.0 0.0 40.0 30.9668 -9.3616 38.7669 -117.817 16.7411 -34.9168 -0.463209 60.0 50.0 30.0 10.0 -120.0 25.0 5.0 -20.0 -60.0))

[image: screenshot_from_2015-05-12 21 13 23]
https://cloud.githubusercontent.com/assets/6636600/7603657/005052b4-f8ec-11e4-8760-8027299e9359.png

(send robot :angle-vector #f(0.0 0.0 -26.0 50.0 -24.0 0.0 0.0 0.0 0.0 -26.0 50.0 -24.0 0.0 0.0 0.0 0.0 0.0 0.0 10.0 -10.0 0.0 -25.0 0.0 0.0 -10.0 15.0 10.0 10.0 0.0 -25.0 0.0 0.0 -10.0 -15.0)) ;; reset-manip-pose

[image: screenshot_from_2015-05-12 21 13 37]
https://cloud.githubusercontent.com/assets/6636600/7603659/01eed3d4-f8ec-11e4-921b-9a41acad720f.png

—
Reply to this email directly or view it on GitHub
#626 (comment)
.

[mmurooka]

つまりどのような状態でも足は動かす．と．

@garaemon も同じ案を提案していました．

[k-okada]

不用意にreset-poseするとハンドがメキメキガッシャンにあるけど，大丈夫かな．大丈夫であればそうしますが．

◉ Kei Okada

On Wed, May 13, 2015 at 1:25 PM, Masaki Murooka notifications@github.com
wrote:

つまりどのような状態でも足は動かす．と．

@garaemon https://github.com/garaemon も同じ案を提案していました．

—
Reply to this email directly or view it on GitHub
#626 (comment)
.

[k-okada]

#627
で作ってみました．confファイルに0/1を書いて下さい．
idlが欲しいとか要望があればおしえてください．

前のバージョンだと腕がぶつ使っても足は動いている，みたいにしていたけどどうしたんだっけな．
というので，先人は結局どうしていたんだろう？足は外して，かならずreset-manip-poseしていたのかな．フィードバックがないからわからんですねぇ．

[snozawa]

前のバージョンだと腕がぶつ使っても足は動いている，みたいにしていたけどどうしたんだっけな．

内部のcollision detectorでは，maskでなくてlimbごとに該当部位のみ止めるというのをやってた気がします．
なので今回は腕と腰の関節のみ止まり，脚はそのまま動きます．

[eisoku9618]

start-jsk/rtmros_tutorials#273 でconfに書きました．

[mmurooka]

先にhrpsys-simulatorで確認しておこうと思って，
変更していただいた #627 を反映していない状態で

上述の2つの関節角を送りましたが，
足裏の水平が保たれなくなるようなことは起きませんでした．
次は実機で確認してみます．もしかしたらstなどが関係しているかも．

[snozawa]

#627ありがとうございました．

#626 (comment)
と並行して，シミュレータ上でhttps://github.com/fkanehiro/hrpsys-base/pull/627が動くのを確認しつつ，
サンプルにテストを追加しました．

ところでこの次の一手としては，
ペアごとに止めるかどうかの有無を決めると良いと思いました．
理由は

• collision maskだと，足をmask 0にしたら足同士の干渉がチェックされないので，チェックするようにしたい
• maskがnumJointsの数ですが，必要なのはlinkの数で，具体的には今はWAIST部分対象外になってる

ためです．

あるとよいものは具体的には，collision_pairの仕様をかえて今までは（例えばの例です）

collision_pair: RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R LLEG_HIP_R:RLEG_HIP_R

のようにしてところを，

collision_pair: RARM_WRIST_P:RLEG_HIP_R:1:0 LARM_WRIST_P:LLEG_HIP_R:1:0 LLEG_HIP_R:RLEG_HIP_R:1:1

としておくというものです．
これでペアごとに決められるように思います．

collision_pairの書き方としては，

軸１：軸２：軸１の動かす動かさないのフラグ：軸２の動かす動かさないのフラグ

もしくは

軸１：軸２

として，フラグをオプショナルにしておいて指定ない場合はフラグ＝１とすると，後方互換性もありそうです．

[k-okada]

maskでも干渉そのものはチェックされると思いますが、どうでしょうか？

またペア毎だと同じリンクが含まれる2つのペアが1/0の時にand? or?の問題がでませんか？

◉ Kei Okada

2015/05/15 9:17、Shunichi Nozawa notifications@github.com のメッセージ:

#627ありがとうございました．

#626 (comment)
と並行して，シミュレータ上でhttps://github.com/fkanehiro/hrpsys-base/pull/627が動くのを確認しつつ，
サンプルにテストを追加しました．

ところでこの次の一手としては，
ペアごとに止めるかどうかの有無を決めると良いと思いました．
理由は

collision maskだと，足をmask 0にしたら足同士の干渉がチェックされないので，チェックするようにしたい
maskがnumJointsの数ですが，必要なのはlinkの数で，具体的には今はWAIST部分対象外になってる
ためです．

あるとよいものは具体的には，collision_pairの仕様をかえて今までは（例えばの例です）

collision_pair: RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R LLEG_HIP_R:RLEG_HIP_R
のようにしてところを，

collision_pair: RARM_WRIST_P:RLEG_HIP_R:1:0 LARM_WRIST_P:LLEG_HIP_R:1:0 LLEG_HIP_R:RLEG_HIP_R:1:1
としておくというものです．
これでペアごとに決められるように思います．

collision_pairの書き方としては，

軸１：軸２：軸１の動かす動かさないのフラグ：軸２の動かす動かさないのフラグ
もしくは

軸１：軸２
として，フラグをオプショナルにしておいて指定ない場合はフラグ＝１とすると，後方互換性もありそうです．

—
Reply to this email directly or view it on GitHub.

[snozawa]

（先ほどの例では）足同士の干渉はチェックされますが，関節上書きがされないので足同士で安全に停止してくれない，ということでした．
（もしかして停止しますでしょうか？）

and/orは，orで良いのではないでしょうか．
具体的には

• ひとつでも自分（関節）のフラグ＝１のペアが干渉してれば，自分は動作停止する

がよさそうということです．

例えば

RLEG_LINK1:RARM_LINK1:0:1

（以下ペア１）
と

RLEG_LINK1:LLEG_LINK1:1:1

（以下ペア２）
とがあって，

• ペア１で干渉したら腕だけとまってほしい．脚が止まってコケないように．
• ペア２で干渉したらどっちの足もとまってほしい．脚が止まってコケても良いので，干渉回避したい

としたいとします．
このときペア１，ペア２も干渉になった瞬間に，腕・脚がそれぞれ止まって欲しいかどうかを考えます．
そうすると，

• 腕はとまるのでOK
• 足はペア１ではコケたくないからとまるな，ペア２ではコケてもよいのでとまれ，となってますが，脚同士の干渉回避を回避するにはコケるのを角度で止めるしかないので，コケてもよいのでとまれとなると思います．

ということで，この例を考えるとor（一個でも動作停止しろといわれたら，停止する）が良さそうにおもいますが，いかがでしょうか．

[k-okada]

なるほど．
RLEG_LINK1:RARM_LINK5
とかあったらどうなるかな．

◉ Kei Okada

2015-05-15 10:23 GMT+09:00 Shunichi Nozawa notifications@github.com:

（先ほどの例では）足同士の干渉はチェックされますが，関節上書きがされないので足同士で安全に停止してくれない，ということでした．
（もしかして停止しますでしょうか？）

and/orは，orで良いのではないでしょうか．
具体的には

• ひとつでも自分（関節）のフラグ＝１のペアが干渉してれば，自分は動作停止する

がよさそうということです．

例えば

RLEG_LINK1:RARM_LINK1:0:1

（以下ペア１）
と

RLEG_LINK1:LLEG_LINK1:1:1

（以下ペア２）
とがあって，

• ペア１で干渉したら腕だけとまってほしい．脚が止まってコケないように．
• ペア２で干渉したらどっちの足もとまってほしい．脚が止まってコケても良いので，干渉回避したい

としたいとします．
このときペア１，ペア２も干渉になった瞬間に，腕・脚がそれぞれ止まって欲しいかどうかを考えます．
そうすると，

• 腕はとまるのでOK

  足はペア１ではコケたくないからとまるな，ペア２ではコケてもよいのでとまれ，となってますが，脚同士の干渉回避を回避するにはコケるのを角度で止めるしかないので，コケてもよいのでとまれとなると思います．

ということで，この例を考えるとor（一個でも動作停止しろといわれたら，停止する）が良さそうにおもいますが，いかがでしょうか．

—
Reply to this email directly or view it on GitHub
#626 (comment)
.

[snozawa]

RLEG_LINK1:RARM_LINK5

これはペア１と同じなので，判断も同じになると思います．
（RLEG_LINK1:RARM_LINK5とペア２が同時に干渉したら，orにして脚を止める）

[k-okada]

でも，左足は動き続けるけどいいのかな．

◉ Kei Okada

On Fri, May 15, 2015 at 11:17 AM, Shunichi Nozawa notifications@github.com
wrote:

RLEG_LINK1:RARM_LINK5

これはペア１と同じなので，判断も同じになると思います．
（RLEG_LINK1:RARM_LINK5とペア２が同時に干渉したら，orにして脚を止める）

—
Reply to this email directly or view it on GitHub
#626 (comment)
.

[mmurooka]

次は実機で確認してみます．もしかしたらstなどが関係しているかも．

ちょっと話戻りますが，
実機で上の2つの姿勢を送ってみたら，
(i)空中でabc,stなし，(ii)空中でabcあり,stなし では足裏が水平なままで，
(iii)地面でabc,stありでは足裏がずれて転びそうになったので，
stが関係していそうでした．

[snozawa]

でも，左足は動き続けるけどいいのかな．

はい，大丈夫だと思います．
左足は，左足関節がフラグ１の干渉ペアが干渉したら止まるはずなので，
この点は今までと同じ挙動なのではと思います．

またこのcollsiion の問題で倒れるのもSTが原因だと思ったので，STを調べてもらいました．
STがセンサフィードバックで関節角を修正しているつもりでも，
実際の関節角度がcollisionによってとまっていると挙動がおかしくなる気がします．

[k-okada]

#637
にlimb毎に止まるようなものをつくりました．これで野沢くんが言っているのと同じ状況にはなるはずです．colilsion_maskとは共存しないので，

#collision_mask: 0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1                                                                                                                              
use_limb_collision: true

みたいにしてください．

[k-okada]

limb枚ではなくて，コリジョンしているリンクからリンクまでの間に有る関節が全部止まる，というものです．
その状態でもう一回ほかのところがコリジョンすると，そこも動かなくなります．
で，一回全部のcollisoinがなくなった状態の姿勢を送らないと，一回止まった関節は動き出すようにはならない，
というふうな挙動が期待されます．あくまでも期待値です．

[snozawa]

ありがとうございます．
ただ，こちらの意図したものと少し違うようでしたので，
取り急ぎサンプルを
#638
に追加しました．

これの "5. Collision mask test" の部分がマスクのテストです．
現状はconfは"collision_mask"をコメントアウトして，"use_limb_collision : true"にしています．
マスクのテストは３つあり，

• 5.1 : 脚同士の干渉＝＞望ましい挙動は，「脚関節が止まる」です．現状機能しています．
• 5.2 : 脚＋腕の干渉＝＞望ましい挙動は，「腕関節のみ止まる」です．最新にしてhttps://github.com/fkanehiro/hrpsys-base/pull/637/のuse_collision_limbにするとうまく動いていないです．
• 5.3 : 脚同士干渉，脚＋腕の干渉の複合バージョン：望ましい挙動は，脚＋腕干渉がおきってるときには腕のみ止まって，脚が干渉し始めると足もとまる，です．現状こちらも5.2と同じで動いてないです

[k-okada]

ああ，足が動き続ける機能は相変わらず欲しかったわけね．
ということで，
use_collision_limb と collision_mask
を並列で設定できるようにしました．

collision_mask: 0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1
#collision_viewer: ture                                                                                                                                                                                 
use_limb_collision: true

で試してみてもらえますでしょうか．
足足だと足が止まるけど，足腕だと腕だけとまります．腕腕はやっていないけど，歩き続けてくれるはず．

 hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0],[1,0,0,0],"rleg"),OpenHRP.AutoBalancerService.Footstep([0,0.0,0],[1,0,0,0],"lleg")])

の挙動がわからないけど，ピコッて足の姿勢が急に動く気がしますが，これはこちらのrecoverモードのもんだいでしょうか．

実際には足足といっているけど，コリジョンしたリンクとリンクの間にある関節，なので，
例えば膝膝がぶつかると足首は動き続けます．これがいいかどうか．これを足首も止めたい，
という挙動にすることもできますが，
今度は肘肘がぶつかった時に手首が止まります．コレはいい気もしますが，例えば，肘胴体が
ぶつかって，手首が動くのは，マニピュレーション班としては嬉しいのかな，とも思ったんだけど．

[k-okada]

#640 の話です

[snozawa]

実際には足足といっているけど，コリジョンしたリンクとリンクの間にある関節，なので，
例えば膝膝がぶつかると足首は動き続けます．これがいいかどうか．これを足首も止めたい，
という挙動にすることもできますが，
今度は肘肘がぶつかった時に手首が止まります．コレはいい気もしますが，例えば，肘胴体が
ぶつかって，手首が動くのは，マニピュレーション班としては嬉しいのかな，とも思ったんだけど．

うーん，これは難しいですね．．．
実際にやってみて判断するかんじでしょうか．

むかしの内部のコードではlimb全体が止まってたきがします．
そのときの感じでいうと，collisiondetectorは自己干渉での自壊を防ぐ最後の最後の砦なので，
動作の成否はもうどうでもいいので止めて，という感じのものでした．

そのときの流れを考えると，limbの一部でなくて全体を止めるのでも悪くないきがします．
理由は以下で，

• 干渉が起きた時に同じlimb内で関節を止める止めないの判断は，全体とめて問題ない，逆に一部動いてもメリットがない．
  腕の干渉なら腕全体が止まる，脚同士の干渉ならコケるの承知で脚全体を止める，というので問題ないきがします．
  逆に，膝膝がぶつかったときに足首が動いていても，もしくは肘肘がぶつかっていて手首が動いていても，ワークスペースの目標足先位置，手先位置は膝・肘が止まった時点でまもられないので，メリットはあまりなさそうです．
• 干渉が起きたときに違うlimbで関節を止める止めないの判断は，止める止めないを選択できるようにするのがよさそう（本prの話題）
  腕・脚が干渉したとして，腕を止めて脚を動かすようにしておくと，腕のマニピュレーションはあきらめて，ただし足の動きはワークスペースの目標足先運動として問題なく実現してくれるので，止める止めないを選択できるとメリットがあります．

[k-okada]

現状、腕脚ぶつかって腕だけとまるけど、次の脚の動きの向きによってはうごか無いか、よりぶつかって壊れるかで、腕は止まるけど脚が動きつづけていい感じ、というのは無いと思っています。go-velocity 0 0 0 でやっているので気がつかないだけで。

なので現実問題としては上半身は上半身でとまる。下半身は下半身でとまる。両方にまたがったら全身止まる、ぐらいかと。

◉ Kei Okada

2015/05/17 4:00、Shunichi Nozawa notifications@github.com のメッセージ:

実際には足足といっているけど，コリジョンしたリンクとリンクの間にある関節，なので，
例えば膝膝がぶつかると足首は動き続けます．これがいいかどうか．これを足首も止めたい，
という挙動にすることもできますが，
今度は肘肘がぶつかった時に手首が止まります．コレはいい気もしますが，例えば，肘胴体が
ぶつかって，手首が動くのは，マニピュレーション班としては嬉しいのかな，とも思ったんだけど．

うーん，これは難しいですね．．．
実際にやってみて判断するかんじでしょうか．

むかしの内部のコードではlimb全体が止まってたきがします．
そのときの感じでいうと，collisiondetectorは自己干渉での自壊を防ぐ最後の最後の砦なので，
動作の成否はもうどうでもいいので止めて，という感じのものでした．

そのときの流れを考えると，limbの一部でなくて全体を止めるのでも悪くないきがします．
理由は以下で，

干渉が起きた時に同じlimb内で関節を止める止めないの判断は，全体とめて問題ない，逆に一部動いてもメリットがない．
腕の干渉なら腕全体が止まる，脚同士の干渉ならコケるの承知で脚全体を止める，というので問題ないきがします． 逆に，膝膝がぶつかったときに足首が動いていても，もしくは肘肘がぶつかっていて手首が動いていても，ワークスペースの目標足先位置，手先位置は膝・肘が止まった時点でまもられないので，メリットはあまりなさそうです．
干渉が起きたときに違うlimbで関節を止める止めないの判断は，止める止めないを選択できるようにするのがよさそう（本prの話題）
腕・脚が干渉したとして，腕を止めて脚を動かすようにしておくと，腕のマニピュレーションはあきらめて，ただし足の動きはワークスペースの目標足先運動として問題なく実現してくれるので，止める止めないを選択できるとメリットがあります．
—
Reply to this email directly or view it on GitHub.