code: def _euler_to_q(self, euler):
          
          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
docs: 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]