diff --git a/python/test/test.py b/python/test/test.py index 511c863fd5ad88e95e342505bf38e09e18310628..a368aa855fd87ccd7b6b79996e0bd334c2bffe21 100644 --- a/python/test/test.py +++ b/python/test/test.py @@ -10,9 +10,9 @@ from hpp_spline import bezier, bezier6, curve_constraints, exact_cubic, from_bez class TestSpline(unittest.TestCase): def test_spline(self): - waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() - waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose() - time_waypoints = matrix([0., 1.]).transpose() + waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T + waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).T + time_waypoints = matrix([0., 1.]).T # testing bezier curve a = bezier6(waypoints6) @@ -39,7 +39,7 @@ class TestSpline(unittest.TestCase): self.assertTrue((a(t) == prim.derivate(t, 2)).all()) self.assertTrue((prim(0) == matrix([0., 0., 0.])).all()) - waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose() + waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).T a0 = bezier(waypoints) a1 = bezier(waypoints, 3.) prim0 = a0.compute_primitive(1) @@ -47,23 +47,23 @@ class TestSpline(unittest.TestCase): for i in range(10): t = float(i) / 10. - self.assertAlmostEqual(norm(a0(t), a1(3 * t))) - self.assertAlmostEqual(norm(a0.derivate(t, 1), a1.derivate(3 * t, 1) * 3.)) - self.assertAlmostEqual(norm(a0.derivate(t, 2), a1.derivate(3 * t, 2) * 9.)) - self.assertAlmostEqual(norm(prim0(t), prim1(t * 3) / 3.)) - self.assertTrue((prim(0) == matrix([0., 0., 0.])).all()) + self.assertAlmostEqual(norm(a0(t) - a1(3 * t)), 0) + self.assertAlmostEqual(norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.), 0) + self.assertAlmostEqual(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.), 0) + self.assertAlmostEqual(norm(prim0(t) - prim1(t * 3) / 3.), 0) + self.assertTrue((prim(0) == matrix([0., 0., 0.]).T).all()) # testing bezier with constraints c = curve_constraints() - c.init_vel = matrix([0., 1., 1.]).transpose() - c.end_vel = matrix([0., 1., 1.]).transpose() - c.init_acc = matrix([0., 1., -1.]).transpose() - c.end_acc = matrix([0., 100., 1.]).transpose() + c.init_vel = matrix([0., 1., 1.]).T + c.end_vel = matrix([0., 1., 1.]).T + c.init_acc = matrix([0., 1., -1.]).T + c.end_acc = matrix([0., 100., 1.]).T - waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose() + waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T a = bezier(waypoints, c) - self.assertAlmostEqual(norm(a.derivate(0, 1), c.init_vel)) - self.assertAlmostEqual(norm(a.derivate(1, 2), c.end_acc)) + self.assertAlmostEqual(norm(a.derivate(0, 1) - c.init_vel), 0) + self.assertAlmostEqual(norm(a.derivate(1, 2) - c.end_acc), 0) # testing polynom function a = polynom(waypoints) @@ -89,10 +89,10 @@ class TestSpline(unittest.TestCase): c.init_acc c.end_acc - c.init_vel = matrix([0., 1., 1.]).transpose() - c.end_vel = matrix([0., 1., 1.]).transpose() - c.init_acc = matrix([0., 1., 1.]).transpose() - c.end_acc = matrix([0., 1., 1.]).transpose() + c.init_vel = matrix([0., 1., 1.]).T + c.end_vel = matrix([0., 1., 1.]).T + c.init_acc = matrix([0., 1., 1.]).T + c.end_acc = matrix([0., 1., 1.]).T a = spline_deriv_constraint(waypoints, time_waypoints) a = spline_deriv_constraint(waypoints, time_waypoints, c) @@ -101,7 +101,7 @@ class TestSpline(unittest.TestCase): a = bezier(waypoints) a_pol = from_bezier(a) - self.assertAlmostEqual(norm(a(0.3), a_pol(0.3))) + self.assertAlmostEqual(norm(a(0.3) - a_pol(0.3)), 0) if __name__ == '__main__':