[irtobot.l] add :rhand :lhand to robot-model limbs

[Naoki-Hiraoka]

ハンドをロボットのlimbとして扱いたい場合があるため，robot-modelクラスのlimbに，新たにrhand, lhandを追加しました．
副作用はありません．

[k-okada]

could you consider adding them in child class?
please add sample program, for example add hand model in demo/ to arm model.

[YoheiKakiuchi]

defaultのlimbが larm,rarm,lleg,rleg,head,torso となっていて、多くのhumanoid型ロボットはこれでいいけど、
単腕ロボットだったり、車輪ロボットだったり、ケンタウロスだったりが設定上どうしたものかとなっていると思っていて、
名前を増やしていくことが解決策としていい方向なのかな。

limbの機能を振り返ると、ほぼjoint-groupか、link-groupの役割なのだろうと思っていて、
slot に limb-list (list :larm :rarm ... :extra-limb) みたいなのを作って、
勝手な名前のlimbが追加でいるようなのがいいのかあと考えていました。

以下、マイナーな思いつきポイント

• angle-vector (joint-list) に入っているjointが、limbに所属する必要があるのはirtrobotのロボットクラスの要求だろうか？ euscolladaを修正するだけで angle-vectorには入るかもしれない。

• メソッドとして :arms,:legs があるのだけど、:hands は必要ないだろうか？

• 名前的にハンドはlimbなのか。ただネーミングとしての問題だけど。

[Naoki-Hiraoka]

• fullbody-inverse-kinematicsを解くのがデフォルトであるヒューマノイドの全身動作では、limbという概念がそもそも不要であること。
• 外界と身体の様々な部位で接触し、接触部位の位置や数が変化する多点接触動作では、end-effectorの相対位置だけでなく、end_effectorの数や親リンク自体も動的に変化するものであるため事前に定義しておくことは困難であること。
• 構造が可変のロボットの場合、limbやend-effectorはclassごとではなくinstanceごとに異なるため、(send *robot* :rarm) (send *robot* :rarm-end-coords)といったclassのmethodを用いる方法は使えないこと。
• limbやend-coordsの情報はhrpsysやtf等の他のソフトウェアとオンラインで同期をとる必要があること。[hrpsys_ros_bridge/HrpsysSeqStateROSBridgeImpl.cpp]fix cop_link_info start-jsk/rtmros_common#1056 (comment)

などといった根深い問題になるため、興味深い話ではありますが、このPRでは現状維持でrhand``lhandを追加するだけの方法をとろうと思います。

[Naoki-Hiraoka]

