add setJointAnglesSequence, very simlar to playPattern but overwrite current motion

[k-okada]

関節角度列の時系列データを送るときに，現在ロボットが動いていても動いていなくても，即座に指令を反映させて欲しい，という希望が有りました．
（playPatternでは，指令した時系列関節角データが全て送り終わってから出ないと次の指令が反映されない）

• playPatternの挙動を替えるという作戦もありえます．
• どちらにせよ，setJointAngleSequenceとsetJointAnglesを混ぜて使うと変になる気がします．(シミュレータが止まる？）
  • ちゃんと理解したわけではないですが，playPaternではqueueにすべての指令値を入れていて，setJointAngles()ではonExecuteの度に指令値を補間して作っている気がします．前者でないsetJointAngleSequenceは作れない気がして，今の実装ですが，setJointAnglesも同じようにしても大丈夫そうでしょうか？

@YoheiKakiuchi @snozawa

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/1954/
Test PASSed.

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/1955/
Test PASSed.

[snozawa]

すいません、遅くなりました

今の実装ですが，setJointAnglesも同じようにしても大丈夫そうでしょうか？

"今の実装"はどうなってますでしょうか。
それからあるべき挙動はどうなりますでしょうか。

あと"今の実装"と"どういう機能であるべきか"の一歩手前くらいの話題として、

• 補間器の挙動
  goalがあたえられて、現在からgoalまでの指令値を毎周期計算する
  hoffarbibなら、終端の位置・速度・加速度を与える必要がある
  もし複数点の経由点を指定するなら、途中の経由点すべての位置・速度・加速度を与えて、その間を補完していく

というのがまずあって、情報としては

• 経由点の位置・速度・加速度情報・またそれらの時刻（スパースなもの）
• 補間された後の、制御周期ごとの情報

になってると思います。

そうすると必要な機能としては
1 時刻t_0のときに時刻t_1, t_2, ...に関節角目標値A_1, A_2,...にいくようにsetJointAngleやsetJointAngleSequenceする
2 スパースな経由点情報が時刻はt_1, t_2,...が、目標関節角度はA_1, A_2,..とqueueにpushされる
3 なにもなければ、毎周期直近の目標時刻t_kと直近の目標値A_kになるように補間する
4 ある時刻にsetJointAngles, setJointAngleSequenceがよばれたら、スパースな経由点情報だけ上書きをする。
例えば新規にsetJointAnglesなどで時刻t_k, t_k+1,...に目標値B_k, B_k+1,...などが指定されたら、今の時刻以降の経由点目標値A__をすべて忘れて（popかcleanして）、あらたに経由点目標値B__でおきかえる。それ以降は同様に補間が毎周期はしる（A__は通らずB__を通るように補間される）

となるんだと思います。
経由点が１個でも２個以上でも必要な処理は同じになります。

ぱっとみ今のmasterの補完器では

• 今のplaypatternの実装ではplayPatternがよばれたらそのときに補間がおこなわれきってしまいます。なので、setJointAngleSequenceを新たに追加しないと上記はできない、というのはおっしゃるとおりと思います。
• playPaternはplayPttern内部でstd::vector<const double*> posなどのキューをもってますが、その関数内部で各制御周期までの補間をしてしまうので、キューは関数を実行しおわると忘れます。
  上記のように経由点をセットするタイミングと実際の補間計算するタイミングをわけるとすると、interpolator.cppのメンバ変数で経由点の目標値（位置・速度・加速度・時刻など）のキューをもつ必要があります。
  多分こちらのPRにもメンバ変数の追加が必要そうにみえます（かんちがいしてたらすいません）

となると思います。

[k-okada]

2015-04-15 19:30 GMT+09:00 Shunichi Nozawa notifications@github.com:

例えば新規にsetJointAnglesなどで時刻t_k, t_k+1,...に目標値B_k,
B_k+1,...などが指定されたら、今の時刻以降の経由点目標値A__をすべて忘れて（popかcleanして）、あらたに経由点目標値B__
でおきかえる。それ以降は同様に補間が毎周期はしる（A__は通らずB__を通るように補間される）

playPatternとsetJointAnglesSequenceの実装はこうなっています．違いはsetJointAnglesSequenceは入力が来た時点で忘れて，
今の速度と連続するようにしている点です．

setJointAnglesは共同は今の速度と連続するようにしています．たあし実装は上のキューに入れる形ではなくて，
毎回キューからだしてきています．なので，setJointAnglesSequenceと共存しないので，setJointAnglesもキューに
いれても問題ないでしょうか？というのが一行目になります．

◉ Kei Okada

[snozawa]

今の速度と連続するようにしている点です．

"今の速度と連続"となってる部分のコードはどちらでしょうか。
また、これは経由点の速度でしょうか。
各制御周期の位置・速度・加速度は、hoffarbibなら
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.cpp#L71
の式にしたがって補間すればかならず連続になるとおもってます。

setJointAnglesは共同は今の速度と連続するようにしています．たあし実装は上のキューに入れる形ではなくて， 毎回キューからだしてきています．なので，setJointAnglesSequenceと共存しないので，setJointAnglesもキューに いれても問題ないでしょうか？というのが一行目になります．

はい、setJointAnglesもキューにいれて、必要に応じて補間し、それが上書きの対象になるので良いきがします。
そうすると、経由点が１個であっても複数であってもまったく同じ方法が摘要できるので、
「経由点が１個なset関数」と「経由点が複数個のset関数」は共存できると思います
（共存＝片方をよんだあとにもう片方をよんでも、上書き機能がちゃんと実行されて普通につかえる）
setJoitnAnglesSequenceの経由点１個バージョンにsetJointAngleというエイリアスの関数を定義しておくくらいなかんじをイメージしています。

[k-okada]

今の速度と連続させるのは
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.cpp#L134
だとおもっているんだけど，ちがうかな．

で，そもそも，setJointAnglesがキューになっていないのはなんでなんでしたでしょうか？ @fkanehiro ?

◉ Kei Okada

2015-04-15 19:44 GMT+09:00 Shunichi Nozawa notifications@github.com:

今の速度と連続するようにしている点です．

"今の速度と連続"となってる部分のコードはどちらでしょうか。
また、これは経由点の速度でしょうか。
各制御周期の位置・速度・加速度は、hoffarbibなら

https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.cpp#L71
の式にしたがって補間すればかならず連続になるとおもってます。

setJointAnglesは共同は今の速度と連続するようにしています．たあし実装は上のキューに入れる形ではなくて，
毎回キューからだしてきています．なので，setJointAnglesSequenceと共存しないので，setJointAnglesもキューに
いれても問題ないでしょうか？というのが一行目になります．

はい、setJointAnglesもキューにいれて、必要に応じて補間し、それが上書きの対象になるので良いきがします。
そうすると、経由点が１個であっても複数であってもまったく同じ方法が摘要できるので、
「経由点が１個なset関数」と「経由点が複数個のset関数」は共存できると思います
（共存＝片方をよんだあとにもう片方をよんでも、上書き機能がちゃんと実行されて普通につかえる）

setJoitnAnglesSequenceの経由点１個バージョンにsetJointAngleというエイリアスの関数を定義しておくくらいなかんじをイメージしています。

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

[snozawa]

あ、すいません、正しくなかったです。
おっしゃるとおり、補間のときにつかってる係数をきめる

https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.cpp#L134

が速度などを連続にするというのであってます。

[fkanehiro]

特に深い理由はないです。
現在の姿勢からある別の姿勢へ遷移するコマンドとして用意していただけです。

2015年4月15日 20:20 Kei Okada notifications@github.com:

今の速度と連続させるのは

https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.cpp#L134
だとおもっているんだけど，ちがうかな．

で，そもそも，setJointAnglesがキューになっていないのはなんでなんでしたでしょうか？ @fkanehiro ?

◉ Kei Okada

2015-04-15 19:44 GMT+09:00 Shunichi Nozawa notifications@github.com:

今の速度と連続するようにしている点です．

"今の速度と連続"となってる部分のコードはどちらでしょうか。
また、これは経由点の速度でしょうか。
各制御周期の位置・速度・加速度は、hoffarbibなら

https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/interpolator.cpp#L71
の式にしたがって補間すればかならず連続になるとおもってます。

setJointAnglesは共同は今の速度と連続するようにしています．たあし実装は上のキューに入れる形ではなくて，
毎回キューからだしてきています．なので，setJointAnglesSequenceと共存しないので，setJointAnglesもキューに
いれても問題ないでしょうか？というのが一行目になります．

はい、setJointAnglesもキューにいれて、必要に応じて補間し、それが上書きの対象になるので良いきがします。
そうすると、経由点が１個であっても複数であってもまったく同じ方法が摘要できるので、
「経由点が１個なset関数」と「経由点が複数個のset関数」は共存できると思います
（共存＝片方をよんだあとにもう片方をよんでも、上書き機能がちゃんと実行されて普通につかえる）

setJoitnAnglesSequenceの経由点１個バージョンにsetJointAngleというエイリアスの関数を定義しておくくらいなかんじをイメージしています。

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

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

[k-okada]

groupを対応しました．あとは，

• https://github.com/k-okada/hrpsys-base/blob/45b00db659b143c18fcd3bdccfd5a4f42047cef5/rtc/SequencePlayer/SequencePlayer.cpp#L387 のzmpの指令もsequenceでもあったほうがいいかな
• @snozawa リクエストの，x,vだけでなく，ほかのももろもろ，は，
  https://github.com/k-okada/hrpsys-base/blob/45b00db659b143c18fcd3bdccfd5a4f42047cef5/rtc/SequencePlayer/seqplay.cpp#L264
  ぐらいのものをいれるのかな．今の位置の指令はsequenceの経由点で速度が連続に成るようにしているけど，他のものも必要？あと，これはgorupにはこれらの指令は必要ない，という理解は正しい？

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/1967/
Test PASSed.

[snozawa]

https://github.com/k-okada/hrpsys-base/blob/45b00db659b143c18fcd3bdccfd5a4f42047cef5/rtc/SequencePlayer/SequencePlayer.cpp#L387 のzmpの指令もsequenceでもあったほうがいいかな

こちらは、setZMPの他にsetZMPSequenceをたす、というかんじでしょうか。
ZMP, basePos, baseRpy,...などは基本的には経由点複数 or 細かく指定する場合には、
JointAnglesの情報とタイムスタンプが同期する必要があります。
なので、ZMPだけsequenceで細かく指定したいことはないので、setZmpSequenceはなくてよさそうです。

今の位置の指令はsequenceの経由点で速度が連続に成るようにしているけど，他のものも必要？

これは、"速度が連続"でなくて、"速度をいいかんじにする"ということでしょうか。
（https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/seqplay.cpp#L235のように速度をきめる）
これでしたら、JointAnglesデータがそうしてるならbasePos, baseRpy, ZMPもこうなってるとうれしいです。

また、

あと，これはgorupにはこれらの指令は必要ない，という理解は正しい？

groupもJointAnglesのサブセットなわけですので、"速度をいいかんじにする"の適用が必要だと思います。
適用するしないの判断が、JointAnglesとJointAnglesのgroupとで統一されてれば良いのだと思います。

[k-okada]

setJointAnglesSequenceFull(pos, vel, acc, zmp, rpy, torque, wrench, optional, tm)

というのはどうかな，とおもたんだけど．で，その引数はこれでいいですか，というのが一つ目で，

setJointAnglesSequenceOfGroupFulll(pos, vel, acc, zmp.... )

は必要ですか，が２つ目ですね．

[snozawa]

setJointAnglesSequenceFull(pos, vel, acc, zmp, rpy, torque, wrench, optional, tm)
は、
setJointAnglesSequenceFull(pos, vel, acc, zmp, pos, rpy, torque, wrench, optional, tm)
で腰位置が必要そうです。

また、
setJointAnglesSequenceOfGroupFulll(pos, vel, acc, zmp.... )
はいらないと思います。
理由は、他の値はほとんどがfullbodyなJointAnglesに関係するものです。
なので、JointAnglesの一部groupをおくりながらzmp, pos, rpy, ...などを指定したくなることはないと思います。

ただ、
setJointAnglesSequenceOfGroupFull(pos, vel, torque)
など、トルクは完全にGroup単位にできるので要望があればあっても良いのかもしれません
（個人的にはまずはなしで良いのではとも思いますが）

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/2054/

Build Log
last 10 lines

[...truncated 2524 lines...]
    at org.jenkinsci.plugins.ghprb.GhprbRepository.createCommitStatus(GhprbRepository.java:122)
    at org.jenkinsci.plugins.ghprb.GhprbBuilds.onCompleted(GhprbBuilds.java:118)
    at org.jenkinsci.plugins.ghprb.GhprbBuildListener.onCompleted(GhprbBuildListener.java:27)
    at org.jenkinsci.plugins.ghprb.GhprbBuildListener.onCompleted(GhprbBuildListener.java:12)
    at hudson.model.listeners.RunListener.fireCompleted(RunListener.java:199)
    at hudson.model.Run.execute(Run.java:1786)
    at hudson.model.FreeStyleBuild.run(FreeStyleBuild.java:43)
    at hudson.model.ResourceController.execute(ResourceController.java:98)
    at hudson.model.Executor.run(Executor.java:374)

Test FAILed.

[k-okada]

setJointAnglesSequenceFull()

をついかしました．実戦投入にはいいテストコードが必要かな．

[k-okada]

@fkanehiro すいません。これ、通るはずなので失敗している所をリスタートおねがいできましでしょうか？

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/2094/
Test PASSed.