RLBench/envs/_quadrocopter/quaternions.py
def omega_from_quat_quat(q1, q2, dt):
    """
    Convert two quaternions and the time difference to angular velocity.

    Parameters:
    -----------
    q1: quaternion
        The old quaternion
    q2: quaternion
        The new quaternion
    dt: float
        The time difference

    Returns:
    --------
    omega_g: ndarray
        The angular velocity in global coordinates
    """
    if vector_norm(q1 - q2) < 1e-8:
        # linearly interpolate
        # the quaternion does not stay on unit sphere -> only for very small
        # rotations!

        # dq/dt
        dq = (q2 - q1) / dt

        # From Diebel: Representing Atitude, 6.6, quaternions are defined
        # differently there: [w, x, y, z] instead of [x, y, z, w]!
        omega = np.array([0.0, 0.0, 0.0], dtype=np.float64)

        # Equivalent, but slower
        # w = np.array([[q2[3], -q2[2], q2[1], -q2[0]],
        #               [q2[2], q2[3], -q2[0], -q2[1]],
        #               [-q2[1], q2[0], q2[3], -q2[2]]], dtype=np.float64)
        #
        # omega = 2 * w.dot(dq)

        omega[0] = 2.0 * (
            q2[3] * dq[0] - q2[2] * dq[1] + q2[1] * dq[2] - q2[0] * dq[3])
        omega[1] = 2.0 * (
            q2[2] * dq[0] + q2[3] * dq[1] - q2[0] * dq[2] - q2[1] * dq[3])
        omega[2] = 2.0 * (
            -q2[1] * dq[0] + q2[0] * dq[1] + q2[3] * dq[2] - q2[2] * dq[3])

        return omega
    else:
        # This function becomes numerically unstable for q1-q2 --> 0

        # Find rotation from q1 to q2
        # unit quaternion -> conjugate is the same as inverse
        # q2 = r * q1 --> r = q2 * inv(q1)
        r = quaternion_multiply(q2, quaternion_conjugate(q1))

        # Angle of rotation
        angle = 2.0 * math.acos(r[3])

        # acos gives value in [0,pi], ensure that we take the short path
        # (e.g. rotate by -pi/2 rather than 3pi/2)
        if angle > math.pi:
            angle -= 2.0 * math.pi

        # angular velocity = angle / dt
        # axis of rotation corresponds to r[:3]
        return angle / dt * r[:3] / vector_norm(r[:3])

    Convert two quaternions and the time difference to angular velocity.

    Parameters:
    -----------
    q1: quaternion
        The old quaternion
    q2: quaternion
        The new quaternion
    dt: float
        The time difference

    Returns:
    --------
    omega_g: ndarray
        The angular velocity in global coordinates
    
Convert two quaternions and the time difference to angular velocity.
Parameters.

quaternion
The old quaternion
q2: quaternion
The new quaternion
dt: float
The time difference



ndarray
The angular velocity in global coordinates
Convert two quaternions and the time difference to angular velocity . Parameters . quaternion The old quaternion q2 : quaternion The new quaternion dt : float The time difference ndarray The angular velocity in global coordinates
def omega_from_quat_quat(q1, q2, dt):
    if vector_norm(q1 - q2) < 1e-8:
        dq = (q2 - q1) / dt
        omega = np.array([0.0, 0.0, 0.0], dtype=np.float64)
         omega = 2 * w.dot(dq)
        omega[0] = 2.0 * (
            q2[3] * dq[0] - q2[2] * dq[1] + q2[1] * dq[2] - q2[0] * dq[3])
        omega[1] = 2.0 * (
            q2[2] * dq[0] + q2[3] * dq[1] - q2[0] * dq[2] - q2[1] * dq[3])
        omega[2] = 2.0 * (
            -q2[1] * dq[0] + q2[0] * dq[1] + q2[3] * dq[2] - q2[2] * dq[3])
        return omega
    else:
         This function becomes numerically unstable for q1-q2 --> 0
         Find rotation from q1 to q2
         unit quaternion -> conjugate is the same as inverse
         q2 = r * q1 --> r = q2 * inv(q1)
        r = quaternion_multiply(q2, quaternion_conjugate(q1))
         Angle of rotation
        angle = 2.0 * math.acos(r[3])
         acos gives value in [0,pi], ensure that we take the short path
         (e.g. rotate by -pi/2 rather than 3pi/2)
        if angle > math.pi:
            angle -= 2.0 * math.pi
         angular velocity = angle / dt
         axis of rotation corresponds to r[:3]
        return angle / dt * r[:3] / vector_norm(r[:3])
def omega_from_quat_quat ( q1 , q2 , dt ) : if vector_norm ( q1 - q2 ) < 1e-8 : dq = ( q2 - q1 ) / dt omega = np . array ( [ 0.0 , 0.0 , 0.0 ] , dtype = np . float64 ) omega [ 0 ] = 2.0 * ( q2 [ 3 ] * dq [ 0 ] - q2 [ 2 ] * dq [ 1 ] + q2 [ 1 ] * dq [ 2 ] - q2 [ 0 ] * dq [ 3 ] ) omega [ 1 ] = 2.0 * ( q2 [ 2 ] * dq [ 0 ] + q2 [ 3 ] * dq [ 1 ] - q2 [ 0 ] * dq [ 2 ] - q2 [ 1 ] * dq [ 3 ] ) omega [ 2 ] = 2.0 * ( - q2 [ 1 ] * dq [ 0 ] + q2 [ 0 ] * dq [ 1 ] + q2 [ 3 ] * dq [ 2 ] - q2 [ 2 ] * dq [ 3 ] ) return omega else : r = quaternion_multiply ( q2 , quaternion_conjugate ( q1 ) ) angle = 2.0 * math . acos ( r [ 3 ] ) if angle > math . pi : angle -= 2.0 * math . pi return angle / dt * r [ : 3 ] / vector_norm ( r [ : 3 ] )
Convert two quaternions and the time difference to angular velocity.
Convert two quaternions and the time difference to angular velocity .
"""
    Convert two quaternions and the time difference to angular velocity.

    Parameters:
    -----------
    q1: quaternion
        The old quaternion
    q2: quaternion
        The new quaternion
    dt: float
        The time difference

    Returns:
    --------
    omega_g: ndarray
        The angular velocity in global coordinates
    """ # linearly interpolate # the quaternion does not stay on unit sphere -> only for very small # rotations! # dq/dt # From Diebel: Representing Atitude, 6.6, quaternions are defined # differently there: [w, x, y, z] instead of [x, y, z, w]! # Equivalent, but slower # w = np.array([[q2[3], -q2[2], q2[1], -q2[0]], #               [q2[2], q2[3], -q2[0], -q2[1]], #               [-q2[1], q2[0], q2[3], -q2[2]]], dtype=np.float64) # # omega = 2 * w.dot(dq) # This function becomes numerically unstable for q1-q2 --> 0 # Find rotation from q1 to q2 # unit quaternion -> conjugate is the same as inverse # q2 = r * q1 --> r = q2 * inv(q1) # Angle of rotation # acos gives value in [0,pi], ensure that we take the short path # (e.g. rotate by -pi/2 rather than 3pi/2) # angular velocity = angle / dt # axis of rotation corresponds to r[:3]
q1
q2
dt