search

start-jsk / rtmros_common

OpenRTM - ROS interoperability packages
http://wiki.ros.org/rtmros_common
12 stars 52 forks source link

関節角指令を階層化したい #545

Closed mmurooka closed 9 years ago

mmurooka commented 9 years ago

首関節はトラックボールから動かして，残りの全身関節角はeusから動かすというように， 関節角指令を階層化したいのですが可能でしょうか． （旧システムのworとかhrみたいなことをやりたいです．）

ros_bridgeのコントローラとして /head_controllerと/fullbody_controllerは出ているのですが， これはお互いに上書きするようになっているでしょうか．

(参考) SampleRobotのコントローラ設定ファイルは以下．HRP2は自動生成． https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/models/SampleRobot_controller_config.yaml

k-okada commented 9 years ago

はい．それぞれ，hrpsysのコントローラにつながっていて，hrpsysは階層的にできてるので， そうなっているとおもいます／そうなっているべきです． 確認してみて下さい．

2014-09-17 16:42 GMT+09:00 Masaki Murooka notifications@github.com:

首関節はトラックボールから動かして，残りの全身関節角はeusから動かすというように， 関節角指令を階層化したいのですが可能でしょうか． （旧システムのworとかhrみたいなことをやりたいです．）

ros_bridgeのコントローラとして /head_controllerと/fullbody_controllerは出ているのですが， これはお互いに上書きするようになっているでしょうか．

(参考) SampleRobotのコントローラ設定ファイルは以下．HRP2は自動生成．

https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/models/SampleRobot_controller_config.yaml

— Reply to this email directly or view it on GitHub https://github.com/start-jsk/rtmros_common/issues/545.

mmurooka commented 9 years ago

確認したらそうなっていて

首関節はトラックボールから動かして，残りの全身関節角はeusから動かすというように，

ができました．

トラックボールの指令が終わって， eusから首も含めて全身送れるようにしたくなった場合， (/head_controllerを無効にしたくなった場合，) どのようにすればよいでしょうか．

k-okada commented 9 years ago

これはたしか removeJointGroup https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/SequencePlayerService_impl.h#L37 しないと，fullbodyが有効にならないんだよね．いわゆるスルーなモードがないですね． @snozawa 何処かで議論していた気がします

ただeuslispユーザだと:angle-vectorすると，eusは常にlimbづつに送るから トラックボール側からhead joint controllerに送る指令を止めればeusからの 全身の指令が有効になる気がします．

garaemon commented 9 years ago

ただeuslispユーザだと:angle-vectorすると，eusは常にlimbづつに送るから トラックボール側からhead joint controllerに送る指令を止めればeusからの 全身の指令が有効になる気がします．

となると、逆に頭の指令値だけ上書くというのはできないということでしょうか?

k-okada commented 9 years ago

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L144 をみると send ri :angle-vector hoge 3000 :ctype :head-controller とすると頭だけに信号を送りそうです．

2014-09-17 21:17 GMT+09:00 Ryohei Ueda notifications@github.com:

ただeuslispユーザだと:angle-vectorすると，eusは常にlimbづつに送るから トラックボール側からhead joint controllerに送る指令を止めればeusからの 全身の指令が有効になる気がします．

となると、逆に頭の指令値だけ上書くというのはできないということでしょうか?

— Reply to this email directly or view it on GitHub https://github.com/start-jsk/rtmros_common/issues/545#issuecomment-55885044 .

YoheiKakiuchi commented 9 years ago

removeJointGroupの件はこちらです。 https://github.com/fkanehiro/hrpsys-base/issues/109

limb単位のコントローラはとしてコントローラの登録が必要ですね。 ちょっとコードがわかりにくい（特に;; add-controllerと書いてあるところなど)のでもう少し簡略化 したいですね。 https://github.com/start-jsk/rtmros_gazebo/blob/master/hrpsys_gazebo_atlas/euslisp/atlas-interface.l#L27-L36 https://github.com/start-jsk/rtmros_gazebo/blob/master/hrpsys_gazebo_atlas/euslisp/atlas-interface.l#L864-L918

garaemon commented 9 years ago

