Acceleration error for free flyer base

[MaximilianDio]

Hello, I encountered an error in the calculation of the classical frame acceleration for freeFlyer bodies.

Assume we have a FreeFlyer body (B) and a frame (i) attached to it. The position from the freeflyer frame to the frame (i) is given by $r_{Bi}$.
Also assume that $x^B$ is a vector x represented in the body frame and $x^W=R^W_B x^B$ is a vector represented in the world frame.
We are given the freeflyer acceleration in world frame $a_{WB}^W, \dot{\omega}$ as well as the velocity $v_{WB}^W, \omega_{WB}^W$

From classical mechanics we can evaluate the acceleration of a frame attached to a free moving object like
$a_{Wi}^W = a_{WB}^W + \dot{\omega} \times r_{Bi}^W + \omega_{WB}^W \times (\omega_{WB}^W \times r_{Bi}^W)$
The acceleration in the body frame is then simply $a_{Wi}^B = R^B_W a_{Wi}^W$

I would like to compose multiple objects and robots using pinocchio. Therefore, I need a way to compute the kinematics and dynamics of a freeflyer object. However, it seems that there is an issue when evaluating the acceleration of a frame attached to a freeflyer object.

Here is a sample code and a sample urdf file:

Note that the frame acceleration changes, when the linear velocity $v_{WB}^W$ of the freeflyer is not 0! - However, the linear velocity should not have any effect on the frame acceleration.

  <?xml version="1.0" encoding="utf-8"?>
  <robot name="object_3d">
  
      <link name="obj_com">
          <inertial>
              <origin xyz="0.0 0.0 0.0" rpy=".0 .0 .0" />
              <mass value="1.0" />
              <inertia ixx="2.0" ixy=".0" ixz=".0" iyy="3.0" iyz=".0" izz="4.0" />
          </inertial>
          <visual>
              <origin xyz="0 0 0" rpy="0 0 0" />
              <geometry>
                  <box size="1.0 1.0 1.0"/>
              </geometry>
              <material name="">
                  <color rgba="0 0 1 1" />
              </material>
          </visual>
      </link>
  
      <joint name="virt_stick_01" type="fixed">
          <parent link="obj_com"/>
          <child link="p_01"/>
          <origin xyz="1.0 0 0" rpy="0 0 0"/>
      </joint>
      <link name="p_01"></link>
  
      <joint name="virt_stick_02" type="fixed">
          <parent link="obj_com"/>
          <child link="p_02"/>
          <origin xyz="-1 0 0" rpy="0 0 0"/>
      </joint>
      <link name="p_02"></link>
  
  </robot>

The following python script was used to evaluate the frame acceleration

import pinocchio as pin
import numpy as np
import copy

np.set_printoptions(formatter={"float": lambda x: "{0:0.3f}".format(x)})

model = pin.buildModelFromUrdf(
    "/home/rtdio/grampc-rob/models/object_3d/object_3d.urdf", pin.JointModelFreeFlyer()
)
data = model.createData()
data_plus = model.createData()

pi_fId = [model.getFrameId("p_01"), model.getFrameId("p_02")]

# compute frames in object frame
###############################
pin.forwardKinematics(model, data, np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]))
pin.updateFramePlacements(model, data)
r_Bi = []
R_Bi = []
for fId in pi_fId:
    r_Bi.append(copy.copy(data.oMf[fId].translation))
    R_Bi.append(copy.copy(data.oMf[fId].rotation))

for i in range(len(pi_fId)):
    print("r_{}: {}".format(i, r_Bi[i]))
print("####################")


# define test configuration/velocity/acceleration
#################################################

# pose from object to world frame
# q = np.array([0.0, 0.0, 0.0, 0, 0, 0, 1])
q = pin.randomConfiguration(model)
q[0:3] = np.random.rand(3)
print(f"q_o:\n{q}")
# object velocity represented in base frame
v_B = np.array([0.0, -1.0, 0.0, 0.0, 0.0, 1.0])
# v_B = np.random.rand(model.nv)
print(f"v_o:\n{v_B}")
# object acceleration represented in base frame
a_B = np.zeros(model.nv)
# a_B = np.random.rand(model.nv)
print(f"a_o:\n{a_B}")

# transform to world frame
R_WB = pin.Quaternion(q[3:7]).toRotationMatrix()
v_W = np.block([R_WB @ v_B[0:3], R_WB @ v_B[3:6]])
a_W = np.block([R_WB @ a_B[0:3], R_WB @ a_B[3:6]])
print("####################")

