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