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}")