computeCoriolisMatrix result changes based on value of universe fixed joint

[abcamiletto]

Bug description

The result of "computeCoriolisMatrix" changes based on the value of the universe joint, even though it should be fixed.

Expected behavior

I would think that no matter which value we set the fixed universe joint to be, the output wouldn't change. If I am misinterpreting what the value of the universe joint is, I would love to get a clarification. I couldn't find any clarification in the docs.

Code

If this is a runtime issue, provide some minimal code to reproduce it:

import pinocchio as pin

# Load the URDF model
model = pin.buildModelFromUrdf(urdf_path)
data = model.createData()
    
q = pin.neutral(model)
v = pin.utils.zero(model.nv) + 1

q[0] = 2.0 # Set any number

# Compute the Coriolis matrix
pin.computeCoriolisMatrix(model, data, q, v)
print(data.C)

Here the larger we set q[0] to be, the larger the coriolis matrix will be.

Thanks for the amazing library!

[jcarpent]

Dear @abcamiletto,

Thanks for raising this issue.
Could you provide the model of your robots to complete the check?

Best,
Justin

[abcamiletto]

Sure, here it is a sample URDF to reproduce the Issue

<robot name="rpf_robot">

  <!-- Base Link -->
  <link name="base_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="5.0"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.2 0.2 0.2"/>
      </geometry>
    </visual>
  </link>

  <!-- Continuous Joint -->
  <joint name="continuous_joint" type="continuous">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
  </joint>

  <!-- Link 1 -->
  <link name="link_1">
    <inertial>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <mass value="3"/>
      <inertia ixx="0.252" ixy="0" ixz="0" iyy="0.252" iyz="0" izz="0.004"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <geometry>
        <cylinder length="1.0" radius="0.05"/>
      </geometry>
    </visual>
  </link>

  <!-- Prismatic Joint -->
  <joint name="prismatic_joint" type="prismatic">
    <parent link="link_1"/>
    <child link="link_2"/>
    <origin xyz="0 0 1" rpy="0 1.5708 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="0" upper="1" effort="100" velocity="0.5"/>
  </joint>

  <!-- Link 2 -->
  <link name="link_2">
    <inertial>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <mass value="5.0"/>
      <inertia ixx="0.42" ixy="0" ixz="0" iyy="0.42" iyz="0" izz="0.016"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.5" rpy="0 0 0"/>
      <geometry>
        <cylinder length="1.0" radius="0.08"/>
      </geometry>
    </visual>
  </link>

  <!-- Fixed Joint -->
  <joint name="fixed_joint" type="fixed">
    <parent link="link_2"/>
    <child link="link_3"/>
    <origin xyz="0 0 1" rpy="0 0 0"/>
  </joint>

  <!-- End Effector (EE) Link -->
  <link name="link_3">
    <inertial>
      <origin xyz="-0.25 0 0" rpy="0 1.57 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.021" ixy="0" ixz="0" iyy="0.021" iyz="0" izz="0.001"/>
    </inertial>
    <visual>
      <origin xyz="-0.25 0 0" rpy="0 1.57 0"/>
      <geometry>
        <cylinder length="0.5" radius="0.05"/>
      </geometry>
    </visual>
  </link>

  <!-- Additional Fixed Joint for EE -->
  <joint name="ee_fixed_joint" type="fixed">
    <parent link="link_3"/>
    <child link="ee_link"/>
    <origin xyz="-0.5 0 0" rpy="0 0 0"/>
  </joint>

  <!-- End Effector Extension Link -->
  <link name="ee_link">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="10.0"/>
      <inertia ixx="0.04" ixy="0" ixz="0" iyy="0.04" iyz="0" izz="0.04"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <sphere radius="0.1"/>
      </geometry>
    </visual>
  </link>

</robot>

[jcarpent]

Your first joint is the unbounded revolute joint parametrized by (cos(q),sin(q)).
Thus, you should normalize your joint configuration vector before sending it to any algorithm.

By simply doing:

import pinocchio as pin
import math

# Load the URDF model
urdf_path = "/tmp/model.urdf"
model = pin.buildModelFromUrdf(urdf_path)
data = model.createData()
    
q = pin.neutral(model)
v = pin.utils.zero(model.nv) + 1

q[0] = math.cos(2.0)
q[1] = math.sin(2.0) # Set any number

assert pin.isNormalized(model,q)

# Compute the Coriolis matrix
pin.computeCoriolisMatrix(model, data, q, v)
print(data.C)

it works well on my side.

[abcamiletto]

Thanks! I tought the first value of q was related to the universe joint. Now it makes much more sense. Thanks a lot!