[euscollada] keep sensor_id correctly from collada

[Naoki-Hiraoka]

https://github.com/start-jsk/rtmros_common/issues/1097 やhttps://github.com/start-jsk/rtmros_common/pull/1080#issuecomment-561094288 で報告されているバグを直すPRです。 SampleRobotやJAXON_REDといった、ロボットのBRMLファイルにForceSensorが書かれている行の順番とsensorIdが一致していないロボットで，euslispのforce-sensorの順番がsensorIdの順番通りにならないバグを直しました.

原因は、euscolladaがcolladaファイル中の誤ったデータを参照してeuslispのforce-sensorのsensor-idとしていたためでした。

SampleRobot VRMLのforce-sensor [lfsensor, rfsensor, lhsensor, rhsensor]の順にsensor_idがふられている https://github.com/fkanehiro/openhrp3/blob/5efda8321a5782f9dee62af470b8753a8a342a7f/sample/model/sample1.wrl#L1112-L1115 https://github.com/fkanehiro/openhrp3/blob/5efda8321a5782f9dee62af470b8753a8a342a7f/sample/model/sample1.wrl#L1254-L1257 https://github.com/fkanehiro/openhrp3/blob/5efda8321a5782f9dee62af470b8753a8a342a7f/sample/model/sample1.wrl#L673-L676 https://github.com/fkanehiro/openhrp3/blob/5efda8321a5782f9dee62af470b8753a8a342a7f/sample/model/sample1.wrl#L865-L868

SampleRobot euslispのforce-sensor(Before this PR) VRMLのsensor_idの順になっていない

1.irteusgl$ load "package://hrpsys_ros_bridge/models/samplerobot.l"
t
2.irteusgl$ (setq *robot* (samplerobot))
#<samplerobot-robot #X55d626a221d8 SampleRobot  0.0 0.0 0.0 / 0.0 0.0 0.0>
3.irteusgl$ send *robot* :force-sensors
(#<cascaded-coords #X55d626b4b690 lhsensor  5.100e-06 300.0 50.0 / -0.003 -0.334 0.198> #<cascaded-coords #X55d626b58810 rhsensor  5.100e-06 -300.0 50.0 / 0.003 -0.334 -0.198> #<cascaded-coords #X55d626b623d8 lfsensor  -0.81 90.017 -668.15 / -0.0 0.001 0.0> #<cascaded-coords #X55d626b62318 rfsensor  -0.81 -89.983 -668.15 / -0.0 0.001 0.0>)

SampleRobot euslispのforce-sensor(After this PR) VRMLのsensor_idの順になっている

1.irteusgl$ load "package://hrpsys_ros_bridge/models/samplerobot.l"
t
2.irteusgl$ (setq *robot* (samplerobot))
#<samplerobot-robot #X5576d41f3220 SampleRobot  0.0 0.0 0.0 / 0.0 0.0 0.0>
3.irteusgl$ send *robot* :force-sensors
(#<cascaded-coords #X5576d43249e8 lfsensor  -0.81 90.017 -668.15 / -0.0 0.001 0.0> #<cascaded-coords #X5576d4333558 rfsensor  -0.81 -89.983 -668.15 / -0.0 0.001 0.0> #<cascaded-coords #X5576d433db70 lhsensor  5.100e-06 300.0 50.0 / -0.003 -0.334 0.198> #<cascaded-coords #X5576d4340000 rhsensor  5.100e-06 -300.0 50.0 / 0.003 -0.334 -0.198>)

このバグによって現在、JAXON_REDが車椅子を押すときに、(send *ri* :set-ref-force #F(0 0 -10) 4000 :rarm)などとすると意図したlimbとは違うlimbの目標力がセットされてしまうという問題が発生しています。

[k-okada]

テストコードをこちらに入れてくれればと思います．

[Naoki-Hiraoka]

JAXON_RED実機で，AutoBalancerをつけた状態で

(send *ri* :set-ref-force #F(50 0 0) 4000 :rarm)

をすると，実機の重心位置が後方に遷移することと，rtprint -n 1 localhost:15005/sh.rtc.rhsensorOutの結果に正しく目標反力が反映されていることを確認しました.

:larm :rleg, :llegについても同様に確認しました(当然ですがlegの場合は重心位置は後方に遷移しませんでした)

[Naoki-Hiraoka]

テストを追加しました. このPR無しの場合の結果はhttps://github.com/jsk-ros-pkg/jsk_model_tools/pull/237 にあります.

https://github.com/jsk-ros-pkg/jsk_model_tools/pull/233 がmergeされると，テストが通るようになります.

[ketaro-m]

僕の環境でも，ローカルでこのブランチを取り込んでビルドしたところ，意図した挙動をすることを確認できました． 実験を進める上でこの変更は必要なのですが，このPRをマージしていただけないでしょうか．

[k-okada]

遅くなりました．マージしました．