def graph_fix_node_test():
G = random_connected_pose_network(10)
target = random_SE2()
assert_exact(G)
graph_fix_node(G, 0, target)
SE2.assert_close(G.node[0]['pose'], target)
assert_exact(G)
def graph_fix_node_test():
G = random_connected_pose_network(10)
target = random_SE2()
assert_exact(G)
graph_fix_node(G, 0, target)
SE2.assert_close(G.node[0]['pose'], target)
assert_exact(G)
def graph_fix_node(G, x, target_pose):
assert G.has_node(x)
q = G.node[x]['pose']
diff = np.dot(target_pose, np.linalg.inv(q))
for n in G.nodes():
q = G.node[n]['pose']
q2 = np.dot(diff, q)
G.node[n]['pose'] = q2
SE2.assert_close(G.node[x]['pose'], target_pose)
def graph_fix_node(G, x, target_pose):
assert G.has_node(x)
q = G.node[x]['pose']
diff = np.dot(target_pose, np.linalg.inv(q))
for n in G.nodes():
q = G.node[n]['pose']
q2 = np.dot(diff, q)
G.node[n]['pose'] = q2
SE2.assert_close(G.node[x]['pose'], target_pose)
def comparison_test():
''' Compares between SE2_from_se2_slow and SE2_from_se2. '''
for pose in SE2.interesting_points():
se2 = se2_from_SE2(pose)
SE2a = SE2_from_se2_slow(se2)
SE2b = SE2_from_se2(se2)
#printm('pose', pose, 'se2', se2)
#printm('SE2a', SE2a, 'SE2b', SE2b)
SE2.assert_close(SE2a, pose)
#print('SE2a = pose Their distance is %f' % d)
SE2.assert_close(SE2b, pose)
#print('SE2b = pose Their distance is %f' % d)
assert_allclose(SE2a, SE2b, atol=1e-8, err_msg='SE2a != SE2b')
assert_allclose(SE2a, pose, atol=1e-8, err_msg='SE2a != pose')
assert_allclose(SE2b, pose, atol=1e-8, err_msg='SE2b != pose')
def comparison_test():
''' Compares between SE2_from_se2_slow and SE2_from_se2. '''
for pose in SE2.interesting_points():
se2 = se2_from_SE2(pose)
SE2a = SE2_from_se2_slow(se2)
SE2b = SE2_from_se2(se2)
#printm('pose', pose, 'se2', se2)
#printm('SE2a', SE2a, 'SE2b', SE2b)
SE2.assert_close(SE2a, pose)
#print('SE2a = pose Their distance is %f' % d)
SE2.assert_close(SE2b, pose)
#print('SE2b = pose Their distance is %f' % d)
assert_allclose(SE2a, SE2b, atol=1e-8, err_msg='SE2a != SE2b')
assert_allclose(SE2a, pose, atol=1e-8, err_msg='SE2a != pose')
assert_allclose(SE2b, pose, atol=1e-8, err_msg='SE2b != pose')
def test_poses_SE2(self):
dynamics = SE2Dynamics(max_linear_velocity=[1, 1],
max_angular_velocity=1)
dt = 0.1
M = 1 # max
Z = 0 # zero
m = -1 # min
tests = [
# format ( (start_xy, start_theta), commands,
# (final_xy, final_theta))
(([0, 0], 0), [Z, Z, Z], ([0, 0], 0)),
(([1, 2], 3), [Z, Z, Z], ([1, 2], 3)),
(([0, 0], 0), [M, Z, Z], ([dt, 0], 0)),
(([0, 0], 0), [m, Z, Z], ([-dt, 0], 0)),
(([0, 0], 0), [Z, M, Z], ([0, dt], 0)),
(([0, 0], 0), [Z, m, Z], ([0, -dt], 0)),
(([0, 0], 0), [Z, Z, M], ([0, 0], dt)),
(([0, 0], 0), [Z, Z, m], ([0, 0], -dt)),
(([0, 0], np.radians(90)), [M, Z, Z], ([0, dt], np.radians(90))),
(([0, 0], np.radians(90)), [Z, M, Z], ([-dt, 0], np.radians(90)))
# TODO: add some more tests with non-zero initial rotation
]
for initial, commands, final in tests:
start_pose = SE2_from_translation_angle(*initial)
final_pose = SE2_from_translation_angle(*final)
start_state = dynamics.pose2state(SE3_from_SE2(start_pose))
final_state = dynamics.pose2state(SE3_from_SE2(final_pose))
commands = np.array(commands)
print('%s -> [%s] -> %s' %
(SE2.friendly(start_pose), commands,
SE2.friendly(final_pose)))
actual, dummy = dynamics.integrate(start_state, +commands, dt)
SE2.assert_close(actual, final_pose)
start2, dummy = dynamics.integrate(final_state, -commands, dt)
SE2.assert_close(start_pose, start2)
def test_poses_SE2(self):
from vehicles_dynamics import SE2Dynamics
dynamics = SE2Dynamics(max_linear_velocity=[1, 1],
max_angular_velocity=1)
dt = 0.1
M = 1 # max
Z = 0 # zero
m = -1 # min
tests = [
# format ( (start_xy, start_theta), commands,
# (final_xy, final_theta))
(([0, 0], 0), [Z, Z, Z], ([0, 0], 0)),
(([1, 2], 3), [Z, Z, Z], ([1, 2], 3)),
(([0, 0], 0), [M, Z, Z], ([dt, 0], 0)),
(([0, 0], 0), [m, Z, Z], ([-dt, 0], 0)),
(([0, 0], 0), [Z, M, Z], ([0, dt], 0)),
(([0, 0], 0), [Z, m, Z], ([0, -dt], 0)),
(([0, 0], 0), [Z, Z, M], ([0, 0], dt)),
(([0, 0], 0), [Z, Z, m], ([0, 0], -dt)),
(([0, 0], np.radians(90)), [M, Z, Z], ([0, dt], np.radians(90))),
(([0, 0], np.radians(90)), [Z, M, Z], ([-dt, 0], np.radians(90)))
# TODO: add some more tests with non-zero initial rotation
]
for initial, commands, final in tests:
start_pose = SE2_from_translation_angle(*initial)
final_pose = SE2_from_translation_angle(*final)
start_state = dynamics.pose2state(SE3_from_SE2(start_pose))
final_state = dynamics.pose2state(SE3_from_SE2(final_pose))
commands = np.array(commands)
print(
'%s -> [%s] -> %s' %
(SE2.friendly(start_pose), commands, SE2.friendly(final_pose)))
actual, dummy = dynamics.integrate(start_state, +commands, dt)
SE2.assert_close(actual, final_pose)
start2, dummy = dynamics.integrate(final_state, -commands, dt)
SE2.assert_close(start_pose, start2)
