Add "emergency freeze" feature

[130s]

緊急時に対応する次の機能が欲しい (というか必要？) です．もう存在していたりするでしょうか？

• 動きをその場で停止し固定する．
• 解除されるまで関節への指示を受け付けない．

Downstream の以下チケットで話題にしています：

• hrpsys の CollisionDetector で衝突検知した時に動き停止する機能を付けたい
• ROS で関節角度が正しく把握できてない時に，関節への指示を受け付けないようにしたい

[fkanehiro]

サーボ偏差が大きくなった時や、力センサに過大な力が作用した時にサーボをOFFにする機能は現状RobotHardware RTCに実装されていますが、外部からの指令を受けて関節を固定する機能はないですね。
また、非常時にサーボをOFFにするのが良いか、固めるのがよいか、時々議論になっていつも結論が出ないところでもあります。

[snozawa]

緊急時に対応する次の機能が欲しい (というか必要？) です．もう存在していたりするでしょうか？
動きをその場で停止し固定する．
解除されるまで関節への指示を受け付けない．

+1

とても欲しい機能です。

hrpsys の CollisionDetector で衝突検知した時に動き停止する機能を付けたい

これは、今のCollisionDetectorの実装では
衝突検知したときに動きが停止されないということでしょうか。

サーボ偏差が大きくなった時や、力センサに過大な力が作用した時にサーボをOFFにする機能は現状RobotHardware RTCに実装されていますが、外部からの指令を受けて関節を固定する機能はないですね。
また、非常時にサーボをOFFにするのが良いか、固めるのがよいか、時々議論になっていつも結論が出ないところでもあります。

一般的な動作停止が必要か否か、必要であれば
どのような挙動にすべきかは議論がいりそうですが、
例えばCollisionDetectorが自己干渉を検知したときのように状況を限定していくと、
動作停止するような挙動が望ましいように思います。

[fkanehiro]

その時にやり方として、

1. RobotHardwareに停止する機能を付けておき、CollisionDetectorは干渉の発生を予測した場合に、その機能をサービス／データポートを使って呼び出す。
2. CollisionDetectorがロボットへのデータフローで最後に来るようにしておいて、CollisionDetector自身が動作停止するような指令を出力する。
   という2つがあると思うのですが、どちらがよいのでしょうか？

[snozawa]

1. RobotHardwareに停止する機能を付けておき、CollisionDetectorは干渉の発生を予測した場合に、その機能をサービス／データポートを使って呼び出す。

個人的には１が良いように思います。
なぜかというと、他のRTCでも動作停止を行うことができるためです。
#217
のissueにでてるような機能も動作停止で実現できて、機能を複数のRTCで使いまわすことが
できるメリットがおおきいように思います。

一方、RobotHardwareにその機能があると良いかといわれると、動作停止機能のみをもつ
別RTCを設けてもよいように思います。

[130s]

個人的には１が良いように思います。
なぜかというと、他のRTCでも動作停止を行うことができるためです。

+1

hrpsys の CollisionDetector で衝突検知した時に動き停止する機能を付けたい

これは、今のCollisionDetectorの実装では
衝突検知したときに動きが停止されないということでしょうか。

私の日本語がまずいですね．正しくは，"下流で衝突検知した時に，hrpsys に動き停止を指示したい" です．

一方、RobotHardwareにその機能があると良いかといわれると、動作停止機能のみをもつ
別RTCを設けてもよいように思います。

ここは私は hrpsys の RTC 群の設計を未だよく理解できていないので，意見ありません．

[fkanehiro]

RobotHardwareに停止する機能をつける場合、停止させるのは簡単なのですが、再開できる必要はありますでしょうか？
再開時に不連続にならないようにするのは誰の責任でしょうか？

[130s]

RobotHardwareに停止する機能をつける場合、停止させるのは簡単なのですが、再開できる必要はありますでしょうか？
再開時に不連続にならないようにするのは誰の責任でしょうか？

はい，私は再開は出来て欲しいと思います．一番最初に私が書いた案では，"解除されるまで関節への指示を受け付けない．" としていまして，"一時停止"のようなものを想定しています．(よく理解できておらず何か間違っていたら指摘頂きたいですが) サーボ off にするわけでは無く同じ姿勢をキープし続ける実装だとすると，不連続かどうか問題にならなかったりするでしょうか？

[snozawa]

私も、動作停止＋再開ができる機能が必要に思います。
不連続にならないように行うのも、「動作停止RTC」が責任をもつのが良いと思います。

CollisionDetectorのコードですと、
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/CollisionDetector/CollisionDetector.cpp#L371
の部分が、動作停止解除時に現在の指令値に少しずつ遷移していく部分にみえます。

[fkanehiro]

確かにRobotHardwareに停止機能をつけてしまうと、シミュレーション時に使えなくなってしまうので、独立したRTCとするのが良さそうですね。そして、そのような機能が既にCollisionDetectorにあるのであれば、CollisionDetectorを干渉の発生を予測する部分と、動作を一時停止/再開する部分（RTC）とに分けるのがよいのではないでしょうか。

[snozawa]

確かに、hrpsys-simulatorのことを考えるとRobotHadwareだと難しいですね。

少し話がそれますが、停止機能が作動中はビープ音で知らせてくれるとわかりやすいと思いました。

[k-okada]

これ，sequence player側でも止める必要はないでしょうか？つまり，sequnce playerでなんかの連続の軌道を動かしているときに，ある瞬間にとまってもらって，また，そこから復帰して元の軌道列にのっかる，というのが一般には期待されている気がします．

機能停止RTCは停止をするけど，resumeするときは今の指令値に徐々に向かうように動くだけなので，最初の指令した起動は満たさないですね．

[k-okada]

FYI @kindsenior

[snozawa]

これ，sequence player側でも止める必要はないでしょうか？つまり，sequnce playerでなんかの連続の軌道を動かしているときに，ある瞬間にとまってもらって，また，そこから復帰して元の軌道列にのっかる，というのが一般には期待されている気がします．

これは非常に重要な機能ですね。
以前同じようなことを考えてみましたが、ちょっと頭がこんがらがってきて保留にしてました。

悩んでいた点は、どこまでを「元の軌道」と判断するか、です。
（言い換えると、その「元の軌道」にのっかって大丈夫か否か、です）

例えば、自己干渉がおきそうでCollisionDetectorな条件で止まったということは、
SequencePlayerから送られてくる「元の軌道」がInvalidであるので、
その「元の軌道」にのっかるのは誤りです。

一方で、SequencePlayerが補間中にロボット体外になにかおいてあって、
そこにぶつかりそうになって「あ」っとおもって何かボタンをおして
SequencePlayerの補間を緊急停止したとします。
このときは、ぶつかりそうになったモノを除去したら
動作を再開してOKなはずなので、「元の軌道」に戻ってほしいところです。

ここのissueで進んでいた議論（だと私が解釈しているもの）は、

　CollisionDetectorの停止機能を一個のRTCとして独立しておくと、SequencePlayerをユーザが緊急停止できたりして良いですね

だと思いますが、@k-okadaさんご指摘の点を踏まえると、

　CollisionDetectorの停止機能はCollisionDetectorの中だけで行い、
　SequencePlayeの緊急停止機能はSequencePlyaerの中で行ったほうが、動作再開ができるので良い

になるのでは、と思っています。
だとしますと、確かにその通りな気がしています。
（さらに、歩行中に止まるような機能はもちろん歩行生成器にいれておくのがよさそうですね）

