[robot-interface] Add option to restrict rotation angle of unlimited rotational joint

[708yamaguchi]

#319 で話されていた無限回転関節問題ですが、
:angle-vector-sequenceの引数で無限回転を制限するかどうかを選択できるようにしました。

テストコードとしてfetch実機に以下のプログラムを送ったところ、コードが腕に絡まないことが確認できました。

(load "package://fetcheus/fetch-interface.l")
(fetch-init)
(send *fetch* :reset-pose)
(send *fetch* :rarm :move-end-pos #f(100 0 0) :world)
(let (avs)
  (dotimes (i 20)
    (send *fetch* :rarm :wrist-r :joint-angle (* i 60))
    (setq avs (append avs (list (send *fetch* :angle-vector)))))
  (send *ri* :angle-vector-sequence-raw avs 3000 :arm-controller 0 :limit-rotation t))

動画

この解決方法で問題なさそうであればtravis用のテストコードを書こうと思うのですが、どうでしょうか？ @k-okada

[k-okada]

はいテストコードを書いてみてください。

[Naoki-Hiraoka]

#319 (comment)

• 基本的に関節の回転量が小さくなるような回転方向に補間する。
• 補間は-180 ~ 180度の範囲内で行うようにして、180(-180)度をまたぐときのみ0度側に遠回りをする。(例えば、無限回転関節のjoint-angleを-178から178にする時に、-178 -> -180(180) -> 178とするのではなく、-178 -> 0 -> 178とする)

2つの補間方法は両立できないので、:limit-rotationがfalseなら前者、trueなら後者になるということでしょうか。
後者の補間方法のとき、180 + 360n度で切り返すのではなく、0+360n度など、異なる角度で切りたいこともあるのではないかと思いました。

[Naoki-Hiraoka]

コードが腕に絡まることを防ぐためには、ハードウェア的に無限に回転させることができないということなので、*ri*を変えるのではなく、おおもとのURDFの関節角度上下限を有限の値に変えた方がよいのではないかと思いました。

[knorth55]

URDFを書き換えると元に戻すのが大変なので、あまり推奨しません
PR2やFetchなど常に動いているロボットは特に
元々無限回転してネジとか閉めれるはずなので、URDFでは残して他で制御すべきかと思います

[Naoki-Hiraoka]

HRP2では、小手先の変更が積み重なった結果、実際に新入生(私)が理解するのが大変なシステムになっていまして、最近このコメントの重みを実感しているところなので、できればおおもとから変えた方がよいのではないかと思います。

[knorth55]

URDF->robot_controller->eusの三段階あると思いますが、URDFから変えるのはその動作をこれ以降しないことをほぼ意味します
無限回転メリットデメリットありますが、今回はコードが絡まることを解決すれば良い話です
HRP2の話はよく知りませんし、複雑なシステムになるには避けたいですが、かと言って元の機能を制限すればよいという話ではないと思います

[k-okada]

https://sourceforge.net/p/jsk-ros-pkg/tickets/91/ https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/a228cc7d3bb5b26b92ed25c7d55f32a64d9d7825/pr2eus/test/pr2-ri-test-arm.l#L90 #319 (comment) でいろいろ書いてあるんだけど読んでみてもよくわからない感じで，状況は整理しないといけないんだけど， pr2の場合は， (dolist (angle (list 0 150 -150)) (send *robot* :larm :wrist-r :joint-angle angle)) としたときは 一回０を経由してもらいたくて，無限回転を積極的に使いたい場合は， (dolist (angle (list 150 170 -170 -150)) (send *robot* :larm :wrist-r :joint-angle angle)) としたらできる， とかなんかなっていて両立できていたかとおもいます． fetchの場合はmoveit 経由している話もあるのと，コントローラの補間機ros_controlベースで， pr2の方はpr2_controller だったかと思うので，

-実機のコントローラは無限回転の場合，最小パスを通るように回転方向を決めるようになっている

みたいにちょっと不思議な仕様になっていて，おそらくfetchはパスを考えずに補間しているので， コントローラへの送り方も少し変えないといけないかもしれない，というところですが， いずれにせよ，テストコードを作って，そもそもこういう挙動になってほしいよね，というのを 共有することが良いとおもいます． URDFで無限回転を制限するとeusのモデルも反映されて，無限回転が使えなくなるのでもったいないですね．

[Naoki-Hiraoka]

無限回転メリットデメリットありますが、今回はコードが絡まることを解決すれば良い話です
元の機能を制限すればよいという話ではない
URDFで無限回転を制限するとeusのモデルも反映されて，無限回転が使えなくなるのでもったいないですね．

fetchの知識に乏しいのでもしかしたら自分はよく理解できていないのかもしれないのですが、
コードの配線を工夫して絡まなくできるのであればこのPRは不要ですし、
それができないのであれば無限回転しようとするとコードが絡まる( = 無限回転できないロボット)なのでそもそもハードウェアによって無限回転機能が制限されており、ソフトウェアが無限回転機能を制限してしまってもったいないという話にはならないのではないかと思います。むしろmoveitが無限回転を利用するプランを出してしまわないようにURDFから変える必要があると思います。

pr2の場合は，
(dolist (angle (list 0 150 -150)) (send robot :larm :wrist-r
:joint-angle angle)) としたときは
一回０を経由してもらいたくて，無限回転を積極的に使いたい場合は，
(dolist (angle (list 150 170 -170 -150)) (send robot :larm :wrist-r
:joint-angle angle)) としたらできる，
とかなんかなっていて両立できていたかとおもいます．

現在のpr2-interfaceの実装を見た限りでは、

• 基本的に関節の回転量が小さくなるような回転方向に補間する。
• 補間は-180 ~ 180度の範囲内で行うようにして、180(-180)度をまたぐときのみ0度側に遠回りをする。(例えば、無限回転関節のjoint-angleを-178から178にする時に、-178 -> -180(180) -> 178とするのではなく、-178 -> 0 -> 178とする)

のいずれにもなっておらず、
実機に指令するときは*robot*のangle-vectorと*ri*のangle-vectorが一致するまで回るようになっています。
#1 (comment)
そのため、(dolist (angle (list 150 170 -170 -150)) ...のようにすると、170と-170の間で-340度回転します。

ところが、kinematics simulatorでは実機の場合と異なる挙動になっているというバグがあり#451 で問題になったので、#452 で直しました。

[knorth55]

fetchの知識に乏しいのでもしかしたら自分はよく理解できていないのかもしれないのですが、
コードの配線を工夫して絡まなくできるのであればこのPRは不要ですし、
それができないのであれば無限回転しようとするとコードが絡まる( = 無限回転できないロボット)なのでそもそもハードウェアによって無限回転機能が制限されており、ソフトウェアが無限回転機能を制限してしまってもったいないという話にはならないのではないかと思います。

ハードウェアを作らない人の目線で話を進めている気がします．
追加する仕様が決定していればそうですが，いま開発段階のハンドのテストなどの視点が足りていないように思います．
平岡くん自体がハンドをつくるとしたときに，最初はケーブルがあってしまうけど試してみたい，ということはあるのではないでしょうか？
ケーブル問題については，山口君が問題を感じてユニットとして分離できる，といった形にしていて，それについてはこのPRは必要ないということになりますが，他の人の今後を考えるとあって損は無いように思えます．

fetchの場合はmoveit 経由している話もあるのと

むしろmoveitが無限回転を利用するプランを出してしまわないようにURDFから変える必要があると思います。

https://github.com/fetchrobotics/fetch_ros/blob/indigo-devel/fetch_moveit_config/launch/planning_context.launch#L10
ここに変更したURDFをロードして，fetcheusのangle-vectorでangle-vector-motion-planに入る前でこのPRのようにangle-vectorをfilterすれば同じようなことができる気がします．

[knorth55]

コードの配線を工夫して絡まなくできるのであればこのPRは不要ですし、

そもそもこのPRはオプションを増やすというものでデフォルト挙動をかえるものではなく，追加機能について必要か不必要かを議論すること自体が不毛だとおもいました．
HRP2がどういった運用をして，何が問題だと感じているのか，平岡くんが共有しているコメントではあまり理解できてないのですが（そもそもHRP2が無限回転するのかも知らないですが）

[Naoki-Hiraoka]

robot-interfaceのオプションを増やす話について

現状では、

controller側には2種類のモードがあります。
A 目標角度になるまで回転する
B 今の関節角度プラスマイナス180度の変位になるように目標角度を解釈し直して回転する

pr2-controllerとkinematics simulatorはB
ros-controllerはA(多分)
になっています。

robot-interface側には4種類のモードがあります。
ア :angle-vectorをそのまま送る
イ 今の指令関節角度から:angle-vectorまでの間に補間する点を細かく追加して送る
ウ 今の指令関節角度からプラスマイナス180度の変位になるように、:angle-vectorを解釈し直してから送る
エ 今の指令関節角度から180+360nの角度をまたがないように、:angle-vectorを解釈し直してから送る

robot-interfaceとkinematics-simulator時のpr2-interfaceはア
実機時のpr2-interfaceはイ (#452 でkinematics-simulator時もイになる予定)
このPRはエ
になっています。

イ、ウ、エのモードをrobot-interfaceに追加して、オプションで選択できるようにするのは便利なので良いと思います。
エについて、180 + 360n度で切り返すのではなく、0+360n度など、異なる角度で切りたいこともあるのではないかと思いました。

[Naoki-Hiraoka]

おおもとから変えた方が良いのではないかという話について

このissueなどで行われている議論と似たようなことがこのPRで行われようとしているように感じ、数年後にどうなったのかを知っている者として老婆心ながら口を出してしまいました。
https://github.com/start-jsk/rtmros_hrp2/issues/450

HRP2では、handは頻繁に交換するだろうからとおおもとからは変えない実装にしてあり、そのぶん

https://github.com/fetchrobotics/fetch_ros/blob/indigo-devel/fetch_moveit_config/launch/planning_context.launch#L10
ここに変更したURDFをロードして，fetcheusのangle-vectorでangle-vector-motion-planに入る前でこのPRのようにangle-vectorをfilterすれば同じようなことができる気がします．

のような技巧を各所に凝らした実装になっています。
ところが、結局何年か経った今でも同じhandが引き続き使われており、
シミュレーションやプランナでハンドが使いにくい、各所に凝らされた技巧を新入生がソースコードを全読みしなければならない、多くのデモが現在の技巧に依存しており容易に変えづらい、といった、おおもとから変えていないことに起因する不便が生じています。

すぐに新しいハンドを開発していくことが確定していて、他の人は今のハンドを使わないのであれば関係ないかもしれないですが、そうでないのであればこのような先例があったということを参考までに共有させていただければと思います。

[knorth55]

@Naoki-Hiraoka

シミュレーションやプランナでハンドが使いにくい、各所に凝らされた技巧を新入生がソースコードを全読みしなければならない、多くのデモが現在の実装に依存しており容易に変えづらい、といった、おおもとから変えていないことに起因する不便が生じています。

ややこしくなるというのは，それはそうで，全てを理解せずとも隠蔽されていれば，ライトユーザは問題ないので，我々などややこしいシステムに詳しくなってしまった人は，うまく隠蔽する手法について考えるべきです．
そこをややこしいからといって大本から解決することができればいいですが，元の機能を制限するような手をとってしまうのは，よい策をとっているとは思いません．
平岡くんが感じた大変さをうまく隠蔽できるような方法を考えていくほうがいいとおもいます．
オプションを追加しまくるというのがひとつで，それをPR2,Fetchではとっています．

ところが、結局何年か経った今でも同じhandが引き続き使われており、

ハードウェアの研究をしている人がいる以上，変えられるように実装していくのが妥当で，昔にきまった方針も理解できます．
そうなると，ハンドを変更していないこと自体が罪なのではないかと個人的にはおもいます．
がんばってもっとハンドをつけかえていくか，ハンドはつけかえられないロボットとあきらめて方針転換すべきかもしれません．
ただハンドを研究している人にHRP2が使われていないという現状をみると，そこらへんの広報からはじめてもいいかもしれません（もしくは自分で作る）

まぁ結局はそのロボットを使う人たちが合意してきめればよいことで，HRP2も平岡くんが昔の人達が決めた方針に異議があるのであれば，そちらで議論をたててみてはどうでしょうか
Fetch，PR2を使いはじめるというのであればぜひ議論をしましょう．