def test_shortest_arc(self):
v1 = np.array([1., 0., 0.])
v2 = np.array([0., 1., 0.])
arc = math_utils.shortest_arc(v1, v2)
rotated = arc * quaternion.x * arc.inverse()
np.testing.assert_almost_equal((quaternion.y - rotated).norm(), 0.0)
np.testing.assert_almost_equal(rotated.norm(), 1.0)
def pid(self):
"""x_d is relative target position"""
if not self.takeoff:
return
if self.x is None:
print(
"PID controller: Failed to get state estimate, skipping frame")
return
x, xdot, xddot, body_orientation = self.x, self.xdot, self.xddot, self.q_b
roll, pitch, yaw = self.rpy(body_orientation)
g_vec = np.array([0.0, 0.0, -9.81])
unit_z = np.array([0.0, 0.0, 1.0])
x_d = self.x_d
# Position controller
self.px_pid.setpoint = x_d[0]
self.py_pid.setpoint = x_d[1]
self.pz_pid.setpoint = x_d[2]
v_d = np.array([0.0, 0.0, 0.0])
v_d[0] = self.px_pid(0.0)
v_d[1] = self.py_pid(0.0)
v_d[2] = self.pz_pid(0.0)
# Velocity controller
self.vx_pid.setpoint = v_d[0]
self.vy_pid.setpoint = v_d[1]
self.vz_pid.setpoint = v_d[2]
a_d = np.array([0.0, 0.0, 0.0])
a_d[0] = self.vx_pid(xdot[0])
a_d[1] = self.vy_pid(xdot[1])
a_d[2] = self.vz_pid(xdot[2])
a_db = self.world_to_body(body_orientation, a_d - g_vec)
# Find desired attitude correction using shortest arc algorithm
q_theta = math_utils.shortest_arc(unit_z, a_db)
roll_d, pitch_d, _ = self.rpy(q_theta)
_, _, yaw_d = self.rpy(self.q_d)
yaw_diff = self._compute_yaw_diff(yaw, yaw_d)
# Apply attitude corrections using attitude PID
self.thetax_pid.setpoint = roll_d
self.thetay_pid.setpoint = pitch_d
self.thetaz_pid.setpoint = yaw_diff
omega_d = np.array([0.0, 0.0, 0.0])
omega_d[0] = self.thetax_pid(
0.0) # 0 because a correction quantity is being tracked
omega_d[1] = self.thetay_pid(0.0)
omega_d[2] = self.thetaz_pid(0.0)
# Send command
c = np.linalg.norm(
a_db) # Desired thrust is norm of desired acceleration
self.command(c, omega_d)
def test_shortest_arc_same(self):
v1 = np.array([1, 0, 0]) * 2.0
v2 = np.array([1., 0., 0.])
arc = math_utils.shortest_arc(v1, v2)
rotated = arc * quaternion.x * arc.inverse()
np.testing.assert_almost_equal(rotated, quaternion.x)
def _orientation_quaternion(self, x):
# The low level controller will use only the yaw angle.
# Returns the quaternion which will rotate the x-axis to
# point towards the next gate.
gaze_direction = self._gaze_direction(x)
return math_utils.shortest_arc(np.array([1., 0., 0.]), gaze_direction)
def solve_for_orientation(f, a):
# Gravity is subtracted
a_without_thrust = (a - f)
return math_utils.shortest_arc(a_without_thrust, gravity_vector)