[k-okada]

（さらに、歩行中に止まるような機能はもちろん歩行生成器にいれておくのがよさそうですね）

なので，歩行生成器の出力がsequencer playerを経由すると，この必要は無いですね．もちろん，一周期遅れていや，という話はありますが．

FYI -> @emijah

[snozawa]

なので，歩行生成器の出力がsequencer playerを経由すると，この必要は無いですね．もちろん，一周期遅れていや，という話はありますが．

いえ、ここではそれは関係のない話におもいます。

SequecePlayerの停止動作を外にするか中にするか、という流れで、確かに中の方が都合が良いところがおおいと思います。
それとまったく同じ理由で、歩行生成器の停止動作を外にするか中にするか、といわれると、歩行動作の停止動作は歩行プログラムで行うのが良いと思います。

端的には、どう歩行中の片足しじきで停止がくると、SequencePlayerで停止してしまうとコケるしかなくなります。
歩行生成RTCの中で停止すると、足を踏み出してバランスとれる状態でとまることになると思います。

[k-okada]

一段階目，緊急停止すると全身が止まる．resumeするととりあえず，そろそろと動き出す．足は地面をするかもしれないから，空中にあげておく．
ニ段階目，緊急停止すると全身が止まる，歩行は上手く安定に止まる．resumeすると，歩行しなおす．

と考えた時に，一気にニ段階目まで作れればいいですが，いつになるの？という話なので，まずは一段階目がデキルというのはメリットでしょう．

2014-07-08 18:42 GMT+09:00 Shunichi Nozawa [email protected]:

なので，歩行生成器の出力がsequencer playerを経由すると，この必要は無いですね．もちろん，一周期遅れていや，という話はありますが．

いえ、ここではそれは関係のない話におもいます。

SequecePlayerの停止動作を外にするか中にするか、という流れで、確かに中の方が都合が良いところがおおいと思います。
それとまったく同じ理由で、歩行生成器の停止動作を外にするか中にするか、といわれると、歩行動作の停止動作は歩行プログラムで行うのが良いと思います。
この２つは、

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

[snozawa]

遅くなりました。

歩行生成器がSequencePlayerを経由するのは個人的にはあまり得策でないように思います。
ただ、以前口頭では「メリットがなくて、デメリットもある」とお伝えしていましたが、確かに

一段階目，緊急停止すると全身が止まる．resumeするととりあえず，そろそろと動き出す．足は地面をするかもしれないから，空中にあげておく．

ができるのはメリットですね。

ただ、そもそも

なので，歩行生成器の出力がsequencer playerを経由すると，この必要は無いですね．もちろん，一周期遅れていや，という話はありますが．

と

一段階目，緊急停止すると全身が止まる．resumeするととりあえず，そろそろと動き出す．足は地面をするかもしれないから，空中にあげておく．

はよくよく考えるとできない気がしてきたので、genericに止める機能をつくるのであれば、
このissueの上のほうにあるように、独立に止める用のRTCをつけるのがかんがえられそうです。

まず、歩行生成器=>SequencePlayerとなっていると
歩行生成器はSequencePlayerに関節各度指令を枚周期送り続けています。
SequencePlayerを経由させてSequencePlayerの出力を緊急停止で止めたとすると、
SequencePlayerは関節各度を出力しないけど、それとは関係なく
歩行生成器は指令値を送ってくる。
そうすると、歩行生成器の関節各指令は
SequencePlayerが現状止めてる各度からどんどん先にすすみます。
SequencePlayerの中にバッファを持つとしても大変だと思います。

また、そもそもSequencePlayer自体が何かのRTCから
関節指令を受け取るように現状なっていなくて、
改造はかなりの手間になりそうな上いろいろなところに影響がでそうです
qInitは唯一の指令値入力ですが、setJointAnglesの最初など
特定のタイミングでしか反映させないので成り立ってます。
StateHolderも込みでこれを行うにしても、やはり大改造が必要で、
システム図をかいてもかなり複雑になります。

ニ段階目，緊急停止すると全身が止まる，歩行は上手く安定に止まる．resumeすると，歩行しなおす． と考えた時に，一気にニ段階目まで作れればいいですが，いつになるの？という話なので，まずは一段階目がデキルというのはメリットでしょう

また、現状のAutoBalancerでは（内部のでは）緊急停止ができるので、
hrpsys-baseのものもその修正は多少必要ですが、それほどかからないと思います。

[k-okada]

上の一段階目ですが，今持っているseqのqueueの動きを一旦止めて，resumeかかったら，今の現在位置からqueueの先頭（止まる前の指令値）まで，位置速度がつながるようにもっていって，そこで，sequencerの働きを再開するという感じですよね．そこで，もし，新しいsetJointAnglesがあったらどうするか，という問題ですが，どうするのあいいですかね．waitしているクライアントだとずっと待っている，ということになりますね．そうでないと送り続けるわけですが，それをqueueに貯めるか，捨てるか，ということでしょうか？僕は捨てるのでいいと思っていましたが，貯めたほうがいいのかな．

hrpsys-baseのものもその修正は多少必要ですが、それほどかからないと思います。

それは素晴らしいことですが，コードの整理（AutoBalncerが歩行生成器なんだっけ？ ik周りのコードをまとめてOpenHRP3に送る，rats経由のコードをどうするか必要ならOpenHRP3側に入れる）を先にやっておかないといけない段階な気がします．このままだと，すぐに，これは汚いから触りたくないので新しいものを作ります．という人が出てきそうです．

[snozawa]

上の一段階目ですが，今持っているseqのqueueの動きを一旦止めて，resumeかかったら，今の現在位置からqueueの先頭（止まる前の指令値）まで，位置速度がつながるようにもっていって，そこで，sequencerの働きを再開するという感じですよね．そこで，もし，新しいsetJointAnglesがあったらどうするか，という問題ですが，どうするの
あいいですかね．waitしているクライアントだとずっと待っている，ということになりますね．そうでないと送り続けるわけですが，それをqueueに貯めるか，捨てるか，ということでしょうか？僕は捨てるのでいいと思っていましたが，貯めたほうがいいのかな．

ちょっと問題が混ざっている気がしますので、分けさせていただいていいでしょうか。

1. (歩行関係なく)「SequencePlayerで止める=>再開」をする方法
   これは確かに、 上の方法でよさそうに思います。
   つまり、seqの経由点がq1, q2, ..., q_{j}, q_{j+1}, ..., q_Nとあって、q_{j}とq_{j+1}との間を補間している途中のq_cで止まったとすると、
   再開はq_cを開始点としてq_c, q_{j+1}, ..., q_Nを新たな経由点として補間するようにすれば良さそうです。
   さらに
   1-1. setJointAngles、playPatternが停止より前か後に指令されたときには、経由点がうわがかれるだけなので、普通のsetJointAnglesのうわがきの挙動とおなじ
   1-2. setJointAngles、playPatternが停止中に指令されたときには、経由点だけ新しいものにうわがいておけば、動作開始後はその経由点を通るようになる。
   となるので、setJointAnglesなどで指令されても動くように思いますが、いかがでしょうか。
