search

jsk-ros-pkg / jsk_control

jsk control ros packages
http://github.com/jsk-ros-pkg/jsk_control
13 stars 51 forks source link

adjacent regular scale をリストで渡せるようにするための変更です。 #708

Closed sshige closed 5 years ago

sshige commented 5 years ago

@mmurooka

adjacent regular scale をリストで渡せるようにするための変更です。

編集し終わったら、WIPを外して通知を送ります。

sshige commented 5 years ago

@mmurooka

adjacent regular scaleのための 作業が終わったように思いますので、 レビューをお願いいたします。

mmurooka commented 5 years ago

travisのtestが失敗しています．たぶんこのPR内での変更が関係あるので，確認してみてください． https://travis-ci.org/jsk-ros-pkg/jsk_control/jobs/504546077

[eus_qp:make] [ROSTEST]-----------------------------------------------------------------------
[eus_qp:make]
[eus_qp:make] [eus_qp.rosunit-optmotiongen_test_sample_inverse_kinematics_statics/all-test][passed]
[eus_qp:make] [eus_qp.rosunit-optmotiongen_test_sample_inverse_kinematics_statics/test-sample-robot-iks][passed]
[eus_qp:make] [eus_qp.rosunit-optmotiongen_test_sample_inverse_kinematics_statics/test-sample-robot-trajectory-iks][FAILURE]
[eus_qp:make] Test:(null-output (consp (sample-robot-reach-trajectory-iks-face :optimize-start-end-torque? t)))
[eus_qp:make] Trace:"^[31m[ERROR] test (null-output (consp (sample-robot-reach-trajectory-iks-face :optimize-start-end-torque? nil))) failed ... ( ^[0mmstart testing [test-sample-robot-trajectory-iks]                                                 

[eus_qp:make] ).^[0m                                                                                                                                                                                                                                     

[eus_qp:make] ?"
[eus_qp:make] Message:""
[eus_qp:make] --------------------------------------------------------------------------------
[eus_qp:make]
[eus_qp:make]
[eus_qp:make] [eus_qp.rosunit-optmotiongen_test_sample_inverse_kinematics_statics/test-sample-robot-trajectory-iks][FAILURE]
[eus_qp:make] Test:(null-output (consp (sample-robot-reach-trajectory-iks-face :optimize-start-end-torque? nil)))
[eus_qp:make] Trace:"^[0mmstart testing [test-sample-robot-trajectory-iks]                                                                                                                                                                               

[eus_qp:make] "
[eus_qp:make] Message:""
[eus_qp:make] --------------------------------------------------------------------------------
[eus_qp:make]
[eus_qp:make]
[eus_qp:make] SUMMARY
[eus_qp:make] m * RESULT: FAILm
[eus_qp:make]  * TESTS: 2
[eus_qp:make]  * ERRORS: 0
[eus_qp:make] m * FAILURES: 1m
sshige commented 5 years ago

@mmurooka

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

原因は、元々のプログラムと新しく書いたプログラムが微妙に異なっていることに あったと考えています。

もともとのプログラムの書き方ですと、

https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L599

で追加された単位行列を次のこのdolistのループ

https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L594

で

https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L595

をすることによって書き換えているように見えますが、 一つ前のループにおけるinstant-config-taskの時変関節角の数が今回のループで対象としているinstant-config-taskの時変関節角の数よりも多い場合、完全に書き換えられるわけではない ように見えます。

つまり https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L595 において書き換わるのは、そのループ対象としているinstant-config-taskの時変関節角×時変関節角 の左上にある部分行列のみで、 一つ前のinstant-config-taskの時変関節角の数が今回のループよりも多い場合は、 その多い分に相当する対角成分が0にならず、1のままになってしまうような書き方に なっているように思います。

そこで、今回は、

https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525R618

のように書き換えを無くし、今回のループのみに対応するような書き方にしたので、 本変更の方が正しい変更になっていると思います。

また今回見て思ったのですが、

num-variant-jointの数が変わる場合、 manualの:adjacent-regular-matrixの非対角成分の -I_{adj} は、 時変関節角の数がキーポーズ毎に 異なる場合は対応する２つのキーポーズの時変関節角の数のうち少ない方の数 の対角成分が1（左上から）でそれ以外の成分は0となるような行列になるので

https://github.com/jsk-ros-pkg/jsk_control/pull/708/commits/f6f48f36379eb7b25c11e02ebb72239a116947eb

で対応させました。

もしかしたらこれでテストは通るかもしれません。(少なくとも手元では前回travisで 失敗となっていた例は通るようになりました）

ただ、問題は残っているように感じていて、 これまでの書き方ですと、共通する関節角度列は少ないindexにあり、かつ対応する関節角のindexは 同じ、というような前提があるように思うのですが、実は内部でうまいこと対応するように なっていたりするのでしょうか？

もしそうなっていなければ、本来は隣接していない関節角同士を隣接させようとするような 場合も、書き方によっては出てきてしまうように思いますので、 対応する関節角は同じindexを持つように書き換えるような 操作が必要かもしれません。

文字で伝わるか不安なのですが、 ご確認いただけますと嬉しいです。

mmurooka commented 5 years ago

もともとのプログラムの書き方ですと、

https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L599

で追加された単位行列を次のイテレーションで

https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L595

にて書き換えているように見えますが、 一つ前のイテレーションにおけるinstant-config-taskの時変関節角の数が今回のイテレーションよりも多い場合、完全に書き換えられるわけではない ように見えます。

つまり https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L595 において書き換わるのは、そのイテレーションでのinstant-config-taskの時変関節角×時変関節角 の左上にある部分行列のみで、 一つ前のinstant-config-taskの時変関節角の数が今回のイテレーションよりも多い場合は、 その多い分に相当する対角成分が0にならず、1のままになってしまうような書き方に なっているように思います。

まずは上の部分までに関してですが，以下の3点くらいの疑問でちゃんと読み取れなかったので，補足してもらえると助かります．

sshige commented 5 years ago

すみません、僕の書き方が悪かったように思います。

ここでいっているイテレーションというのは、SQPのイテレーションのことではなく、 このdolistのループのことをさしています。 https://github.com/jsk-ros-pkg/jsk_control/pull/708/files#diff-13d05ed74c65e8c0a0a90b53b09d0525L594

イテレーションというのは，SQPのイテレーションのことだよね？その途中でvariant-joint-listやinvariant-joint-listが変わることは想定していない（これまでもそういう例は出てきてない）と思うんだけれど，どうだろう．

途中でvariant-joint-listが変わるというのは、trajectoryの中にあるinstant-config-task毎にvariant-joint-listが変わるということを指しています。

上の説明を書き換えましたので、もう一度見てもらえると助かります。

sshige commented 5 years ago

@mmurooka

こちらコメントいただいた箇所の修正が出来たように思いますので、 チェックしていただけないでしょうか？

mmurooka commented 5 years ago

ありがとう．OKです．