From [email protected] on November 16, 2012 01:35:23

簡単に実装できそうな方法はちょっと思いつかないですが、

・関節のグループを定義するIFをSequencePlayerServiceに追加する。
・内部ではそれが呼ばれた時にそのグループ用のInterpolatorを生成する。
・それらのグループに対して、setJointAngles()等を呼べるようにする。
・onExecute()では全身用のInterpolator, 各グループのInterpolatorを順に（生成した順？）呼び出して、順次上書きしていき、最後に外へ出力する。

という感じかなぁ、と思いますがどうでしょうか？

from hrpsys-base.

From [email protected] on November 16, 2012 01:44:02

なるほど．グループですね．それでいい気がします．もう実装済みだったりしますか？

from hrpsys-base.

From [email protected] on November 16, 2012 02:35:09

いえいえ、未実装です。

from hrpsys-base.

From [email protected] on November 16, 2012 06:01:32

r574 で実装してみました。
例によって最低限のテストしかしていないので、想定した使い方ができるか確認をお願いします。

from hrpsys-base.

From [email protected] on December 06, 2012 00:46:41

ありがとうございます．

試してみました．おそらく想定している範囲では動いていることを確認できました．
追加で
・addJointGroupしたときに急激に指令値がかわり，０に固定される．
　　　現在の関節角度にするべきか，徐々に０に動くべきか，最初のsetJointAnglesOfGroupeまでは上書きしないべきか．
　　　その場合は，syncみたいなメソッドを呼ぶと上書き解除モードにするべきか．
・removeJointGroupしたときに急激に指令値が変わる．徐々に最初のもどっていってほしい．
・waitInterpolation("rarm")としたら，そのgroupのインターポレーションを待ってほしい．
ができるといいともいました．

seq_svc.playPattern(
    [[0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1.3,0,0,0,0,0,0,0,0,0,0,0,0],
     [0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0.5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.3,0,0,0,0,0,0,0,0,0,0,0,0],
     [0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1.3,0,0,0,0,0,0,0,0,0,0,0,0],
     [0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0.5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.3,0,0,0,0,0,0,0,0,0,0,0,0],
     [0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1.3,0,0,0,0,0,0,0,0,0,0,0,0],
     [0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0.5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.3,0,0,0,0,0,0,0,0,0,0,0,0]],
    [[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]],[[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]],[20,20,20,20,20])
time.sleep(5)
seq_svc.addJointGroup("larm", ["L_SHOULDER_P","L_SHOULDER_R","L_SHOULDER_Y","L_ELBOW_P","L_WRIST_Y","L_WRIST_R"])
seq_svc.addJointGroup("rarm", ["R_SHOULDER_P","R_SHOULDER_R","R_SHOULDER_Y","R_ELBOW_P","R_WRIST_Y","R_WRIST_R"])
#seq_svc.setJointAngle("CHEST_P", 0.0, 20.5)                                                                                
#seq_svc.waitInterpolation()                                                                                                
print("start")
seq_svc.setJointAnglesOfGroup("larm", [0,0,0,-1,0,0], 20)
print("start larm")
time.sleep(2)
print("start rarm")
seq_svc.setJointAnglesOfGroup("rarm", [0,0,0, 1,0,0], 20)

print("wait")

time.sleep(2)
print("remove")
seq_svc.removeJointGroup("larm")
seq_svc.removeJointGroup("rarm")
seq_svc.waitInterpolation()
print("done")

#seq_svc.setJointAngles([0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],10)     
#seq_svc.setJointAnglesWithMask([0,0,0,0,0,0,0,0,0,0,0,0,0,0,1.3,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]\

, [0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],1.2)
seq_svc.waitInterpolation()
#seq_svc.setJointAngle("CHEST_P", 0, 2)
#seq_svc.waitInterpolation()
log_svc.save("/tmp/walk")
print("save done")

Attachment: Screenshot-2012-12-06 17:44:16.png

from hrpsys-base.

From [email protected] on December 09, 2012 04:25:16

いずれかの関節が動いている最中にグループを追加したり削除したりできる機能は必要でしょうか？それとも止まっている時だけでよいでしょうか？

from hrpsys-base.

From [email protected] on December 09, 2012 19:03:03

削除はキャンセルの意味であれば，あるといいとおもいます．

現状の研究室の古いsequencerでは，addgroupの部分は決め打ちになっていて，
setJointAngles() と sync() というコマンドが有ります．set..で上書きスタートで，
syncすると，上書きをやめて，全身のsequencerの軌道を使います．

今回はどうするかですが，removeがsyncの意味だとすると，動いている最中に削除する機能は必要に思います．
そうでなくて，syncメソッドを作るのであれば，必要有りません．

実際にはinterpratorにsyncというメソッドがあるので，名前は違う物が良さそうです．

from hrpsys-base.

From [email protected] on December 09, 2012 19:04:00

Cc: [email protected] [email protected]

from hrpsys-base.

From [email protected] on December 16, 2012 05:34:09

