[wip] add end-coords-interpolation

[k-okada]

move robot in cartesian space interpolation

[k-okada]

手先の作業空間で補間して動くモードです． @wkentaro @yuyu2172 @pazeshun @knorth55
https://code.google.com/archive/p/hrpsys-base/issues/104 とかで確かにリクエストはあったけど今までpr2eus/robot-interface.l には入れてなくて，まぁ，各自で頑張って書いて下さいというスタンスだったんですが，見てると結構苦労しているか，案外自分では書いていないなぁということでつくろうかと思いました．https://github.com/euslisp/jskeus/pull/391　が必要です．

似たようなコードを書いている人がいたらおしえて下さい．例えばIK解いた後にNullSpaceで元の姿勢に近づくようにしているかとか，いくつか気になりました．

[pazeshun]

私の理解が正しいのかわかりませんが、これは補完された経由点全てでIKが解けないとangle-vectorを送ること自体がキャンセルされるという理解でよろしいでしょうか？
MITもこういう感じらしいので、これでいいとは思うのですが、実際どの程度成功するのかはテストしてみたいです。

[k-okada]

そうです．テストしましょう．いまは非常にプリミティブな実装になっているので，もうすこし賢くはできるはずです．

[pazeshun]

このPRでmasterをrebaseした状態で、jsk-ros-pkg/jsk_robot#746, jsk-ros-pkg/jsk_robot#747, start-jsk/jsk_apc#1987 とともに実機で実験してみました。
軽くテストした感じではちゃんと動いているようで、たいてい成功しているようです。
もう使いたいレベルなのですが、テストコードなどちゃんと考えたほうがよいのでしょうか

[pazeshun]

こちらですが、

• 最終姿勢が与えたangle-vectorと異なることが許されている気がする
• (send robot :angle-vector)で初期姿勢を取っているが、これはjoint_statesから取っているため、[Baxter] Baxter's arm goes slightly down when trajectory starts jsk_robot#738 と同様の問題が起こる恐れがある
  の二点において修正が必要そうな気がするので、方法を考えてみます

[pazeshun]

一番目の問題点を、 #273 で解決できたように思います。

[pazeshun]

二番目の問題点については、Baxter specificだと思うので、pr2eusとしては #273 で十分だと思っています。

[k-okada]

commited by #273