C(q,v)v+g(q) ≠ nonLinearEffects

[matheecs]

C(q,v)v+g(q) ≠ nonLinearEffects

When I use JointModelSphericalZYX as part of floting joint, the result C*v+g is not equal to nonLinearEffects (nle).

Expected behavior

$$C(q,v)v+g(q) = nle$$

Code

import pinocchio as pin
import numpy as np
np.set_printoptions(precision=8, suppress=True, linewidth=250)

# create a model with a JointModelSphericalZYX
urdf_file = "mybot.urdf"
jc = pin.JointModelComposite()
jc = jc.addJoint(pin.JointModelTranslation())
jc = jc.addJoint(pin.JointModelSphericalZYX())
model = pin.buildModelFromUrdf(
    urdf_file, jc
)
data = model.createData()

q_home = np.zeros(model.nq)
v_home = np.array([0.1] * model.nv)

pin.computeCoriolisMatrix(model, data, q_home, v_home)
pin.nonLinearEffects(model, data, q_home, v_home)
pin.computeGeneralizedGravity(model, data, q_home)

# check C(q,v)v+g(q) == nle
if not np.allclose(data.nle, data.C@v_home + data.g):
    print(f"nle = {data.nle}")
    print(f"Cv+g= {data.C@v_home + data.g}")

Output:

nle = [-0.00993994 -0.00706209 56.54352842 -0.00136424 -0.52168281 -0.00489886 -0.25487205 -0.25479257]
Cv+g= [-0.00628687 -0.0028713  56.54406614 -0.0005245  -0.52431665 -0.00165649 -0.25502346 -0.25493177]

The results are inconsistent.

Additional context

Model file (mybot.urdf):

<?xml version="1.0" encoding="utf-8"?>
<robot name="mybot">
  <link name="body">
    <inertial>
      <origin xyz="0.00037 0 0.01792" rpy="0 0 0" />
      <mass value="4.935" />
      <inertia ixx="0.036517" ixy="-3.49E-05" ixz="1.11E-05" iyy="0.0058391" iyz="6.41E-06"
        izz="0.039681" />
    </inertial>
  </link>
  <link name="left_link">
    <inertial>
      <origin xyz="0.064144 0 -0.047389" rpy="0 0 0" />
      <mass value="0.394" />
      <inertia ixx="0.000169" ixy="-1E-09" ixz="0.000195" iyy="0.0019044" iyz="0" izz="0.0018657" />
    </inertial>
  </link>
  <joint name="left_joint" type="revolute">
    <origin xyz="0 0.1 -0.5" rpy="0 0 0" />
    <parent link="body" />
    <child link="left_link" />
    <axis xyz="0 1 0" />
    <limit lower="-0.7" upper="0.7" effort="200" velocity="23" />
  </joint>
  <joint name="left_front_joint" type="fixed">
    <parent link="left_link" />
    <child link="lf_contact" />
    <origin xyz=" 0.10 0 -0.06" />
  </joint>
  <joint name="left_back_joint" type="fixed">
    <parent link="left_link" />
    <child link="lb_contact" />
    <origin xyz="-0.03 0 -0.06" />
  </joint>
  <link name="lf_contact">
    <inertial>
      <mass value="0.01" />
      <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" />
    </inertial>
  </link>
  <link name="lb_contact">
    <inertial>
      <mass value="0.01" />
      <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" />
    </inertial>
  </link>
  <link name="right_link">
    <inertial>
      <origin xyz="0.064144 0 -0.047389" rpy="0 0 0" />
      <mass value="0.394" />
      <inertia ixx="0.000169" ixy="1E-09" ixz="0.000195" iyy="0.0019044" iyz="0" izz="0.0018657" />
    </inertial>
  </link>
  <joint name="right_joint" type="revolute">
    <origin xyz="0 -0.1 -0.5" rpy="0 0 0" />
    <parent link="body" />
    <child link="right_link" />
    <axis xyz="0 1 0" />
    <limit lower="-0.7" upper="0.7" effort="200" velocity="23" />
  </joint>
  <joint name="right_front_joint" type="fixed">
    <parent link="right_link" />
    <child link="rf_contact" />
    <origin xyz=" 0.10 0 -0.06" />
  </joint>
  <joint name="right_back_joint" type="fixed">
    <parent link="right_link" />
    <child link="rb_contact" />
    <origin xyz="-0.03 0 -0.06" />
  </joint>
  <link name="rf_contact">
    <inertial>
      <mass value="0.01" />
      <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" />
    </inertial>
  </link>
  <link name="rb_contact">
    <inertial>
      <mass value="0.01" />
      <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" />
    </inertial>
  </link>
</robot>

System

• OS: Ubuntu 22.04 (Intel x86-64)
• Pinocchio version: pinocchio3-preview

[jcarpent]

Thanks for reporting this issue @matheecs. It is coming from the fact we do not fully support JointModelComposite. We will try to fix it in the following weeks.

Best, Justin