def get_feedback_response(self, state, control, dt):
X = slice(0, 3)
V = slice(3, 6)
Z = slice(6, 9)
RPY = slice(6, 9)
OM = slice(9, 12)
U = slice(0, 1)
AA = slice(1, 4)
pos = state[X]
vel = state[V]
rpy = state[RPY]
angvel = state[OM]
rot = Rotation.from_euler('ZYX', rpy[::-1])
z = rot.apply(np.array((0, 0, 1)))
skew_z = mathu.skew_matrix(z)
rot_inv = rot.inv().as_matrix()
pos_vel = state[:6]
accel_des = -self.K_pos.dot(pos_vel - np.hstack(
(self.pos_des, self.vel_des))) + self.acc_des + g3
anorm = np.linalg.norm(accel_des)
K_x = np.zeros((self.n_control, self.n_state))
K_u = np.zeros((self.n_control, self.n_control))
dadx = -self.K_pos[:, X]
dadv = -self.K_pos[:, V]
duda = self.U.duda(accel_des, z)
dudz = self.U.dudz(accel_des, z)
dudx = duda.dot(dadx)
dudv = duda.dot(dadv)
K_x[U, X] = dudx
K_x[U, V] = dudv
K_x[U, Z] = dudz
z_des = accel_des / anorm
dzda = (1.0 / anorm) * (np.eye(3) - np.outer(z_des, z_des))
dzdx = dzda.dot(dadx)
dzdv = dzda.dot(dadv)
K_x[AA, X] = self.K_att[:, :3].dot(rot_inv).dot(skew_z.dot(dzdx))
K_x[AA, V] = self.K_att[:, :3].dot(rot_inv).dot(skew_z.dot(dzdv))
# TODO Fix this.
K_x[AA,
Z] = -self.K_att[:, :3].dot(rot_inv).dot(mathu.skew_matrix(z_des))
K_x[AA, OM] = -self.K_att[:, 3:6]
K_u[U, U] = 1
K_u[AA, AA] = np.eye(3)
return K_x, K_u
def get_ABCD(self, state, control, dt):
X = slice(0, 3)
V = slice(3, 6)
RPY = slice(6, 9)
Z = slice(6, 9)
OM = slice(9, 12)
U = slice(0, 1)
AA = slice(1, 4)
rpy = state[RPY]
angvel = state[OM]
rot = Rotation.from_euler('ZYX', rpy[::-1])
z = rot.apply(np.array((0, 0, 1)))
ang_world = rot.apply(angvel)
if not self.use_feedback:
u = control[U][0]
else:
pos_vel = state[:6]
accel_des = -self.K_pos.dot(pos_vel - np.hstack(
(self.pos_des, self.vel_des))) + self.acc_des + self.g_vec
u = self.U.u(accel_des, z)
A = np.zeros((self.n_state, self.n_state))
B = np.zeros((self.n_state, self.n_control))
C = np.zeros((self.n_out, self.n_state))
D = np.zeros((self.n_out, self.n_control))
A[X, X] = np.eye(3)
A[X, V] = dt * np.eye(3)
A[V, V] = np.eye(3)
A[V, Z] = u * dt * np.eye(3)
A[Z, Z] = np.eye(3) + dt * mathu.skew_matrix(ang_world)
#A[Z, OM] = -dt * mathu.skew_matrix(z).dot(rot.as_matrix())
A[Z, OM] = -dt * rot.as_matrix().dot(
mathu.skew_matrix(np.array((0, 0, 1))))
A[OM, OM] = np.eye(3)
B[V, U] = dt * np.array([z]).T
B[OM, AA] = dt * np.eye(3)
C[X, X] = np.eye(3)
if self.use_feedback:
K_x, K_u = self.get_feedback_response(state, control, dt)
A += B.dot(K_x)
B = B.dot(K_u)
return A, B, C, D
def __init__(self,
*,
mass,
motor_thrust_coeffs,
motor_torque_scale,
inertia,
motor_arm_length,
motor_spread_angle,
motor_inertia=0.0,
center_of_mass=np.zeros(3),
**kwargs):
self.motor_thrust = np.poly1d(motor_thrust_coeffs)
self.mass = mass
self.I = inertia
self.I_inv = np.linalg.inv(self.I)
self.motor_I = np.zeros((3, 3))
self.motor_I[2, 2] = motor_inertia
dcms = motor_arm_length * np.cos(motor_spread_angle)
dsms = motor_arm_length * np.sin(motor_spread_angle)
self.motor_dirs = np.array((1.0, 1.0, -1.0, -1.0))
self.mixer = np.zeros((4, 4))
self.mixer[0, :] = np.ones(4)
self.mixer[1, :] = np.array((-dsms, dsms, dsms, -dsms))
self.mixer[2, :] = np.array((-dcms, dcms, -dcms, dcms))
self.mixer[3, :] = -motor_torque_scale * self.motor_dirs
self.mixer_inv = np.linalg.inv(self.mixer)
# torque = com x (0, 0, thrust) = [com]_x * (0, 0, thrust)
self.com_thrust_torque = skew_matrix(center_of_mass)[:, 2][:,
np.newaxis]
for k, v in kwargs.items():
setattr(self, k, v)
def get_feedback_response(self, state, control, dt, nou=False):
X = slice(0, 3)
V = slice(3, 6)
RPY = slice(6, 9)
OM = slice(9, 12)
U = slice(0, 1)
AA = slice(1, 4)
pos = state[X]
vel = state[V]
rpy = state[RPY]
angvel = state[OM]
rot = Rotation.from_euler('ZYX', rpy[::-1])
z = rot.apply(np.array((0, 0, 1)))
skew_z = mathu.skew_matrix(z)
skew_angvel_w = mathu.skew_matrix(rot.apply(angvel))
z_dot = skew_angvel_w.dot(z)
roll, pitch, yaw = rpy
dzdrpy = np.array(((np.sin(yaw) * np.cos(roll) -
np.sin(roll) * np.cos(yaw) * np.sin(pitch),
np.cos(roll) * np.cos(yaw) * np.cos(pitch),
np.sin(roll) * np.cos(yaw) -
np.cos(roll) * np.sin(yaw) * np.sin(pitch)),
(-np.sin(roll) * np.sin(yaw) * np.sin(pitch) -
np.cos(yaw) * np.cos(roll),
np.cos(roll) * np.sin(yaw) * np.cos(pitch),
np.cos(roll) * np.cos(yaw) * np.sin(pitch) +
np.sin(yaw) * np.sin(roll)),
(-np.cos(pitch) * np.sin(roll),
-np.sin(pitch) * np.cos(roll), 0)))
dzdotdrpy = skew_angvel_w.dot(dzdrpy)
dzdotdang = -skew_z.dot(rot.as_matrix())
u, udot = self.int_u, self.int_udot
k1 = 840 * np.eye(3) / self.duration**4
k2 = 480 * np.eye(3) / self.duration**3
k3 = 120 * np.eye(3) / self.duration**2
k4 = 16 * np.eye(3) / self.duration**1
start_acc = u * z - g3
start_jerk = u * z_dot + udot * z
pos_err = pos - self.pos_des
vel_err = vel - self.vel_des
acc_err = start_acc - self.acc_des
jerk_err = start_jerk - self.jerk_des
djerkdrpy = u * dzdotdrpy + udot * dzdrpy
daccdrpy = u * dzdrpy
dsnapdrpy = -k3.dot(daccdrpy) - k4.dot(djerkdrpy)
snap = -k1.dot(pos_err) - k2.dot(vel_err) - k3.dot(acc_err) - k4.dot(
jerk_err) + self.snap_des
dv1drpy = dsnapdrpy.T.dot(z) + snap.dot(
dzdrpy) + 2 * u * z_dot.T.dot(dzdotdrpy)
v1 = snap.dot(z) + u * z_dot.dot(z_dot)
u_factor = 1.0 / u
z_ddot = u_factor * (snap - v1 * z - 2 * udot * z_dot)
dzddotdrpy = u_factor * (dsnapdrpy - np.outer(dv1drpy, z) -
v1 * dzdrpy - 2 * udot * dzdotdrpy)
dalphadrpy = skew_z.dot(
dzddotdrpy - skew_angvel_w.dot(dzdotdrpy)) - mathu.skew_matrix(
z_ddot - skew_angvel_w.dot(z_dot)).dot(dzdrpy)
djerkdang = u * dzdotdang
dsnapdang = -k4.dot(djerkdang)
dv1dang = dsnapdang.T.dot(z) + 2 * u * z_dot.T.dot(dzdotdang)
dzddotdang = u_factor * (dsnapdang - np.outer(dv1dang, z) -
2 * udot * dzdotdang)
# TODO XXX This is missing the omega cross zdot term.
dalphawdang = skew_z.dot(dzddotdang)
dalphabdang = rot.inv().apply(dalphawdang)
dsnapdpos = -k1
dsnapdvel = -k2
dv1dpos = dsnapdpos.dot(z)
dv1dvel = dsnapdvel.dot(z)
dzddotdpos = u_factor * (dsnapdpos - np.outer(dv1dpos, z))
dzddotdvel = u_factor * (dsnapdvel - np.outer(dv1dvel, z))
dalphadpos = skew_z.dot(dzddotdpos)
dalphadvel = skew_z.dot(dzddotdvel)
K_x = np.zeros((self.n_control_sys, self.n_state))
K_x[AA, RPY] = dalphadrpy
K_x[AA, OM] = dalphabdang
K_x[AA, X] = dalphadpos
K_x[AA, V] = dalphadvel
K_u = np.zeros((self.n_control_sys, self.n_control))
if not nou:
K_u[U, U] = 1
K_u[AA, AA] = np.eye(3)
return K_x, K_u
def get_ABCD(self, state, control, dt):
X = slice(0, 3)
V = slice(3, 6)
RPY = slice(6, 9)
OM = slice(9, 12)
U = slice(0, 1)
AA = slice(1, 4)
pos = state[X]
vel = state[V]
ind = self.iter - 1 if self.iter >= len(self.zs) - 1 else self.iter
u, udot = self.zs[ind]
rpy = state[RPY]
angvel = state[OM]
rot = Rotation.from_euler('ZYX', rpy[::-1])
z = rot.apply(np.array((0, 0, 1)))
z_dot = mathu.skew_matrix(rot.apply(angvel)).dot(z)
roll, pitch, yaw = rpy
dzdrpy = np.array(((np.sin(yaw) * np.cos(roll) -
np.sin(roll) * np.cos(yaw) * np.sin(pitch),
np.cos(roll) * np.cos(yaw) * np.cos(pitch),
np.sin(roll) * np.cos(yaw) -
np.cos(roll) * np.sin(yaw) * np.sin(pitch)),
(-np.sin(roll) * np.sin(yaw) * np.sin(pitch) -
np.cos(yaw) * np.cos(roll),
np.cos(roll) * np.sin(yaw) * np.cos(pitch),
np.cos(roll) * np.cos(yaw) * np.sin(pitch) +
np.sin(yaw) * np.sin(roll)),
(-np.cos(pitch) * np.sin(roll),
-np.sin(pitch) * np.cos(roll), 0)))
A = np.zeros((self.n_state, self.n_state))
B = np.zeros((self.n_state, self.n_control))
C = np.zeros((self.n_out, self.n_state))
D = np.zeros((self.n_out, self.n_control))
A[X, X] = np.eye(3)
A[V, V] = np.eye(3)
A[X, V] = dt * np.eye(3)
A[RPY, RPY] = np.eye(3)
A[OM, OM] = np.eye(3)
#A[V, Z] = u * dt * np.eye(3)
A[V, RPY] = u * dt * dzdrpy
# TODO Need to fix this because om is in the body frame!
#A[Z, OM] = -dt * mathu.skew_matrix(z)
# Below taken from Tal and Karaman 2018 - Accurate Tracking of ...
A[RPY, OM] = dt * np.array(
((1, np.sin(roll) * np.tan(pitch), np.cos(roll) * np.tan(pitch)),
(0, np.cos(roll), -np.sin(roll)),
(0, np.sin(roll) / np.cos(pitch), np.cos(roll) / np.cos(pitch))))
B[V, U] = dt * np.array([z]).T
B[OM, AA] = dt * np.eye(3)
# Trying 2D mats.......
#ct = np.cos(rpy[0])
#st = np.sin(rpy[0])
#A[V, 6:7] = u * dt * np.array(((0, -ct, -st),)).T
#A[6, 9] = dt
#########################33
C[X, X] = np.eye(3)
if self.use_feedback:
oldu = self.int_u
oldudot = self.int_udot
self.int_u = u
self.int_udot = udot
K_x, K_u = self.get_feedback_response(state, control, dt)
self.int_u = oldu
self.int_udot = oldudot
A = A + B.dot(K_x)
B = B.dot(K_u)
return A, B, C, D
def get_feedback_response(self, state, control, dt):
X = slice(0, 3)
V = slice(3, 6)
RPY = slice(6, 9)
OM = slice(9, 12)
U = slice(0, 1)
AA = slice(1, 4)
pos = state[X]
vel = state[V]
rpy = state[RPY]
angvel = state[OM]
rot = Rotation.from_euler('ZYX', rpy[::-1])
z = rot.apply(np.array((0, 0, 1)))
roll, pitch, yaw = rpy
dzdrpy = np.array((
(np.sin(yaw) * np.cos(roll) - np.sin(roll) * np.cos(yaw) * np.sin(pitch), np.cos(roll) * np.cos(yaw) * np.cos(pitch), np.sin(roll) * np.cos(yaw) - np.cos(roll) * np.sin(yaw) * np.sin(pitch)),
(-np.sin(roll) * np.sin(yaw) * np.sin(pitch) - np.cos(yaw) * np.cos(roll), np.cos(roll) * np.sin(yaw) * np.cos(pitch), np.cos(roll) * np.cos(yaw) * np.sin(pitch) + np.sin(yaw) * np.sin(roll)),
(-np.cos(pitch) * np.sin(roll), -np.sin(pitch) * np.cos(roll), 0)
))
skew_z = mathu.skew_matrix(z)
pos_vel = state[:6]
accel_des = -self.K_pos.dot(pos_vel - np.hstack((self.pos_des, self.vel_des))) + self.acc_des + g3
anorm = np.linalg.norm(accel_des)
K_x = np.zeros((self.n_control, self.n_state))
dadx = -self.K_pos[:, X]
dadv = -self.K_pos[:, V]
duda = self.U.duda(accel_des, z)
dudz = self.U.dudz(accel_des, z)
dudx = duda.dot(dadx)
dudv = duda.dot(dadv)
K_x[U, X] = dudx
K_x[U, V] = dudv
K_x[U, RPY] = dudz.dot(dzdrpy)
z_des = accel_des / anorm
dzda = (1.0 / anorm) * (np.eye(3) - np.outer(z_des, z_des))
dzdx = dzda.dot(dadx)
dzdv = dzda.dot(dadv)
# We assume yaw is zero
#assert abs(rpy[2]) < 1e-6
deulerdz = np.zeros((3, 3))
a1 = 1 / np.sqrt(1 - z_des[1] ** 2)
deulerdz[0, 1] = -a1
deulerdz[1, 0] = 1 / np.sqrt(1 - (z_des[0] * a1) ** 2)
deulerdz[1, 1] = z_des[0] * z_des[1] / (np.sqrt(1 - (z_des[0] * a1) ** 2) * ((1 - z_des[1]**2) ** (3.0/2)))
#des_phi = np.arcsin(-z_des[1])
#des_th = np.arctan2(z_des[0], z_des[2])
#print(dzdrpy)
#print(deulerdz)
#print(-1 / np.cos(des_phi))
#print(1 / (1 + np.tan(des_th)**2))
#input()
#deulerdz[0, 1] = -1 / np.cos(des_phi)
#deulerdz[1, 0] = 1 / (1 + np.tan(des_th) ** 2)
#deulerdz[1, 1] = deulerdz[1, 0] * (-z_des[0] / (z_des[2] ** 2))
K_x[AA, X] = self.K_att[:, :3].dot(deulerdz.dot(dzdx))
K_x[AA, V] = self.K_att[:, :3].dot(deulerdz.dot(dzdv))
K_x[AA, RPY] = -self.K_att[:, :3]
K_x[AA, OM] = -self.K_att[:, 3:6]
K_u = np.zeros((self.n_control, self.n_control))
K_u[U, U] = 1
K_u[AA, AA] = np.eye(3)
return K_x, K_u