[panda_eus] Make :gripper method work & enable to get gripper status while moving

[pazeshun]

#3 の副作用として、(send *panda* :rarm :gripper :joint-list)の返り値に指ジョイントが含まれないようになり、グリッパの指の現在位置を知る(send *ri* :gripper :rarm :position)が常に0を返すようになってしまいました。
(send *ri* :stop-grasp :rarm :width 0.1)とかでは、現在位置から目標位置までの距離と目標所要時間から指の速度を計算して指令しているので、この速度が非常に小さくなり、ゆっくり動くようになってしまっていました。

この問題を解消するため、end-coordsの親リンクとして*_hand_tcpを使うのをやめ、*_handを使うようにしました。
yamlにURDF由来の数字が出てきてしまって二重管理になるのですが、URDF上の値をそのままコピーするだけで足し算とかは必要ないなので、まだセーフだと考えています。

end-coordsの位置が以前と変化していないこと、(send *ri* :gripper :rarm/:larm :position)が機能することは確認しました。

また、(send *ri* :get-start-grasp-result :rarm :wait nil)などとすると、現在のグリッパの動きを待つことなく前回の動作の結果を返すようにしました。
ただし、:wait nilの場合は、:start-graspを一回でも呼んだ後でないと、#1 (comment) と同じ読みにくいエラーが出てしまいます。
これは解決が難しく、エラーメッセージを読みやすくするのはあくまでオプショナルだと考えてスキップしました。

さらに、(send *ri* :get-start-grasp-status :rarm)などとすると、最後に指令した:start-graspの動作の現在の状態を返すようにしました。
actionlib_msgs::GoalStatusのstatusの値が返ってくるので、現在も動作を実行中かどうか、動作が正常終了したのかどうかなどを判定できます。

[pazeshun]

@yuki-asano 大変お待たせして申し訳ありません。JSKのデモでの動作確認は取れたのですが、ご確認いただいてもよろしいでしょうか。

[yuki-asano]

遅くなりました。ありがとうございます。 @pazeshun