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)