全身のsequencerの出力が動いている時にsyncを行なうとどういう挙動になるのでしょうか？

from hrpsys-base.

From [email protected] on December 18, 2012 22:02:48

徐々に全身のsequencerの軌道にちかづきます．CollisionDetectotor.cpp の274行目ぐらいの挙動でしょうか．この部分のコードは中西くんが詳しいです． http://code.google.com/p/hrpsys-base/source/browse/tags/3.1.3/rtc/CollisionDetector/CollisionDetector.cpp

Cc: [email protected]

from hrpsys-base.

From [email protected] on December 22, 2012 19:55:37

r602 で対応して見ました。確認をお願いします。

• SequencePlayerService::waitInterpolationOfGroup(groupName)を追加しました
• グループを削除する際にはそのグループは補間中でないことが必要です
• グループによる補間開始、グループの削除時には位置は連続ですが、速度、加速度は不連続です

from hrpsys-base.

From [email protected] on January 11, 2013 03:03:40

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

jointGroupが補間中でない場合はm_seq->isEmpty()はtrueになっていると思いますが、
この状態で、
seqplay::removeJointGroup()
が呼ばれて、
removeが呼ばれるとstateがremovingになっても、onExecuteのm_seq->isEmptyではtrueなので
m_seq->get()が呼ばれず、groupInterpolatorのget()が呼ばれていないように思います。

groupInterpolatorのisEmpty()でstateも見るようにするのでしょうか？
というのはあっていますか？それとも使い方がまちがっていますでしょうか？

HRP4Cでいかのようにして確認しています。

print("start")

seq_svc.playPattern(

[[0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.3,0,0,0,0,0,0,0,-0.3,0,0,0,0],

[0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0.5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1.3,0,0,0,0,0,0,0,-1.3,0,0,0,0],

[0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-0.3,0,0,0,0,0,0,0,-0.3,0,0,0,0],

[0,0,0,0,0,0,0,0,0,0,0,0,0,0, 0.5,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1.3,0,0,0,0,0,0,0,-1.3,0,0,0,0]],

[[0,0,0],[0,0,0],[0,0,0]],[[0,0,0],[0,0,0],[0,0,0]],[2,2,2])

time.sleep(1)

print("start larm")
print("start rarm")
seq_svc.addJointGroup("larm", ["L_SHOULDER_P","L_SHOULDER_R","L_SHOULDER_Y","L_ELBOW_P","L_WRIST_Y","L_WRIST_R"])
seq_svc.addJointGroup("rarm", ["R_SHOULDER_P","R_SHOULDER_R","R_SHOULDER_Y","R_ELBOW_P","R_WRIST_Y","R_WRIST_R"])
#seq_svc.setJointAngle("CHEST_P", 0.0, 20.5)                                                                               
#seq_svc.waitInterpolation()                                                                                               
seq_svc.setJointAnglesOfGroup("larm", [-2, 1,0, 0,0,0], 3)
#seq_svc.setJointAnglesOfGroup("rarm", [-2,-1,0, 0,0,0], 3)                                                                
#    print("wait")                                                                                                         
#time.sleep(5)                                                                                                             
#seq_svc.waitInterpolationOfGroup("larm")                                                                                  
#seq_svc.waitInterpolationOfGroup("rarm")                                                                                  
#seq_svc.waitInterpolation()                                                                                               
print("remove")
seq_svc.waitInterpolationOfGroup("larm")
time.sleep(1)
seq_svc.removeJointGroup("larm")
#print seq_svc.removeJointGroup("rarm")                                                                                    
#time.sleep(3)                                                                                                             
print("wait")
seq_svc.waitInterpolation()
print("done")

from hrpsys-base.

From [email protected] on January 11, 2013 07:57:58

もう一ヶ所、以下の用にしないとうまく動かない気がします。 http://stackoverflow.com/questions/263945/what-happens-if-you-call-erase-on-a-map-element-while-iterating-from-begin-to Index: ../build/hrpsys-base/rtc/SequencePlayer/seqplay.cpp

--- ../build/hrpsys-base/rtc/SequencePlayer/seqplay.cpp (リビジョン 615)
+++ ../build/hrpsys-base/rtc/SequencePlayer/seqplay.cpp (作業コピー)
@@ -264,15 +264,17 @@
{
interpolators[Q]->get(o_q);
std::map<std::string, groupInterpolator *>::iterator it;

• for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
• for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); ){
  groupInterpolator *gi = it->second;
  if (gi){
  gi->get(o_q);
  if (gi->state == groupInterpolator::removed){

•           groupInterpolators.erase(it);
  

•           groupInterpolators.erase(it++);
          delete gi;
  

•           continue;
      }
  }
  

•   ++it;
  

  }
  interpolators[ZMP]->get(o_zmp);
  interpolators[ACC]->get(o_accel);

from hrpsys-base.

From [email protected] on January 11, 2013 08:56:35

• グループによる補間開始、グループの削除時には位置は連続ですが、速度、加速度は不連続です