send ri :angle-vector hoge 3000 :ctype :head-controller とすると頭だけに信号を送りそうです．

(send *ri* :angle-vector hoge 3000 :head-controller)

ですかね

YoheiKakiuchi commented 9 years ago

robot-interfaceでlimb単位コントロールできるサンプルを追加しています。 https://github.com/start-jsk/rtmros_tutorials/pull/82 https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/43

mmurooka commented 9 years ago

話をちゃんと追えていなくて，改めて質問させていただきたいのですが， 全身関節角をeusの(send *ri* :angle-vector)から送りつつ， 頭の関節角だけを別プログラムから上書く/上書かないを動的に切り替えられるようにしたいのですが， これはどのようにしたらいいでしょうか．

現状確認できていることは， /head_controller/follow_joint_trajectory_action/goal をpublishすると， 頭関節のみeusの指令を上書くことはできますが head_controllerのpublishをやめてもeusの関節角指令に復帰してくれずに最後にpublishした関節角にとどまってしまいます．

頭の関節角のみ上書くプログラムは， 以下のボールマウスから /head_controller/follow_joint_trajectory_action/goal をpublishするものを使っています． https://github.com/jsk-ros-pkg/jsk_control/blob/master/jsk_teleop_joy/scripts/head_control_by_trackball.py なので，できればこのままpythonからできたら嬉しいですが， eusからの方が楽であればそれでもいいとも思います．

k-okada commented 9 years ago

hrpsys側でseqが複数あるのはわかるかな？全身，頭，腕，，，とあります． で，一回でも頭に命令が来ると，あとはその値がほじされるので，頭をdisableする必要があります． 今だと，removeJointGroupして，もういっかいaddJointGroupするんだったかな． でもこれはいまいちですね，もし本当にこれしかないとすると，なにか考えたほうがいいですね． 古いhrpsysのときはSYNCみたいなものを作ったんだっけ？新しいIDLを追加しちゃうか， どこかのモードでいれるか．．．．

そのへんの話は https://github.com/fkanehiro/hrpsys-base/issues/215 ではカバーされていないですね．

2014-11-02 16:27 GMT+09:00 Masaki Murooka notifications@github.com:

話をちゃんと追えていなくて，改めて質問させていただきたいのですが， 全身関節角をeusの(send ri :angle-vector)から送りつつ， 頭の関節角だけを別プログラムから上書く/上書かないを動的に切り替えられるようにしたいのですが， これはどのようにしたらいいでしょうか．

現状確認できていることは， /head_controller/follow_joint_trajectory_action/goal をpublishすると， 頭関節のみeusの指令を上書くことはできますが head_controllerのpublishをやめてもeusの関節角指令に復帰してくれずに最後にpublishした関節角にとどまってしまいます．

頭の関節角のみ上書くプログラムは， 以下のボールマウスから /head_controller/follow_joint_trajectory_action/goal をpublishするものを使っています．

https://github.com/jsk-ros-pkg/jsk_control/blob/master/jsk_teleop_joy/scripts/head_control_by_trackball.py なので，できればこのままpythonからできたら嬉しいですが， eusからの方が楽であればそれでもいいとも思います．

— Reply to this email directly or view it on GitHub https://github.com/start-jsk/rtmros_common/issues/545#issuecomment-61396993 .

mmurooka commented 9 years ago

hrpsys側でseqが複数あるのはわかるかな？全身，頭，腕，，，とあります．

あまりちゃんとわかっていないです． rtls hrp2017c:15005/ で seq.rtc はひとつしかありませんが，これが複数のseqを含んでいるイメージでしょうか．

とりあえずちゃんとした解決策の議論を見つつ， eusから:add-joint-group，:remove-joint-groupでやりたいことができるか確認してみます．

mmurooka commented 9 years ago

以下で neck-pitch, neck-yaw を0.1[rad]に上書きした後に上書きを解除することができました． ただ，eusから :add-joint-group しても，ROSトピックの /head_controller/follow_joint_trajectory_action/goal のようなものは rostopic list で現れないため pythonから送ることはできなさそうでした．

