ImpedanceControllerの関数で、startImpedanceを追加し、setImpedanceParamのJointPath指定方法をかえる #232

snozawa · 2014-06-16T09:04:02Z

ImpedanceController.idlを変えようと思います(=>@snozawa)。

setImpedanceParam
startImpedanceと(純粋な)setImpedanceParamにわける
setImpedanceParamで、JointPath指定方法をかえる
今はname, base_name, target_nameをsetImpedanceParamの引数にいれて
指定してますが、引数ではnameに相当するもので指定する。
今はエンドエフェクタの情報はconfにいれてあるので、引数ではnameを指定するだけにする

（注意点は、virtual force sensorRTCの入力でも動いているので、それを保つようにしておく）

k-okada · 2014-06-16T13:33:56Z

それぞれ理由は何でしょう？

snozawa · 2014-06-17T05:48:34Z

少し広めにみると、パラメータをセットする関数、モードをstart, endさせる関数をどうするかは
ImpedanceControllerだけでなく他のRTC (CollisionDetector, Stabilizer, AutoBalancer...etc)
でも概ね同じ基準で考える必要があります。
Start, Stopのやり方がRTCごとに異なっていると、混乱が生じます

なので上記の２変更点はもう少しいうと

(a) RTCの関数でsetParameterXXXとStartXXXとをわけるか、setParameterXXXとで同じにすべきか
(b) RTCの関数でJointPath系列やエンドエフェクタ情報を指定するのに、confでかくか、
　引数であたえることにすべきか
という議論になると思います。

それぞれ、対応する直しかたは

