def planar_ctrl_law(e_p_2d, e_v_2d): e_p = np.array([e_p_2d[0], e_p_2d[1], 0.]) e_v = np.array([e_v_2d[0], e_v_2d[1], 0.]) norm_e_p = utils.norm2(e_p) norm_e_v = utils.norm2(e_v) if norm_e_p > utils.EPS: # calculate axis to rotate about axis_p = utils.cross(np.array([0., 0., 1.]), e_p / norm_e_p) # calculate angle target theta_p = utils.smooth_symmetric_clip(K_P * norm_e_p, a_clip=THETA_MAX) q_p = quaternion.from_axis_angle(axis_p, theta_p) if (norm_e_p < utils.EPS) and norm_e_v > utils.EPS: # damping terms axis_v = utils.cross(np.array([0., 0., 1.]), e_v / norm_e_v) theta_v = utils.smooth_symmetric_clip(K_V * norm_e_v, a_clip=THETA_MAX) q_v = quaternion.from_axis_angle(axis_v, theta_v) else: q_v = np.array([1., 0., 0., 0.]) # get resultant quaternion q_xy = quaternion.product(q_p, q_v) axis_tot, theta_tot = quaternion.to_axis_angle(q_xy) # saturate angle to limit theta_clip = utils.smooth_symmetric_clip(theta_tot, a_clip=THETA_MAX) q_xy = quaternion.from_axis_angle(axis_tot, theta_clip) else: q_xy = np.array([1., 0., 0., 0.]) return q_xy
def test_kalman_f_matrix(self): """ Sensor: Check Orientation Kalman F matrix derivative matches the numeric version """ for i in range(10): a_theta = quaternion.euler_fix_quadrant(2 * np.pi * np.random.rand()) e_axis = np.random.rand(3) n_body = np.random.rand(3) q = quaternion.from_axis_angle(e_axis, a_theta) u = 5 * np.random.rand(4) dt = 0.001 s = np.concatenate(( np.zeros(3), q, np.zeros(3), n_body, np.ones(4), )) f_alg = sensor.kalman_orientation_f(s, dt) f_num = sensor.kalman_orientation_f_finite_difference(s, u, dt) self.assertTrue( np.isclose(f_alg, f_num, atol=5e-3).all(), f"Error in f derivative\n{f_num}\n{f_alg}")
def test_rot_mat_der(self): """ Quaternions: Check the rotation matrix derivative matches the numeric version """ for i in range(10): a_0 = quaternion.euler_fix_quadrant(2 * np.pi * np.random.rand()) e_0 = np.random.rand(3) # make a non unit quaternion q = quaternion.from_axis_angle(e_0, a_0) eps = 0.0001 for i_q in range(4): dq = np.zeros(4) dq[i_q] = eps r_pos = quaternion.to_rot_mat(quaternion.conjugate(q + dq)) r_neg = quaternion.to_rot_mat(quaternion.conjugate(q - dq)) r_num = (r_pos - r_neg) / 2 / eps # get algebraic solution r_alg = quaternion.rot_mat_der(q, i_q, b_inverse=True) self.assertTrue( np.isclose(r_num, r_alg, rtol=1e-3).all(), f"Error in rot mat derivative\n{r_num}\n{r_alg}")
def test_inverses(self): """ Quaternions: Check inverse rotation matrix conjugate """ for i in range(10): a_0 = quaternion.euler_fix_quadrant(2 * np.pi * np.random.rand()) e_0 = np.random.rand(3) # make a non unit quaternion q = quaternion.from_axis_angle(e_0, a_0) + np.array( [0., 0., 0.5, 0.]) q_conj = quaternion.conjugate(q) r = quaternion.to_rot_mat(q) r_inv = np.linalg.inv(r) r_conj = quaternion.to_rot_mat(q_conj) self.assertTrue( np.isclose(r_conj, r_inv).all(), f"Conjugate and inverse mismatch\n{r_inv}\n{r_conj}")
def test_integrate_der(self): """ Quaternions: Check the integration derivative matches the numeric version """ for i in range(10): a_0 = quaternion.euler_fix_quadrant(2 * np.pi * np.random.rand()) e_0 = np.random.rand(3) n_body = np.random.rand(3) q = quaternion.from_axis_angle(e_0, a_0) dt = 0.001 # compute analytical derivative q_der_q_alg, q_der_n_alg = quaternion.integrate_der(q, n_body, dt) eps = 0.00001 q_der_q_num = np.zeros((4, 4)) for i_q in range(4): dq = np.zeros(4) dq[i_q] = eps q_p = quaternion.integrate(q + dq, n_body, dt) q_n = quaternion.integrate(q - dq, n_body, dt) q_der_q_num[:, i_q] = (q_p - q_n) / 2 / eps q_der_n_num = np.zeros((4, 3)) for i_n in range(3): dn = np.zeros(3) dn[i_n] = eps q_p = quaternion.integrate(q, n_body + dn, dt) q_n = quaternion.integrate(q, n_body - dn, dt) q_der_n_num[:, i_n] = (q_p - q_n) / 2 / eps self.assertTrue( np.isclose(q_der_q_num, q_der_q_alg, atol=5e-3).all(), f"Error in integral derivative wrt q" f"\n{q_der_q_num}\n{q_der_q_alg}") self.assertTrue( np.isclose(q_der_n_num, q_der_n_alg, atol=5e-3).all(), f"Error in integral derivative wrt n" f"\n{q_der_n_num}\n{q_der_n_alg}")
def test_kalman_h_matrix(self): """ Sensor: Check Orientation Kalman H matrix derivative matches the numeric version """ a_theta = quaternion.euler_fix_quadrant(2 * np.pi * np.random.rand()) e_axis = np.random.rand(3) n_body = np.random.rand(3) q = quaternion.from_axis_angle(e_axis, a_theta) s = np.concatenate(( np.zeros(3), q, np.zeros(3), n_body, np.zeros(4), )) h_alg = sensor.kalman_orientation_h(s) h_num = sensor.kalman_orientation_h_finite_difference(s) self.assertTrue( np.isclose(h_alg, h_num, rtol=1e-3).all(), f"Error in h derivative\n{h_num}\n{h_alg}")