def calc_att_cmd(self, Tc_vec, Tc, yaw_des):
s_yaw = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
R = np.empty([3, 3])
R[:, 2] = Tc_vec / Tc
R[:, 1] = mt.skew(R[:, 2]) @ s_yaw
R[:, 1] /= mt.norm2(R[:, 1])
R[:, 0] = mt.skew(R[:, 1]) @ R[:, 2]
return quat.from_R(R)
def calc_att_cmd(self, thrust_vec, yaw_des):
thrust_mag = mt.norm2(thrust_vec)
s_yaw = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
R = np.empty([3,3])
R[:,2] = thrust_vec / thrust_mag
R[:,1] = mt.skew(R[:,2]) @ s_yaw
R[:,1] /= mt.norm2(R[:,1])
R[:,0] = mt.skew(R[:,1]) @ R[:,2]
return quat.from_R(R)
def rotp(self, vec3):
"""Passive rotation of 3d vector (same as q.R @ vec3)
e.g. vec_b = q_i2b.rotp(vec_i)
"""
qbar_skew = mt.skew(self.qbar)
temp = 2 * qbar_skew @ vec3
return vec3 - self.q0 * temp + qbar_skew @ temp
def rota(self, vec3):
"""Active rotation of 3d vector (same as q.R.T @ vec3)
e.g. vec_i = q_i2b.rota(vec_b)
"""
qbar_skew = mt.skew(self.qbar)
temp = 2 * qbar_skew @ vec3
return vec3 + self.q0 * temp + qbar_skew @ temp
def naive_triangulate_points(x_a, x_b, translation, rotation):
"""
Assumes points satisfy the epipolar constraint. Uses the eigenvector
corresponding the to smallest eigenvalue of A.T @ A as depth solution
"""
n = x_a.shape[1]
R = rotation.R
# R = rotation
# create A matrix
x_a_rot = R @ x_a
A = np.zeros((3 * n, n + 1))
for i, x in enumerate(x_b.T):
x_b_cross = mt.skew(x)
A[3 * i:3 * (i + 1), i] = x_b_cross @ x_a_rot[:, i]
A[3 * i:3 * (i + 1), -1] = x_b_cross @ translation
# get Lambda by taking the eigenvector corresponding the smallest
# singular value of A
u, s, vh = np.linalg.svd(A)
# normalize so that gamma = 1
Lambda = vh[-1, :] / vh[-1, -1]
# multiply to get 3d points in previous frame
p_a = x_a * Lambda[:-1]
p_a = p_a[:, np.bitwise_and(Lambda[:-1] < 20.0, Lambda[:-1] > 1.0)]
# get points in new body frame
p_b = R @ p_a + translation.reshape(3, 1)
return p_b
def calc_ang_vel_des(self, att_cmd, pos, vel, yaw, traj):
pos_des = traj.pos
vel_des = traj.vel
accel_des = traj.accel
jerk_des = traj.jerk
yaw_des = traj.yaw
yaw_rate_des = traj.yaw_rate
pos_err = pos - pos_des
vel_err = vel - vel_des
R = att_cmd.R
r1 = R[:, 0]
r2 = R[:, 1]
r3 = R[:, 2]
s_yaw = np.array([np.cos(yaw_des), np.sin(yaw_des), 0])
s_yaw_dot = np.array([
-yaw_rate_des * np.sin(yaw_des), yaw_rate_des * np.cos(yaw_des), 0
])
a = accel_des - self.g * np.array(
[0., 0., 1.]) - self.kp * pos_err - self.kd * vel_err
accel_err = -self.kp * pos_err - self.kd * vel_err
a_dot = jerk_des - self.kp * vel_err - self.kd * accel_err
r3_dot = -a_dot / np.linalg.norm(a) + a * (
a_dot @ a) / np.linalg.norm(a)**3
num = mt.skew(r3) @ s_yaw
num_dot = mt.skew(r3_dot) @ s_yaw + mt.skew(r3) @ s_yaw_dot
r2_dot = num_dot / np.linalg.norm(num) - num * (
num_dot @ num) / np.linalg.norm(num)**3
r1_dot = mt.skew(r2_dot) @ r3 + mt.skew(r2) @ r3_dot
R_dot = np.zeros((3, 3))
R_dot[:, 0] = r1_dot
R_dot[:, 1] = r2_dot
R_dot[:, 2] = r3_dot
ang_vel_des = mt.vee(R.T @ R_dot)
return ang_vel_des
def from_two_unit_vectors(v1, v2):
"""Create Quaternion from 2 unit vectors"""
assert v1.size == 3
assert v2.size == 3
u = v1.copy()
v = v2.copy()
d = u @ v
if d < 1.0:
invs = (2 * (1 + d))**-0.5
qbar = mt.skew(u) @ v * invs
q0 = 0.5 / invs
q_arr = np.array([q0, *qbar])
return Quaternion(q_arr)
def analytical_update(x_a, x_b, translation, rotation):
"""
Computes the analytic solution to the point match correction problem
(see Section 12.5 of Multiple View Geometry book)
"""
n = x_a.shape[1]
x_a = x_a[:2, :].reshape(1, n, 2)
x_b = x_b[:2, :].reshape(1, n, 2)
x_a_cor, x_b_cor = cv.correctMatches(
mt.skew(translation) @ rotation.R, x_a, x_b)
x_a_cor = x_a_cor[0, :, :].T
x_b_cor = x_b_cor[0, :, :].T
x_a_cor = np.block([[x_a_cor], [np.ones(n)]])
x_b_cor = np.block([[x_b_cor], [np.ones(n)]])
return x_a_cor, x_b_cor
def R(self):
"""Get passive rotation matrix
e.g. R_i2b = q_i2b.R
"""
qbar_skew = mt.skew(self.qbar)
return np.eye(3) + 2 * (-self.q0 * qbar_skew + qbar_skew @ qbar_skew)