Move end-coords of single panda and dual_panda to the center of the fingers

[pazeshun]

For jsk-ros-pkg#1799 (comment)

[pazeshun]

Not tested in real robot

[pazeshun]

現時点での変更：

Before	After

@yuki-asano 右側の写真のように、end-coordsが完全には指先の中心にないのですが、これはどういった意図でしょうか。特に意図がない場合、そのうちjsk-ros-pkg#1799 (comment) とか言われてend-coordsを変えられて、 @yuki-asano のデモが動かなくなる未来が想像できる状態かと思いました。
もし可能なのであれば、この機会に変えて、end-coordsを指先中心に持ってきた方がよいのではないかと思いました。

また、現在はpanda_link8の子としてend-coordsを定義しているかと思いますが、これはどういった意図でしょうか。
eusのコードでは、まれに、angle-vectorに入っている最後のjointの子リンクの子リンクとしてend-coordsが定義されていることを想定しているコードがあった気がして、angle-vectorに入っている最後のjointの子リンクはpanda_link7なので、そのようなコードは動かなくなってしまう状態になっています（ただし、そうなっているロボットは他にもあった気がして、pandaだけが特別よくない状態になっている、というわけでもないです）。

[yuki-asano]

@pazeshun 確認ありがとうございます．

右側の写真のように、end-coordsが完全には指先の中心にないのですが、これはどういった意図でしょうか。

確かに若干中心からずれていますね．意図は思い出せないので，切れがよい数字とかそんな感じだったんじゃないかと思います．
ここを見てみると，0.1034というのが正しい値のようです．
https://github.com/frankaemika/franka_ros/blob/358406952597c3735d43625595650901be84a5d8/franka_description/robots/panda/panda.urdf.xacro#L12

これでyamlを修正すると，指の中心になるようです．この値でよさそうなら変更するのは問題ないです．

また、現在はpanda_link8の子としてend-coordsを定義しているかと思いますが、これはどういった意図でしょうか。

これも意図は思い出せないのですが，本家でもlink8を基準にしているのでそれを参考にした，とかはありうるかもしれないです．
https://github.com/frankaemika/franka_ros/blob/358406952597c3735d43625595650901be84a5d8/franka_description/robots/common/franka_robot.xacro#L17
もし，link7からだとすると，グリッパリンクの長さを測って足し合わせていかないといけないと思うのですが，自作じゃないロボットだとCAD入手も難しく計測も怪しくなってしまうので，本家プログラムの記述に出来るだけ合わせて算出を分かりやすくしたとかはありそうです．

[pazeshun]

@yuki-asano

これも意図は思い出せないのですが，本家でもlink8を基準にしているのでそれを参考にした，とかはありうるかもしれないです．
https://github.com/frankaemika/franka_ros/blob/358406952597c3735d43625595650901be84a5d8/franka_description/robots/common/franka_robot.xacro#L17

こちらですが、本家的に一番正確なのはpanda_handかと思ったのですが、これで直してみてもよろしいでしょうか。
panda_link8 -> (franka_hand.xacroのxyz引数とrpy引数でずらす) ->panda_hand -> (franka_hand.xacroのtcp_xyz引数とtcp_rpy引数でずらす) -> panda_hand_tcp
という位置関係なので、yamlの値がtcp_xyzとtcp_rpyのみに由来しているようにするためには、panda_handを親リンクにしないといけなさそうです。
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/panda/panda.urdf.xacro#L8-L10
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L26
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_hand.xacro#L6-L10
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_hand.xacro#L22-L26
ただし、xyzは0 0 0でしたが、rpyは0 0 ${-pi/4}なので、panda_handを親リンクにした場合は、このrpyの分だけyamlのrotateを変更しないといけなそうです。
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L23
(以上、バージョン0.10.1で確認しましたが、最新版でも同じ気がします。)
間違いがあればご教授いただけますと助かります。

もし，link7からだとすると，グリッパリンクの長さを測って足し合わせていかないといけないと思うのですが，自作じゃないロボットだとCAD入手も難しく計測も怪しくなってしまうので，本家プログラムの記述に出来るだけ合わせて算出を分かりやすくしたとかはありそうです．

ちなみに、link7にした場合でも、グリッパリンクの長さを測る必要はなくて、URDFに書いてあって、すごく切りの悪い数値というわけでもないです。
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_arm.xacro#L134

[pazeshun]

あっ、panda_hand_tcpを親リンクにしてend-coodsを作るのが、yamlにURDFからコピーした数字をかかないで済むようになるので、最もきれいな気がしてきました。
どうでしょうか。

[yuki-asano]

@pazeshun 詳しく調べてくれてありがとうございます．

あっ、panda_hand_tcpを親リンクにしてend-coodsを作るのが、yamlにURDFからコピーした数字をかかないで済むようになるので、最もきれいな気がしてきました。

これでうまくいきそうなら，良さそうに思います．

[pazeshun]

変更しました。

dual_panda:

Before this PR	After this PR
rarm_link7 -> rarm-end-coords	rarm_link7 -> rarm_link8 -> rarm_hand -> rarm_hand_tcp -> rarm-end-coords

Before this PRのend-coordsをローカルx軸方向に0.0104mオフセットしたものがAfter this PRのend-coords。
∵0.107 + 0.1034 - 0.2 = 0.0104
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_arm.xacro#L134
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L8
https://github.com/jsk-ros-pkg/jsk_robot/blob/d6bf03c7e9727daa0935f823efe422279739be78/jsk_panda_robot/panda_eus/models/dual_panda.yaml#L24

panda:

Before this PR	After this PR

Before this PRのend-coordsをローカルx軸方向に0.0034mオフセットしたものがAfter this PRのend-coords。
∵0.1034 - 0.1 = 0.0034
https://github.com/frankaemika/franka_ros/blob/0.10.1/franka_description/robots/common/franka_robot.xacro#L8

jsk_robot/jsk_panda_robot/panda_eus/models/panda.yaml

Line 12 in 653bb9b

translate: [0, 0, 0.1]

これでデモの方も対応できないか確認してみます。

[yuki-asano]

@pazeshun こちらいかがでしょう？自分の方は大丈夫です．

[pazeshun]

@yuki-asano すみません、ずっとバタついていて、週明けに取り組もうと考えていたところでした。もう少々お待ち下さい…

[pazeshun]

@yuki-asano 大変遅くなりましたが、デモのコードを書き換えることで、このPRで動くようにしました。
mergeしていただいても大丈夫です。

[yuki-asano]

@pazeshun ありがとうございます．