# compute forward kinematics for grasping points
################################################
pin.forwardKinematics(model, data, q, v_B, a_B)
pin.computeJointJacobians(model, data, q)
pin.computeJointJacobiansTimeVariation(model, data, q, v_B)
pin.updateFramePlacements(model, data)

gp_W_pose = []
gp_W_pose_analytic = []

gp_B_twist = []
gp_B_twist_derivative = []

gp_W_twist_analytic = []
gp_W_twist_derivative_analytic = []

gp_B_twist_analytic = []
gp_B_twist_derivative_analytic = []

# compute kinematics for each grasping point
for i, fId in enumerate(pi_fId):
    # obtain kinematics via pinocchio
    #################################
    T_gp = data.oMf[fId]
    v_gp_B = pin.getFrameVelocity(model, data, fId, pin.ReferenceFrame.LOCAL)
    a_gp_B = pin.getFrameClassicalAcceleration(
        model, data, fId, pin.ReferenceFrame.LOCAL
    )

    gp_W_pose.append(
        np.block([T_gp.translation, pin.Quaternion(T_gp.rotation).coeffs()])
    )
    gp_B_twist.append(np.block([v_gp_B.linear, v_gp_B.angular]))
    gp_B_twist_derivative.append(np.block([a_gp_B.linear, a_gp_B.angular]))

    # obtain kinematics via analytical formula
    ###########################################
    r_gp_W = q[0:3] + R_WB @ R_Bi[i] @ r_Bi[i]
    q_qp_W = pin.Quaternion(q[3:7]) * pin.Quaternion(R_Bi[i])

    v_gp_W = np.block(
        [v_W[0:3] + np.cross(v_W[3:6], R_WB @ R_Bi[i] @ r_Bi[i]), v_W[3:6]]
    )
    a_gp_W = np.block(
        [
            a_W[0:3]
            + np.cross(a_W[3:6], R_WB @ R_Bi[i] @ r_Bi[i])
            + np.cross(v_W[3:6], np.cross(v_W[3:6], R_WB @ R_Bi[i] @ r_Bi[i])),
            a_W[3:6],
        ]
    )

    gp_W_pose_analytic.append(np.block([r_gp_W, q_qp_W.coeffs()]))
    gp_W_twist_analytic.append(v_gp_W)
    gp_W_twist_derivative_analytic.append(a_gp_W)

    # transform to body frame
    gp_B_twist_analytic.append(np.block([R_WB.T @ v_gp_W[0:3], R_WB.T @ v_gp_W[3:6]]))
    gp_B_twist_derivative_analytic.append(
        np.block([R_WB.T @ a_gp_W[0:3], R_WB.T @ a_gp_W[3:6]])
    )

# check results
for i in range(len(pi_fId)):

    print(f"r_gp_W_{i}:\n{gp_W_pose[i]}")
    print(f"r_gp_W_ana_{i}:\n{gp_W_pose_analytic[i]}")
    assert np.allclose(gp_W_pose[i], gp_W_pose_analytic[i]), "pose is not correct"

    print(f"v_gp_B_{i}:\n{gp_B_twist[i]}")
    print(f"v_gp_B_ana_{i}:\n{gp_B_twist_analytic[i]}")
    assert np.allclose(gp_B_twist[i], gp_B_twist_analytic[i]), "velocity is not correct"

    print(f"a_gp_B_{i}:\n{gp_B_twist_derivative[i]}")
    print(f"a_gp_B_ana_{i}:\n{gp_B_twist_derivative_analytic[i]}")
    assert np.allclose(
        gp_B_twist_derivative[i], gp_B_twist_derivative_analytic[i]
    ), "acceleration is not correct"

    print("####################")

It seems that the calculation of the classical acceleration assumes a fixed base frame.
It can be easily corrected by subtracting the cross product of the angular velocity and the linea velocity of the base frame.

a_gp_B = pin.getFrameClassicalAcceleration(
        model, data, fId, pin.ReferenceFrame.LOCAL
    )
gp_B_twist_derivative.append(
        np.block(
            [
                a_gp_B.linear - np.cross(v_gp_B.angular, v_B[0:3]),
                a_gp_B.angular,
            ]
        )
    )

[jcarpent]

@MaximilianDio We currently have limited human resources to answer from Pinocchio's team. We hope the community will be able to provide help in the meantime.