(a') ImpedanceControllerをsetXXとStartXXとに分ける方式に直すべきか、
　他のRTCをsetXXX方式に統一すべきか
(b') ImpedanceControllerのJointPahtやエフェクタ指定方法をconfにするか、
　他のRTCをconfでなくて引数で指定する方式になおすべきか
ということになります。

上記の観点で考えると、個人的にはそれぞれ

(a'') setParameterXXとStartXXとがわかれているほうが良いと思います。
理由は
- わかりやすい。setParameterXXの中でstartがなされる現状の方法のデメリットは、
  setXXXな名前のメソッドなので値をセットすると思いきや中で自動でインピーダンス制御がスタートするので、
  思いがけず実機が動いて危険、というのが実際ありました
  （何年も前、hrpsys-baseにImpedanceControllerがはいりたてくらいのころ）
- 実際の実装も、startXXXな部分とsetXXXな部分とでif文でわかれる形になっているので、わかれた方が
  みやすいと思います[1]。
(b'') JointPathやエンドエフェクタ情報はconfで書いておく方がいいと思います[2]。
理由は
- 引数でロボット固有の情報を与えずにすむ.
  　例えば
  　setImpedanceParam("rarm", "RARM_LINK5", "RARM_LINK0") (現状バージョン)
  　と書いたコードは書く場所も多く、ミスも多く、他のロボットにも適用できないですが、
  　setImpedanceParam("rarm") (変更したいバージョン)
  　としておけば改善されます。
  　また、ロボット固有情報をインタプリタ側で指定しないので、
  　そこでのバグがはいりにくく、Euslispでできるけどpythonではできない、
  　ということもなくなります[3]。
- 引数が少なくなるため、使いやすい
- エンドエフェクタ・JointPath情報をstaticに設定するかどうか。
  　今まで制御器側に動的エンドエフェクタ・JointPath情報を設定したいことはなく、
  　staticに（起動時に）きまっていて問題ないと思います。
  　また、今VRMLにはこれらの情報がはいってないですが、Colladaではmanipulatorタグ、
  　Euslispもモデル情報としてこれらをもっていて、やはりstaticで良いと思います。
  　またsetParameterXXXと関係しますが、動的にJointPathなどをセットするようにしてしまうと、
  　その前にはRTCはJointPathなどの情報を持たないので、別なパラメータをセット・ゲットできなくなります

[1] https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/ImpedanceController/ImpedanceController.cpp#L566
[2] https://github.com/start-jsk/hrpsys/blob/master/samples/samplerobot/SampleRobot.conf.in
[3] start-jsk/rtmros_gazebo#64

k-okada · 2014-06-17T09:14:40Z

https://github.com/start-jsk/rtmros_hironx/blob/groovy-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py#L66

にあるようなグループとの関連はどうなるでしょうか？グループとImpedanceのグループは同一なことが想定されていますか？

同一であれば，このなかのinitでseqのグループを作るときに，impedanceのグループみたいなものもつくるのはどうでしょうか．今のままでも"rarm"をキーにして，いるので，base_nameとtarget_nameが""だったら更新せず，KMDだけ使う，のはどうでしょう．

confは最終的にROSから制御できないのであまり良くない気がしています．

snozawa · 2014-06-18T05:50:16Z

https://github.com/start-jsk/rtmros_hironx/blob/groovy-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py#L66
にあるようなグループとの関連はどうなるでしょうか？グループとImpedanceのグループは同一なことが想定されていますか？

はい、そうです。
groupの名前のように、limb自体に"rarm"のようなラベルをつけて使いたいということでした。
ただ、今end-effector情報で使っているのは"rarm"でなくて":rarm"のようにコロンをつけてしまってました。
これは、コロンなしの"rarm"に統一するのでいいでしょうか。

同一であれば，このなかのinitでseqのグループを作るときに，impedanceのグループみたいなものもつくるのはどうでしょうか．今のままでも"rarm"をキーにして，いるので，base_nameとtarget_name
が""だったら更新せず，KMDだけ使う，のはどうでしょう．

こちらは、end-effector設定など今confにかいている情報を、
confでなくてpythonスクリプト側に記述する、ということでしょうか。
確かに、それもよさそうですね。
また、seqのgroupを見るかかぎり、ここからエンドエフェクタ情報などをとってくることも可能と思います。

ただし、運用はどうしましょう。２点くらいご相談させていただきたい点をあげると、

初期化時に欲しい情報はやはり現状confにいれるしかない
例えばdtは初期化時に必要なので、confにかくしかないです。
エンドエフェクタ情報なども現状に初期化時に欲しいときがあり、onActivate後かモード開始後にすることはできなくはないですが、
どうしても初期化時に欲しい情報が今後でてきたらconfはいると思います。
（本当は、エンドエフェクタ等はVRML側にはいっていると良いものなので）
どのファイルをメンテナンスするか
今はconfは自動生成ファイルになっていて、各ロボットごとの設定がかきやすいです。
一方pythonスクリプトは、hrpsys_config.py はStableRTC userは標準のものを使い、UnstableRTC userはそれを継承して実機やシミュレータで使っています [1]。
pythonで設定を書く場合は、pythonに設定をかき、そのpythonスクリプトをコミットすることになるでしょうか。

confは最終的にROSから制御できないのであまり良くない気がしています

こちらが一点よくわかりませんでした。
pythonにしたらしたで、python側に直接groupの設定を書かなければいけないと思いますが、
「ROSから制御できる」というのがよくわかりませんでした。
confにしてもpythonにしても、何かの情報から自動生成できるのが良いと思います。
今 Unstable RTCが使ってるエンドエフェクタ情報のようなconfに書いている情報は、
cmakeにかいてはいますが、その記述自体はEuslispのモデル情報から自動生成したものです。
[1] start-jsk/rtmros_gazebo#61

k-okada · 2014-06-18T11:23:22Z

（本当は、エンドエフェクタ等はVRML側にはいっていると良いものなので）

その方向はかなりマジメに考えていいと思います．何かコメントあるでしょうか？ @fkanehiro さん．Chorenoidのモデルだと対応されていたりしますか？

confは最終的にROSから制御できないのであまり良くない気がしています
こちらが一点よくわかりませんでした。

pythonに書いたら，そこから何らかのCORBAを通じてpluginに命令を送ります．
https://github.com/start-jsk/rtmros_hironx/blob/groovy-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py#L117

ということはROSからもサービスで通信出来ます．チョットだけなにか変えたいというときに，
上の方式だとmy_hironx_client.py を作って対応する人はいそうです．褒められることではないんけれども．
confをチョットだけ変えてもコミットするとコンフリクトするので，こちらのほうが楽そうな気がしますが
どうでしょうか？（チョットだけかえるを助長するのは良くないですが）

#234 の議論もconfで対応できるか検討してみて下さい．

fkanehiro · 2014-06-18T11:55:56Z

こちらでは、手先のリンクはどれで、足先のリンクはどれ、といったロボットの形態よってまちまちな情報はHumanoidノードのinfoというフィールドに文字列で入れて、ModelLoaderから読みだして使っています。
ChoreonoidではVRMLを参照するYAMLファイルにそのような情報を書くようにしているようです。

fkanehiro · 2014-06-18T12:06:24Z

化けてるようです。

k-okada · 2014-06-18T12:16:12Z

なんと！！！

どういうのか教えて下さい。
hrp4cにそれをいれておきかえましょう @YouheiKakiuchi
OpenHRP3のホームページに情報をいれましょう @snozawa
Hironx/nextageのモデルファイルも更新しましょう @130s
ColladaWriterを対応させましょう @k-okada

fkanehiro · 2014-06-18T23:22:57Z

例えば、某ロボットでは以下のような情報を入れています。

    "LEG_END RLEG_JOINT5 LLEG_JOINT5",
    "ARM_END RARM_JOINT6 LARM_JOINT6",
    "FOOT_SIZE XX XX XX XX",

emijah · 2014-06-19T00:05:30Z

この情報ってColladaに入っていませんでしたっけ？

k-okada · 2014-06-19T04:54:15Z

colladaには入っているけど，wrlにないのが問題です @emijah

k-okada · 2014-06-19T05:05:57Z

あまり何が起こるか，チャントわかっていないですが，

PROTO JointGroup [
  exposedField MFNode baseJoint
  exposedField MFNode targetJoint
  exposedField SFVec3f translation # endcoords
  exposedField SFRotation rotation # endcoords
]

みたいなのをつくって，

　joints [
   ]
   segments [
   ]
   jointGroups [
      DEF LARM JointGroup { baseJoint LARM_JOINT0 targetJoint LARM_JOINT6 translation 0 0 0 rotation 1 0 0 0 }
   ]

みたいにするとオオゴトでしょうか？

emijah · 2014-06-19T05:29:26Z

@k-okada wrlに入れる話にするということですね。
これが通るとcolladaとの違いが埋まりますね。

fkanehiro · 2014-06-19T05:42:06Z

新しいノードを定義したりすると、ModelLoaderの中にも手を入れないといけないですね、っていうのと、エンドエフェクタの座標系ってIKする時に欲しいっていうのは分かるんですが、その座標系っていつも同じでしょうか？あるときは把持する中心の座標系だったり、ある時は手の先端だったりしません？そういったアプリというか状況によって変化する情報はモデルファイルには書かずに、モデルファイルには常に変わることのないスタティックな情報だけを書くのがよいのでは、と考えています。

k-okada · 2014-06-19T06:33:41Z

新しいノードを定義したりすると、ModelLoaderの中にも手を入れないといけないですね、

これはCORBAのメッセージも型が変わって，下位互換性がなくなったりしますでしょうか？

fkanehiro · 2014-06-19T06:37:32Z

はい、それらの追加情報を格納する場所をどこかに確保する必要があります。

snozawa · 2014-06-19T07:09:16Z

新しいノードを定義したりすると、ModelLoaderの中にも手を入れないといけないですね、っていうのと、エンドエフェクタの座標系ってIKする時に欲しいっていうのは分かるんですが、その座標系っていつも同じでしょうか？あるときは把持する中心の座標系だったり、ある時は手の先端だったりしません？そういったアプリというか状況によって変化する情報はモデルファイルには書かずに、モデルファイルには常に変わることのないスタティックな情報だけを書くのがよいのでは、と考えています。

エンドエフェクタ情報は、確かに用途によって使い分ける必要がありそうですので、

デフォルト値を入れておく、
必要があれば、変更可能にもしておく
とするのはいかがでしょうか。

ちなみにこちらでの運用方法ですと、実はエンドエフェクタを変えることがあまりないです。
というのも、

Euslisp, MoveIt,..など上位ソフトウェアで「エンドエフェクタをかえつつ」プランニングし、
hrpsys-baseレイヤの部分ではそれに基づき動作制御・微修正することになる

ので、hrpsys-baseレイヤでは「エンドエフェクタをスタティックでなくかきかえる」ことが
必要になったことが今まではございませんでした。

fkanehiro · 2014-06-19T09:25:29Z

こちらではエンドエフェクタのリンク名まではスタティックと見なしています。
そういう意味では位置や姿勢はゼロベクトルと単位行列をデフォルト値としている、と見ることもできるかと思います。

snozawa · 2014-06-19T17:48:41Z

こちらではエンドエフェクタのリンク名まではスタティックと見なしています。
そういう意味では位置や姿勢はゼロベクトルと単位行列をデフォルト値としている、と見ることもできるかと思います。

具体的に、hrpsys-baseでHumanoidのPROTOのInfoフィールドを読み込んでいるところはどちらでしょうか？
MString型で読み込み自体はOpenHRP3だと思いますが、
さらにその文字列からリンク名などを取得している部分を教えていただけると助かります。
もしhrpsys-abseにそのコードがありましたら、エンドエフェクタの（デフォルトの）位置姿勢オフセット量などは追加可能でしょうか？

fkanehiro · 2014-06-19T23:28:47Z

hrpsys-baseの中にはInfoフィールドを使っているコードは無いですね。
使っているところでは以下のような感じにしています。

void setPropertiesFromInfoField(motion_generator::HumanoidBodyPtr body,
                                OpenHRP::BodyInfo_var bodyInfo)
{
    OpenHRP::StringSequence *info = bodyInfo->info();
    for (unsigned int i=0; i<info->length(); i++){
        const char *s = (*info)[i];
        std::istringstream is(s);
        std::string key;
        is >> key;
        if (key == "LEG_END"){
            std::string rend, lend;
            is >> rend >> lend;
            body->ankleLink[RIGHT] = body->link(rend);
            body->ankleLink[LEFT ] = body->link(lend);
        }else if(key == "ARM_END"){
            std::string rend, lend;
            is >> rend >> lend;
            body->wristLink[RIGHT] = body->link(rend);
            body->wristLink[LEFT ] = body->link(lend);

snozawa · 2014-07-24T08:43:26Z

こちらで案がでている

JointGroup
"HEAD HEAD_JOINT0 HEAD_JOINT1"のようなもので、名前(HEAD)とジョイントのリストをいれる
エンドエフェクタ情報
"RARM RARM_JOINT6 0 0 0 1 0 0 0" のようなもので、名前（RARM）とエフェクタリンク（のジョイント、RARM_JOINT6)、オフセット(位置x3と姿勢x4,クォータニオン)くらいの情報をいれる

の２種を新たにinfoに入れる、というのを進めてみたいですが、いかがでしょうか。

k-okada · 2014-07-24T08:49:35Z

やるなら，Modelの定義としてちゃんと入れたいけどなぁ．やっぱAPIこわれるだろうか？
前みたいに最後に追加したらOKみたいな話にはならないのかな．．

snozawa · 2014-07-24T08:59:18Z

やるなら，Modelの定義としてちゃんと入れたいけどなぁ．やっぱAPIこわれるだろうか？

Modelの定義といいますと、VRMLにinfoとしてかくだけでなく、数個前のコメントにあるような

PROTO JointGroup
....

PROTO EndEffector
....

とかくルールにして、ModelLoaderも修正する、という形でしょうか。
確かに、個人的には、そのほうが良いと思います。

　APIが壊れてない＝新しい要素を追加してないIDLから作ったプログラムが、新しい要素を追加したIDLから作ったプログラムと、（新しい要素以外は）エラーなく通信できる

でしょうか。

また「最後に追加」が、idlファイルの最後か、structに何かたすなら、structの最後というのもOKなのか、
APIがどのようにしたら壊れるかをあまり理解できてないのですが、どこを参照したらよいでしょうか。

fkanehiro · 2014-07-25T09:35:13Z

最後に追加するのはOK、というのは単に僕の経験則でしかないので、正確なところはCORBAのC++マッピングの仕様をみていただくしかないと思います。
同様に僕の予想でしかないですが、構造体へのメンバの追加はNGだと思います。

fkanehiro · 2014-07-25T09:38:37Z

ちなみにOpenHRPのVRMLがベースとしているh-animではSiteというノードがあり、これがエンドエフェクタを表すようなので、これを使うのが正しい方法かもしれません。
https://h-anim.org/Specifications/H-Anim1.1/#site

snozawa · 2014-07-26T06:56:21Z

最後に追加するのはOK、というのは単に僕の経験則でしかないので、正確なところはCORBAのC++マッピングの仕様をみていただくしかないと思います。
同様に僕の予想でしかないですが、構造体へのメンバの追加はNGだと思います。

こちらありがとうとございます。追加はむずかしそうですね。

ちなみにOpenHRPのVRMLがベースとしているh-animではSiteというノードがあり、これがエンドエフェクタを表すようなので、これを使うのが正しい方法かもしれません。

Siteも利用する難しさは同じではないでしょうか？
つまり、VRMLパーサとその構造体はOpenHRP3側で実装・定義してありますので、
Siteを利用する場合は
　ModelLoaderでSite読み込み部分を追加
　idlなどにSiteに対応する部分を追加
しなければならないので、「追加してIDLのAPIをこわす」というのに該当してしまうのではないでしょうか。
（どこか勘違いしていたらすいません）

fkanehiro · 2014-07-26T07:03:21Z

はい、そういう意味では同じなのですが、同じ意味のノードを独自に定義するよりは、既存のものを使うのがよいのでは、ということです。

snozawa · 2014-07-27T07:31:39Z

はい、そういう意味では同じなのですが、同じ意味のノードを独自に定義するよりは、既存のものを使うのがよいのでは、ということです。

確かに，おっしゃるとおりだと思います．
そうしますと，OpenHRP3側に追加するのであれば

JointGroup情報：新たに独自ノードを追加して記述
エンドエフェクタ情報:Siteノードで記述して記述
でいくとして，実際問題APIが変わるような大幅な変更はOpenHRP3で可能なのでしょうか．

それが非常に難しい場合は，上記の２点をInfoで行い，実際パースする部分もhrpsys-baseに追加するのが現実的だと思います．

fkanehiro · 2014-07-28T09:34:54Z

前者の方が手間がかかる、APIが変わるということはありますが、非常に難しい、という程でもないと思います。
問題は誰がやるかということで…

…name, e.g., :rarm => rarm. This change is based on JointGroup name discussed in fkanehiro#232

snozawa mentioned this issue Jun 16, 2014

(hrpsys_gazebo_atlas) :start-impedanceでエラー start-jsk/rtmros_gazebo#64

Closed

k-okada mentioned this issue Jun 18, 2014

(hrpsys_config.py) getCurrentPosition w/o arg returns all EEF positions (fix to https://github.com/start-jsk/rtmros_hironx/issues/102) #234

Closed

snozawa mentioned this issue Jul 4, 2014

atlasでインピーダンス制御を入れると手が振動する start-jsk/rtmros_gazebo#74

Closed

snozawa mentioned this issue Jul 24, 2014

SequencePlayerでZMP、waistPos, waistRpy出力の変更 #272

Open

snozawa mentioned this issue Aug 12, 2014

setTargetPoseの参照フレームを設定できるようにする． #184

Closed

snozawa added a commit to snozawa/hrpsys-base that referenced this issue Aug 14, 2014

(AutoBalancer, GaitGenerator, Stabilizer, sample) : Fix end effector …

29bfe86

…name, e.g., :rarm => rarm. This change is based on JointGroup name discussed in fkanehiro#232

snozawa mentioned this issue Aug 14, 2014

Fix end effector name #301

Merged

mmurooka mentioned this issue Sep 30, 2014

using only RTM message for servo on/off in hrpsys_dashbord start-jsk/rtmros_common#548

Open

snozawa mentioned this issue Oct 28, 2014

Add end effector frame to robot model start-jsk/rtmros_common#585

Open

k-okada mentioned this issue Mar 17, 2015

[hironx_client.py] fix startImpedance and stopImpedance for new IDL > hrpsys 315.3.0 start-jsk/rtmros_hironx#338

Merged

snozawa mentioned this issue Sep 24, 2015

setTargetPoseRelative()がうまく動作しない #820

Open

eisoku9618 mentioned this issue Nov 11, 2015

Add ee as param #878

Merged

snozawa mentioned this issue Mar 1, 2016

VirtualForceまわりでエンバグしている #959

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

ImpedanceControllerの関数で、startImpedanceを追加し、setImpedanceParamのJointPath指定方法をかえる #232

ImpedanceControllerの関数で、startImpedanceを追加し、setImpedanceParamのJointPath指定方法をかえる #232

snozawa commented Jun 16, 2014

k-okada commented Jun 16, 2014

snozawa commented Jun 17, 2014

k-okada commented Jun 17, 2014

snozawa commented Jun 18, 2014

k-okada commented Jun 18, 2014

fkanehiro commented Jun 18, 2014

fkanehiro commented Jun 18, 2014

k-okada commented Jun 18, 2014

fkanehiro commented Jun 18, 2014

emijah commented Jun 19, 2014

k-okada commented Jun 19, 2014

k-okada commented Jun 19, 2014

emijah commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

k-okada commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

snozawa commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

snozawa commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

snozawa commented Jul 24, 2014

k-okada commented Jul 24, 2014

snozawa commented Jul 24, 2014

fkanehiro commented Jul 25, 2014

fkanehiro commented Jul 25, 2014

snozawa commented Jul 26, 2014

fkanehiro commented Jul 26, 2014

snozawa commented Jul 27, 2014

fkanehiro commented Jul 28, 2014

ImpedanceControllerの関数で、startImpedanceを追加し、setImpedanceParamのJointPath指定方法をかえる #232

ImpedanceControllerの関数で、startImpedanceを追加し、setImpedanceParamのJointPath指定方法をかえる #232

Comments

snozawa commented Jun 16, 2014

k-okada commented Jun 16, 2014

snozawa commented Jun 17, 2014

k-okada commented Jun 17, 2014

snozawa commented Jun 18, 2014

k-okada commented Jun 18, 2014

fkanehiro commented Jun 18, 2014

fkanehiro commented Jun 18, 2014

k-okada commented Jun 18, 2014

fkanehiro commented Jun 18, 2014

emijah commented Jun 19, 2014

k-okada commented Jun 19, 2014

k-okada commented Jun 19, 2014

emijah commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

k-okada commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

snozawa commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

snozawa commented Jun 19, 2014

fkanehiro commented Jun 19, 2014

snozawa commented Jul 24, 2014

k-okada commented Jul 24, 2014

snozawa commented Jul 24, 2014

fkanehiro commented Jul 25, 2014

fkanehiro commented Jul 25, 2014

snozawa commented Jul 26, 2014

fkanehiro commented Jul 26, 2014

snozawa commented Jul 27, 2014

fkanehiro commented Jul 28, 2014