2. そもそも歩行生成器がSequencePlayerに入力を与える方法
   そもそも歩行生成器がSequencePlayerに入力を与える方法は、よく理解できていません。
   歩行生成=>SequencePlayerの順番にするとのことでしたが、どのような構成を想定されてますか？
   SequencePlayerはsetJointAnglesやStateHolderのqInitから時々入力をもらって、毎制御周期ではそれを補間により刻んで出力する、という機能であるのに対し、
   歩行生成のRTCは毎制御周期でずっと指令値を出しています。
   なので、歩行生成器=>SequencePlayerの順番になると、歩行生成器からSequencePlayerに
   常に指令がいっているので、SequencePlayerが他からもらう指令（setJointAnglesなど）と競合すると思います。

それは素晴らしいことですが，コードの整理（AutoBalncerが歩行生成器なんだっけ？ ik周りのコードをまとめてOpenHRP3に送る，rats経由のコードをどうするか必要ならOpenHRP3側に入れる）を先にやっておかないといけない段階な気がします．このままだと，すぐに，これは汚いから触りたくないので新しいものを作ります．という人が出てきそうです．

これはおっしゃるとおりだと思います。
#114
#171
を確認しつつ、週明けくらいまでにはOpenHRP3にパッチが出せそうなところまで整理してみます。

[k-okada]

そもそも歩行生成器がSequencePlayerに入力を与える方法

完全に話が別になっていますが，たとえば歩行生成器は今から３歩分の軌道を生成してながして，
これを毎周期上書きする，みたいなことになったりするんじゃないかな，と思ったんだけど違うかな．

[snozawa]

完全に話が別になっていますが，

そうですね。
「歩行生成器＝＞SequencePlayerであれば停止機能も共有できます」の話の前段階として、そもそも「歩行生成器＝＞SequencePlayer」とする方法があまり分からなかったので、ご質問させていただきました。

たとえば歩行生成器は今から３歩分の軌道を生成してながして， これを毎周期上書きする，みたいなことになったりするんじゃないかな，と思ったんだけど違うかな．

歩行生成＝＞SequencePlayer　なシステムで、
歩行生成器が３歩分の軌道を生成してSequencePlayerに毎周期関節角度を流し続けていて、
そのうえでSequencePlayerは毎周期どのように関節角度を上書きをすることになりますでしょうか？

[130s]

悩んでいた点は、どこまでを「元の軌道」と判断するか、です。
（言い換えると、その「元の軌道」にのっかって大丈夫か否か、です）

この点の議論は特に進んでいない，という理解で良いでしょうか．

例えば、自己干渉がおきそうでCollisionDetectorな条件で止まったということは、
SequencePlayerから送られてくる「元の軌道」がInvalidであるので、
その「元の軌道」にのっかるのは誤りです。

一方で、SequencePlayerが補間中にロボット体外になにかおいてあって、
そこにぶつかりそうになって「あ」っとおもって何かボタンをおして
SequencePlayerの補間を緊急停止したとします。
このときは、ぶつかりそうになったモノを除去したら
動作を再開してOKなはずなので、「元の軌道」に戻ってほしいところです。

上に例示頂いてるように，ロボットが決めてくれて構わない場合と，人じゃないと決められない場合の二種類あると思います．その判断を受け付ける interface はどこが良いのでしょうか．停止 RTC が一括して受け，SeqPlayer 等の停止機能を持つ (ことになる) 各 RTC に送るのかなと思っていましたがそうでもないでしょうか．

[k-okada]

一時停止したとしてその時の時間をt0，姿勢（指令値＝現在値としておく）をP(t0)とします．
一時停止中も時間はt1,t2,t3,,,,と流れ，その時のもともと想定していたロボットの姿勢はこうだった，という姿勢はP_ref(t1),
P_ref(t2).... と変化していきます．
一方で，実際のロボットの姿勢をP_act(t1),
P_act(t2)...とすると，一時停止中にロボットの姿勢を人が動かしてもよい場合（動かせる場合は）
P_act(t1) != P(t0), P_act(t2) != P(t0) になります．

で，tnの時に一時停止が解除されたとします．その時の実際のロボットの姿勢はP_act(tn)，一時停止前のロボットの姿勢はP(t0),
もし一時停止が押されていなければ，こうなっていたはずだったという姿勢はP_ref(tn)となります．

もし，P_act(t1) = P_act(t2) = P(t0) ，つまり一時停止中はロボットの姿勢を動かせない，という状況であれば，
P(tn) = P_ref(t1), P(tn+1), P_ref(t2), ... となるように一時中断，再開，という感じで動かす手があります．
．．．． （A)

もうひとつは，一時停止中の，一時停止が押されていなければ，こうなってたはずだったという姿勢をはスキップして，
一時停止が解除された時に，もし，時停止が押されていなければ，こうなってたはずだったという姿勢に追いつくようにする，という
パターンも考えられます．たとえばtnからtn+1までの間でこうなってたはずだった姿勢に近づくように動いて
tn+1 以降で，P_ref(tn+1)に追従するようにする，ということで，
P(tn+k) = (1-k)P(t0) + kP_ref(tn+1) (0 < k < 1)
P(tn+k) = P_ref(tn+k) ... (k>1) .... (B)
とすることになります．現状のCollisionDetectorの復帰はこの様な感じで書かれている，ということでいいでしょうか．

一方，一旦停止中に姿勢が変わるとすると，（B)のパターンと同じ考えで
「一時停止が押されていなければ，こうなってたはずだったという姿勢をはスキップして，
一時停止が解除された時に，もし，時停止が押されていなければ，こうなってたはずだったという姿勢に追いつくようにする，」
とすると，
P(tn+k) = (1-k)P_act(tn) + kP_ref(tn+1) (0 < k < 1)
P(tn+k) = P_ref(tn+k) ... (k>1) ...(C)
となりますし，また，（A)と同じ方向性で，一時中断，したところから再開して欲しい，というのであれば，

P(tn+k) = (1-k)P_act(tn) + kP_ref(t1) (0 < k < 1)
P(tn+k) = P_ref(t1+k) ... (k>1)
というふうになります． ...(D)

なんとなくですが，一般的に一時中断して復帰して下さい，といわれると，（A)または(D)をイメージしますがどうでしょうか．
という感じでまずは方向性を決める必要があると思います．
ただ，例えばテーブルのしたに手があって，，その手を上に持っていく指令をつくっちゃったとして，ぶつかりそう！となって
一時停止をおしたとします．そこでテーブルの下から手を移動させて，ぶつからなさそうな位置に手を持ってきて，
そこで復帰させると，またテーブルのしたに手がはいって，そこから上に手を持って行って，ぶつかる，ということになるので，
なんとも言いがたいところではあります．とすると，（C)でいいのかな，という気もします．と言うふうによくわからなくなってきます．
どれがいいでしょうか？

一方，さて，これをどう実装しようか，と考えた時にseqで対応するから，あるいは，全ての最後に一時停止RTCをつくるか，
ということになります，（A),(D)だと，じゃあどれだけP_ref()を覚えておくバッファを付けておきますか？無限？みたいな
話も出てくるかと思います．
また，実装面を考えるとCollisionDetectorと整合性をとるにはどうするか，というのも問題になる気がします．

2014-08-14 9:22 GMT+09:00 Isaac I.Y. Saito [email protected]:

悩んでいた点は、どこまでを「元の軌道」と判断するか、です。
（言い換えると、その「元の軌道」にのっかって大丈夫か否か、です）

この点の議論は特に進んでいない，という理解で良いでしょうか．

例えば、自己干渉がおきそうでCollisionDetectorな条件で止まったということは、
SequencePlayerから送られてくる「元の軌道」がInvalidであるので、
その「元の軌道」にのっかるのは誤りです。

