def print_minimum(self):
self.info('print_minimum:')
connections = self.compute_connections(only_close_enough=False)
self.show_connections(connections, 'all of them')
nodes1 = list(self.tree1.G.nodes())
nodes2 = list(self.tree2.G.nodes())
# TODO: do not compute all of this
Db = construct_matrix_iterators((nodes1, nodes2),
lambda n1, n2: self.distance(n1, n2))
Dp = construct_matrix_iterators((nodes1, nodes2),
lambda n1, n2: self.distance_prediction(n1, n2))
Db_min, m = md_argmin(Db)
n1 = nodes1[m[0]]
n2 = nodes2[m[1]]
assert_allclose(Db_min, self.distance(n1, n2))
self.info('Minimum distance_branch: %g between %s and %s' %
(Db_min, self.tree1.node_friendly(n1),
self.tree2.node_friendly(n2)))
Dp_min, m = md_argmin(Dp)
n1 = nodes1[m[0]]
n2 = nodes2[m[1]]
assert_allclose(Dp_min, self.distance_prediction(n1, n2))
self.info('Minimum distance_prediction: %g between %s and %s' %
(Dp_min, self.tree1.node_friendly(n1),
self.tree2.node_friendly(n2)))
def test_coords1():
vl = np.array([
0,
1,
0,
])
va = np.array([np.deg2rad(20), 0, 0])
vel = {
'F': vl,
'FL': vl + va,
'FR': vl - va,
'B': (-vl),
'BL': (-vl + va),
'BR': (-vl - va),
}
def make_motion(v):
A = se2.algebra_from_vector(v)
Q = SE2.group_from_algebra(A)
return Q
motions = dictmap(make_motion, vel)
print motions
for k, v in motions.items():
print(' - %s: %s -> %s' % (k, vel[k], SE2.friendly(v)))
names = sorted(vel.keys())
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2), SE2.multiply(q2, q1))
def same(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, q2)
def anti(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, SE2.inverse(q2))
cD = construct_matrix_iterators((names, names), commuting)
aD = construct_matrix_iterators((names, names), anti)
D = construct_matrix_iterators((names, names), same)
r = Report('test_coords1')
r.table('D', data=D, cols=names, rows=names, fmt='%f')
r.table('aD', data=aD, cols=names, rows=names, fmt='%f')
r.table('cD', data=cD, cols=names, rows=names, fmt='%f')
r.to_html('out/test_coords1/test_coords1.html')
def test_coords1():
vl = np.array([0, 1, 0, ])
va = np.array([np.deg2rad(20), 0, 0])
vel = {
'F': vl,
'FL': vl + va,
'FR': vl - va,
'B': (-vl),
'BL': (-vl + va),
'BR': (-vl - va),
}
def make_motion(v):
A = se2.algebra_from_vector(v)
Q = SE2.group_from_algebra(A)
return Q
motions = dictmap(make_motion, vel)
print motions
for k, v in motions.items():
print(' - %s: %s -> %s' % (k, vel[k], SE2.friendly(v)))
names = sorted(vel.keys())
def commuting(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(SE2.multiply(q1, q2),
SE2.multiply(q2, q1))
def same(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, q2)
def anti(a, b):
q1 = motions[a]
q2 = motions[b]
return SE2.distance(q1, SE2.inverse(q2))
cD = construct_matrix_iterators((names, names), commuting)
aD = construct_matrix_iterators((names, names), anti)
D = construct_matrix_iterators((names, names), same)
r = Report('test_coords1')
r.table('D', data=D, cols=names, rows=names, fmt='%f')
r.table('aD', data=aD, cols=names, rows=names, fmt='%f')
r.table('cD', data=cD, cols=names, rows=names, fmt='%f')
r.to_html('out/test_coords1/test_coords1.html')
def get_distance_matrix(self, plans, action_distance):
D_ij = lambda p1, p2: action_distance(
self.plan2action(p1),
self.plan2action(p2))
D = construct_matrix_iterators((plans, plans), D_ij)
return plans, D
