wheelのintertiaのoriginがおかしい

[shmpwk]

@YoheiKakiuchi

以下のようにWheelをattachしてつくってみたのですが、wheelのinertiaのoriginが移動しています。

     <origin rpy="0 0 0" xyz="0.0 -0.012 0.0015"/>

これは正しくはすべて0だと思うのですが、どうでしょうか？
このInertiaがおかしいためホイールがついたロボットがGazebo上で滑っていきました。

GazeboでのInertiaの表示

assebled_robot.urdf

<?xml version="1.0" ?>
<!-- This file was generated by robot assembler -->
<robot name="assembled_robot"
       xmlns:xi="http://www.w3.org/2001/XInclude"
       xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
       xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
       xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
       xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
       xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
       xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics"
       xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:macro name="gazebo_link_reference" params="link_name mu1:=0.9 mu2:=0.9" >
    <gazebo reference="${link_name}">
      <mu1>${mu1}</mu1>
      <mu2>${mu2}</mu2>
    </gazebo>
  </xacro:macro>
  <xacro:macro name="gazebo_transmission" params="joint_name" >
    <transmission name="${joint_name}_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${joint_name}">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
      <actuator name="${joint_name}_motor">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
  </xacro:macro>

  <xacro:gazebo_link_reference link_name="BASE" />
  <link name="BASE">
  <inertial>
     <mass value="0.1"/>
     <origin rpy="0 0 0" xyz="0.0 -0.012 0.0015"/>
     <inertia ixx="3.202708e-05" ixy="0.0" ixz="0.0" iyy="2.077708e-05" iyz="3.549874e-37" izz="2.478750e-05"/>
  </inertial>
  <visual>
     <origin xyz="0.000000 0.000000 0.000000" rpy="0.00000000 -0.00000000 0.00000000"/>
     <geometry>
     <mesh filename="package://robot_assembler/meshes/xm430_dynamixel.dae" scale="1.000000 1.000000 1.000000"/>
     </geometry>
  </visual>
  <collision>
     <origin xyz="0.000000 0.000000 0.000000" rpy="0.00000000 -0.00000000 0.00000000"/>
     <geometry>
     <mesh filename="package://robot_assembler/meshes/xm430_dynamixel.dae" scale="1.000000 1.000000 1.000000"/>
     </geometry>
  </collision>
  </link>
  <xacro:gazebo_link_reference link_name="LINK0" />
  <link name="LINK0">
  <inertial>
     <mass value="0.1"/>
     <origin rpy="0 0 0" xyz="-0.05 -0.0485 0.022"/>
     <inertia ixx="3.202708e-05" ixy="-2.019484e-35" ixz="1.514613e-35" iyy="2.478750e-05" iyz="2.894099e-21" izz="2.077708e-05"/>
  </inertial>
  <visual>
     <origin xyz="-0.050000 -0.050000 0.010000" rpy="-1.57079633 0.00000000 0.00000000"/>
     <geometry>
     <mesh filename="package://robot_assembler/meshes/wheel.dae" scale="0.001000 0.001000 0.001000"/>
     </geometry>
  </visual>
  <collision>
     <origin xyz="-0.050000 -0.050000 0.010000" rpy="-1.57079633 0.00000000 0.00000000"/>
     <geometry>
     <mesh filename="package://robot_assembler/meshes/wheel.dae" scale="0.001000 0.001000 0.001000"/>
     </geometry>
  </collision>
  </link>
  <xacro:gazebo_transmission joint_name="JOINT0" />
  <joint name="JOINT0" type="revolute">
     <parent link="BASE"/>
     <child link="LINK0"/>
     <origin xyz="0.000000 0.000000 0.019000" rpy="0.00000000 -0.00000000 0.00000000"/>
     <axis xyz="0 0 1"/>
     <limit effort="200" lower="-3.14159" upper="3.14159" velocity="40"/>
  </joint>

  <!-- Gazebo plugin for ROS Control -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
  </gazebo>
</robot>

cc. @ykawamura96

[ykawamura96]

もしかしたら、僕が作成したwheelの設定が間違っていたかもしれません。
具体的には、

robot_assembler/config/robot_assembler_parts_settings.yaml

Lines 905 to 906 in 0220451

	center-of-mass: [0.0, -0.012, 0.0015]
	inertia-tensor: [3.202708e-05, 0.0, 0.0, 0.0, 2.077708e-05, 0.0, 0.0, 0.0, 2.478750e-05]

の部分が間違えている可能性があります。
ただしくは、 center-of-massが[0 0 0]ということでしょうか？

[shmpwk]

ただしくは、 center-of-massが[0 0 0]ということでしょうか？

そのようにしてみましたが、まだgazeboで表示されるinertiaがおかしいです。
(急ぎのissueではなく、かつurdfを直接修正すれば済むので、今日直してもらわなくて大丈夫です。)

[knorth55]

https://github.com/agent-system/robot_assembler/blob/master/config/robot_assembler_parts_settings.yaml

このファイルが間違っているのようですが、どれが正解かはよくわかりません。
河村くんが設定してくれたので河村くんが見てくれたほうが解ると思います。
このIssueの一番上のコメントにGazeboでのInertiaの可視化の画像を追加しました。
今のコードでやると、InertiaがWheelの中心どころかWheelの外が原点として設定されています。
（そもそもWheelなのにInertiaが上下左右で非均等なのもおかしいですが……）

[ykawamura96]

可視化するとわかりやすくおかしいですね...
CADを確認したところ、CADの原点そもそも車輪の中心でなかったのが原因なようです。
直してPRを出したいと思います。