search

RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.29k stars 1.26k forks source link

SpatialInertia::IsPhysicallyValid() fails for reasonable spatial inertia #16058

Closed matheecs closed 2 years ago

matheecs commented 2 years ago

Current error messages from loading URDF. For example:

RuntimeError: The resulting spatial inertia:
 mass = 0.634
 com = [    0 0.016 -0.02]ᵀ
 I =
  0.0023989    0.000245     1.3e-05
   0.000245   0.0023566  0.00020438
    1.3e-05  0.00020438 0.000570304
 is not physically valid. See SpatialInertia::IsPhysicallyValid()

Details: macOS Monterey, Python 3.9.7 (homebrew), binary release

jwnimmer-tri commented 2 years ago

+@mitiguy please see what you can find out here.

At minimum, it looks like we have a bug in Drake that we're printing out truncated values for the inertia values, rather than the full-double precision. That makes it difficult to check the math using only the error message.

mitiguy commented 2 years ago

@jwnimmer-tri Looking at it now.
@matheecs Can you send me a .urdf file and/or more information about the underlying object that causes this error. For example, was the underlying object a sphere, cylinder, or a generic shape?

matheecs commented 2 years ago

@mitiguy There is the original scripts and .urdf file:

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 1e-3)
parser = Parser(plant)
parser.AddModelFromFile("mini_cheetah.urdf")
plant.Finalize()

mini_cheetah.urdf

<robot name="mini_cheetah">
  <link name="body">
    <inertial>
      <mass value="3.3"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.036203" iyz="0" izz="0.042673"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_body.obj"/>
      </geometry>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_body.obj"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <!--  -->
  <!-- FL0 -->
  <link name="front_left_hip">
    <inertial>
      <mass value="0.54"/>
      <origin xyz="0.0 0.036 0."/>
      <inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045" 
                iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="0. 0. -1.5708" xyz="-0.055 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/>
    </collision>
  </link>
  <joint name="front_left_hip_roll" type="revolute">
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0.19 0.049 0.0"/>
    <parent link="body"/>
    <child link="front_left_hip"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="front_left_hip_roll"/>
    <actuator name="front_left_hip_roll"/>
  </transmission>
  <!-- FL1 -->
  <link name="front_left_upper_leg">
    <inertial>
      <mass value="0.634"/>
      <origin xyz="0.0 0.016 -0.02"/>
      <inertia ixx="0.001983" ixy="0.000245" ixz="0.000013" iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0 -1.5708 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="front_left_hip_pitch" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
    <parent link="front_left_hip"/>
    <child link="front_left_upper_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="front_left_hip_pitch"/>
    <actuator name="front_left_hip_pitch"/>
  </transmission>
  <!-- FL2 -->
  <link name="front_left_lower_leg">
    <inertial>
      <mass value="0.064"/>
      <origin xyz="0.0 0.0 -0.209"/>
      <inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0 3.141592 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="front_left_knee" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
    <parent link="front_left_upper_leg"/>
    <child link="front_left_lower_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="front_left_knee"/>
    <actuator name="front_left_knee"/>
  </transmission>
  <!--  -->
  <!-- FR0 -->
  <link name="front_right_hip">
    <inertial>
      <mass value="0.54"/>
      <origin xyz="0.0 0.036 0."/>
      <inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045" 
                iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="3.141592 0 1.5708" xyz="-0.055 0 0"/>
    </collision>
  </link>
  <joint name="front_right_hip_roll" type="revolute">
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0.19 -0.049 0.0"/>
    <parent link="body"/>
    <child link="front_right_hip"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="front_right_hip_roll"/>
    <actuator name="front_right_hip_roll"/>
  </transmission>
  <!-- FR1 -->
  <link name="front_right_upper_leg">
    <inertial>
      <mass value="0.634"/>
      <origin xyz="0.0 0.016 -0.02"/>
      <inertia ixx="0.001983" ixy="0.000245" ixz="0.000013" iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0 -1.5708 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="front_right_hip_pitch" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
    <parent link="front_right_hip"/>
    <child link="front_right_upper_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="front_right_hip_pitch"/>
    <actuator name="front_right_hip_pitch"/>
  </transmission>
  <!-- FR2 -->
  <link name="front_right_lower_leg">
    <inertial>
      <mass value="0.064"/>
      <origin xyz="0.0 0.0 -0.209"/>
      <inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0 3.141592 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="front_right_knee" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
    <parent link="front_right_upper_leg"/>
    <child link="front_right_lower_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="front_right_knee"/>
    <actuator name="front_right_knee"/>
  </transmission>
  <!--  -->
  <!-- BL0 -->
  <link name="back_left_hip">
    <inertial>
      <mass value="0.54"/>
      <origin xyz="0.0 0.036 0."/>
      <inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045" 
                iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="3.141592 0.0 -1.5708" xyz="0.055 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="3.141592 0 -1.5708" xyz="0.055 0 0"/>
    </collision>
  </link>
  <joint name="back_left_hip_roll" type="revolute">
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="-0.19 0.049 0.0"/>
    <parent link="body"/>
    <child link="back_left_hip"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="back_left_hip_roll"/>
    <actuator name="back_left_hip_roll"/>
  </transmission>
  <!-- BL1 -->
  <link name="back_left_upper_leg">
    <inertial>
      <mass value="0.634"/>
      <origin xyz="0.0 0.016 -0.02"/>
      <inertia ixx="0.001983" ixy="0.000245" ixz="0.000013" iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0 -1.5708 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="back_left_hip_pitch" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
    <parent link="back_left_hip"/>
    <child link="back_left_upper_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="back_left_hip_pitch"/>
    <actuator name="back_left_hip_pitch"/>
  </transmission>
  <!-- BL2 -->
  <link name="back_left_lower_leg">
    <inertial>
      <mass value="0.064"/>
      <origin xyz="0.0 0.0 -0.209"/>
      <inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0 3.141592 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="back_left_knee" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
    <parent link="back_left_upper_leg"/>
    <child link="back_left_lower_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="back_left_knee"/>
    <actuator name="back_left_knee"/>
  </transmission>
  <!--  -->
  <!-- BR0 -->
  <link name="back_right_hip">
    <inertial>
      <mass value="0.54"/>
      <origin xyz="0.0 0.036 0."/>
      <inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045" 
                iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="0.0 0.0 1.5708" xyz="0.055 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_abad.obj"/>
      </geometry>
      <origin rpy="0 0 1.5708" xyz="0.055 0 0"/>
    </collision>
  </link>
  <joint name="back_right_hip_roll" type="revolute">
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="-0.19 -0.049 0.0"/>
    <parent link="body"/>
    <child link="back_right_hip"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="back_right_hip_roll"/>
    <actuator name="back_right_hip_roll"/>
  </transmission>
  <!-- BR1 -->
  <link name="back_right_upper_leg">
    <inertial>
      <mass value="0.634"/>
      <origin xyz="0.0 0.016 -0.02"/>
      <inertia ixx="0.001983" ixy="0.000245" ixz="0.000013" iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_upper_link.obj"/>
      </geometry>
      <origin rpy="0 -1.5708 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="back_right_hip_pitch" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
    <parent link="back_right_hip"/>
    <child link="back_right_upper_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="back_right_hip_pitch"/>
    <actuator name="back_right_hip_pitch"/>
  </transmission>
  <!-- BR2 -->
  <link name="back_right_lower_leg">
    <inertial>
      <mass value="0.064"/>
      <origin xyz="0.0 0.0 -0.209"/>
      <inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/mini_lower_link.obj"/>
      </geometry>
      <origin rpy="0 3.141592 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="back_right_knee" type="revolute">
    <axis xyz="0 -1 0"/>
    <origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
    <parent link="back_right_upper_leg"/>
    <child link="back_right_lower_leg"/>
  </joint>
  <transmission type="SimpleTransmission">
    <joint name="back_right_knee"/>
    <actuator name="back_right_knee"/>
  </transmission>