は添付のもので速度が連続になったりしますでしょうか？

Attachment: diff2.patch

from hrpsys-base.

From [email protected] on January 11, 2013 09:04:22

• グループを削除する際にはそのグループは補間中でないことが必要です
  で以下の様にしないのはなにか理由があるでしょうか？美しくないという漢字でしょうか。

いずれにせよ
(defmethod hrpsys-interface
(:remove-joint-group (gname)
(send self :wait-interpolation-of-group gname)
(send super :remove-joint-group gname))
)
みたいにしちゃうと思うので、このままでもいいとは思いますが。。。。

bool SequencePlayer::removeJointGroup(const char *gname)
{
waitInterpolationOfGroup(gname);
Guard guard(m_mutex);
return m_seq->removeJointGroup(gname);
}

from hrpsys-base.

From [email protected] on January 12, 2013 00:25:30

Comment 13 はなぜでしょうか？

from hrpsys-base.

From [email protected] on January 12, 2013 01:07:30

eraseしたあと、プログラムが固まることがありました
以下の状況かと思っています http://marupeke296.com/TIPS_No12_ListElementErase.html

from hrpsys-base.

From [email protected] on January 12, 2013 01:07:31

eraseしたあと、プログラムが固まることがありました
以下の状況かと思っています http://marupeke296.com/TIPS_No12_ListElementErase.html

from hrpsys-base.

From [email protected] on January 12, 2013 02:40:52

ん？コメント13の変更だと
it++; continue;
するか
it++;
するかで元のプログラムと同じに思えるのですが。it++してからdeleteするのがポイント？
それとリンク先のページはlistの話で、listの場合はeraseがイテレータを返すようですが、mapの場合は返さないようなので同じようには書けないですね。
類似の問題があるのかもしれませんが。

from hrpsys-base.

From [email protected] on January 12, 2013 03:09:42

なるほど。こういうことですね。 http://d.hatena.ne.jp/Ox8000ffff/20100225/1267201650

from hrpsys-base.

From [email protected] on January 12, 2013 06:58:12

r618 で修正しました。

from hrpsys-base.

From [email protected] on January 14, 2013 23:04:12

ありがとうございます。確認しました。一旦これで先に進みたいと思います。

後で議論になったばあいのメモ：
全身のintepolatorが動いている途中でremoveすると結構激しく動きますね。

seqplay::getで以下の部分は毎回周期よばれて、o_qには現在の出力姿勢がかかれている
というのでいいですよね？
例えば添付のグラフですとカーソルがあるあたりで、jointGroupがremoveされてていて、
水色の関節の動きが、青い関節にだんだんあっていくというはずなんですが、一度逆側にふれる
のはなぜでしょうか。

    double v[m_dof];
    interpolators[Q]->get(o_q, v);
    std::map<std::string, groupInterpolator *>::iterator it;
    for (it=groupInterpolators.begin(); it!=groupInterpolators.end();){
            groupInterpolator *gi = it->second;
            if (gi){
                    gi->get(o_q, v);

Attachment: hrpsys.py

from hrpsys-base.

From [email protected] on January 15, 2013 03:25:21

画像が添付されていないようです。

from hrpsys-base.

From [email protected] on January 15, 2013 03:26:34

失礼しました

Attachment: Screenshot_from_2013-01-15 15:59:21.png

from hrpsys-base.

From [email protected] on February 11, 2013 03:42:14

removeがどの瞬間に呼ばれたのかはっきりとは分かりませんが、水色よりもまだ青色の方が小さいときに呼ばれたということはないでしょうか？明らかに逆ぶれしているように見えるときは既に青の方が大きいですが、その少し前のまだ青が小さい内にremoveが呼ばれて下に向かって動き出したところで青がどんどん上に動いていってそれに追従しようとした、と考えると辻褄があいます。

from hrpsys-base.

From [email protected] on February 27, 2013 19:39:48

#66( http://code.google.com/p/hrpsys-base/issues/detail?id=66 ) の問題と関係していたりしますでしょうか？

また，こにも書きましたが以下のワーニングを表示するというのはどうでしょうか？何が起こっているか把握できるのは助かるかとは思うのですが＞金広さん

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

from hrpsys-base.

From [email protected] on March 02, 2013 18:27:34

警告を出すのはよろしいかと思いますが、速度リミットをこえている間ずっと出続けられるのも困るのでその辺は注意が必要かと思います。

push/popに関しては、確かになくせそうですね。

from hrpsys-base.

From [email protected] on March 07, 2013 00:57:32

push/popに関しては、確かになくせそうですね。

お願いいたします！

from hrpsys-base.

From [email protected] on March 09, 2013 18:13:42

と思いましたが、ファイルからパターンをロードする際にキューが必要になるのでやはり消せないですね。

from hrpsys-base.

From [email protected] on March 18, 2013 02:47:21

trunkに反映されていなかったので，反映させました． r632 です．

from hrpsys-base.

From [email protected] on March 24, 2013 06:07:33

Status: Fixed

from hrpsys-base.