please` add sample program, for example add hand model in demo/ to arm model.

I added limbs (rarm, rhand) to demo/sample-arm-model.l.

[Naoki-Hiraoka]

メソッドとして :arms,:legs があるのだけど、:hands は必要ないだろうか？

:handsを追加しました

[Naoki-Hiraoka]

下流のPRがこれに依存しているため、お手すきのときにmergeしていただけますと幸いです。

[k-okada]

I added limbs (rarm, rhand) to demo/sample-arm-model.l.

サンプルプログラムを見ると，メソッド内でしかhandが使われていないのでは？このPRの必要性がわかるようにしてください．
何がしたいんだろう？

• :angle-vector でハンドも返したい？
• :joint-list もそうなる？
  など必要機能一覧を教えてほしくて，で，それの利用例をサンプルコードに入れてください．

僕は，

could you consider adding them in child class?
euscolladaを修正するだけで angle-vectorには入るかもしれない。

で良いとなる気がします．ここは伝わっていないと思うけど，グリッパはHRP2以外にもいろいろあって，関節とアクチュエータの数が同じなものは少数派です．
なので，
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/pr2-utils.l のように，子クラスで，定義しても，
send pr2 :larm :gripper :joint-angle
send pr2 :larm :gripper :joint-list
send pr2 :larm :gripper :links
が可能になります．その上で，:lhand, :rhand という名前にしましょうというルールにしているのが，このPRの目的なんじゃないか，というのが僕の予想です．そのためにも，サンプルプログラムをPRの必要性がわかるような例を入れてください．

[Naoki-Hiraoka]

このPRの目的は2つありますが、よく考えるとこのPRは不要でした。

1. ハンドの関節やリンクを、全身の他の関節やリンクとあらゆる点において全く同じように扱えるようにしたい

これは

could you consider adding them in child class?
euscolladaを修正するだけで angle-vectorには入るかもしれない。

で良かったです。

2. HRP2や他の様々なロボットが、各child classでハンドの:joint-listや:angle-vectorなどを個別に実装しているが、中身はほとんど同じなので、robot-modelの方で統一したい。(関節とアクチュエータの数が同じでない場合の対応も、各ロボットハンド個別で対応するのではなく、mimic jointのようなものをirteusに実装すれば対応できる。)

これは

名前を増やしていくことが解決策としていい方向なのかな。
根深い問題になる

:rhand :lhandを追加するだけでは根本的解決になりませんでした。

[k-okada]

簡単に諦める必要はなくて，サンプルプログラムは追加してみてください．というのと，これがmergeされないと，下流のソフトが使えないことはなくて，下流のアプリの.rosinstall にこのブランチを指定しておけば，ユーザからはwstool update, catkin build で中身がな何かは意識せず使えるのと，その.rosinstallに個々のURLが乗っていれば，このページからリンクが見えるので，どういう利用方法がされているかわかるのと，そこで使っていて，下流のソフトで１ヶ月使って問題ありあせん，というのがmerge催促の強いメッセージになります．

[Naoki-Hiraoka]

やっぱり2を解決したいので、

slot に limb-list (list :larm :rarm ... :extra-limb) みたいなのを作って、

構造が可変のロボットの場合、limbやend-effectorはclassごとではなくinstanceごとに異なるため、(send robot :rarm) (send robot :rarm-end-coords)といったclassのmethodを用いる方法は使えない

の方針をとって、(send robot :limb :rarm) (send robot :limb :rarm :end-coords)などを中心で使っていく形に変更しました。
rleg, lleg, torso, head, rarm, larmはこれまで通り、(send robot :rarm) (send robot :rarm-end-coords)といったclassのmethodを用いたり、rarm rarm-end-coords rarm-root-linkといったclassのslotを用いることができるようにして、後方互換性を残しました。

• (send self :add-limb [limb名] :end-coords [end-coords] :links [linkのリスト] :root-link [root-link]) で好きなlimbを追加できる。
• (send self :limb [limb名] ...) で好きなlimbが使える。
• rleg, lleg, torso, head, rarm, larmはこれまで通り、(send *robot* :rarm) (send *robot* :rarm-end-coords)といったclassのmethodも使える

[Naoki-Hiraoka]

roseus sample-centaur-robot.l
(setq *robot* (instance sample-centaur-robot :init))
(objects *robot*)
(send *robot* :reset-pose)
(send *robot* :limb :rbleg :move-end-pos #F(0 0 100))
(send *robot* :limb :rbleg :move-end-pos #F(0 0 -100))
(send *robot* :move-centroid-on-foot '(:lfleg :lbleg) '(:rfleg :lfleg :rbleg :lbleg))

[Naoki-Hiraoka]

というのと，これがmergeされないと，下流のソフトが使えないことはなくて

すみません。これについては自分はまだよく理解できていません。
mergeされないと、start-jsk/rtmros_choreonoid#328 (comment) のように、他の複数のPRに依存したPRや、他のPRを内包せざるを得ないPRがどんどん出現し、これらのPRに依存する開発をさらに進めてPRを出していくための管理が大変になっていくように思います。その結果、PRを出さずに自分だけの巨大なbranchで開発したくなる誘惑がどんどん大きくなってきました。

下流のアプリの.rosinstall にこのブランチを指定しておけば

この方法で解決すると思って試したことがありました。しかし、自分の.rosinstallだけを管理することは比較的容易でしたが、rtmros系は自分だけでなく複数の人が独自の.rosinstallをもっていて、自分のrosinstallと他の人のrosintallの内容を両方同時に使いたい場合も多いので、mergeされない差分が大きくなっていくと、各人のrosinstall間の整合性をとることが困難でした。

そのため、出したPRはなるべく早くにmergeするのがよいのだろうと考え、そのために最近は自分はテストやドキュメントの追加を急いで行うようにしようと考えているのですが、そうではないとすると上記問題をどのように解決したら良いのでしょうか。

[Naoki-Hiraoka]

サンプルプログラムとして、sample-centaur-robot-model.lを作成し、walk-motion.lに歩行デモを追加しました。

[Naoki-Hiraoka]

まとめると，

• slotに(limb-list (:rarm (:links ...) (:end-coords ...) (:root-link ...) (:collision-avoiance-links ...)) ...)を追加しました.
• (send *robot* :add-limb :rarm :links ... :end-coords ... :root-link ... :collision-avoiance-links ...)で追加でき，(send *robot* :remove-limb :rarm)で削除できます
• (send *robot* :rarm ...)でlimbにアクセスできます．
• 従来使えていた，slot変数rarm rarm-end-coords rarm-root-linkを使う方法や，メンバ関数:rarm-end-coords,:rarm-root-linkを使う方法も，依然としてサポートしています．ただし，:rarm :larm :rleg :lleg :head :torso限定です．
• 各サンプルロボットモデルのlimbの設定のしかたを，このPRの新しい方法に変えました．
• ケンタウロス型ロボットモデルを追加し，歩行のサンプル・テストを追加しました．