SequencePlayer内部のhoffarbib補間について

[fkanehiro]

From ik0...@gmail.com on January 27, 2013 18:06:45

問題:
hrp4rでマニピュレータを対話的に操作するrosのインタフェース(interactive_marker)を用いて右腕を動かす実験において,
rosからの関節指令値を受けた際,SequencePlayerのqRefの出力が大きく(関
節角度制限を大きく上回る程度に)変化するという現象が確認されております.
腕はしばらくすると最終的に正しい目標位置に到達します.
発生する条件は不明ですが, setJointAnglesに与えるtmが大きいと発生しやすい傾向にあるように思えます.
条件が不明なため再現性を調べることが困難ですが, 実験中に1~2回起こる程度の発生頻度現象が再発しています.

調査内容:
不正な関節角度が与えられた後, 腕が最終的に正しい目標に到達することから
SequencePlayer内部の補間において不正な関節角度が与えられることを疑って
SequencePlayer/interpolator.cppにおいて,
linear_interpolation関数の出力gxとhoffarbib関数の出力xのR_ELBOW_Pについてログを取り, 比較して見ました.

結果:
現象が再発した際に得られたグラフとrtcdのログを添付いたします.
linearの出力はSequencePlayer::setJointAngles直前の関節角度指令値(ログ内のseqplayer)及びseqplay.cpp内でinterpolator::setGoalでgxに与えられた値(ログ内のseqplay->setGoal)に即座に追従していますが,hoffarbibの場合は大きく関節角度が湾曲して追従していることがわかります.おそらくこれが不正な関節角度の原因と思われます.

以上の結果からSequencePlayerの補間で問題が起こっていると思われるのですが, 原因に心当たりはございますでしょうか.

Attachment: rtcd-20130126173523.log.hrp4r hoffarbib_log.png

Original issue: http://code.google.com/p/hrpsys-base/issues/detail?id=66

[fkanehiro]

From ik0...@gmail.com on January 27, 2013 01:38:01

追記です.
ログを眺めていたところ,
関節角度目標値に到達する直前に目標値が上書きされた場合に問題の現象が発生しやすい可能性がありそうだということがわかりました. 添付のグラフでもtime = 4000 [*5msec]近くで目標値到達直前に関節角度目標値が変更されています.
interactive markerはユーザがmarkerの位置を変えると目標関節角度が上書きされるため, 「tmが長いと現象が発生しやすい」というのは「tmが長いと目標値到達直前の目標値変更が発生しやすい」という事である可能性があります.

確認よろしくお願いいたします.

[fkanehiro]

From kei.ok...@gmail.com on January 27, 2013 01:57:59

interactive_markerのときはinterpolation modeをlinearで動かすのはどうでしょうか。

それとは別に、値がとばないような工夫は必要ですね。関連するスレッドは以下 http://code.google.com/p/hrpsys-base/issues/detail?id=2&can=1

Owner: kei.ok...@gmail.com

[fkanehiro]

From fkaneh...@gmail.com on January 27, 2013 04:46:35

目標値と補間時間はどのように与えているでしょうか？
添付されたグラフだと2度gxが変化していますが、setJointAngles()を呼んでいるのも2回でしょうか？それとも同じgxで何度も呼んでいるでしょうか？

[fkanehiro]

From ik0...@gmail.com on January 27, 2013 08:33:15

目標値と補間時間は目標値が変化するごとにsetJointAngles()を呼んで与えています.
添付したグラフ上でもgxが変化している点でsetJointAngles()が呼ばれており, 呼んだ回数は2回です.

[fkanehiro]

From kei.ok...@gmail.com on January 27, 2013 21:06:06

再現できる入力をつくれないかな。添付のコードをrtc/SequencePlayerにいれて、
CMakeLists.txtに以下を追加して、テストコードを作ってみてください。

プログラムはみたら分かると思いますが、関節各目標値（angle_vector_sequence）、補完時間（ interpolation_time）のsetJointAnglesに与える情報と、
それを与えた時刻をtime_from_startに入れてください。軸は1軸だけを取り上げるのでいいと思います。

std::vector<double> angle_vector_sequence; // target angle vector                                     
std::vector<double> interpolation_time; // interpolation time                                         
std::vector<double> time_from_start; // time from start     

+add_executable(test_seqplay test_seqplay.cpp ${comp_sources})
+target_link_libraries(test_seqplay ${libs})

+set(target SequencePlayer SequencePlayerComp test_seqplay)

Attachment: test_seqplay.cpp

[fkanehiro]

From ik0...@gmail.com on February 12, 2013 21:20:29

遅くなりまして申し訳ありません.
テストコードを作成しました.
実機での出力より若干エラーが小さくなっていますが
hrpsys( r627 )のSequencePlayerでは添付のような出力を吐きます.
確認よろしくお願いいたします.

Attachment: hoffarbib-20130213.png test_seqplay.cpp

[fkanehiro]

From kei.ok...@gmail.com on February 18, 2013 09:10:57

