[Tests][Python] add unit tests for piecewise_se3

python/test/test.py

@@ -10,7 +10,7 @@ from numpy.linalg import norm
 from curves import (CURVES_WITH_PINOCCHIO_SUPPORT, Quaternion, SE3Curve, SO3Linear, bezier, bezier3,
                     bezier_from_hermite, bezier_from_polynomial, cubic_hermite_spline, curve_constraints, exact_cubic,
                     hermite_from_bezier, hermite_from_polynomial, piecewise_bezier_curve,
-                    piecewise_cubic_hermite_curve, piecewise_polynomial_curve, polynomial, polynomial_from_bezier,
+                    piecewise_cubic_hermite_curve, piecewise_polynomial_curve,piecewise_SE3_curve, polynomial, polynomial_from_bezier,
                     polynomial_from_hermite)
 
 eigenpy.switchToNumpyArray()
@@ -526,6 +526,106 @@ class TestCurves(unittest.TestCase):
         self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
         return
 
+    def test_piecewise_se3_curve(self):
+      init_quat = Quaternion.Identity()
+      end_quat = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0)
+      init_rot = init_quat.matrix()
+      end_rot = end_quat.matrix()
+      waypoints = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
+      min = 0.2
+      max = 1.5
+      translation = bezier(waypoints, min, max)
+      # test with bezier
+      se3 = SE3Curve(translation, init_rot, end_rot)
+      pc = piecewise_SE3_curve(se3)
+      self.assertEqual(pc.num_curves(),1)
+      self.assertEqual(pc.min(), min)
+      self.assertEqual(pc.max(), max)
+      pmin = pc(min)
+      pmax = pc(max)
+      self.assertTrue(isclose(pmin[:3, :3], init_rot).all())
+      self.assertTrue(isclose(pmax[:3, :3], end_rot).all())
+      self.assertTrue(isclose(pmin[0:3, 3], translation(min)).all())
+      self.assertTrue(isclose(pmax[0:3, 3], translation(max)).all())
+      # add another curve :
+      end_pos2 = array([-2,0.2,1.6])
+      max2 = 2.7
+      se3_2 = SE3Curve(translation(max).reshape(-1,1),end_pos2.reshape(-1,1),end_rot,end_rot,max,max2)
+      pc.append(se3_2)
+      self.assertEqual(pc.num_curves(),2)
+      pmin2 = pc(max)
+      pmax2 = pc(max2)
+      self.assertTrue(isclose(pmin2[:3, :3], end_rot).all())
+      self.assertTrue(isclose(pmax2[:3, :3], end_rot).all())
+      self.assertTrue(isclose(pmin2[0:3, 3], se3_2.translation(max)).all())
+      self.assertTrue(isclose(pmax2[0:3, 3], se3_2.translation(max2)).all())
+      self.assertTrue(pc.is_continuous(0))
+      self.assertFalse(pc.is_continuous(1))
+
+      # check if error are correctly raised :
+      with self.assertRaises(ValueError): # time intervals do not match
+        se3_3 = SE3Curve(se3_2(max2),se3_2(max2-0.5),max2-0.5,max2+1.5)
+        pc.append(se3_3)
+      with self.assertRaises(ValueError):
+        se3_3 = SE3Curve(se3_2(max2),se3_2(max2-0.5),max2+0.1,max2+1.5)
+        pc.append(se3_3)
+
+      # TODO : serialization
+
+      se3_3 = SE3Curve(se3(max),se3_2(max2-0.5),max2,max2+1.5)
+      pc.append(se3_3)
+      self.assertFalse(pc.is_continuous(0))
+
+
+    if CURVES_WITH_PINOCCHIO_SUPPORT:
+
+        def test_piecewise_se3_curve_linear_pinocchio(self):
+            print("test piecewise SE3 pinocchio")
+            init_quat = Quaternion.Identity()
+            end_quat = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0)
+            init_rot = init_quat.matrix()
+            end_rot = end_quat.matrix()
+            init_translation = array([0.2, -0.7, 0.6])
+            end_translation = array([3.6, -2.2, -0.9])
+            init_pose = SE3.Identity()
+            end_pose = SE3.Identity()
+            init_pose.rotation = init_rot
+            end_pose.rotation = end_rot
+            init_pose.translation = init_translation.reshape(-1,1)
+            end_pose.translation = end_translation.reshape(-1,1)
+            min = 0.7
+            max = 12.
+            se3 = SE3Curve(init_pose, end_pose, min, max)
+            pc = piecewise_SE3_curve(se3)
+            self.assertEqual(pc.num_curves(),1)
+            p = pc.evaluateAsSE3(min)
+            self.assertTrue(isinstance(p, SE3))
+            self.assertTrue(pc.evaluateAsSE3(min).isApprox(init_pose, 1e-6))
+            self.assertTrue(pc.evaluateAsSE3(max).isApprox(end_pose, 1e-6))
+            self.assertEqual(pc.min(), min)
+            self.assertEqual(pc.max(), max)
+            self.assertTrue(isclose(pc.rotation(min), init_rot).all())
+            self.assertTrue(isclose(pc.translation(min), init_translation).all())
+            self.assertTrue(isclose(pc.rotation(max), end_rot).all())
+            self.assertTrue(isclose(pc.translation(max), end_translation).all())
+            # add another curve :
+            end_translation2 = array([-2., 1.6, -14.])
+            end_pose2 = SE3.Identity()
+            end_pose2.rotation = end_rot
+            end_pose2.translation = end_translation2.reshape(-1,1)
+            max2 = 23.9
+            se3_2 = SE3Curve(end_pose, end_pose2, max, max2)
+            pc.append(se3_2)
+            self.assertEqual(pc.num_curves(),2)
+            p = pc.evaluateAsSE3(max2)
+            self.assertTrue(isinstance(p, SE3))
+            self.assertTrue(pc.evaluateAsSE3(max2).isApprox(end_pose2, 1e-6))
+            self.assertEqual(pc.min(), min)
+            self.assertEqual(pc.max(), max2)
+            self.assertTrue(isclose(pc.rotation(max2), end_rot).all())
+            self.assertTrue(isclose(pc.translation(max2), end_translation2).all())
+
+
     def test_conversion_piecewise_curves(self):
         __EPS = 1e-6
         waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()