#38 eusモデルをcolladaに変換する際に，質量情報も保持したい

[Murooka Masaki]

eusモデルをgazeboに描画しようとしていてurdfに変換しないといけないので，
[eusモデル] == convert-rbrain-model-to-collada ==> [collada] ==
collada_to_urdf ==> [urdf]
の順番にurdfを作成しました．

convert-rbrain-model-to-collada の処理の中で質量情報が無くなってしまいurdfファイルに
手書きで質量情報を加える必要がありました．
（加えたらgazebo上にJointありモデルが描画できました）

convert-rbrain-model-to-collada で質量情報を維持するためには，
質量情報に関するcolladaタグ（library_physics_modelsなど）を追加するように
jsk-ros-pkg/euslisp/jskeus/irteus/irtcollada.l
を編集する必要があると思います．

[Yohei Kakiuchi]

[r944]で 室岡くんの作ってくれた、library_physics_models, library_physics_sceneを加えるコードをマージしました。
現状ではそのまま、ダミーのmassとweightを入れています。ここは要修正。

また、ルートのノードにダミー(root_node)を入れているために、うまくphysicsが解決できない問題がのこっている。

[Yohei Kakiuchi]

また、ルートのノードにダミー(root_node)を入れているために、うまくphysicsが解決できない問題がのこっている。

[r945]で root_node -> visual1/node0 に変更した。
これで名前が解決できるようにするために、nodeにsidを追加した。node0はルートノード。

[Yohei Kakiuchi]

現状ではそのまま、ダミーのmassとweightを入れています。ここは要修正。

[r946]でmass, inertia, mass_frameをeusモデルから変換したものを入れるようにしました。

[Murooka Masaki]

(send link :inertia-tensor)
の返り値が単位行列になっているeusモデルのリンクをurdfに変換すると，
対応するリンクのinertiaは，
<inertia ixx="1e-09" ixy="0" ixz="0" iyy="1e-09" iyz="0" izz="1e-09"/>
となります．

urdfのinertiaの全成分があまりに小さいとgazebo上に表示される物体がくずれてしまうようです．
上のinertiaタグ内の1e-09を1e-03に変えたところ，gazebo上に物体が正しく表示されました．

irtcollada.l内の
(setq inertia (scale 1.0e-9 (float-vector (aref inertia 0 0) (aref inertia 1 1) (aref inertia 2 2))))
の行で，慣性主軸方向の慣性モーメントを1.0e-9倍しており，慣性モーメントが小さくなりすぎているのではないかと思います．
1.0e-9倍にはどのような意味（単位の変換？）がありますでしょうか？

[Yohei Kakiuchi]

eusで使っている慣性行列の単位は g・mm^2 で質量は g です。
urdfでは、kg・m^2 , kg を使っているので、そのための単位変換です。

小さすぎる、慣性を入れるとgazeboの計算が収束しないのはその通りだと思います。
変換時か、eusモデルの初期値にもっと大きい値を入れるのがいいと思います。

根本的には、重心位置、質量、慣性行列がそれぞれある程度もっともらしくないと、
gazeboでは変な挙動をすると思います。

[Murooka Masaki]

なるほど，eusモデルの質量情報に非現実的な値が入っていたんですね．

現状は，hrpsys_gazebo_tutorials/scripts/eus2urdf_for_gazebo_pyscript.pyで，
変換後のurdfファイルのinertiaを置換して対応しているのですが，
変換前のeusモデルに正しそうな質量情報を上書きしてから変換する方が，良さそうです．

それっぽいマスパラメータをeusモデルの各リンクに与えるeus関数みたいなものがあると良さそうですね．

[Yohei Kakiuchi]

まずは、適切なmassを設定して、それぞれのリンクのboundingboxの中心を重心、直方体の慣性行列をセットするようなことで良いのではないかと思います。