一方で、SequencePlayerが補間中にロボット体外になにかおいてあって、
そこにぶつかりそうになって「あ」っとおもって何かボタンをおして
SequencePlayerの補間を緊急停止したとします。
このときは、ぶつかりそうになったモノを除去したら
動作を再開してOKなはずなので、「元の軌道」に戻ってほしいところです。

上に例示頂いてるように，ロボットが決めてくれて構わない場合と，人じゃないと決められない場合の二種類あると思います．その判断を受け付ける
interface はどこが良いのでしょうか．停止 RTC が一括して受け，SeqPlayer 等の停止機能を持つ (ことになる) 各 RTC
に送るのかなと思っていましたがそうでもないでしょうか．

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

[snozawa]

詳細な解説大変ありがとうございます．
だいぶイメージがわいてきました．
（個人的には，研究室内部でのCollisionDetectorとhrpsys-baseのが(B)に近いため，(B)をイメージしてました）

少し話を整理させていただきたいのと，質問があります．

• 人が手で動かす
  人が動かすという状況は，手と机の例ですと
  　「緊急停止」=>「サーボオフ」=>「手を机の上に持っていく」=>「サーボオン」
  という手順になってるということでよろしいでしょうか．
• (B), (C), (D)について
  (B), (C), (D)で(0 < k < 1)となってる場合と(1 > k)の場合とで場合分けされているのはなぜでしょうか．
  現状のCollisionDetectorでは，場合わけせずm_recoverTime (2.5秒デフォルト)くらいで
  今の補間された指令値まで復帰します．
  (もともとseqのどこかの指令値を止めておいて復帰する，
  というスタイルにCollisionDetectorが現状ないので比較が難しいですが)
  (B), (C), (D)で書いていただいた方法ですと，
  かならず ( tn+1 - tn )秒でもとに復帰することになるので，
  ものすごく細かくtrajectoryを与えた場合に(tn+1 - tnが小さい場合)，
  過大な速度を出すことになりそうです．
• StateHolderを導入
  StateHolderをいれると，もう少し話がすっきりするかもしれません．
  SequencePlayerはStateHolderとペアで動くことを想定されてますが，
  今回の(B)と(C)との違いにある，現在姿勢をかえるかどうかの違いは，
  StateHolderでgoActualで代用できる気がします．
  つまり，SequencePlayerはStateHolderの現在値をqInitとしてとってきて，
  そこから指令関節角度列まで補間を繰り返してくので，
  goActualしていればqInitがP_act(tn)になって，
  　P_act(tn) => P_ref(なにか) => P_ref(なにか) ...
  と補間され，goActualしてなければqInitはP(tn)のままで，
  　P(tn) => P_ref(なにか) => P_ref(なにか) ...
  と補間されていくと思います．
  そうすると一個上の復帰までの時間の問題も，
  　P_act(tn) => P_ref(なにか)
  か
  　P(tn) => P_ref(なにか)
  かの最初の補間時間を，適度にゆっくりな値にしておけばよさそうです．
• SequencePlayerの指令値を止めておいて，N個スキップするインターフェースにする
  それで，スキップすべきかどうかはかなり状況依存なきがします．
  個人的には，(B)が良いなと思っていましたが，確かに(D)などのようなものも必要だと
  このissueで感じました．
  なので，
  　SequencePlayerの指令値を止めておいて，N個スキップするインターフェースにする
  というのはいかがでしょうか．
  そうすると，
  　SequencePlayerの停止直後のP_ref(t1)から再開したい
  といったものや
  　SequencePlayerの停止関係なく現在最新の指令値P_ref(tn+1)から再開したい
  といった，２種類の要望に答えられるものがひとつの実装でできそうなきがします．
  具体的には，
  • step. 1
    　緊急停止指令をサービスポートでSequencePlayerに与える
  • step. 2
    　SequencePlayerが停止軌道を生成し，P_ref(t1) ...以降を保存しておく
  • step. 3
    　ユーザが再開するときに，どこから再開するかserviceで指示する(P_ref何個めか)
    　P_ref(t1)すると，停止直後の軌道から，P_ref(t2)だとそのいっこ後から．．．になり，
    　P_ref(tn+1)などにすると，現在の軌道を目標にして復帰していく
    といったかんじになります．
    もしかすると，P_refのqueそれぞれを可視化するGUIがあって，どこをスキップさせてどこをスキップさせないか，などが表示されるといいかもしれません．
• misc
  （実は，(A)の場合分けがちょっとよくわかりませんでした．．．）

まとめますと，

• 人が手で動かすことによる実姿勢からの復帰か，停止時の姿勢からの復帰か
  => StateHolderのgoActualするかどうかで実装
• スキップすべきかどうか
  => ユーザがきめられるようにする
• 補間軌道列をスキップするかどうか選ぶ方法
  => SequencePlayerが停止指令をうけつけ，それによって，
  SequencePlayerが補間指令をqueで保存しておく．
  その上で，ユーザがどこまでスキップしてどこまでスキップしないで，
  動作再開するかを選べるようにする．

（何か勘違いをしている箇所あればよろしくお願いします）

[k-okada]

A-Dは中断したところから再開，B-Cは今の指令値に近づいて再開，の違いでしょうか．
A-Bは中断中は現在位置が動かないことを仮定，C-Dは動いてもOK,ということで本質的な違いは無いです．ABで作っても，CDにバージョンアップすることになりそうです．

• 人が動かすという状況は，手と机の例ですと
  「緊急停止」=>「サーボオフ」=>「手を机の上に持っていく」=>「サーボオン」
  という手順になってるということでよろしいでしょうか．

はい．

• (B), (C), (D)について
  (B), (C), (D)で(0 < k < 1)となってる場合と(1 > k)の場合とで場合分けされているのはなぜでしょうか．
  現状のCollisionDetectorでは，場合わけせずm_recoverTime (2.5秒デフォルト)くらいで
  今の補間された指令値まで復帰します．
  (もともとseqのどこかの指令値を止めておいて復帰する， というスタイルにCollisionDetectorが現状ないので比較が難しいですが)
  (B), (C), (D)で書いていただいた方法ですと， かならず ( tn+1 - tn )秒でもとに復帰することになるので，
  ものすごく細かくtrajectoryを与えた場合に(tn+1 - tnが小さい場合)， 過大な速度を出すことになりそうです．

P(tn+k) = (1-k)P(t0) + kP_ref(tn+1) (0 < k < 1)

k = 10 で上の式に入れると -9 * P(t0) + 9 * P_ref(tn+1) でおかしくならないでしょうか？

tn+1 ＝ tn秒＋１秒と読んで下さい．t(n+1)秒では無いです
( tn+1 - tn )秒 ＝ 1秒
です．なので，１秒で動いて下さい，ということです．１秒ではなくてここを２秒でも３秒でも，あるいは関節のdisplacementで秒数を計算してもなんでもいいです．

• StateHolderを導入
  StateHolderをいれると，もう少し話がすっきりするかもしれません．
  SequencePlayerはStateHolderとペアで動くことを想定されてますが，
  今回の(B)と(C)との違いにある，現在姿勢をかえるかどうかの違いは， StateHolderでgoActualで代用できる気がします．
  つまり，SequencePlayerはStateHolderの現在値をqInitとしてとってきて，
  そこから指令関節角度列まで補間を繰り返してくので，
  goActualしていればqInitがP_act(tn)になって，
  P_act(tn) => P_ref(なにか) => P_ref(なにか) ...
  と補間され，goActualしてなければqInitはP(tn)のままで，
  P(tn) => P_ref(なにか) => P_ref(なにか) ...
  と補間されていくと思います．
  そうすると一個上の復帰までの時間の問題も，
  P_act(tn) => P_ref(なにか)
  か
  P(tn) => P_ref(なにか)
  かの最初の補間時間を，適度にゆっくりな値にしておけばよさそうです．

