adds to new feature of the collisions data output

hmatsushita commented 7 years ago

接触情報の出力を追加しました。

以下のコマンドで取得することが可能です。

$ rostopic echo /choreonoid/[world name]/contacts/physics

出力は以下のメッセージタイプを使用しています。

http://docs.ros.org/jade/api/gazebo_msgs/html/msg/ContactState.html

fkanehiro commented 7 years ago

出力情報が正しいどうかを確認するために、1辺の長さが1mで重さが1kgの立方体をz=0の平面上に落とした場合の出力内容を教えてください。それともう少し複雑なシミュレーション実行時にWorldRosItem有りの場合と無しの場合でどの程度のオーバヘッドがあるか確認して下さい。

hmatsushita commented 7 years ago

出力情報の検証について承知いたしました。これまでのご指摘の内容および、Travis CI 環境でのビルドエラーを含め対応いたしますので、暫しお時間をください。

hmatsushita commented 7 years ago

詳細な調査は実施しておりませんが、Travis CI 環境でのビルドエラーにつきましては、simtrans のビルドに失敗している事が原因のようです。

ローカル環境で HEAD ブランチの simtrans をビルドしたところ、同様のエラーが生じました。

byte-compiling build/bdist.linux-x86_64/egg/simtrans/sdf.py to sdf.pyc
  File "build/bdist.linux-x86_64/egg/simtrans/sdf.py", line 215
    am.effortlimit = [float(limit.find('effort').text]
                                                     ^
SyntaxError: invalid syntax

fkanehiro commented 7 years ago

修正しました。

hmatsushita commented 7 years ago

ご対応ありがとうございます。問題が解決されている事を確認いたしました。

hmatsushita commented 7 years ago

大変遅くなりましたが、改修を実施いたしました。 1m 辺で質量 1Kg の箱を平面上へ落とした際の、それぞれの出力を以下に記します。

gazebo_ros_pkgs

collision1_name: box::BOX01KG::BOX01KG_collision
collision2_name: ground_plane::link::collision
wrenches: 
  - 
    force: 
      x: 8.38148103009e-08
      y: 7.29408721805e-08
      z: 2.42549989724
    torque: 
      x: -1.21274991215
      y: 1.21274990671
      z: 5.43696906022e-09
  - 
    force: 
      x: 1.110008861e-07
      y: 1.26416553395e-07
      z: 2.42550049698
    torque: 
      x: -1.21275018528
      y: -1.21275030399
      z: 1.18708719748e-07
  - 
    force: 
      x: 1.03224456528e-07
      y: 1.21875188838e-07
      z: 2.42550049698
    torque: 
      x: 1.21275030943
      y: 1.21275019688
      z: -1.12549822683e-07
  - 
    force: 
      x: 1.74928947366e-07
      y: 1.51736157764e-07
      z: 2.42549910891
    torque: 
      x: 1.21274963032
      y: -1.21274964192
      z: -1.15963948013e-08
total_wrench: 
  force: 
    x: 4.72969100295e-07
    y: 4.72968772177e-07
    z: 9.70200000011
  torque: 
    x: -1.57683906599e-07
    y: 1.57684875601e-07
    z: -5.28676330834e-13
contact_positions: 
  - 
    x: -0.5
    y: -0.5
    z: -5.13261655399e-12
  - 
    x: 0.5
    y: -0.5
    z: -5.03065908567e-12
  - 
    x: -0.5
    y: 0.5
    z: -4.7516798158e-12
  - 
    x: 0.5
    y: 0.5
    z: -4.64972234748e-12
contact_normals: 
  - 
    x: 0.0
    y: 0.0
    z: 1.0
  - 
    x: 0.0
    y: 0.0
    z: 1.0
  - 
    x: 0.0
    y: 0.0
    z: 1.0
  - 
    x: 0.0
    y: 0.0
    z: 1.0
depths: [5.132672065144561e-12, 5.030714596821626e-12, 4.751735326949498e-12, 4.649777858626564e-12]

choreonoid_ros_pkg

collision1_name: box100x100x100_1Kg::BOX01KG
collision2_name: Floor::BASE
wrenches: 
  - 
    force: 
      x: 0.00882850540173
      y: 0.00306734565972
      z: 1.98872501845
    torque: 
      x: 0.992004021538
      y: 0.996377327601
      z: -0.00594056313686
  - 
    force: 
      x: -0.00169757954078
      y: -0.00485463176235
      z: 1.94355439685
    torque: 
      x: -0.973981889811
      y: 0.97374638431
      z: 0.00158151909795
  - 
    force: 
      x: -0.00779683402757
      y: 0.000756038393521
      z: 1.94079101355
    torque: 
      x: 0.968093829154
      y: 0.00219274993046
      z: 0.00388831618305
  - 
    force: 
      x: 0.000186724333353
      y: 0.00139787980389
      z: 2.95970649263
    torque: 
      x: -1.48336333536
      y: -1.4765996315
      z: 0.000790986822188
  - 
    force: 
      x: 0.000140208766187
      y: -0.000502214019223
      z: 0.973874532447
    torque: 
      x: 0.48568496118
      y: -0.485950573886
      z: -0.000320522274295
total_wrench: 
  force: 
    x: -0.000338975067083
    y: -0.000135581924445
    z: 9.80665145392
  torque: 
    x: -0.0115624132982
    y: 0.00976625645651
    z: -2.63307976167e-07
contact_positions: 
  - 
    x: -0.500950039019
    y: 0.498877452626
    z: -9.55627477151e-09
  - 
    x: -0.501076580765
    y: -0.501070893962
    z: -3.92639726288e-09
  - 
    x: -0.00106672624076
    y: 0.498814193495
    z: -9.45402536812e-09
  - 
    x: 0.498837269532
    y: -0.501249084458
    z: -3.72157754613e-09
  - 
    x: 0.49904994952
    y: 0.498650938163
    z: -9.35116539758e-09
contact_normals: 
  - 
    x: -2.03660806002e-10
    y: 5.62980541454e-09
    z: 1.0
  - 
    x: -2.03660806002e-10
    y: 5.62980541454e-09
    z: 1.0
  - 
    x: -2.03660806142e-10
    y: 5.62980541843e-09
    z: 1.00000000069
  - 
    x: -2.03660806002e-10
    y: 5.62980541454e-09
    z: 1.0
  - 
    x: -2.03660806002e-10
    y: 5.62980541454e-09
    z: 1.0
depths: [0.00010000386263015214, 0.00010000386263015214, 0.0001000038626992058, 0.00010000386263015212, 0.00010000386263015211]

fkanehiro commented 7 years ago

collision?_nameの部分がgazeboは３つ、choreonoidは２つの単語でできているように見えますね
ボックスの重心位置がど真ん中だったり、落とす位置が原点の上だったりすると、間違いがあっても気づかなかったりするので、ずらしてみると良いと思います。

hmatsushita commented 7 years ago

collision?_name で gazebo が 3 つとなっているのは、URDF モデルの contact センサの指定において、任意の collision 名の指定が可能となっており、ここで指定した collision 名がリンク名の後に追加されている事によります。現状の choreonoid への実装では、collision 名の指定を考慮していなかったため、ご指摘の差異が生じております。 WorldRosItem のプロパティで collision 名の指定を可能とする (\<link name>=\<collision name>, ... のような形式)、もしくは collision 名のサフィックスを指定可能とし自動生成した collision 名 (リンク名 + サフィックス) を付与する等の対応が考えられますが、いかがいたしましょう。

重心位置につきましては試行いたします。ご教示ありがとうございます。

hmatsushita commented 7 years ago

collision 名の指定を省略した場合、リンク名の後に collision が付与されるようです。

hmatsushita commented 7 years ago

ボックスの重心位置を 0.48, 048, -0.48 へ変更し、落とす位置を x=1.0, y=1.0 へずらした設定でシミュレーションを実行いたしました。

gazebo_ros_pkgs

collision1_name: box::BOX01KG::BOX01KG_collision
collision2_name: ground_plane::link::collision
wrenches: 
  - 
    force: 
      x: -0.00543354679265
      y: -0.00537807189568
      z: 9.36843577062
    torque: 
      x: 0.187261153974
      y: -0.187260044477
      z: 1.10949793948e-06
  - 
    force: 
      x: 0.000757746401787
      y: 0.00522677394745
      z: 0.139524867919
    torque: 
      x: -0.136629835082
      y: -0.00280565228641
      z: 0.0008471269527
  - 
    force: 
      x: 0.00528209903143
      y: 0.000812923129929
      z: 0.139523590844
    torque: 
      x: 0.00280673027947
      y: 0.136627477046
      z: -0.000902306647959
  - 
    force: 
      x: -0.00060637512191
      y: -0.000661548792643
      z: 0.0545157706184
    torque: 
      x: -0.0534386861819
      y: 0.0534375827085
      z: 5.40701973191e-05
total_wrench: 
  force: 
    x: -7.64813462877e-08
    y: 7.63890554251e-08
    z: 9.702
  torque: 
    x: -6.37009585025e-07
    y: -6.37008366243e-07
    z: 5.63107503335e-18
contact_positions: 
  - 
    x: 1.5
    y: 1.5
    z: -1.69657621285e-11
  - 
    x: 1.5
    y: 0.5
    z: -1.31717369979e-11
  - 
    x: 0.5
    y: 1.5
    z: -1.12321534147e-11
  - 
    x: 0.5
    y: 0.5
    z: -7.4381282841e-12
contact_normals: 
  - 
    x: 0.0
    y: 0.0
    z: 1.0
  - 
    x: 0.0
    y: 0.0
    z: 1.0
  - 
    x: 0.0
    y: 0.0
    z: 1.0
  - 
    x: 0.0
    y: 0.0
    z: 1.0
depths: [1.6965762128506867e-11, 1.3171736997932802e-11, 1.1232153414670758e-11, 7.438128284096694e-12]

choreonoid_ros_pkg

collision1_name: box100x100x100_1Kg::BOX01KG
collision2_name: Floor::BASE
wrenches: 
  - 
    force: 
      x: 0.000340935506691
      y: -4.9874001578e-05
      z: 0.091242029293
    torque: 
      x: 0.136863005735
      y: -0.0456209928625
      z: -0.000536340106164
  - 
    force: 
      x: -1.01484974968e-05
      y: -6.13979273313e-06
      z: 0.0462784550986
    torque: 
      x: 0.0231427444865
      y: -0.0231392214909
      z: 2.00512442198e-06
  - 
    force: 
      x: -0.000402057163064
      y: 0.00027639472059
      z: 0.117206872952
    torque: 
      x: 0.175810249772
      y: -0.117212280738
      z: 0.000879493013068
  - 
    force: 
      x: 9.40712407343e-05
      y: -0.00025383806791
      z: 0.148913978737
    torque: 
      x: 0.0744569295139
      y: -0.223358499106
      z: -0.000427771429823
  - 
    force: 
      x: -9.31360198357e-05
      y: -0.000127004568255
      z: 9.40297885052
    torque: 
      x: 14.1035242222
      y: -14.1044670241
      z: -5.0812156524e-05
total_wrench: 
  force: 
    x: -7.03349329704e-05
    y: -0.000160461709886
    z: 9.8066201866
  torque: 
    x: 14.5137971518
    y: -14.5137980183
    z: -0.000133425555021
contact_positions: 
  - 
    x: 0.499999877655
    y: 1.49999956244
    z: 1.01954483744e-08
  - 
    x: 0.499999870228
    y: 0.500075989978
    z: 1.662144683e-08
  - 
    x: 1.00004604106
    y: 1.49999955878
    z: 8.69973720841e-09
  - 
    x: 1.49991628282
    y: 0.499999554859
    z: 1.06510572584e-08
  - 
    x: 1.49999987771
    y: 1.49989958935
    z: 7.20464532478e-09
contact_normals: 
  - 
    x: 2.9914812007e-09
    y: 6.42654277848e-09
    z: 1.0
  - 
    x: 2.9914812007e-09
    y: 6.42654277848e-09
    z: 1.0
  - 
    x: 2.99148120118e-09
    y: 6.4265427795e-09
    z: 1.00000000016
  - 
    x: 5.97171341673e-09
    y: 3.44631051852e-09
    z: 1.0
  - 
    x: 5.97171341673e-09
    y: 3.44631051852e-09
    z: 1.0
depths: [9.999317107724428e-05, 9.999317107724428e-05, 9.999317109323164e-05, 9.999317107612644e-05, 9.999317107612644e-05]

いずれの接触座標もボックスと床の接触面内に収まっている事、指定した重心位置に掛かる力が集中している事を確認いたしました。

fkanehiro commented 7 years ago

total_torqueが違うのでなにか間違っているようです。

hmatsushita commented 7 years ago

ご確認および、ご指摘ありがとうございます。調査・修正いたします。

hmatsushita commented 7 years ago

total_torque の結果に差異が生じている件ですが、モデルの重心位置を原点から変更した場合、choreonoid と gazebo の挙動に違いがあり結果に差異が出ております。具体的には、重心位置を原点から変更した場合、choreonoid は重心位置に近い接触座標の force, torque の値が変化しますが、gazebo は重心位置に近い接触座標の force の値は変化するものの torque の値に大きな変化がなく、この事が原因で差異が生じております。

chorenoid・重心位置に近い接触座標の force, torque

wrenches:
  - 
    force: 
      x: -1.59006347345e-05
      y: -0.00033787219034
      z: 9.25793355432
    torque: 
      x: 4.62803328996
      y: -4.62896661248
      z: -0.000160987375071

total_wrench: 
  force: 
    x: -1.63328151221e-05
    y: -0.000346785824278
    z: 9.80664761762
  torque: 
    x: 4.70719482715
    y: -4.70720455188
    z: -0.000158607962464

gazebo・重心位置に近い接触座標の force, torque

wrenches: 
  - 
    force: 
      x: -0.00543354679262
      y: -0.00537807189565
      z: 9.36843577057
    torque: 
      x: 0.187261153973
      y: -0.187260044475
      z: 1.10949793947e-06

total_wrench: 
  force: 
    x: -7.64813462856e-08
    y: 7.63890554028e-08
    z: 9.70199999995
  torque: 
    x: -6.37009585067e-07
    y: -6.37008366118e-07
    z: 4.00477177462e-18

choreonoid の torque の値が先に添付したデータと異なるのは、モデルの momentsOfInertia の設定が gazebo (URDF) のモデルに設定した値と異なっており、これを修正した事によるものです。不手際があり申し訳ございません。

自身の感覚としましては、重心が掛かっている接触点の torque が変化する choreonoid の方が正しい挙動のように見受けられますが、実際のところはどうなのでしょうか。

ご指摘、ご指示など頂ければ幸いです。よろしくお願いいたします。

fkanehiro commented 7 years ago

どちらが正しいかはその定義（どの座標系で力・トルクを出ししているか）によるので一概には言えません。まずは、重心と落とす位置を一度に変えずに片方だけ変えるとどうなるか見てみてはどうでしょうか。

hmatsushita commented 7 years ago

承知いたしました。もう少しバリエーションを増やして確認作業を実施いたします。

hmatsushita commented 7 years ago

すみません。 total_torque の件ではありませんが、collision?_name のフォーマット違いについてはいかがいたしましょうか。

fkanehiro commented 7 years ago

合わせられるところは合わせたほうがよいと思います。

hmatsushita commented 7 years ago

collision?_name の件についても承知いたしました。安直ではありますが、取り急ぎ '::collision' の文字列をリンク名に続けて付与いたしました。

fkanehiro commented 7 years ago

マニュアルの更新もお願いします。

hmatsushita commented 7 years ago

マニュアルの更新、承知いたしました。

crankler commented 5 years ago

How can you get the true time stamp from this kind of topic in ROS ?

fkanehiro / choreonoid_ros_pkg

adds to new feature of the collisions data output #75