ComputeCoriolisMatrix has numerical error?

[XuanLin]

Hi,

I want to ask one question on getting Coriolis matrix of floating base dynamics. First, could you confirm that when using floating base dynamics, the root joint derivatives (dx/dt, dy/dt, dz/dt, w_x, w_y, w_z) and second derivatives (ddx/dtdt, ddy/dtdt, ddz/dtdt, alpha_x, alpha_y, alpha_z) are all expressed in the base (or body) frame instead of the world (universe) frame, such that the mass matrix and Coriolis matrix should be independent of position and orientation (x, y, z, theta_x, theta_y, theta_z)? This is also the convention from slide 4 of https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/6-dynamics2_ex.pdf.

On top of that, when I use pinocchio.computeCoriolisMatrix. I noticed that the result depends on position and orientation. I use the following code for a simple two-link floating base robot:

import pinocchio
import numpy as np

urdf_filename = '/urdf_debug.urdf'

# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename, pinocchio.JointModelFreeFlyer())

for item in model.names: 
    print(item)

# Create data required by the algorithms
data = model.createData()

x_theta_1 = np.array([0.0, 0.0, 0.0,   0.01, -0.03, -0.03, 1.00])
x_theta_2 = np.array([0.0, 0.0, 0.0,   0.01,  0.13, -0.11, 0.99])

q_qx1 = np.concatenate((x_theta_1, np.zeros(model.nq-7)))
dq_qx1 = np.concatenate((np.zeros(6), np.ones(model.nq-7)))
pinocchio.forwardKinematics(model, data, q_qx1)
M1 = np.copy(pinocchio.crba(model, data, q_qx1))
C1 = np.copy(pinocchio.computeCoriolisMatrix(model, data, q_qx1, dq_qx1))
G1 = np.copy(pinocchio.computeGeneralizedGravity(model, data, q_qx1))

q_qx2 = np.concatenate((x_theta_2, np.zeros(model.nq-7)))
dq_qx2 = np.concatenate((np.zeros(6), np.ones(model.nq-7)))
pinocchio.forwardKinematics(model, data, q_qx2)
M2 = np.copy(pinocchio.crba(model, data, q_qx2))
C2 = np.copy(pinocchio.computeCoriolisMatrix(model, data, q_qx2, dq_qx2))
G2 = np.copy(pinocchio.computeGeneralizedGravity(model, data, q_qx2))

# Mass matrix is independent of both
print("Mass matrix ========================================================================")
print(M1 == M2)

# Is coriolis matrix independent of q? If dq=0, then yes!
print("Coriolis matrix ======================================================================")
print(C1 == C2)

# Gravity matrix is independent of position but depends on orientation!
print("Gravity vector ======================================================================")
print(G1 == G2)

I input two different position and orientation parameters: x_theta_1 and x_theta_2. At the end of the code, I compare the results. The mass matrix is independent of both x and theta, which makes sense. The gravity vector depends on orientation but is independent of position, which also makes sense. However, the Coriolis matrix seems to depend on both position and orientation. The dependency on position seems to be from numerical error as the difference is small, but becomes larger when the difference in position is larger. The dependency on orientation is more significant. For example, the small angle difference in my code already causes the matrix norm to be np.linalg.norm(C1-C2) = 0.00075. I am not familiar with Featherstone's algorithm running under the hood. I wonder if I am not using this function correctly or if there is some underline issue?

The .urdf I am using is:

<?xml version="1.0" encoding="utf-8"?>

<robot name="debug">

<link name="body">
  <inertial>
    <origin xyz="0 0 0" rpy="0 0 0" />
    <mass value="10" />
    <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
  </inertial>
</link>

<link name="limb1">
    <inertial>
      <origin
        xyz="0.01530842 -0.00007279 -0.04673530"
        rpy="0 0 0" />
      <mass
        value="2" />
      <inertia
        ixx="1"
        ixy="0"
        ixz="0"
        iyy="1"
        iyz="0"
        izz="1" />
    </inertial>
</link>

<joint name="joint1" type="revolute">
  <origin xyz="-0.099895 0.0625 -0.024942" rpy="0 0.7854 0" />
  <parent link="body" />
  <child link="limb1" />
  <axis xyz="0 0 1" />
  <limit lower="-1.8" upper="1.8" effort="300" velocity="1000" />
</joint>

</robot>

[jcarpent]

You're quantities:

x_theta_1 = np.array([0.0, 0.0, 0.0,   0.01, -0.03, -0.03, 1.00])
x_theta_2 = np.array([0.0, 0.0, 0.0,   0.01,  0.13, -0.11, 0.99])

are not normalized for the quaternion part.

[XuanLin]

Hi,

Thanks for the reply! The quaternions are copied from Gazebo with first 2 decimals hence not so precise. I used a more careful quantities:

x_theta_1 = np.array([0.0, 10.0, 0.0,   0.00,            0.00, np.sin(np.pi/2), np.cos(np.pi/2)])
x_theta_2 = np.array([10.0, 0.0, 0.0,   0.00, np.sin(np.pi/2),            0.00, np.cos(np.pi/2)])

And the difference in norm is 5.16e-14. This is much better, but still worse than the mass and gravity matrices. Is it suggested that I always put 0s in position and [0, 0, 0, 1] orientation?