[HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal comes #1103

Naoki-Hiraoka · 2020-12-29T03:21:09Z

fullbody_controller/follow_joint_trajectory_action/goalに届いたgoalに、ロボットの全関節の名前が含まれていない場合に、含まれていない関節が意図せずに動いてしまうバグを直すPullRequestです。

rosの標準的なjoint_trajectory_controllerは、goalにロボットの全関節の名前が含まれていない場合、エラーを返すか、含まれている関節の指令値だけを更新します。(http://wiki.ros.org/joint_trajectory_controller のallow_partial_joints_goal)

HrpsysSeqStateROSBridgeは、hrpsysのSetJointAnglesインタフェースが一部の関節の指令値だけを更新する機能に対応していないので、goalに含まれている関節の指令値はgoalの角度に、含まれていない関節の指令値は現在のactualの角度に、なるように更新しています。actualの角度は現在の指令値とは異なるので、goal含まれていない関節の指令値が意図せずに変化してしまっていました。

このPull Requestでは、goalに含まれていない関節の指令値は現在の指令値になるように更新するようにしました。

…omes

Masanori-Konishi · 2022-06-16T04:19:44Z

@kindsenior start-jsk/rtmros_choreonoid#362 で、一度ロボットに:go-wheelを呼んだ後だと、ロボットにeuslispから:angle-vectorを送るたびに、車輪が意図せずに大きく動いてしまうという問題がありました。車輪は、SequencePlayerが出力した指令関節角度にAutoBalancerが別の指令値を(上書きではなく)足し込む、という処理を行っています。車輪はeusmodelのanlge-vectorに含まれていないため、このPullRequestで指摘されているバグによって、angle-vectorを送るたびに車輪の指令値が現在のactual値に上書きされ、AutoBalancerが足しこんでいた値の分だけ動いてしまうことが原因でした。

このPullRequestを使うと、意図せずに動いてしまう挙動がなくなりました。

(執筆　Naoki-Hiraoka)

pazeshun · 2022-06-16T12:32:14Z

@Naoki-Hiraoka
このPRの存在に気がついてなくて申し訳なかったのですが、何点か質問があります。

rosの標準のjoint_trajectory_controllerは、allow_partial_joints_goalがfalseだからエラーを返す、という理解であってるでしょうか？それに本当に合わせるなら、デフォルトではエラーを返すようにした方がよいように思います。今出先で調べられないんですが、hrpsys_ros_bridge.launchに引数を追加するのって難しいんでしたっけ、そうだったかもしれません。
これ、例えば起動直後とか、非常停止から復帰した直後とかに部分的な指令が送られても、大丈夫なものなのでしょうか。hrpsysの仕様がよくわかってなくて申し訳ないですが。

pazeshun · 2022-06-16T13:23:09Z

1つめの質問に関してですが、もしかして、根本の問題は、hrpsysにおける関節数と、robot-interfaceにおける関節数が違っていて、その場合には必ず、:angle-vectorから送られない関節が出てくるので、僕の指摘に従うと必ずエラーが出てしまうということでしょうか。
そのようなロボットの例を教えてほしいです。
僕の理解不足で、あらゆるロボットでこの問題が起こります、とかだったら申し訳ないけど…。
もしそうじゃないとしたら、fullbody-controllerじゃなくてlimbごとのcontroller(もしくはわざともっと細かくわけたcontroller)で送るのが一般的なのかなって気がしました。

Naoki-Hiraoka · 2022-06-17T03:17:26Z

rosの標準のjoint_trajectory_controllerは、allow_partial_joints_goalがfalseだからエラーを返す、という理解であってるでしょうか？それに本当に合わせるなら、デフォルトではエラーを返すようにした方がよいように思います。今出先で調べられないんですが、hrpsys_ros_bridge.launchに引数を追加するのって難しいんでしたっけ、そうだったかもしれません。
? 1つめの質問に関してですが、もしかして、根本の問題は、hrpsysにおける関節数と、robot-interfaceにおける関節数が違っていて、その場合には必ず、:angle-vectorから送られない関節が出てくるので、僕の指摘に従うと必ずエラーが出てしまうということでしょうか。
そのようなロボットの例を教えてほしいです。

hrpsysにおける関節数と、robot-interfaceにおける関節数が違う例として、次の2つがあります. (どちらも僕自身はあまり使っていません)

https://github.com/start-jsk/rtmros_choreonoid/tree/master/hrpsys_choreonoid_tutorials のJAXON_JVRC
- ハンドがhrpsysには含まれているがrobot-interfaceには含まれていない
[WIP] auto-stabilizer for jaxon wheel rtmros_choreonoid#362 のJAXON_JVRC
- 車輪がhrpsysには含まれているがrobot-interfaceには含まれていない

僕が当時このPullRequestを出したときには、HRP2JSKNTSの全身行動制御用のc++の独自のfullbody_controllerのclientを使っていて、当時つま先関節をうまく扱えなかったため、つま先関節がhrpsysでは含まれているがclient側には含まれていませんでした。

そのため、あらゆるロボットのrobot-interfaceの通常使用でこの問題が起こるわけではありませんが、あらゆるロボットでちょっとallow_partial_joints_goalの機能を利用して動かしたいというケースはあるのかなと思います。

僕自身は、allow_partial_joints_goalをROS paramから受け取れるようにしてrosの標準に合わせれば良いと思いますが、今既にmasterにmergeされているhttps://github.com/start-jsk/rtmros_choreonoid/tree/master/hrpsys_choreonoid_tutorials のJAXON_JVRC が今のHrpsysSeqStateROBBridgeのエラーを返さない仕様を利用してしまっているように、後方互換性を失い既存のコードが動かなくなる可能性があります。hrpsys_ros_bridge.launchに引数を追加するプログラム作業自体は簡単ですが、後方互換性を確保するまでの開発制度が難しそうに思います。

そのため、エラーを返さない仕様のままでいいかなと思っています。後方互換性のために徐々にrosの標準からかけ離れていって使いにくさが閾値を超えたら、新しく別のHrpsyqStateROSBridgeを作れば良いと思います。

これ、例えば起動直後とか、非常停止から復帰した直後とかに部分的な指令が送られても、大丈夫なものなのでしょうか。hrpsysの仕様がよくわかってなくて申し訳ないですが。

少なくとも今よりも悪化することはほぼ無いと思います。

このPull Requestでは、goalに含まれていない関節の指令値は現在のStateHolderのqOutの値になるように更新します。

rtmros_common/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch

Line 126 in 07f7e19

    
           <rtconnect from="sh.rtc:qOut" to="HrpsysSeqStateROSBridge0.rtc:mcangle" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>

したがって、指令関節角度を送ったときに、HrpsysSeqStateROSBridgeにStateHolderのqOutが届いていなかったり、変な値になっていると、危険ということになります。しかし、もともとのhrpsysの仕様として、指令関節角度を送ったときに、SequencePlayerにStateHolderのqOutが届いていなかったり、変な値になっていると危険な処理が既に存在します。

具体的には、もともとのhrpsysで、HrpsysSeqStateROSBridgeに指令関節角度が送られると、SeqencePlayerのsetJointAnglesまたはsetJointAnglesSequenceが呼ばれます。SeqencePlayerのsetJointAnglesやsetJointAnglesSequenceでは、目標軌道の時刻0の指令関節角度として、StateHolderのqOutの値を使います。

rtmros_common/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp

Lines 217 to 227 in 07f7e19

    
           if ( duration.length() == 1 ) { 
        
             m_service0->setJointAngles(angles[0], duration[0]); 
        
           } else { 
        
             // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API 
        
             if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) { 
        
               OpenHRP::dSequenceSequence rpy, zmp; 
        
               m_service0->playPattern(angles, rpy, zmp, duration); 
        
             } else { 
        
               m_service0->setJointAnglesSequence(angles, duration); 
        
             } 
        
           }

https://github.com/fkanehiro/hrpsys-base/blob/72af4f664a426c581924228d94fd98babe0e6723/rtc/SequencePlayer/SequencePlayer.cpp#L421
https://github.com/fkanehiro/hrpsys-base/blob/72af4f664a426c581924228d94fd98babe0e6723/rtc/SequencePlayer/SequencePlayer.cpp#L754
https://github.com/fkanehiro/hrpsys-base/blob/72af4f664a426c581924228d94fd98babe0e6723/rtc/SequencePlayer/SequencePlayer.cpp#L765
https://github.com/fkanehiro/hrpsys-base/blob/72af4f664a426c581924228d94fd98babe0e6723/python/hrpsys_config.py#L375

SequencePlayerとStateHolderは同一プロセスで立ち上げることが多いですが、HrpsysSeqStateROSBridgeとStateHolderは別プロセス、場合によっては別PCで立ち上げるため、通信の信頼度が違うということは言えますが、気にする程では無いのでは無いかと思います。

pazeshun · 2022-06-17T06:53:08Z

hrpsysにおける関節数と、robot-interfaceにおける関節数が違う例として、次の2つがあります. (どちらも僕自身はあまり使っていません)
* https://github.com/start-jsk/rtmros_choreonoid/tree/master/hrpsys_choreonoid_tutorials のJAXON_JVRC
  
  * ハンドがhrpsysには含まれているがrobot-interfaceには含まれていない

* https://github.com/start-jsk/rtmros_choreonoid/pull/362 のJAXON_JVRC
  * 車輪がhrpsysには含まれているがrobot-interfaceには含まれていない

なるほど、choreonoid上でハンドを動かせるモデルを用意した場合には、hrpsysにハンド関節を含まざるを得ず、ros_bridge以上のレイヤは実機と同じにしたいので、hrpsysとrobot-interfaceと関節数が異なってくることをすっかり忘れていました。
（後からこのコメント読む人向けの情報ですが、例えばhttps://github.com/start-jsk/rtmros_hrp2/issues/450#issuecomment-268421960 で指摘されていて、関連情報はhttps://github.com/start-jsk/rtmros_hrp2/issues/578 から辿れると思います。）
もうちょっと詳しく教えてほしいんですが、関節への指令角度が現在角度に上書きされると困るのはどういう場合でしょうか。
例えば、関節が柔らかめのロボット（PR2, Baxter, Panda, ...）の場合は、腕に外力（重力、環境との接触力・・・）が加わって指令角度と現在角度が乖離している時に、指令角度が現在角度で上書きされると、外力に抗う力が失われる、というのがあります。
JAXONのハンド関節でもそうなっているということでしょうか。
確かに、特にシミュレーションだとそうなりやすいのかも、とは感じました。

僕が当時このPullRequestを出したときには、HRP2JSKNTSの全身行動制御用のc++の独自のfullbody_controllerのclientを使っていて、当時つま先関節をうまく扱えなかったため、つま先関節がhrpsysでは含まれているがclient側には含まれていませんでした。

@Naoki-Hiraoka のシステムではまだこの問題は残っていますか？
ちゃんと理解できているかわかりませんが、これはつま先関節が含まれていないclientが悪いような気がして、euslispでいうと(send *ri* :state :reference-vector)で取得できるような指令角度をつま先関節に入れておくようにすればいいだけだったのではないかという気がします。
というより、今までは1関節だけ動かしたい、みたいな時はそうしているのではないかという気がします。
（仕様がわかってないんですが、(send *ri* :state :reference-vector)とStateHolderのqOutが違う可能性もありますね、そうだったらすみません。）

また、@Masanori-Konishi の車輪付きロボットで起こる不具合を読んであれっと思ったのですが、AutoBalancerによる関節角度変更ってStateHolderのqOutには反映されないんでしょうか。
もしそうだとすると、例えばAutoBalancerを入れて動かしていた状態からAutoBalancerを切って部分的なangle-vectorを送ると、angle-vectorに含まれていない関節はqOutの位置まで勝手に動くことになりますか？

具体的には、もともとのhrpsysで、HrpsysSeqStateROSBridgeに指令関節角度が送られると、SeqencePlayerのsetJointAnglesまたはsetJointAnglesSequenceが呼ばれます。SeqencePlayerのsetJointAnglesやsetJointAnglesSequenceでは、目標軌道の時刻0の指令関節角度として、StateHolderのqOutの値を使います。

を読むと、angle-vectorに含まれている関節でも、急に動く動きが出てきてもおかしくなさそうな感じですが・・・。
何か勘違いしていると思うので、教えてください。

起動直後とか、非常停止から復帰した直後とかに部分的な指令が送られた場合であっても、危険性が悪化するわけではない、ということは理解しました、詳しく説明してくれてありがとうございます。

Naoki-Hiraoka · 2022-06-17T07:39:14Z

もうちょっと詳しく教えてほしいんですが、関節への指令角度が現在角度に上書きされると困るのはどういう場合でしょうか。

かたい位置制御のロボットであっても、指令角度が現在角度で上書きされると外力の方向にわずかに動いてしまうため、何度もangle-vectorを送っているとそれが蓄積し大きな動きになってしまいます。

また、AutoBalancerといったStateHolderの先のrtcによって指令関節角度が上書きされている場合、指令角度が現在角度で上書きされると、StateHolderの指令関節角度が大きく変化してしまいます。その結果、AutoBalancerといったStateHolderの先のrtcの挙動にも予期せぬ影響を与えます。悪影響が出ないような実装をされているrtcもあれば、出るような実装をされているrtcもあります。

@Naoki-Hiraoka のシステムではまだこの問題は残っていますか？

おっしゃっている方法も含めこの問題の解決方法はいろいろあるので、この問題は今は残っていません。そのため、まあいいかと2020年にこのPull Requestを出してから放置してきました。が、今回再び@Masanori-Konishi がこのバグで困っていたため、やっぱりこのバグを直しておくべきだと思った次第です。

（仕様がわかってないんですが、(send ri :state :reference-vector)とStateHolderのqOutが違う可能性もありますね、そうだったらすみません。）

恐らく同じだと思います。

AutoBalancerによる関節角度変更ってStateHolderのqOutには反映されないんでしょうか。
もしそうだとすると、例えばAutoBalancerを入れて動かしていた状態からAutoBalancerを切って部分的なangle-vectorを送ると、angle-vectorに含まれていない関節はqOutの位置まで勝手に動くことになりますか？

反映されません。 AutoBalancerを切った段階で、全関節はqOutの位置まで勝手に動きます。

余談ですが、このへんの仕様については、各自の研究で都合のようにカスタマイズするべきところですが、カスタマイズするための方法として、従来のstart-jskの標準システムはhrpsysのhrpsys_config.pyという一つのファイルに対してみんなでブランチを切って編集するという方法をとってきました。そのため、コンフリクトしたり、各自の個人の研究のための部分とみんなで共有する部分の切り分けが困難だったりと、とても不便でした。僕の自分の研究では、hrpsys_config.pyを使っていません。

angle-vectorに含まれている関節でも、急に動く動きが出てきてもおかしくなさそうな感じですが・・・。
何か勘違いしていると思うので、教えてください。

基本的には前回の周期のSeqencePlayerの出力がStateHolderのqOutに入っているため、問題ありません。

servoOn時にStateHolderでgoActualしたときの姿勢をSeqencePlayerに反映するために、StateHolderのqOutを読む仕様になっているのだと思います。

pazeshun · 2022-06-17T16:23:20Z

かたい位置制御のロボットであっても、指令角度が現在角度で上書きされると外力の方向にわずかに動いてしまうため、何度もangle-vectorを送っているとそれが蓄積し大きな動きになってしまいます。

また、AutoBalancerといったStateHolderの先のrtcによって指令関節角度が上書きされている場合、指令角度が現在角度で上書きされると、StateHolderの指令関節角度が大きく変化してしまいます。その結果、AutoBalancerといったStateHolderの先のrtcの挙動にも予期せぬ影響を与えます。悪影響が出ないような実装をされているrtcもあれば、出るような実装をされているrtcもあります。

ありがとうございます、理解しました。

多分このPRの内容で大丈夫だとは思うのですが、動かしたくない関節への指令値どうするか問題では何度か痛い目を見ていて不安が拭えないので、週明けにHiroで実験してみたいと思います。

pazeshun · 2022-06-28T10:37:25Z

こちらのPRを適用して、Hiroで以下のようなテストコードを実行してみました。
https://gist.github.com/pazeshun/a98bc90485ec4cd89243f3494c642639
robot-interfaceのrarm-controllerの送り先をfullbody_controllerに指定することで、goalに含まれない関節が存在する状態を作り出した上で、繰り返し動作を行わせて、動作前後でのgoalに含まれない関節の角度のdiffを計算するものです。
このコードを走らせても、diffは非常に小さいことを確認しました。

1.irteusgl$ test
;; (make-irtviewer) executed
[ WARN] [1656411684.646091801]: [fullbody_controller/follow_joint_trajectory_action] feedback-cb #<control_msgs::followjointtrajectoryactionfeedback #X55875f310f18> received wrong       goal
...
[ INFO] [1656412296.884039561]: larm av diff: #f(0.0005 0.0 0.0 0.0 0.001172 0.0)
t

pazeshun · 2022-06-28T10:55:09Z

このPRを適用しない状態だと、適用した場合と比べて大きなdiffが出ることを確認しました。

[ INFO] [1656413573.930113692]: larm av diff: #f(-0.0045 -0.000429 0.012 0.052326 0.003516 0.001308)

Naoki-Hiraoka · 2022-06-28T10:58:35Z

@pazeshun ご確認ありがとうございました。

@Masanori-Konishi @kindsenior その後wheelは問題なく動いていますか?

Masanori-Konishi · 2022-06-28T11:05:59Z

二週間程度がっつり動かしていますが、当該の問題点は解消していて、その後も特に問題は起きていないです。

Naoki-Hiraoka · 2022-06-28T11:15:00Z

ありがとうございます。

@k-okada こちらのmergeをお願いできますでしょうか。

Naoki-Hiraoka · 2022-06-29T03:07:15Z

ありがとうございます。

[HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal c…

facea76

…omes

Merge branch 'master' into fix-fullbody-controller

9c909ab

Masanori-Konishi mentioned this pull request Jun 16, 2022

Auto stabilizer wheel fix fullbody controller go wheel no wait kindsenior/rtmros_common#3

Merged

pazeshun approved these changes Jun 28, 2022

View reviewed changes

k-okada merged commit 535106b into start-jsk:master Jun 29, 2022

Naoki-Hiraoka deleted the fix-fullbody-controller branch June 29, 2022 03:07

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal comes #1103

[HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal comes #1103

Naoki-Hiraoka commented Dec 29, 2020

Masanori-Konishi commented Jun 16, 2022

pazeshun commented Jun 16, 2022

pazeshun commented Jun 16, 2022

Naoki-Hiraoka commented Jun 17, 2022 •

edited

Loading

pazeshun commented Jun 17, 2022 •

edited

Loading

Naoki-Hiraoka commented Jun 17, 2022

pazeshun commented Jun 17, 2022

pazeshun commented Jun 28, 2022

pazeshun commented Jun 28, 2022

Naoki-Hiraoka commented Jun 28, 2022

Masanori-Konishi commented Jun 28, 2022

Naoki-Hiraoka commented Jun 28, 2022

Naoki-Hiraoka commented Jun 29, 2022

[HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal comes #1103

[HrpsysSeqStateROSBridge.cpp] set body to reference angle when Goal comes #1103

Conversation

Naoki-Hiraoka commented Dec 29, 2020

Masanori-Konishi commented Jun 16, 2022

pazeshun commented Jun 16, 2022

pazeshun commented Jun 16, 2022

Naoki-Hiraoka commented Jun 17, 2022 • edited Loading

pazeshun commented Jun 17, 2022 • edited Loading

Naoki-Hiraoka commented Jun 17, 2022

pazeshun commented Jun 17, 2022

pazeshun commented Jun 28, 2022

pazeshun commented Jun 28, 2022

Naoki-Hiraoka commented Jun 28, 2022

Masanori-Konishi commented Jun 28, 2022

Naoki-Hiraoka commented Jun 28, 2022

Naoki-Hiraoka commented Jun 29, 2022

Naoki-Hiraoka commented Jun 17, 2022 •

edited

Loading

pazeshun commented Jun 17, 2022 •

edited

Loading