[optmotiongen] :play-motionで力を描画させる

[sshige]

:play-motionで力を描画出来るようにするための変更です。

@mmurooka 現状のプログラムですと、隣あったinstant-config-task同士で、 contact-target-coords-listに入っているcoordsが前から順番に同じものでなければ うまくいかないプログラムになってしまっているのですが、 これをうまく回避する方法というのはあったりするのでしょうか？ contact-target-coordsに名前は入っていないように思います。

現状では、例えば以下のような書き方をされているプログラムですとうまく描画されないです。 (make-coords :pos (float-vector 300 200 0)) と (send (car robot-env-list) :contact "rarm" :contact-coords) が対応している。

[mmurooka]

現状のプログラムですと、隣あったinstant-config-task同士で、 contact-target-coords-listに入っているcoordsが前から順番に同じものでなければ うまくいかないプログラムになってしまっているのですが、 これをうまく回避する方法というのはあったりするのでしょうか？ contact-target-coordsに名前は入っていないように思います。

trajectory-configuration-taskの:generate-robot-state-listというメソッドは， 計画された動作から，robot-state-listというのを返すメソッドで，この中で上記の問題に部分的に対応しているので参考になるかもしれません． https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/optmotiongen/euslisp/trajectory-configuration-task.l#L871 https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/optmotiongen/euslisp/instant-configuration-task.l#L2415-L2424 で，指定されたlimbの接触がcontact-listの何番目かを探しています． 結局，contact-attention-coordsのnameを使えば良いということだと思います．

[sshige]

@mmurooka コメントいただきありがとうございます。

contact-attention-coordsのnameで対応するように変更しました。 現状、robotにかかる力のみ対応しています。

[mmurooka]

closedですが， https://github.com/mmurooka/euslib/tree/murooka-branch/demo/murooka/demo_20190422 のRHP3の膝立ち遷移動作のプログラムで， eus_qpをこのPRのbranchにして， :visualize-force? tして，

roscd demo_20190422/euslisp
roseus
(load "./knee-stand-trajectory.l")
(setup-rhp3)
(stand-knee-trajectory)
(stand-knee-trajectory) ;; 一回目は失敗するので二回呼ぶ必要がある 

とすると，左足だけしか接触力の矢印が表示されません． 右脚や膝でも矢印が出るのが正しいですが，確認できますか．

[sshige]

遅くなりました。

contact coordsの名前を見て、リストの順序を判定しているので、 contact-coordsに名前が入っていなかったことが原因だと思います。

https://github.com/mmurooka/euslib/pull/11/files

のようにすると表示されると思います。

ご確認いただけますか？

[mmurooka]

https://github.com/mmurooka/euslib/pull/11/files のようにすると表示されると思います。

表示されるようになりました．

ただ，どっちが正解か難しいところかもしれませんが，表示に問題があるように思って， 時間ステップiで手先接触がなくて，時間ステップi+1で手先接触が新たにできた，というときに， iからi+1の間に，接触力が，ゼロから時間ステップi+1で計画された接触力に向かって徐々に大きくなる様子を表示すべきだと思います． 実機にref-forceとして与える場合も， ref-forceが不連続になって，ロボットが急に動くことになってしまうことを回避するために， そのように与えることになると思います．

[sshige]

ただ，どっちが正解か難しいところかもしれませんが，表示に問題があるように思って， 時間ステップiで手先接触がなくて，時間ステップi+1で手先接触が新たにできた，というときに， iからi+1の間に，接触力が，ゼロから時間ステップi+1で計画された接触力に向かって徐々に大きくなる様子を表示すべきだと思います．

これもありだとは思うのですが、上記のような実装にするとすると、例えば今回の膝つき動作の際には、膝と床の接触がないのにもかかわらず、力が描画されることになるというように考えています。 膝と床が接触した瞬間から大きな力が入るので、計画動作によって生じる力を描画する、という意味では今のバージョンの方がよいのではないでしょうか？

実機にref-forceとして与える場合も， ref-forceが不連続になって，ロボットが急に動くことになってしまうことを回避するために， そのように与えることになると思います．

すみません、この文章をちゃんとわかっていないのですが、ref-forceを与えるというのはどういった時の話でしょうか？今回のパターンの例ですと、膝立ち動作中の手にref-forceが入るというイメージであっていますか？

[mmurooka]

これもありだとは思うのですが、上記のような実装にするとすると、例えば今回の膝つき動作の際には、膝と床の接触がないのにもかかわらず、力が描画されることになるというように考えています。

https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/optmotiongen/euslisp/sample/sample-sqp-optimization-trajectory.l とかは，接触を追加するときには， 新しい接触部位は，まず，kinematicsの条件を追加して，次の姿勢で，contactの条件も追加するというようにしているので，

膝と床の接触がないのにもかかわらず、力が描画されることになる

という問題は生じませんね． ただ，今回の膝立ち遷移動作はそのようになっておらず，確かに上の問題が生じてしまうので難しいところです．

ref-forceを与えるというのはどういった時の話でしょうか？

ここで生成された動作を， angle-vector-sequence-fullで送るような場合です． このときは，計画されたangle-vector, ref-force, zmpなどをまとめて送って，ロボットがそれらの情報通りに動いてくれることが期待されます． 今回の動作の，膝などの接触は，そもそもref-forceを指定できませんが， 上のsample-sqp-optimization-trajectory.lでは，そのように送れると思います．

以下の例で，描画1は左足の接触が新たに生じた時に，力が徐々に増えている様子が表示されますが，後者では突然ある程度大きい力が表示されます． 前者の徐々に接触力が増えるデータをpatternファイルに保存して，load-patternしてgazeboで動作を確認していますが，後者のように突然ref-forceが変化するpatternファイルにすると良くない挙動になると思います．

roscd eus_qp/optmotiongen/euslisp/sample
roseus
(load "./sample-sqp-optimization-trajectory.l")
(sample-sqp-optimization-trajectory :optimize-start-end-torque? nil :visualize-loop? nil)
;; 描画1
(play-robot-state-list
     :rs-list *rs-list*
     :robot-env *robot-env*
     :joint-name-list (send-all (send *robot-env* :robot :joint-list-orig) :name)
     :root-link-name (send (car (send *robot-env* :robot :links)) :name)
)
;; 描画2
(send *trajectory-config-task* :play-animation
 :robot-env *robot-env*
 :loop? nil
 :visualize-force? t
)

[sshige]

@mmurooka

オフラインで相談させていただいたように、力が生じていないときでも、ref-forceを与えたい場所である(=接触させたい場所である)のならば、 力が働かないというcontact-constraintを加えてもらうようにしたいと思います。

上のサンプルの修正も追加し、徐々に力が加わるように描画されることを確認しました。 https://github.com/jsk-ros-pkg/jsk_control/pull/717/commits/acb2383289031f982a68a07d04eb9d6812d15af0

ご確認、お願いします。

[mmurooka]

一部修正して https://github.com/jsk-ros-pkg/jsk_control/pull/722 でマージしました．