def test_fk_a_in_b(self): km = ArticulationModel() p1 = translation3(1, -3, 5) axis1 = vector3(1, -2, 5) axis1 /= norm(axis1) p2 = rotation3_axis_angle(axis1, 1.67) p3 = translation3(-7, 1, 0) axis2 = vector3(5, 0, 3) axis2 /= norm(axis2) p4 = rotation3_axis_angle(axis2, -0.6) p5 = translation3(0, 0, 4) p6 = frame3_rpy(1.2, -0.3, 0.67, [1, 2, 3]) f_a = Frame('world', p1, p1) f_b = Frame('a', f_a.pose * p2, p2) f_c = Frame('b', f_b.pose * p3, p3) f_d = Frame('c', f_c.pose * p4, p4) f_e = Frame('b', f_b.pose * p5, p5) f_f = Frame('world', p6, p6) f_g = Frame('lol', se.eye(4)) km.set_data('a', f_a) km.set_data('b', f_b) km.set_data('c', f_c) km.set_data('d', f_d) km.set_data('e', f_e) km.set_data('f', f_f) km.set_data('g', f_g) d_in_b = fk_a_in_b(km, f_d, f_b) b_in_d = fk_a_in_b(km, f_b, f_d) d_in_e = fk_a_in_b(km, f_d, f_e) d_in_f = fk_a_in_b(km, f_d, f_f) self.assertEquals(d_in_b, f_c.to_parent * f_d.to_parent) self.assertEquals(b_in_d, inverse_frame(f_c.to_parent * f_d.to_parent)) self.assertEquals( d_in_e, inverse_frame(f_e.to_parent) * f_c.to_parent * f_d.to_parent) self.assertEquals(d_in_f, inverse_frame(f_f.pose) * f_d.pose) with self.assertRaises(Exception): fk_a_in_b(km, f_d, f_g)
def test_collect_chain(self): km = ArticulationModel() f_a = Frame('world', None) f_b = Frame('a', None) f_c = Frame('b', None) f_d = Frame('c', None) f_e = Frame('b', None) km.set_data('a', f_a) km.set_data('b', f_b) km.set_data('c', f_c) km.set_data('d', f_d) km.set_data('e', f_e) chain = collect_chain(km, f_d) self.assertEquals(chain, [f_a, f_b, f_c, f_d])