def f(self, x, u):
"""
@type x: 12x1 numpy.array
@param x: State vector [x, y, z,
roll, pitch, yaw,
x_vel, y_vel, z_vel,
roll_vel, pitch_vel, yaw_vel]
@type u: 6x1 numpy.array
@param u: Input vector [force_x, force_y, force_z
torque_roll, torque_pitch, torque_yaw]
Note force is in the quad frame and torque is attitude
(or Euler) torques.
"""
attitude = tuple(np.squeeze(x[3:6]))
linear_vel = rigid.Vector(*x[6:9], reference=self.reference)
attitude_vel = tuple(np.squeeze(x[9:12]))
force = u[0:3]
torque_attitude = u[3:6]
output = self.dynamics(attitude=attitude,
linear_vel=linear_vel,
attitude_vel=attitude_vel,
force=force,
torque_attitude=torque_attitude)
x_dot = output[0]
return x_dot
def diffeomorphism_full_state_control(self):
d = 1
xi_bar = np.array([[0, 0, 1]]).T
zeta_bar = xi_bar + np.array([[0, 0, d]]).T
m = self.M
g = self.G
R = self.rot_mat
x = self.state
xi = x[0:3]
eta = x[3:6]
xi_dot = x[6:9]
eta_dot = x[9:12]
phi = eta[0]
theta = eta[1]
psi = eta[2]
phi_dot = eta_dot[0]
theta_dot = eta_dot[1]
psi_dot = eta_dot[2]
zeta = (self * rigid.Vector(*(0, 0, -d))).array
chi = np.vstack([zeta - zeta_bar, psi, xi_dot, psi_dot])
W = mechanics.att_vel_to_ang_vel_jacobian(phi, theta)
W_inv = np.linalg.inv(W)
W_dot = mechanics.time_derivative_att_vel_to_ang_vel_jacobian(
phi, theta, phi_dot, theta_dot)
A = np.hstack(
[np.zeros((8, 4)),
np.vstack([np.identity(4), np.zeros((4, 4))])])
b = np.array([[-1 / m * R[0, 2], d * R[0, 1], -d * R[0, 0]],
[-1 / m * R[1, 2], d * R[1, 1], -d * R[1, 0]],
[-1 / m * R[2, 2], d * R[2, 1], -d * R[2, 0]]])
B = np.vstack([
np.zeros((4, 4)),
np.hstack([b, np.zeros((3, 1))]),
np.array([[0, 0, 0, 1]])
])
G = np.array([[0, 0, 0, 0, 0, 0, -g, 0]]).T
QQ = np.diag([100, 100, 10, 1, 1, 1, 1, 1])
RR = np.diag(np.array([1, 1, 1, 1]))
K, _, _ = lqr(A, B, QQ, RR)
# mu_bar = np.array([[-g*m*R[2, 2], g/d*R[2, 1], -g/d*R[2, 0], 0]]).T
mu_bar = np.array([[g * m, 0, 0, 0]]).T
mu_delta = -np.dot(K, chi)
mu = mu_bar + mu_delta
F = mu[0, 0]
alpha = mu[1:4]
J = generialized_coordinate_inertia(self.I, phi, theta, psi)
C = quad_coriolis(self.I, phi, theta, phi_dot, theta_dot, psi_dot)
tau_eta = np.dot(J, np.dot(W_inv,
(alpha - np.dot(W_dot, eta_dot)))) + np.dot(
C, eta_dot)
return np.vstack([F, tau_eta])
def force_local(self, value):
if isinstance(value, np.ndarray):
force = self.rotation.inv() * \
rigid.Vector(*value, reference=self.reference)
self.force = force.z
else:
raise Exception("Invalid argument: " + str(value) + "\n" + \
"Valid types: 3x1 numpy.array")
def torque(self, value):
force = self.force
if isinstance(value, np.ndarray) or isinstance(value, tuple):
torque = rigid.Vector(*value, reference=self)
self._pwm = tuple(
np.squeeze(
force_torque_to_pwm(-force.z, torque.array, self.L, self.C_F,
self.C_T)))
def force(self, value):
torque = self.torque
if not isinstance(value, rigid.Vector):
value = rigid.Vector(z=-float(value), reference=self)
self._pwm = tuple(
np.squeeze(
force_torque_to_pwm(-value.z, self.torque.array, self.L,
self.C_F, self.C_T)))
def pid_control(self):
u_bar = self.input_desired
# e = self.state_err[0:6]
e_pos = self.rotation.inv() * rigid.Vector(*self.state_err[0:3],
reference=self.reference)
e_vel = self.rotation.inv() * rigid.Vector(*self.state_err[3:6],
reference=self.reference)
e = np.vstack([e_pos.array, e_vel.array])
# e_i = self.state_err_cum_sum[0:6]
e_i_pos = self.rotation.inv() * rigid.Vector(
*self.state_err_cum_sum[0:3], reference=self.reference)
e_i_vel = self.rotation.inv() * rigid.Vector(
*self.state_err_cum_sum[3:6], reference=self.reference)
e_i = np.vstack([e_i_pos.array, e_i_vel.array])
# e_d = self.state_err[6:12]
e_d_pos = self.rotation.inv() * rigid.Vector(*self.state_err[6:9],
reference=self.reference)
e_d_vel = self.rotation.inv() * rigid.Vector(*self.state_err[9:12],
reference=self.reference)
e_d = np.vstack([e_d_pos.array, e_d_vel.array])
P = self.pid_P
I = self.pid_I
D = self.pid_D
u_delta = np.dot(P, e) + np.dot(I, e_i) + np.dot(D, e_d)
return u_bar + u_delta
def __init__(self,
params=QuadParams(),
time=0,
position=(0, 0, 0),
attitude=(0, 0, 0),
linear_vel=(0, 0, 0),
attitude_vel=(0, 0, 0),
pwm=None,
reference=frames.LOCAL):
QuadParams.__init__(self,
name=params.name,
M=params.M,
I=params.I,
L=params.L,
C_F=params.C_F,
C_T=params.C_T)
frames.AircraftState.__init__(self,
name=params.name,
time=time,
position=position,
attitude=attitude,
linear_vel=linear_vel,
attitude_vel=attitude_vel,
reference=reference)
self._finite_state = ['LANDED']
self._init_state = self.state
self.state_desired = self.state
if pwm is None:
self.pwm = (0, 0, 0, 0)
self.force = rigid.Vector(z=-self.G * self.M, reference=self)
else:
self.pwm = pwm
self._init_pwm = self.pwm
# # LQR attitude controller
# self.controller = self.lqr_attitude_control
# LQR position and attitude controller
self.controller = self.lqr_position_attitude_control
def dynamics(self,
attitude=None,
linear_vel=None,
attitude_vel=None,
pwm=None,
force=None,
torque=None,
torque_attitude=None):
if attitude is None:
attitude = self.attitude
if linear_vel is None:
linear_vel = self.linear_vel
if attitude_vel is None:
attitude_vel = self.attitude_vel
if pwm is None:
if force is None:
force = self.force
if torque is None and torque_attitude is None:
torque_attitude = np.array([self.torque_attitude]).T
elif torque is not None and torque_attitude is None:
torque_attitude = mechanics.ang_torque_to_att_torque(
self.attitude, torque.array)
elif torque is not None and torque_attitude is not None:
raise Exception(
"Invalid argument. Either use torque or torque_attitude, but not both."
)
elif pwm is not None and force is None and torque is None:
(force, torque) = pwm_to_force_torque(np.array(pwm), self.L,
self.C_F, self.C_T)
# force_local = self.rotation * rigid.Vector(x=0, y=0, z=force, reference=self)
force = rigid.Vector(x=0, y=0, z=-force, reference=self)
torque = rigid.Vector(*torque, reference=self)
torque_attitude = mechanics.ang_torque_to_att_torque(
self.attitude, torque.array)
else:
raise Exception(
"Invalid arguments. Either use pwm or (force and torque), but not both."
)
(x_dot_trans, x_dot_rot, A_trans, B_trans, A_rot, B_rot) = \
quad_model.quad_dynamics(self.G, self.M,
self.I[0, 0], self.I[1, 1], self.I[2, 2],
linear_vel.x, linear_vel.y, linear_vel.z,
attitude[0], attitude[1], attitude[2],
attitude_vel[0], attitude_vel[1], attitude_vel[2],
-force.z,
torque_attitude[0, 0], torque_attitude[1, 0], torque_attitude[2, 0])
# Rearrange the ordering of the states from
# [pos pos_vel att att_vel] to [pos, att, pos_vel, att_vel]
i3 = np.identity(3)
z3 = np.zeros((3, 3))
# Note: For this similarity transform T_inv = T.T = T
T_inv = np.vstack([
np.hstack([i3, z3, z3, z3]),
np.hstack([z3, z3, i3, z3]),
np.hstack([z3, i3, z3, z3]),
np.hstack([z3, z3, z3, i3])
])
f_bar = np.vstack([x_dot_trans, x_dot_rot])
x_dot = np.dot(T_inv, f_bar)
A_left = np.vstack([A_trans, np.zeros((6, 6))])
A_right = np.vstack([np.zeros((6, 6)), A_rot])
A_bar = np.hstack([A_left, A_right])
A = np.dot(T_inv, np.dot(A_bar, T_inv))
B_top = np.hstack([B_trans, np.zeros((6, 3))])
B_bot = np.hstack([np.zeros((6, 1)), B_rot])
B_bar = np.vstack([B_top, B_bot])
B = np.dot(T_inv, B_bar)
# Temporary
# x_dot[0:3] = np.zeros((3,1))
if self.finite_state[0] == 'LANDED':
x_dot[2, 0] = max(x_dot[2, 0], 0)
return (x_dot, A, B, A_trans, B_trans, A_rot, B_rot)
def torque(self):
(force, torque) = pwm_to_force_torque(np.array(self._pwm), self.L,
self.C_F, self.C_T)
return rigid.Vector(*torque, reference=self)