def test_rigid_transform_local_to_world_when_rotation_is_180_deg_and_position_is_non_zero(
):
R = angle_to_rotation_matrix(np.deg2rad(180.))
t = np.array([3.5, -4.])
p_local = np.array([1., 2.])
assert assert_array_equal(rigid_transform_local_to_world(R, t, p_local),
np.array([2.5, -6.])) is None
def test_rigid_transform_local_to_world_when_rotation_is_zero_and_position_is_non_zero(
):
R = angle_to_rotation_matrix(0)
t = np.array([3.5, -4.])
p_local = np.array([1., 2.])
assert assert_array_equal(rigid_transform_local_to_world(R, t, p_local),
np.array([4.5, -2.])) is None
def inv_observe_range_bearing(X_r, p_local_polar):
"""
Simulate inverse range-bearing sensor reading of a landmark in world coordinates
:param X_r: robot pose (true)
:param p_local_polar: (range, bearing) polar coordinate position of a landmark in local ref frame
:return: estimated position of landmark in rectangular coordinates (world ref frame)
"""
t = X_r[:2]
alpha = X_r[2]
R = transforms.angle_to_rotation_matrix(alpha)
return transforms.rigid_transform_local_to_world(
R, t, transforms.polar_to_rect(p_local_polar))
def observe_range_bearing(X_r, p_world_rect):
"""
Simulate a range-bearing sensor reading of a landmark in world coordinates
:param X_r: robot pose (true)
:param p_world_rect: (x, y) rectangular coordinate position of a landmark in world ref frame
:return: range-bearing (polar coordinate) measurement of a landmark (local ref frame)
"""
t = X_r[:2]
alpha = X_r[2]
R = transforms.angle_to_rotation_matrix(alpha)
return transforms.rect_to_polar(
transforms.rigid_transform_world_to_local(R, t, p_world_rect))
def move(r, u, n):
"""
Move robot using control input and perturbation
:param r: current robot pose in world ref frame [x; y; alpha]
:param u: control signal in local ref frame [d_x; d_alpha]
:param n: perturbation to control input in local ref frame [d_x; d_y; d_alpha]
:return: new robot pose in world ref frame [x; y; alpha]
"""
# current pose in world ref frame
x, y, alpha = r
# control signal in local ref frame
d_x_local_u, d_alpha_local_u = u
d_x_local_n, d_alpha_local_n = n
d_x_local = d_x_local_u + d_x_local_n
d_p_local = np.array([d_x_local, 0])
d_alpha_local = d_alpha_local_u + d_alpha_local_n
# # convert control input position + noise to world reference frame
# d_x_local = u[0] + n[0]
# d_y_local = u[1] + n[1]
# d_alpha = u[2] + n[2]
alpha_new = alpha + d_alpha_local
R_new = angle_to_rotation_matrix(alpha_new)
# move to new position
d_x_world, d_y_world = np.dot(R_new, d_p_local)
# d_x_world, d_y_world = rigid_transform_local_to_world(angle_to_rotation_matrix(alpha + d_alpha_local),
# [d_x_local, 0],
# [0, 0])
r_new = np.array([x + d_x_world, y + d_y_world, alpha_new])
# r_new = [d_x_world, d_y_world, alpha + d_alpha]
return r_new
def test_angle_to_rotation_matrix_if_angle_is_180_deg():
assert assert_array_almost_equal(
angle_to_rotation_matrix(np.deg2rad(180.)),
np.array([[-1, 0], [0, -1.]])) is None
def test_angle_to_rotation_matrix_if_angle_is_zero():
assert assert_array_equal(angle_to_rotation_matrix(0),
np.array([[1, 0], [0, 1.]])) is None