stack-of-tasks / pinocchio

A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
http://stack-of-tasks.github.io/pinocchio/
BSD 2-Clause "Simplified" License
1.7k stars 366 forks source link

Wrong inertia properties extracted from data class using python interface. #2188

Closed TJBetter closed 4 months ago

TJBetter commented 4 months ago

Hello all,

Thanks for this making this great repo public, we really like it.

I wonder if you could clarify what does the class data contains? We are using the latest version, and we are trinh to extract the inertia properties from the data class from the cheatsheet (here):

def main():

    urdf_filename = ('2d_planar.urdf')
    # Load the urdf model
    model = pinocchio.buildModelFromUrdf(urdf_filename)

    data = model.createData()
    print('data mass is {}'.format(list(data.mass)))

if __name__ == '__main__':
    main()

But the above code gives me data mass is [-1.0, -1.0, -1.0], which doesn't match the urdf file:

 <?xml version="1.0" ?>

<robot name="myexample" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <link name="base_link">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 10"/>
            <geometry>
                <box size="1 1 1"/>
            </geometry>
        </visual>
        <inertial>
          <mass value="1"/>
          <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
          <inertia ixx="0.0217284832211" ixy="0.1" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
        </inertial>
    </link>

    <joint name="joint_0" type="revolute">
        <axis xyz="1 0 0" />
        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5" />
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <parent link="base_link"/>
        <child link="link1"/>
    </joint>

    <link name="link1">
        <visual>
                <origin rpy="0 0 0" xyz="0 0 0.0"/>
                <geometry>
                    <cylinder radius="0.35" length="0.4"/>
                </geometry>
        </visual>
        <inertial>
          <mass value="2"/>
          <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
          <inertia ixx="0.0217284832211" ixy="0.2" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
        </inertial>
    </link>

    <joint name="joint_1" type="revolute">
        <axis xyz="1 0 0" />
        <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5" />
        <origin rpy="0 0 0" xyz="0 0 1.0"/>
        <parent link="link1"/>
        <child link="link2"/>
    </joint>

    <link name="link2">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0.2"/>
                <geometry>
                    <cylinder radius="0.35" length="0.4"/>
                </geometry>
        </visual>
        <inertial>
          <mass value="3.42"/>
          <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
          <inertia ixx="0.0217284832211" ixy="0.3" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
        </inertial>
    </link>
</robot>

In addition, I wonder if you may know if there is a python api for this great repo? Thank you.

fabinsch commented 4 months ago

Hi @TJBetter, as you did not call any algorithm this behavior is expected. If you just want to get the mass, you could call

In [2]: pin.computeSubtreeMasses?
Docstring:
computeSubtreeMasses( (Model)model, (Data)data) -> None :
    Compute the mass of each kinematic subtree and store it in the vector data.mass.
Type:      function

and this will fill data.mass, see Section Center of Mass on the cheat sheet.