スタート時刻あるいはゴール時刻で速度，加速度が大きいと，こういう現象が起こる気がします． http://mplab.ucsd.edu/wordpress/tutorials/minimumJerk.pdf をみて添付のoctaveコードをみてみてもやはり同じ現象になると思います．
また， r628 でquintic spline, cubic spline　を使っても同じになるとおもいます．
定性的には
a.angle_vector_sequence += 0.00, -1.15781, -0.784511;
a.interpolation_time += 0.10, 0.10, 0.50;
a.time_from_start += 0.00, 0.30, 0.38;
という状態から
a.angle_vector_sequence += 0.00, -1.15781, -0.784511;
a.interpolation_time += 0.10, 0.10, 1.50;
a.time_from_start += 0.00, 0.30, 0.38;
としてもオーバーシュートしますが，
a.angle_vector_sequence += 0.00, -1.15781, -0.784511;
a.interpolation_time += 0.10, 1.50, 0.50;
a.time_from_start += 0.00, 0.30, 1.38;
とするとオーバシュートしません．つまり３番目の目標値を与えるときの現在速度，加速度が大きいと
この現象が起きていると思います．

気になるのは，そもそもなぜこういう目標があたえられているかということ．:angle-vectorをつかっていれば， http://code.google.com/p/rtm-ros-robotics/issues/detail?id=50 の問題さえなければ，遷移時間はそれほど大きくならないはず．
どのような関節角度でどのような目標関節をどのような目標遷移時間で送られているかしりたいところです．

とりあえず気をつけて使う，的な解決方法しか思いつかないのが残念なんですが，
interpolatorのなかで，明らかに速度が大きくなりすぎている場合にワーニングでも
だしておこうかとおもうんですが，seqから今の速度などはとれないですよね？とれるように
したパッチをdebug.patchでおくりましたが．もうちょっと綺麗に書けそうです．
例えばinterpolatorでpush/pop関数がありますが，いまはx,v,aがスロット変数なので
もう必要ないというのはただしいでしょうか？

Cc: fkaneh...@gmail.com ik0...@gmail.com

Attachment: debug.patch minjerk.m

[fkanehiro]

From kei.ok...@gmail.com on February 27, 2013 19:41:01

ik0313さん．以下のものはどうでしょうか？

気になるのは，そもそもなぜこういう目標があたえられているかということ．:angle-vectorをつかっていれば， http://code.google.com/p/rtm-ros-robotics/issues/detail?id=50 の問題さえなければ，遷移時間はそれほど大きくならないはず．
どのような関節角度でどのような目標関節をどのような目標遷移時間で送られているかしりたいところです．

とりあえず気をつけて使う，的な解決方法しか思いつかないのが残念なんですが，

[fkanehiro]

From ik0...@gmail.com on February 28, 2013 01:48:48

関節角度の送信記録は
以前添付したログファイル rtcd-20130126173523.log.hrp4r の内に
seqplay->setGoal: <関節角度> <遷移時間>
の形で出ています. なおこの値は右腕肘関節(R_ELBOW_P)についてのものです.
これを見ると基本的に問題が発生していない部分では0.5から1.5程度の遷移時間が送られているようですが, 問題の箇所では20.7と大きな値が送られていることが分かります.

使用しているinteractive_markerでは関節角度マーカを移動させるごとにikを解き,
結果の関節角度をsetJointAnglesしていると認識しているのですが,
ros(interactive_marker)とrtcdの間の通信が遅くなることがあることが分かっており,
それが原因で遷移時間が長くなっているのかもしれません.

[fkanehiro]

From kei.ok...@gmail.com on March 22, 2013 02:33:38

seqplay->setGoal: <関節角度> <遷移時間>
の形で出ています. なおこの値は右腕肘関節(R_ELBOW_P)についてのものです.
これを見ると基本的に問題が発生していない部分では0.5から1.5程度の遷移時間が送られているようですが, 問題の箇所では20.7と大きな値が送られていることが分かります.

これの遷移時間はなにできまっているものですか？毎回違うのはなにか計算しているのですか？

[fkanehiro]

From yk.at....@gmail.com on March 24, 2013 21:06:35

遷移時間の単位は秒だと思っていれば良いですか？
eusのsend :angle-vectorのレベルでは、遷移時間は固定値（300ms程度？）ですが、
遷移が終了するより先に、send :angle-vectorで目標を上書きします。
遷移時間を変更しそうなのは、 http://code.google.com/p/rtm-ros-robotics/issues/detail?id=50 ここですかね。
その上書き周期が最速33msです。
（ikに時間がかかると増えるはずで、実態がどれぐらいで回っているか計測していない）

そもそも、遷移時間が長いとこの問題が起こりそうなのでしょうか？

スタート時刻あるいはゴール時刻で速度，加速度が大きい
には当てはまらない気がしますが、遷移時間の長さで発散する速度や加速度が変わるのでしょうか？

[fkanehiro]

From kei.ok...@gmail.com on April 01, 2013 18:38:18

問題の箇所では20.7と大きな値が送られていることが分かります.

大きな値が送られた時は問題ではなく，小さい時に問題のように思いますが，ちがうでしょうか？
以前作ってくれたテストプログラムは小さい時間のときのプログラムに見えます．

遷移時間が短いと，終端の速度加速度を守ろうとして，途中の速度加速度が発散すると理解しています． Issue 50 を反映させてからあとに試したて，問題が再現するか見たいところです．

[kindsenior]

これ，今どうなっていますでしょうか？ @orikuma

[k-okada]

これ，今どうなっていますでしょうか？@orikuma