Create q array from euler angles
        :param euler: array [roll, pitch, yaw] in rad
        :returns: array q which represents a quaternion [w, x, y, z]

def _euler_to_q(self, euler):
        """
        Create q array from euler angles
        :param euler: array [roll, pitch, yaw] in rad
        :returns: array q which represents a quaternion [w, x, y, z]
        """
        assert(len(euler) == 3)
        phi = euler[0]
        theta = euler[1]
        psi = euler[2]
        c_phi_2 = np.cos(phi / 2)
        s_phi_2 = np.sin(phi / 2)
        c_theta_2 = np.cos(theta / 2)
        s_theta_2 = np.sin(theta / 2)
        c_psi_2 = np.cos(psi / 2)
        s_psi_2 = np.sin(psi / 2)
        q = np.zeros(4)
        q[0] = (c_phi_2 * c_theta_2 * c_psi_2 +
                s_phi_2 * s_theta_2 * s_psi_2)
        q[1] = (s_phi_2 * c_theta_2 * c_psi_2 -
                c_phi_2 * s_theta_2 * s_psi_2)
        q[2] = (c_phi_2 * s_theta_2 * c_psi_2 +
                s_phi_2 * c_theta_2 * s_psi_2)
        q[3] = (c_phi_2 * c_theta_2 * s_psi_2 -
                s_phi_2 * s_theta_2 * c_psi_2)
        return q