つまり，SequencePlayerはStateHolderの現在値をqInitとしてとってきて，
そこから指令関節角度列まで補間を繰り返してくので，

のタイミングは新しい目標値が与えられた時ですよね？であるとすると，ものすごく先の時刻（n+1000)の目標値が与えられて，
n+100で一時停止してn+200で復帰したら破綻しませんでしょうか？

• SequencePlayerの指令値を止めておいて，N個スキップするインターフェースにする
  それで，スキップすべきかどうかはかなり状況依存なきがします．
  個人的には，(B)が良いなと思っていましたが，確かに(D)などのようなものも必要だと このissueで感じました．
  なので，
  SequencePlayerの指令値を止めておいて，N個スキップするインターフェースにする
  というのはいかがでしょうか．
  そうすると，
  SequencePlayerの停止直後のP_ref(t1)から再開したい
  といったものや
  SequencePlayerの停止関係なく現在最新の指令値P_ref(tn+1)から再開したい
  といった，２種類の要望に答えられるものがひとつの実装でできそうなきがします． 具体的には，

  • step. 1
    緊急停止指令をサービスポートでSequencePlayerに与える

  • step. 2
    SequencePlayerが停止軌道を生成し，P_ref(t1) ...以降を保存しておく

  • step. 3
    ユーザが再開するときに，どこから再開するかserviceで指示する(P_ref何個めか)
    P_ref(t1)すると，停止直後の軌道から，P_ref(t2)だとそのいっこ後から．．．になり，
    P_ref(tn+1)などにすると，現在の軌道を目標にして復帰していく といったかんじになります．

    もしかすると，P_refのqueそれぞれを可視化するGUIがあって，どこをスキップさせてどこをスキップさせないか，などが表示されるといいかもしれません．

    • misc
      （実は，(A)の場合分けがちょっとよくわかりませんでした．．．）

（A/D)はpick and place などのプログラムを作っていると，pickした時に止めて，なんかして（微妙に物体の位置を調整など？）,
placeしたい，
とおもうので（D)が望まれる気がします．ゆーっくり階段を登っている状況で，ちょっと止めて段差を調整して再開，を考えても（A/D)じゃないでしょうか．

Bが必要な状況はどういうのがあるかな．

また，RTC側を複雑にすると色々面倒では？という気がします．作るのもメンテするのも．なので，
RTCはシンプルにしておくのがいいともっていて，基本は（D)にしておいて，元の軌道を無視したければ，
clearを送って，再開すると同時に新しい軌道を送る，という感じになるんじゃないかな．

ただ，Dだとバッファを持たないといけないので，本当はBを作っておいて，なんかユーザ側で送るとDになる，というのが
楽なんですが

まとめますと，

• 人が手で動かすことによる実姿勢からの復帰か，停止時の姿勢からの復帰か
  => StateHolderのgoActualするかどうかで実装
• スキップすべきかどうか
  => ユーザがきめられるようにする
• 補間軌道列をスキップするかどうか選ぶ方法
  => SequencePlayerが停止指令をうけつけ，それによって， SequencePlayerが補間指令をqueで保存しておく．
  その上で，ユーザがどこまでスキップしてどこまでスキップしないで， 動作再開するかを選べるようにする．

（何か勘違いをしている箇所あればよろしくお願いします）

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

[snozawa]

Bが必要な状況はどういうのがあるかな．

一度整理しますと、
　(B)と(C)：途中動作列をスキップする
　(A)と(D)：途中動作列をスキップしない
だとして、それぞれ必要な状況はこのissueこれまでの議論で既にでてると思っていて、この使い分けについては
　途中動作列が適切ならスキップすべき(B,Cにすべき)、適切でないならスキップしないべき(A,Dにすべき)
となると思います。

これまででてる例ですと、
　テーブルの下から徐々に上に手を伸ばす軌道をSequencePlayerにおくる、この軌道がテーブルと干渉する軌道になってしまっている
としたときに、
　テーブルにぶつかる軌道は不適切なので、B,Cでスキップしてから一時停止解除する
　（この場合は、Cタイプで、サーボを落としてテーブル上まで手をもってきてから一時停止解除する）
としてもいいですし、
　テーブルをどかすかロボットを動かして、元軌道が再生されても問題ないようにしてから、
　A,Dで元軌道に復帰しつつ一時停止解除する
としてもいいと思うので、

• B,CもしくはA,Dそれぞれ欲しい状況は、それぞれある
• どちらがよいかは決められる場合もあるし、上記のように状況or対処しだいでかわることもある（上記例は、緊急停止状況がまったく同じでも、対処方法が違うとBCorADどちらにしたら良いかかわる例）

だと思います。

P(tn+k) = (1-k)P(t0) + kP_ref(tn+1) (0 < k < 1) k = 10 で上の式に入れると -9 * P(t0) + 9 * P_ref(tn+1) でおかしくならないでしょうか？ tn+1 ＝ tn秒＋１秒と読んで下さい．t(n+1)秒では無いです ( tn+1 - tn )秒 ＝ 1秒 です．なので，１秒で動いて下さい，ということです．１秒ではなくてここを２秒でも３秒でも，あるいは
関節のdisplacementで秒数を計算してもな

すいません、こちら大変な勘違いしてました。
図を作って見ましたので、はってみます。

一点確認なのですが、２個まえくらいのコメントに書いてある
P_refは、SequencePlayer補間の経由点でなくて、補間した後の各制御刻みの出力値でしょうか。

また，RTC側を複雑にすると色々面倒では？という気がします．作るのもメンテするのも．なので， RTCはシンプルにしておくのがいいともっていて，基本は（D)にしておいて，元の軌道を無視したければ， clearを送って，再開すると同時に新しい軌道を送る，という感じになるんじゃないかな．

そもそもこれをRTC側でやるとすると、どうしてもかなり複雑になるのではと思っていました。

一つ前で私が書いた方法はできるだけ複雑にしないために、
SequencePlayerの補間経由点だけを止めて保持するといいのでは、というつもりでした。

こうするメリットはほぼ今の方法をかえなくてすんで、例えば

P(tn+k) = (1-k)P_act(tn) + kP_ref(tn+1) (0 < k < 1)
P(tn+k) = P_ref(tn+k) ... (k>1) ...(C)

は復帰方法も改めて実装せず、SequencePlayerの補間をそのままつかえると思います。
つまり、一時停止開始点の姿勢からP_ref(t1) ...を通る補間計算をしなおすか(B,Cのケース)、
P_ref(tn+1)...を通る計算をしなおすことになります(A,Dのケース)。

一時停止解除時の開始姿勢をP_actかPかの違いも、
servoOffしてgoActualしておけば現状でもP_act(CかD)になりますしs、
そうしなければP(AかB)になると思います。

また、一時停止時に保持しておくデータは補間後の各制御刻みではなくて
SequencePlayerの補間点の数の分だけなので、バッファ的に保持するデータはあまり多くなく、

のタイミングは新しい目標値が与えられた時ですよね？であるとすると，ものすごく先の時刻（n+1000)の目標値が与えられて， n+100で一時停止してn+200で復帰したら破綻しませんでしょうか？

