Missing sequences for funcction biorbd::utils::Quaternion::eulerDotToOmega

[pyomeca]

Missing sequences for funcction biorbd::utils::Quaternion::eulerDotToOmega

[EveCharbie]

code to be translated to c++ :

def matrix_calc(Q, seq):
    seq_num = np.zeros((3,))
    for i in range(3):
        if seq[i] == 'x':
            seq_num[i] = 0
        elif seq[i] == 'y':
            seq_num[i] = 1
        elif seq[i] == 'z':
            seq_num[i] = 2

    vect_units = np.zeros((3, 3))
    for i in range(3):
        vect_units[i, int(seq_num[i])] = 1

    Rot_mat_1 = biorbd.Rotation_fromEulerAngles(np.array([Q[0], 0, 0]), seq).to_array()
    Rot_mat_2 = biorbd.Rotation_fromEulerAngles(np.array([Q[0], Q[1], 0]), seq).to_array()

    matrix = np.zeros((3, 3,))
    matrix[:, 0] = np.eye(3) @ vect_units[0, :]
    matrix[:, 1] = Rot_mat_1 @ vect_units[1, :]
    matrix[:, 2] = Rot_mat_2 @ vect_units[2, :]
    return matrix

def Qdot_to_omega(Q, Qdot, seq):

    matrix = matrix_calc(Q, seq)
    omega = matrix @ Qdot

    return omega

def omega_to_Qdot(Q, omega, seq):

    matrix = np.linalg.inv(matrix_calc(Q, seq))
    Qdot = matrix @ omega

    return Qdot

from http://www.stengel.mycpanel.princeton.edu/Quaternions.pdf

[EveCharbie]

@fbailly could you confirm before I translate it since you implemented the 'xyz' sequence ?