Inertial Data ignored

[AndrewJSchoen]

This was initially found from within the k library, but basically the inertial data seems to be ignored on input to the read_from_string function. For example, with the URDF example provided in this repo, the initial link has the following specification:

<link name="root">
    <inertial>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <mass value="1"/>
      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
    </inertial>
    <visual>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
      <geometry>
        <box size="0.2 0.2 0.4" />
      </geometry>
      <material name="Cyan">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder radius="1" length="0.5"/>
      </geometry>
    </collision>
  </link>

However, when loaded, it has the following parsed data:

Link { 
    name: "root_body", 
    inertial: Inertial { 
        origin: Pose { 
            xyz: [0.0, 0.0, 0.0], 
            rpy: [0.0, 0.0, 0.0] 
        }, 
        mass: Mass { 
            value: 0.0 
        }, 
        inertia: Inertia { 
            ixx: 0.0, ixy: 0.0, ixz: 0.0, iyy: 0.0, iyz: 0.0, izz: 0.0 
        } 
    },
    ...
}

This leads to functions like center_of_mass failing.

Edit: Confirming this, I added a check in the test function:

assert_eq!(robot.links[0].inertial.mass.value, 1.0);

And it fails as expected:

running 2 tests
test funcs::it_works ... FAILED
test utils::it_works ... ok

failures:

---- funcs::it_works stdout ----
thread 'funcs::it_works' panicked at 'assertion failed: `(left == right)`
  left: `0.0`,
 right: `1.0`', src/funcs.rs:186:5

[taiki-e]

Fixed in #57