は問題にならないと思います。
もし問題があるとすれば、そもそもplayPatternかloadPatternでこれだけの数の指令値を受け付けられない
という状況だと思います。
（このP_refの解釈の点、こちらで誤解があるかもしれませんが）

あと、補間のようなコードを新たに書かずにすむ他に、
地味に線形な補間でなくてhoffarbib補間をそのままつかえて
間をつなぐ軌道もなめらかになる点もメリットかと思いました。

ただ、もちろんシンプルさでいうと

ただ，Dだとバッファを持たないといけないので，本当はBを作っておいて，なんかユーザ側で送るとDになる，というのが 楽なんですが

がもっともシンプルだと思います。
変更も
　停止する機能をCollisionDetectorから分離して使えるようにする
だけですんで、ユーザは「動作再開」のようなことをしたければ、「再開」させたい姿勢と同じ指令値をSeqに再送信する
ようになると思います。

[snozawa]

すいません、絵をはりわすれました。
縦軸が姿勢Pで、横が時刻です。

[k-okada]

図であっていると思います．

P_refは、SequencePlayer補間の経由点でなくて、補間した後の各制御刻みの出力値でしょうか。

はいそうです．A-Dで説明したのは基本的な挙動の話でその実装の時はいろいろやり方があると思います．
上に書いてあるように補間経由点を保持すればいいと思いますが，それでも理屈的には無限の経由点を保持できないので，
例えば腕を振り続けるプログラムを流しておいて，一時停止を押して，次の日また，再開したらなにかおかしいことにはなりそうです．
（一時停止が当たり前になると，こういう使い方になるのはPR2を見ていると，そうなりそうです）

よくわかっていないですが，P_key()が経由点，P_ref()が出力として，

P_key(t0),P_ref(t1), P_ref(t2), P_ref(t3), P_key(t4)

という出力が得られるときに，P_ref(t2)で停止した場合，この瞬間に補間器に残っている経由点はP_key(t4)ですが，
P_key(t2), P_key(t4) ... という補間器の経由点に変更して，qInitをつかって，P_act(t2) ...
P_key(t2)の補間を作ってから
P_key(t2) .. P_key(t4)の補間を実行する，ということになりますか？これって実装は簡単になるでしょうか．

とまぁ，いろいろつらつらと考えていると，
一時停止が掛かったら，補間器を止めて中のバッファもクリアする，というふうにして，
（これはRTCを変更せずにつくれる？，あるいはservo/motor offになったらクリアというプログラムにする？）
そこからサーボオンというか一時停止復帰，とした時に，現在位置にじっくり移動する，というような
プログラムにしておけば，あとは上位はユーザで書いておいて下さい，というのがコントローラとしては
良い挙動なのかな，というきがしてきました．

結局現状のサーボオン／オフと一時停止の違いを考えてみると，ユーザからは，その違いはなくて，
もしかしたら，今のサーボオン／オフでいいかもしれない．
実験が終わったら押すボタンでしかないと．サーボオンオフといったら，本当にサーボがきれて，
一時停止はサーボが入ったまま，という違いはあるかもしれないし，ないかもしれないけど．
（PR2だとrun/stopでサーボは切れるが，歩行ロボットや産業用を考えるとサーボが入ったままでfreezeして欲しいかも）

あるいは，今の瞬間補間器に入っている指令は再生して欲しい，かもしれなけど，あまり効果はないかな．と思うと
やはりシンプルに一時停止されたら今の補間器をクリアして，ストップして，その間も上位では腕を振り続けるプログラムは
動いていても，復帰した時に，うまく接続されればいい，という感じです．
又，上で軌道をプランした時は，もう一回プランしなおす，という感じです．

どうでしょうか．

[snozawa]

はいそうです．A-Dで説明したのは基本的な挙動の話でその実装の時はいろいろやり方があると思います．
上に書いてあるように補間経由点を保持すればいいと思いますが，それでも理屈的には無限の経由点を保持できないので，

SequencePlayer/interpolator.cppにすでにあるqueを使い、
それを一時とめておくようなものを考えていましたが、
よくよく考えるとinterpolator.cppのq,dq,ddqは
各時刻指令値のqueだったので、少し思い違いしてたようです。

例えば腕を振り続けるプログラムを流しておいて，一時停止を押して，次の日また，再開したらなにかおかしいことにはなりそうです． （一時停止が当たり前になると，こういう使い方になるのはPR2を見ていると，そうなりそうです）

すいません、ちょっとこちらのイメージしていたのが違ったようでした。
（一時停止を当たり前のように常用するというよりは、
これまでの議論ででてきたような、もう少しemergency感のある状況を想定してました）

やはり

あるいは，今の瞬間補間器に入っている指令は再生して欲しい，かもしれなけど，あまり効果はないかな．と思うと やはりシンプルに一時停止されたら今の補間器をクリアして，ストップして，その間も上位では腕を振り続けるプログラムは 動いていても，復帰した時に，うまく接続されればいい，という
感じです． 又，上で軌道をプランした時は，もう一回プランしなおす，という感じです．

のようにシンプルなのがよさそうですね。

うまく接続する部分は、一時停止時間中に指令されてた軌道（各時刻指令値or補間経由点...etc）
を再生する方法（前回までのA、D）でなければ、結局 CollisionDetectorから
freeze機能を抜き出すRTCをつくるだけで良いように思うのですが、いかがでしょうか。

つまり、

• 一時停止時間中に指令されてた軌道を再生せず、最新のになってほしい場合(B,C)
  FreezeするRTCにfreezeの指令を送る
  =>
  停止している
  =>
  停止解除する
  =>
  freezeRTCに入力されてくる最新の軌道に徐々に復帰していく
  (もしseqに腕ふり指令値を送りつづけてたら、その途中のタイミングで徐々にそこに漸近していく)
• 一時停止時間中に指令されてた軌道を再生したい場合(A,D)
  FreezeするRTCにfreezeの指令を送る
  =>
  停止している
  =>
  停止解除する
  =>
  上側がseqに送ったけど再生されなかった停止時間中の軌道を、
  なんとかして再生してあげる。

[k-okada]

2014-08-19 2:38 GMT+09:00 Shunichi Nozawa [email protected]:

うまく接続する部分は、一時停止時間中に指令されてた軌道（各時刻指令値or補間経由点...etc）
を再生する方法（前回までのA、D）でなければ、結局 CollisionDetectorから
freeze機能を抜き出すRTCをつくるだけで良いように思うのですが、いかがでしょうか。

そうすると，そのRTCからSeqplayerの補間器をクリアしなければいけなくなりませんか？
stableRTCだけ考えるとseqplayerで全てやるのが単純のような気がします．

一方で，impedanceとかwalkを考えると，それらの出力を何処で止めるか，ということになるかとも
思います．結局だれかが止めなければいけないとすると，それようのRTCを作って，全てのRTC
に非常停止対応の口をつける，ということになるでしょうか．．．．．

あるいは，RTCでstopの状態に遷移させるとemergenceの時の挙動になるようにする，みたいな
規定をあらたに作る，というのはどうでしょうか？

[snozawa]

そうすると，そのRTCからSeqplayerの補間器をクリアしなければいけなくなりませんか？ stableRTCだけ考えるとseqplayerで全てやるのが単純のような気がします．

SequencePlayerの補間機をクリアしようとすると、そうなりますね。
すいません、一つ前のコメントの

