Floating base rnea cross term

[dsami2]

I have a question that probably comes down to some frame convention that I'm missing. I have a point mass URDF:

<?xml version="1.0" encoding="utf-8"?>
<robot name="PointMass">
    <link name="point_mass">
        <inertial>
            <mass value="1.0"/>
            <origin xyz="0.0  0.0  0.0" rpy="0.0 0.0 0.0"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>
    </link>
</robot>

And I have a minimal test script calling RNEA here:

import pinocchio as pin
from pinocchio.utils import *
import numpy as np

model = pin.buildModelFromUrdf("point_mass.urdf", pin.JointModelFreeFlyer())
data = pin.Data(model)

q = np.array([0,0,0,0,0,0,0])
qdd = np.array([0,0,0,0,0,0])

qd = np.array([0,0,0,0,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [0.   0.   9.81 0.   0.   0.  ]

qd = np.array([0,0,1,0,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [0.   0.   9.81 0.   0.   0.  ]

qd = np.array([0,0,1,1,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [ 0.   -1.    9.81  0.    0.    0.  ]

So, the first two show the gravity force in Z. The third one also shows a negative force in Y, which looks to me to be like a cross product term: omega x v = [1 0 0] x [0 0 1] = [0 -1 0] I don't understand where that term comes from, could somebody explain?

[dsami2]

I have a modified version of the same test where I add 1 revolute dof:

<?xml version="1.0" encoding="utf-8"?>
<robot name="1DofMass">
    <link name="root_link"/>
    <link name="point_mass">
        <inertial>
            <mass value="1.0"/>
            <origin xyz="0.0  0.0  0.0" rpy="0.0 0.0 0.0"/>
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
        </inertial>
    </link>

    <joint name="joint" type="revolute">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <axis xyz="1.0 0.0 0.0" />
        <parent link="root_link" />
        <child link="point_mass" />
        <limit effort="0.0" lower="0.0" upper="0.0" velocity="0.0" />
    </joint>
</robot>

And modified script to change the floating body angular velocity to be a velocity in the revolute joint instead:

import pinocchio as pin
from pinocchio.utils import *
import numpy as np

model_1dof = pin.buildModelFromUrdf("1dof_mass.urdf", pin.JointModelFreeFlyer())
data_1dof = pin.Data(model_1dof)

q = np.array([0,0,0,0,0,0,0,0])
qdd = np.array([0,0,0,0,0,0,0])

qd = np.array([0,0,0,0,0,0,0])
pin.computeAllTerms(model_1dof, data_1dof, q, qd)
print(pin.rnea(model_1dof, data_1dof, q, qd, qdd))
# prints [0.   0.   9.81 0.   0.   0.  ]

qd = np.array([0,0,1,0,0,0,0])
pin.computeAllTerms(model_1dof, data_1dof, q, qd)
print(pin.rnea(model_1dof, data_1dof, q, qd, qdd))
# prints [0.   0.   9.81 0.   0.   0.  ]

qd = np.array([0,0,1,0,0,0,1])
pin.computeAllTerms(model_1dof, data_1dof, q, qd)
print(pin.rnea(model_1dof, data_1dof, q, qd, qdd))
# prints [0.   0.   9.81 0.   0.   0.  ]

In my mind, this is basically the same model as the point mass above, except that I added a massless link in between the floating base and the point mass. But now, that cross term has gone away.

[jcarpent]

To shortly answer your question, the [0, -1, 0] is coming from the Coriolis/Centrifugal effects that you can compute via: pin.computeCoriolisMatrix(model,data,q,qd) @ qd

[jcarpent]

Also, don't forget to use pin.neutral(model) to compute a configuration vector, otherwise, it will not be properly normalized.

import pinocchio as pin
from pinocchio.utils import *
import numpy as np

model = pin.buildModelFromUrdf("point_mass.urdf", pin.JointModelFreeFlyer())
data = pin.Data(model)

q = pin.neutral(model)
qdd = np.array([0,0,0,0,0,0])

qd = np.array([0,0,0,0,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [0.   0.   9.81 0.   0.   0.  ]

qd = np.array([0,0,1,0,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [0.   0.   9.81 0.   0.   0.  ]

qd = np.array([0,0,1,1,0,0])
pin.computeAllTerms(model, data, q, qd)
print(pin.rnea(model, data, q, qd, qdd))
# prints [ 0.   -1.    9.81  0.    0.    0.  ]