def on_run(self, *args):
if self.last_run_time is None:
return
dt = self.this_run_time - self.last_run_time
self.revolve_angle += self.degrees_per_second * dt
if self.revolve_angle > 390:
Heading(self.initial_heading)()
Pitch(0)()
Roll(0)()
self.finish()
return
rot_2d = rotate((0, 1), self.revolve_angle)
rotation_axis = np.array((rot_2d[0], rot_2d[1], 0))
conic_quat = quat_from_axis_angle(rotation_axis, self.conic_angle_rad)
# TODO How can we achieve the same effect without using heading?
heading, pitch, roll = conic_quat.hpr()
Heading(heading)()
Pitch(pitch)()
Roll(roll)()
def on_run(self, *args):
if self.last_run_time is None:
return
dt = self.this_run_time - self.last_run_time
self.revolve_angle += self.degrees_per_second * dt
if self.revolve_angle > 390:
Heading(self.initial_heading)()
Pitch(0)()
Roll(0)()
self.finish()
return
rot_2d = rotate((0, 1), self.revolve_angle)
rotation_axis = np.array((rot_2d[0], rot_2d[1], 0))
conic_quat = quat_from_axis_angle(rotation_axis, self.conic_angle_rad)
pointing_vector = conic_quat * np.array((0, 0, 1))
# TODO How can we achieve the same effect without using heading?
heading, pitch, roll = conic_quat.hpr()
Heading(heading)()
Pitch(pitch)()
Roll(roll)()
def predict_from_ref(measured, reference): ref_norm = reference / np.linalg.norm(reference) measured_norm = measured / np.linalg.norm(measured) axis = np.cross(measured_norm, ref_norm) angle = np.arccos(ref_norm.dot(measured_norm)) return quat_from_axis_angle(axis, angle)
def predict(state):
""" Predicts the next state given the current, f in the literature.
State is a Numpy array and dt is float in seconds. """
quat, angular_vel = get_quat_ang_vel(state)
angle = np.linalg.norm(angular_vel)
if abs(angle) < 1e-7:
return state
axis = angular_vel / angle;
rotate_quat = quat_from_axis_angle(axis, angle * dt)
return make_state(rotate_quat * quat, angular_vel)
def get_thrust_vectoring_offset(self):
angle = self.thrust_vectoring_data.angle_from_value(
self.vector_angle.get())
return quat.quat_from_axis_angle(
self.thrust_vectoring_data.vector_axis, angle)
def get_thrust_vectoring_offset(self):
angle = self.thrust_vectoring_data.angle_from_value(self.vector_angle.get())
return quat.quat_from_axis_angle(self.thrust_vectoring_data.vector_axis, angle)