関節角度をうわがくRTCにGoActual相当のモードなどを作る

[fkanehiro]

From [email protected] on August 23, 2013 22:06:33

現在
SequencePlayer -> StateHolder -> RTC1 -> RTC2 ... -> RobotHardware
のように、センサフィードバックを用いるなどして
関節角をずっと上書き続けるモードをもつRTCが、
StateHolderとRobotHardwareの間にはさまっています。

RTC1, RTC2などがそれで、hrpsys_config.pyの例では
ImpedanceControllerやSoftErrorLimiterが該当します。

GoActualはStateHolderのみに有効なので、
servoOn時にGoActualしても、
もしRTC1, ...が関節角うわがきモードの場合、
RobotHardwareまで届いてくる関節角は
GoActualされてないものになります。

RTC1, ...のRTC群にはすべて、
関節角をうわがかないモードへと遷移するなどする必要があります。

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

[fkanehiro]

From [email protected] on November 30, 2013 03:06:04

GoActualはSeqのfullbodyだけしか有効ではなくGroupには効いていないとずっと思っていましたが，実はそうではなくて，seqのgroupも効いていますよね，という確認です．

ここで扱っている問題はImpedanceなどのプラグインについてGoActualが必要というような議論で良いですよね？

[fkanehiro]

From [email protected] on December 03, 2013 22:44:27

GoActualはSeqのfullbodyだけしか有効ではなくGroupには効いていないとずっと思っていましたが，実はそうではなくて，seqのgroupも効いていますよね，という確認です．

ちょっと質問が把握できてないかもしれないですが、答えは多分yesだと思います。

GoActualはStateHolderの機能です、
seq -> sh -> ...
となっている並びのうち、一番最後にくるshだけgoActualすれば
よいというものです。

seqはこのときには、goActualのように実機関節に合わせないですが、
次のsetJointAnglesがよばれたときには、
setInitialState関数がseqの中で呼ばれて、
これは実はStateHolderがもってる値をとってきているので、
この時点でseqもgoActualされたのと同じになります。

また、このsetInitialState関数はみたところsetJointAnglesだけでなくて、
setJointAnglesofGroupでも同様に呼ばれているので、
groupにもgoActualされた結果は伝播してくると思います。

ここで扱っている問題はImpedanceなどのプラグインについてGoActualが必要というような議論で良いですよね？

はい、そうです。

seq -> sh -> ....
のshより手前のものは、

• shがgoActualする
• seqなど手前のものは、動作する(関節保管を開始する..etc)直前にはshからqInitのようなものをもらうことで、goActualと同じことをする

となっています。
shより後のstなどは、stopSTのようなかんじで、
何もしないモードへの遷移が必要になりそう、ということでした。

[fkanehiro]

From [email protected] on December 04, 2013 02:04:14

seqはこのときには、goActualのように実機関節に合わせないですが、
次のsetJointAnglesがよばれたときには、
setInitialState関数がseqの中で呼ばれて、
これは実はStateHolderがもってる値をとってきているので、
この時点でseqもgoActualされたのと同じになります。

ですが，以下のようにqInitはqOutとつながっていることが重要．

leus@starohost:~/urata/build/projects/trans_vm/bin$ rtcat -l localhost:15005/seq.rtc:qInit
-DataInPort: qInit
  dataport.data_type          IDL:RTC/TimedDoubleSeq:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  Any
  port.port_type              DataInPort
 +Connected to  /localhost:15005/sh.rtc:qOut

となっている必要がありこれは，

/hrpsys/lib/python2.7/dist-packages/hrpsys_config.py

で

    connectPorts(self.sh.port("qOut"), self.seq.port("qInit"))

と定義されているから，大丈夫のはず．

[fkanehiro]

From [email protected] on December 04, 2013 02:06:42

ただ，指令値と現在地が定常的に異なるロボットだとどうなるのか？という別の問題もある．

[fkanehiro]

From [email protected] on December 04, 2013 02:14:53

ただ，指令値と現在地が定常的に異なるロボットだとどうなるのか？という別の問題もある．

関節位置サーボゲインが低い、というような場合ですよね。
どのような問題がありますでしょうか。

