Quaternion of a JointModelFreeFlyer

[xiaosuyang]

Hello.
I give a quaternion [0.707,0,0,0.707] to JointModelFreeFlyer which let the base_link rotate about Z axis 90 degree, but the JointModelFreeFlyer's oMi is :

R =
           1            0            0
           0 -2.22045e-16           -1
           0            1 -2.22045e-16

which is rotate about X axis 90 degree.

[xiaosuyang]

the quat is[w,x,y,z] in pinocchio,not the order [x,y,z,w]

[jorisv]

Hello,

Actually the quaternion in the generalized position vector (q) is stored in [x, y, z, w] order.
The full position vector for the FreeFlyer is [X, Y, Z, QX, QY, QZ, QW] where X, Y, Z is the translation vector and QX, QY, QZ, QW is the quaternion.

You can see it with the following sample code:

import numpy as np
import pinocchio as pin

# Create a model with only a FreeFlyer joint
m = pin.Model()
m.addJoint(0, pin.JointModelFreeFlyer(), pin.SE3.Identity(), "t")
d = m.createData()
# q_vec = [X, Y, Z, QX, QY, QZ, QW]
q_vec = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1])
pin.forwardKinematics(m, d, q_vec)
print("Identity quaterion:", q_vec[3:])
print(d.oMi[-1].rotation)
print()

# We create a quaternion from an AngleAxis rotated around X
aa = pin.AngleAxis(np.pi / 2, np.array([1, 0, 0]))
quat = pin.Quaternion(aa)
q_vec[3:] = quat.coeffs()
pin.forwardKinematics(m, d, q_vec)
print("X rotated quaterion:", q_vec[3:])
print(d.oMi[-1].rotation)
print()

# We create a quaternion from an AngleAxis rotated around Y
aa = pin.AngleAxis(np.pi / 2, np.array([0, 1, 0]))
quat = pin.Quaternion(aa)
q_vec[3:] = quat.coeffs()
pin.forwardKinematics(m, d, q_vec)
print("Y rotated quaterion:", q_vec[3:])
print(d.oMi[-1].rotation)
print()

# We create a quaternion from an AngleAxis rotated around Z
aa = pin.AngleAxis(np.pi / 2, np.array([0, 0, 1]))
quat = pin.Quaternion(aa)
q_vec[3:] = quat.coeffs()
pin.forwardKinematics(m, d, q_vec)
print("Z rotated quaterion:", q_vec[3:])
print(d.oMi[-1].rotation)
print()

The output is:

Identity quaterion: [0. 0. 0. 1.]
[[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]

X rotated quaterion: [0.70710678 0.         0.         0.70710678]
[[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  2.22044605e-16 -1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  2.22044605e-16]]

Y rotated quaterion: [0.         0.70710678 0.         0.70710678]
[[ 2.22044605e-16  0.00000000e+00  1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  0.00000000e+00]
 [-1.00000000e+00  0.00000000e+00  2.22044605e-16]]

Z rotated quaterion: [0.         0.         0.70710678 0.70710678]
[[ 2.22044605e-16 -1.00000000e+00  0.00000000e+00]
 [ 1.00000000e+00  2.22044605e-16  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  1.00000000e+00]]

In the output, you can see that the last part of the quaternion always hold a value. If the order was [w,x,y,z] like you said, the last value should be 0 for X and Y rotation.

[xiaosuyang]

and I use the following code to get the se3 of freeflyerjoint:

use the [w,x,y,z] order， the SE3 of the freeflyerjoint is right. It is because of the C++ code?

[jcarpent]

We follow the convention of Eigen storage.