(send *ri* :add-joint-group "head_for_teleop" (send-all (send *robot* :head :joint-list) :name))
(send *ri* :set-joint-angles-of-group "head_for_teleop" #f(0.1 0.1) 5.0)
(send *ri* :remove-joint-group "head_for_teleop")
snozawa commented 9 years ago

今だと，removeJointGroupして，もういっかいaddJointGroupするんだったかな． でもこれはいまいちですね，

これは、JointGroupのadd/removeとは別に、enable化/disable化するようにするほうがよいということでしょうか。 確かにそうですね。

古いhrpsysのときはSYNCみたいなものを作ったんだっけ？

古いシステムのものでは、部位ごとのseqが別プラグインとしてスタティックに生成されていて、 指令値がくると上書きモード、sync関数を呼べば上書きしないモードになってました。

rtls hrp2017c:15005/ で seq.rtc はひとつしかありませんが，これが複数のseqを含んでいるイメージでしょうか．

のご質問は、一つのRTCに含まれてる、というのであってると思います。

ところで、古い方では部位ごとに別RTCになってましたが、新しい方で１RTCですべて行うようにした pros/consはそれぞれ何だったでしょうか。

k-okada commented 9 years ago

ただ，eusから :add-joint-group しても，ROSトピックの /head_controller/follow_joint_trajectory_action/goal のようなものは rostopic list で現れないため pythonから送ることはできなさそうでした．

最終的にはhrpsysにrosbridge経由で送っているのでeusでできるけど，pythonでできないということは無いです．もう一度確認してください．

ところで、古い方では部位ごとに別RTCになってましたが、新しい方で１RTCですべて行うようにした

requestしたらこうなって出てきた，気がします．

2014-11-04 10:37 GMT+09:00 Shunichi Nozawa notifications@github.com:

今だと，removeJointGroupして，もういっかいaddJointGroupするんだったかな． でもこれはいまいちですね，

これは、JointGroupのadd/removeとは別に、enable化/disable化するようにするほうがよいということでしょうか。 確かにそうですね。

古いhrpsysのときはSYNCみたいなものを作ったんだっけ？

古いシステムのものでは、部位ごとのseqが別プラグインとしてスタティックに生成されていて、 指令値がくると上書きモード、sync関数を呼べば上書きしないモードになってました。

rtls hrp2017c:15005/ で seq.rtc はひとつしかありませんが，これが複数のseqを含んでいるイメージでしょうか．

のご質問は、一つのRTCに含まれてる、というのであってると思います。

ところで、古い方では部位ごとに別RTCになってましたが、新しい方で１RTCですべて行うようにした pros/consはそれぞれ何だったでしょうか。

— Reply to this email directly or view it on GitHub https://github.com/start-jsk/rtmros_common/issues/545#issuecomment-61580540 .

mmurooka commented 9 years ago

最終的にはhrpsysにrosbridge経由で送っているのでeusでできるけど，pythonでできないということは無いです

確かにそうでした．

eusから :add-joint-group, :set-joint-angles-of-group した場合は， SequencePlayerのサービスが呼ばれていて， このサービスはrequestの中にgroup名をもっており， 新しい名前のtopic,serviceができることはないということでした． https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/SequencePlayer/SequencePlayerService_impl.cpp#L153-L156

自分の整理ようにまとめると， ROSのレイヤから関節角を送る方法は以下の3通りくらいあるみたいで， この中で A がROS（eus）から動的に関節グループを登録・削除できる． それで，関節グループを毎回登録・削除するのではなく， グループを保持したまま有効・無効にできたらなお良いということですね．

A.
service： /SequencePlayerServiceROSBridge/setJointAnglesOfGroup
node： /SequencePlayerServiceROSBridge
B.
topic： /fullbody_controller/command
node： /HrpsysSeqStateROSBridge
C.
topic: /head_controller/command （などのlimbごとにあるトピック）
node: /HrpsysJointTrajectoryBridge

pythonからはAのサービスを送ればよいので， とりあえず毎回add,removeする方法であればやりたいことができそうです．

snozawa commented 9 years ago

ところで、古い方では部位ごとに別RTCになってましたが、新しい方で１RTCですべて行うようにした requestしたらこうなって出てきた，気がします．

requestって何でしたっけ？

snozawa commented 9 years ago

ところで、古い方では部位ごとに別RTCになってましたが、新しい方で１RTCですべて行うようにした

今のスタイルのように１RTCでできてると、動的にGroupがaddできたりするので、便利ですね。

自分の整理ようにまとめると， ROSのレイヤから関節角を送る方法は以下の3通りくらいあるみたいで， この中で A がROS（eus）から動的に関節グループを登録・削除できる． それで，関節グループを毎回登録・削除するのではなく， グループを保持したまま有効・無効にできたらなお良いということですね．

そうですね、個人的にも登録・削除と有効・無効が別なのがよさそうに思います。 登録は、jointのgroupをしていしないといけなくて、 groupの名前とジョイントの名前リストを与えないといけない。 登録後、有効無効だけできるようにすれば、groupの名前だけ引数に与えればOKですね。

k-okada commented 9 years ago

requestって何でしたっけ？ https://github.com/fkanehiro/hrpsys-base/issues/36

です．金広さんがさくっと実装してくれました．これみると，やっぱ最近は忙しいんですかね．．．

2014-11-05 8:30 GMT+09:00 Shunichi Nozawa notifications@github.com:

ところで、古い方では部位ごとに別RTCになってましたが、新しい方で１RTCですべて行うようにした requestしたらこうなって出てきた，気がします．

requestって何でしたっけ？

— Reply to this email directly or view it on GitHub https://github.com/start-jsk/rtmros_common/issues/545#issuecomment-61734591 .

snozawa commented 9 years ago

requestって何でしたっけ？ https://github.com/fkanehiro/hrpsys-base/issues/36 です．金広さんがさくっと実装してくれました．これみると，やっぱ最近は忙しいんですかね．．．

なるほど、ありがとうございます、このタイミングだったんですね。

さっきオフラインではなしていて@mmurookaさんのcommentにちょとほそくで、 A、B、Cのうち、Aは多分つかわない方向になると思います。 なぜなら、jointtrajecctoryを介さず直接seqをよんでるためです。

@k-okadaさん ちなみに、HrpsysSeqStaetROsBridgeとHrpsysJointTrajectoryBridgeの両方に setJointAnglesがあるのですが、fullbody_controller的につかうと今どちらが使われていますか？

使われてない方は消してOKでしょうか？

YoheiKakiuchi commented 9 years ago

setJointAnglesがあるのですが、fullbody_controller的につかうと今どちらが使われていますか？

HrpsysSeqStaetROsBridge が使われます。 HrpsysJointTrajectoryBridgeこっちはグループだけ使うことを想定していますが、 group nameの指定が無いときはfullbodyになるように書いてありますね。 問題がありそうなら修正しますが、実質は使っていないと思います。

YoheiKakiuchi commented 9 years ago

うまく説明できるか分からないけど、joint groupを使った今の作戦だと、 現状の2つのソースがあってどちらかの入力を選ぶという目的には使えるけど、 関節角度を送りたいところが3つ以上になると破綻するように思う。

いい案が浮かばないので、とにかく動く方法でいいとは思いますが、 PR2の今のデモが優先度が決められなくなっていて、joint groupを使う作戦はPR2には使えないなとか、考えながら。

snozawa commented 9 years ago

HrpsysSeqStaetROsBridge が使われます。 HrpsysJointTrajectoryBridgeこっちはグループだけ使うことを想定していますが、 group nameの指定が無いときはfullbodyになるように書いてありますね。 問題がありそうなら修正しますが、実質は使っていないと思います。

これは差し支えないなら、使ってない方を削除するとよさそうに思います。 HrpsysSeqStateROSBridgeでステートやりとりを、 HrpsysJointTrajectoryBridgeでfullbodyもxx_controllerをやるとROSノードの名前と合致してるきがしますが、 どうでしょうか。 （HrpsysSeqStateROSBridgeのsetJointAnglesをけすほうこう）

mmurooka commented 9 years ago

とりあえず addGroup, removeGroupを使って https://github.com/jsk-ros-pkg/jsk_control/pull/131 でやりたいことはできました．

mmurooka commented 9 years ago

いったんcloseします．