一時停止時間中に指令されてた軌道を再生せず、最新のになってほしい場合(B,C)
一時停止時間中に指令されてた軌道を再生したい場合(A,D

の場合わけは、今の状況だと

• 一時停止時間中に指令されてた軌道を再生しない場合(B,C) (緊急停止解除時には動いてない状態とする)
• 一時停止時間中に指令されてた軌道を再生したい場合(A,D)
  となるでしょうか。

stableRTCだけだとSequenePlayerでもよさそうですが、StateHolder以降で
関節角度を動かすimpedanceのようなものがあるときには、

• どれか一つのRTCで緊急停止しておく(freezeRTC)
• （停止機能は上だけでも十分ですが）緊急停止解除時に勝手に動かないように、freezeRTCに指令をおくらない状態にしておく。 これはRTCごとにまちまちですが、SequencePlayerなら補間機クリア、impedanceならstopImpedance、歩行なら歩行停止、なかんじになります。

その上で、停止解除時に止まってることを保証する方法は

• 何もしないモード遷移関数をつくる
  関節角度をうわがくRTCにGoActual相当のモードなどを作る #131
• stop(onDeactivateをよぶ？)にする。

とがありそうですが、確かにstopでできるなら新たに追加がなくてよさそうかもしれませんね。

ちなみに、内部のOpenHRP2制御系にRTCのがわをかぶせて使っていた時には、
servoONのpythonスクリプトのなかで

def servoON():
   ...
   goActual()
   for rtc in rtclist:
        rtc.stop # impedanceとかのうわがきモードもここで止めてくれる
        rtc.start
   ...

のようなことをして、サーボONから復帰してたことがありました。

[k-okada]

@fkanehiroさん
stop/startをしたら各RTCでどのような挙動を示すべきか，というのは何か指標が有りますでしょうか？
また，これを使っていかに議論しているような一時停止／復帰 （あるいはサーボオフ／サーボオン）の
時の状態遷移に活用する，というのはどう思われますでしょうか？

2014-08-19 16:21 GMT+09:00 Shunichi Nozawa [email protected]:

そうすると，そのRTCからSeqplayerの補間器をクリアしなければいけなくなりませんか？
stableRTCだけ考えるとseqplayerで全てやるのが単純のような気がします．

SequencePlayerの補間機をクリアしようとすると、そうなりますね。
すいません、一つ前のコメントの

一時停止時間中に指令されてた軌道を再生せず、最新のになってほしい場合(B,C)
一時停止時間中に指令されてた軌道を再生したい場合(A,D

の場合わけは、今の状況だと

• 一時停止時間中に指令されてた軌道を再生しない場合(B,C) (緊急停止解除時には動いてない状態とする)
• 一時停止時間中に指令されてた軌道を再生したい場合(A,D) となるでしょうか。

stableRTCだけだとSequenePlayerでもよさそうですが、StateHolder以降で
関節角度を動かすimpedanceのようなものがあるときには、

• どれか一つのRTCで緊急停止しておく(freezeRTC)
• （停止機能は上だけでも十分ですが）緊急停止解除時に勝手に動かないように、freezeRTCに指令をおくらない状態にしておく。
  これはRTCごとにまちまちですが、SequencePlayerなら補間機クリア、impedanceならstopImpedance、歩行なら歩行停止、なかんじになります。

その上で、停止解除時に止まってることを保証する方法は

• 何もしないモード遷移関数をつくる
  関節角度をうわがくRTCにGoActual相当のモードなどを作る #131 関節角度をうわがくRTCにGoActual相当のモードなどを作る #131
• stop(onDeactivateをよぶ？)にする。

とがありそうですが、確かにstopでできるなら新たに追加がなくてよさそうかもしれませんね。

ちなみに、内部のOpenHRP2制御系にRTCのがわをかぶせて使っていた時には、
servoONのpythonスクリプトのなかで

def servoON():
...
goActual()
for rtc in rtclist:
rtc.stop # impedanceとかのうわがきモードもここで止めてくれる
rtc.start
...

のようなことをして、サーボONから復帰してたことがありました。

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

[fkanehiro]

上の議論はあまりフォローできてないのですが、
stop/startについては、RTMのdeactivate_component, activate_componentを呼んでいるだけ
ですのでhrpsysのレベルでこういった挙動を想定している、ということはありません。

[k-okada]

これから想定していく，というのはどう思われますでしょうか？
例えば，stop->startとなるときには，どのような姿勢からでも動くようにする（急に目標位置変更くするようなことはしない），みたいなルールを作ってみる．みたいな，

2014-08-21 9:37 GMT+09:00 fkanehiro [email protected]:

上の議論はあまりフォローできてないのですが、
stop/startについては、RTMのdeactivate_component, activate_componentを呼んでいるだけ
ですのでhrpsysのレベルでこういった挙動を想定している、ということはありません。

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

[fkanehiro]

はい、安全のためにはそうすべきだと思います。SequencePlayerはそうしています。
ただそのためには結構実装がややこしくなるところもあるので、痛し痒しの所はありますが。
後はフィルタ的な動作をするコンポーネントだとstopしてしまうとそこでデータフローが止まってしまうので、スルーするモードに移行するような機能が必要になる場合がよくありますが、そういうところも揃えられると良いかもしれませんね。

[snozawa]

ImpedanceController, Stabilizer, AutoBalancerなどのフィルタ的に関節角度をうわがくRTCを、
上がきしないモードに移行するようにするのを試そうとしています。
#131

@fkanehiro さん

後はフィルタ的な動作をするコンポーネントだとstopしてしまうとそこでデータフローが止まってしまうので、スルーするモードに移行するような機能が必要になる場合がよくありますが、そういうところも揃えられると良いかもしれませんね。

こちら、stopでデータフローが止まると問題あるでしょうか。
stop/startで上書きしないモードに移行するより、別途上書きしないモードにもどる関数をIDLで定義して、
hrpsys_config.pyのservoOn関数などでgoActualのあとなどに追加するのがよいでしょうか。

また、stop/startされると上書きしないモードに移行するものを試していますが、
該当するRTCを、stopしてDeactivateがよばれて、startしてActivateがよばれると、
hrpsys_config.pyのstartがかえってこないことがあります。

具体的には、startを呼んでも時々onActivated関数が呼ばれてないようで、start関数がかえってきません。
（下記のprint文がでてこない）
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/ImpedanceController/ImpedanceController.cpp#L204
さらによくよくみるとonExecuteの中のprint文がでてくるので、Activate状態にはなっていそう、というものです。

よろしくお願いします。

[fkanehiro]

ImpedanceController, Stabilizer, AutoBalancerなどのフィルタ的に関節角度をうわがくRTCを、
上がきしないモードに移行するようにするのを試そうとしています。
#131 #131

@fkanehiro https://github.com/fkanehiro さん

後はフィルタ的な動作をするコンポーネントだとstopしてしまうとそこでデータフローが止まってしまうので、スルーするモードに移行するような機能が必要になる場合がよくありますが、そういうところも揃えられると良いかもしれませんね。

こちら、stopでデータフローが止まると問題あるでしょうか。

使い方次第のところはありますが、フィルタ的な動作を期待している場合にデータフローが止まってしまうと
困る場合が多いのではと思います。
例えば、seq->stという接続をしていて、stを止めてしまうとseqが何を出力してもロボットが動かないとか。

stop/startで上書きしないモードに移行するより、別途上書きしないモードにもどる関数をIDLで定義して、
hrpsys_config.pyのservoOn関数などでgoActualのあとなどに追加するのがよいでしょうか。

はい、モードの移行方法はいくつかあると思います。
サービスを使うか、データポートを使うか、コンフィギュレーション変数を使うか。こちらで実装したものは
コンフィギュレーション変数を使っているものが多いです。pythonから使う分には、一番RTC側の変更が
少なくてすむので。

また、stop/startされると上書きしないモードに移行するものを試していますが、
該当するRTCを、stopしてDeactivateがよばれて、startしてActivateがよばれると、
hrpsys_config.pyのstartがかえってこないことがあります。

具体的には、startを呼んでも時々onActivated関数が呼ばれてないようで、start関数がかえってきません。
（下記のprint文がでてこない）

https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/ImpedanceController/ImpedanceController.cpp#L204
さらによくよくみるとonExecuteの中のprint文がでてくるので、Activate状態にはなっていそう、というものです。

よろしくお願いします。

これは変ですね。RTM本体の問題か、rtm.pyの問題か切り分けて頂いて、適切な場所にご報告頂ければ
と思います。

[snozawa]

使い方次第のところはありますが、フィルタ的な動作を期待している場合にデータフローが止まってしまうと 困る場合が多いのではと思います。 例えば、seq->stという接続をしていて、stを止めてしまうとseqが何を出力してもロボットが動かないとか。

なるほど、そうですね。
実装上は、stop/startが呼ばれると何秒かかけて遷移するようなものは難しそうですが
（stop/startの中でwaitInterpolationのように遷移終了をまつのができないと思います）、
サーボオフ時にgoActualのように一制御周期で戻ってくるのはできそうです。
サーボオフ時なので、データフローが一瞬とまったり飛ぶのは問題なさそうかと思っています。

サービスを使うか、データポートを使うか、コンフィギュレーション変数を使うか。こちらで実装したものは コンフィギュレーション変数を使っているものが多いです。pythonから使う分には、一番RTC側の変更が 少なくてすむので。

これは、どのようにされてますでしょうか。
具体的には、フィルタ的RTCはいくつかあると思いますが、
それらすべてで同じ名前のコンフィギュレーション変数を使い、
servoOn時に同じ名前でフィルタRTCにアクセスして関節角度上書きモードから
素通しモードへ遷移する、というようなことをされておりますでしょうか。

これは変ですね。RTM本体の問題か、rtm.pyの問題か切り分けて頂いて、適切な場所にご報告頂ければ と思います。

こちら、まだあまりわけられていませんが、少しだけ状況がわかったので別途issueにします。

[fkanehiro]

状態遷移が完了するのを待ちたいとなると、サービスポートを使うしかないですね。

こちらでは手抜きして、コンフィギュレーション変数で状態遷移のフラグを立てて、呼出側で適当に待つ、ということをやっています。
hrpsys-baseの中にはないのですが、コンフィギュレーション変数名としてはboolean型のisEnabledという変数を使って0を１にするとフィルタ機能有効化、１を0にするとフィルタ機能無効化、としています。
状態遷移には１秒ほど時間をかけてフィルタありとなしの出力を線形補間するので、サーボのON・OFFは気にする必要はないです。

2014-10-10 15:18 GMT+09:00 Shunichi Nozawa [email protected]:

使い方次第のところはありますが、フィルタ的な動作を期待している場合にデータフローが止まってしまうと 困る場合が多いのではと思います。
例えば、seq->stという接続をしていて、stを止めてしまうとseqが何を出力してもロボットが動かないとか。

なるほど、そうですね。
実装上は、stop/startが呼ばれると何秒かかけて遷移するようなものは難しそうですが
（stop/startの中でwaitInterpolationのように遷移終了をまつのができないと思います）、
サーボオフ時にgoActualのように一制御周期で戻ってくるのはできそうです。
サーボオフ時なので、データフローが一瞬とまったり飛ぶのは問題なさそうかと思っています。

サービスを使うか、データポートを使うか、コンフィギュレーション変数を使うか。こちらで実装したものは
コンフィギュレーション変数を使っているものが多いです。pythonから使う分には、一番RTC側の変更が 少なくてすむので。

これは、どのようにされてますでしょうか。
具体的には、フィルタ的RTCはいくつかあると思いますが、
それらすべてで同じ名前のコンフィギュレーション変数を使い、
servoOn時に同じ名前でフィルタRTCにアクセスして関節角度上書きモードから
素通しモードへ遷移する、というようなことをされておりますでしょうか。

これは変ですね。RTM本体の問題か、rtm.pyの問題か切り分けて頂いて、適切な場所にご報告頂ければ と思います。

こちら、まだあまりわけられていませんが、少しだけ状況がわかったので別途issueにします。

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

[snozawa]

状態遷移が完了するのを待ちたいとなると、サービスポートを使うしかないですね。

なるほど、わかりました。
コンギュレーションに関しても、ありがとうございます。

@k-okada さん
フィルタ機能ON/OFFの遷移を、start/stopでなくて、共通の名前のサービスポートにするのはいかがでしょうか。
例えば、フィルタ的RTCは必ずON/OFFのためのenable()/disable()関数をidlにかいて定義しておくルールにする、などです。

サーボON/OFFの復帰に関して、実用上は、
start/stop(それぞれonActivated()/onDeactivated()に対応)で問題なさそうです
一方、start/stopでやると

• 遷移完了をまつようなことがいれられない
• 状態遷移がおこるのをしらなくてstart/stopをよんでしまうユーザ（はあまりいなそうですが）がいたら、関節指令値が一期にとんで問題がおこる
• サーボOFF状態からの復帰だけでなく、サーボON状態での（このissueで議論されているような）緊急停止からの復帰のような場合に対応できない

などがありそうです。

[k-okada]

2014-10-14 10:23 GMT+09:00 Shunichi Nozawa [email protected]:

start/stop(それぞれonActivated()/onDeactivated()に対応)で問題なさそうです
一方、start/stopでやると

• 遷移完了をまつようなことがいれられない

  状態遷移がおこるのをしらなくてstart/stopをよんでしまうユーザ（はあまりいなそうですが）がいたら、関節指令値が一期にとんで問題がおこる

  サーボOFF状態からの復帰だけでなく、サーボON状態での（このissueで議論されているような）緊急停止からの復帰のような場合に対応できない

これ？ 本当にstart/stop ではできないことでしょうか？ 遷移完了をstart/stop
で待てない，というのはどこかそうなっていますでしょうか？
onActivateに失敗する，というパターンもできるし，onActivateで待つ，というパターンも書けると思いますが．できないでしょうか．

最後はサーボOFFしたらstopの状態に入ればいいんだと思ったりするけど．
http://www.openrtm.org/OpenRTM-aist/html/E3839EE3838BE383A5E382A2E383AB2FRTCE38397E383ADE382B0E383A9E3839FE383B3E382B0E585A5E99680.html

[snozawa]

onExecutionとonActivatedが同じスレッドで、状態によって順番によばれてくと理解しており、
wait的なものを入れるのができないと思います。
（まつと、いつまでたってもonExecuteが実効されない、onExecuteが実効されないと遷移もされていかない、のデッドロックになります）
他のidlに書いてるサービスポートでwaitができるのは、別になってるからだと思います。

最後はサーボOFFしたらstopの状態に入ればいいんだと思ったりするけど．

サーボOFFしたら何もしてない状態に遷移する、は確かにいいですね。