例えば,servo ONしたときに現在関節角度に指令値がかわる分には
問題ないのではないでしょうか。

[fkanehiro]

From [email protected] on December 04, 2013 04:02:09

中岡くんに手伝ってもらって，

０）サーボオンする
１）右手に指令値を送る
２）左手に指令値を送る
３）サーボオフする
４）ロボットの身体をもって適当に関節を動かす
５）サーボオンする
６）右手に指令値を送る
７）指令値が不連続になる．

という問題があることが分かりました．問題は，seqplay::setJointAnglesOfGroupで以下のようになっていて

    Guard guard(m_mutex);
    if (!setInitialState()) return false;

    if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false;
    return m_seq->setJointAnglesOfGroup(gname, angles, tm);

となっていて，resetJointGroup(）のなかで，qInit(shの出力指令値．GoActualしたあとは現在値を出力指令値にセットする）をinterpolatorの現在位置と目標位置にセットしています

{
    char *s = (char *)gname; while(_s) {_s=toupper(_s);s++;}
    groupInterpolator *i = groupInterpolators[gname];
    if (i){
        i->set(full);

```
の部分です．
```

void interpolator::set(const double *x_, const double *v_)
{
  for (int i=0; i<dim; i++){
    gx[i] = x[i] = x_[i];

```
ですね．

で，この時にgroupのinterpolatorで，いまsetJointAnglesOfGroupした関節だけは出力値が現在位置にリセットされるのですが，それ以外のgroupの関節は出力は前の値ののままなので，指令値が不連続になる，ということになっています．

そこで以下のようにresetJointGroupで自分以外の関節も，それが稼働中でなければgi->setを読んで，現在の指令値（goACtualの結果を反映した値）で補間器の出力をリセットするというふうに変更しました．これで大丈夫だと思います．


```

sleus@starohost:~/ros/fuerte/rtm-ros-robotics/openrtm_common/hrpsys/build/hrpsys-base-source/rtc/SequencePlayer$ svn diff

# Index: seqplay.cpp

--- seqplay.cpp (リビジョン 918)
+++ seqplay.cpp (作業コピー)
@@ -415,6 +415,17 @@
    groupInterpolator *i = groupInterpolators[gname];
    if (i){
        i->set(full);
-       std::map<std::string, groupInterpolator *>::iterator it;
-        for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
-           if ( it->first != std::string(gname) ) { // other 
-               groupInterpolator *gi = it->second;
-               if (gi && (gi->state == groupInterpolator::created || gi->state == groupInterpolator::working) && gi->inter->isEmpty()) {
-                   gi->set(full);
-               }
-           }
-       }
-        // update for non working interpolators
-   
      return true;
  }else{
      std::cerr << "[resetJointGroup] group name " << gname << " is not installed" << std::endl;

```
```

[fkanehiro]

From [email protected] on December 04, 2013 07:02:23

なるほど、それは確かにまずかったですね。
コミットありがとうございます。

[snozawa]

#215
の議論で、上書きモードはRTCのstop->startで各RTCで実装するということになっており、
このissueの議題は官僚していると思いますので、closeでお願いします。

[snozawa]

こちらの上書きモードのRTCのgoActual相当の入力への同期方法ですが、
今はstop->startしたら上書きモードをやめるようになっています。

やはりserviceポートが良いきがしますがいかがでしょうか。

stopの状態になるとRTCの大体の機能がそこなわれてしまい扱いが難しい気がしています。
現状起こっている問題は、stop->startしている途中のstopな状態でserviceポートを
外から呼ばれると、rtcがstopなので無視されてしまいます。
外からサービスポートを呼ぶものは基本的にはrtcdと独立に動いているので、
いちいちisActiveのようなかたちでチェックしてない構造になっていて、
たとえisActiveをやったとしてもすごく複雑になる気がします。

やはり、通常のモード切り替えはサービスポートでやっているので、
servoOn/Off時のgoActual相当の上がかないモードへの遷移も、
サービスポートが自然に思いますが、いかがでしょうか。

（また、現状もしros serviceからよぼうとしたら、サービスでないと動かないと思います）

よろしくお願いいたします。

[snozawa]

@fkanehiroさん
こちらreopenでお願いできますでしょうか。
よろしくお願いいたします。