Parsing urdf - breaking change 2.7 - 3.0

[budzianowski]

Bug description

Running the same code results in the following error in 3.0.0 Assertion failed: (model.check(data) && "data is not consistent with model."), function forwardKinematics, file kinematics.hxx, line 88. It is hard to debug this so opening the issue.

Reproduction steps

import pinocchio as pin
from pinocchio.robot_wrapper import RobotWrapper

robot = RobotWrapper.BuildFromURDF("humanoid.urdf")
model = robot.model
data = robot.data
q_norm = pin.neutral(model)
pin.forwardKinematics(model, data, q_norm)

<robot name="humanoid">

  <!-- Pelvis -->
  <link name="pelvis">
    <inertial>
      <origin xyz="0 0 0.37" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.002"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.09"/>
      </geometry>
      <material name="gray">
        <color rgba="0.6 0.6 0.6 1"/>
      </material>
    </visual>
  </link>

 <!-- Torso -->
  <link name="torso">
    <inertial>
      <origin xyz="0 0 0.75" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.002"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.11"/>
      </geometry>
      <material name="gray">
        <color rgba="0.6 0.6 0.6 1"/>
      </material>
    </visual>
  </link>

  <joint name="abdomen_x" type="revolute">
    <parent link="pelvis"/>
    <child link="torso"/>
    <origin xyz="0 0 0.35" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.047" upper="1.047" effort="600" velocity="1.0"/>
    <dynamics damping="60" friction="0"/>
  </joint>

  <joint name="abdomen_y" type="revolute">
    <parent link="pelvis"/>
    <child link="torso"/>
    <origin xyz="0 0 0.35" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.047" upper="1.57" effort="600" velocity="1.0"/>
    <dynamics damping="60" friction="0"/>
  </joint>

  <joint name="abdomen_z" type="revolute">
    <parent link="pelvis"/>
    <child link="torso"/>
    <origin xyz="0 0 0.35" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.872" upper="0.872" effort="600" velocity="1.0"/>
    <dynamics damping="60" friction="0"/>
  </joint>

 <!-- Until now works -->
  <!-- Head -->

 <link name="head">
    <inertial>
      <origin xyz="0 0 0.125" rpy="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.095"/>
      </geometry>
      <material name="gray">
        <color rgba="0.6 0.6 0.6 1"/>
      </material>
    </visual>
  </link>

  <joint name="neck_x" type="revolute">
    <parent link="torso"/>
    <child link="head"/>
    <origin xyz="0 0 0.375" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-0.872" upper="0.872" effort="50" velocity="1.0"/>
    <dynamics damping="5" friction="0"/>
  </joint>

</robot>

System

• OS: Mac and Linux
• Pinocchio version: 2.7 and 3.0.0

[jorisv]

Hello @budzianowski,

You have compiled pinocchio 3 in Debug mode. Did you have also compiled pinocchio 2 in Debug mode ? If it's not the case it's normal the assert is silent when running the same code with pinocchio 2.

Here the model.check(data) function return False, that mean you model and data are somehow not right. I have check your URDF and this part trigger me:

  <joint name="abdomen_x" type="revolute">
    <parent link="pelvis"/>
    <child link="torso"/>
    <origin xyz="0 0 0.35" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.047" upper="1.047" effort="600" velocity="1.0"/>
    <dynamics damping="60" friction="0"/>
  </joint>

  <joint name="abdomen_y" type="revolute">
    <parent link="pelvis"/>
    <child link="torso"/>
    <origin xyz="0 0 0.35" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.047" upper="1.57" effort="600" velocity="1.0"/>
    <dynamics damping="60" friction="0"/>
  </joint>

  <joint name="abdomen_z" type="revolute">
    <parent link="pelvis"/>
    <child link="torso"/>
    <origin xyz="0 0 0.35" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.872" upper="0.872" effort="600" velocity="1.0"/>
    <dynamics damping="60" friction="0"/>
  </joint>

Here, you have 3 joints that link from/to the same body. It's not right and create an invalid Model.K I don't know if URDF allow to create composite joint. You can add intermediate body (torso2, torso3) to simulate that. You can also use MJCF that support composite joint.

[budzianowski]

Thanks - I will check pin2 although I can't find any debug flags in CMakeLists?

[jorisv]

It's the CMAKE_BUILD_TYPE variable. By default this variable is not set and assert are activated.

If you have installed pinocchio 2 from a package manager (pypi, conda, ...) it's build in release mode.