</robot>

Thanks.

mitiguy commented 2 years ago

@matheecs Thank you for the URDF/SDF file -- much appreciated.

Problem #1: The inertia properties in one or more URDF tags are not valid.

For example, one of the problematic URDF \<inertial> tags is: 

  <inertial>
      <mass value="0.634"/>
      <origin xyz="0.0 0.016 -0.02"/>
      <inertia ixx="0.001983" ixy="0.000245" ixz="0.000013" iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
    </inertial>

The problem is that this is not a physically valid inertia. That body has a numerical value for its product of inertia Ixy whose absolute value is too large relative to its moment of inertia Izz. Real physical bodies require: 2 * abs(Ixy) <= Izz.

Another way to say this: The PRINCIPAL moments of inertia for that body about Bcm are: [Imin = 0.0004078925; Imedium = 0.001790823; Imax = 0.002295285] Real physical bodies require the "triangle-inequality" Imax <= Imin + Imed.

Problem #2: The Drake error message is confusing because the inertia properties are shifted away from the center of mass. In the URDF, the inertia properties are associated with the body's center of mass Bcm (which in our syntax is I_BBcm). Drake shifts the inertia properties to the body's origin (which in our syntax is I_BBo).

RuntimeError: The resulting spatial inertia: mass = 0.634 com = [ 0 0.016 -0.02]ᵀ I = 0.0023989 0.000245 1.3e-05 0.000245 0.0023566 0.00020438 1.3e-05 0.00020438 0.000570304 is not physically valid. See SpatialInertia::IsPhysicallyValid()

Problem #3: Regardless of the shifting (which also causes confusion), the Drake error message is not particularly helpful because it gives little additional information.

matheecs commented 2 years ago

@mitiguy Thank you for answering my question. I really do not know the necessary condition of "triangle-inequality", and the doc only describe the following details of IsPhysicallyValid():

The checks performed are:

No NaN entries.
Non-negative mass.
Non-negative principal moments about the center of mass.
Principal moments about the center of mass must satisfy the triangle inequality:
Ixx + Iyy >= Izz
Ixx + Izz >= Iyy
Iyy + Izz >= Ixx

We may add "triangle-inequality" condition Imax <= Imin + Imed to the doc.