test.py 60 KB
Newer Older
1
import os
Guilhem Saurel's avatar
Guilhem Saurel committed
2
import unittest
3
from math import sqrt
Guilhem Saurel's avatar
Guilhem Saurel committed
4

5
import eigenpy
6
import numpy as np
7
from numpy import array, array_equal, isclose, random, zeros
8
from numpy.linalg import norm
9
import pickle
Guilhem Saurel's avatar
Guilhem Saurel committed
10
11
12
from curves import (CURVES_WITH_PINOCCHIO_SUPPORT, Quaternion, SE3Curve, SO3Linear, bezier, bezier3, convert_to_bezier,
                    convert_to_hermite, convert_to_polynomial, cubic_hermite_spline, curve_constraints, exact_cubic,
                    piecewise, piecewise_SE3, polynomial)
13

14
15
eigenpy.switchToNumpyArray()

16
if CURVES_WITH_PINOCCHIO_SUPPORT:
Guilhem Saurel's avatar
Guilhem Saurel committed
17
18
    from pinocchio import SE3, Motion

19

20
class TestCurves(unittest.TestCase):
Guilhem Saurel's avatar
Guilhem Saurel committed
21
    # def print_str(self, inStr):
22
23
24
    #   print inStr
    #   return

Guilhem Saurel's avatar
Guilhem Saurel committed
25
    def compareCurves(self, c1, c2):
26
27
        t_min = c1.min()
        t_max = c1.max()
Guilhem Saurel's avatar
Guilhem Saurel committed
28
29
30
31
32
33
34
35
        self.assertEqual(t_min, c2.min(), "c1 min : " + str(t_min) + " ; c2 min : " + str(c2.min()))
        self.assertEqual(t_max, c2.max(), "c1 max : " + str(t_max) + " ; c2 max : " + str(c2.max()))
        self.assertTrue(
            norm(c1.derivate(t_min, 1) - c2.derivate(t_min, 1)) < 1e-10,
            "dc1(tmin) = " + str(c1.derivate(t_min, 1)) + " ; dc2(tmin) = " + str(c2.derivate(t_min, 1)))
        self.assertTrue(
            norm(c1.derivate(t_max, 1) - c2.derivate(t_max, 1)) < 1e-10,
            "dc1(tmax) = " + str(c1.derivate(t_max, 1)) + " ; dc2(tmax) = " + str(c2.derivate(t_max, 1)))
36
37
        t = t_min
        while t < t_max:
Guilhem Saurel's avatar
Guilhem Saurel committed
38
39
40
            self.assertTrue(
                norm(c1(t) - c2(t)) < 1e-10, " at t = " + str(t) + " c1 = " + str(c1(t)) + " ; c2 = " + str(c2(t)))
            t = t + 0.01
41

42
    def test_bezier(self):
43
        print("test_bezier")
44
45
46
47
        # To test :
        # - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
        # - Variables : degree, nbWayPoints
        __EPS = 1e-6
48
        waypoints = array([[1., 2., 3.]]).T
49
        a = bezier(waypoints, 0., 2.)
50
51
        t = 0.
        while t < 2.:
52
            self.assertTrue(norm(a(t) - array([1., 2., 3.]).T) < __EPS)
53
            t += 0.1
54
55
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        # time_waypoints = array([0., 1.]).transpose()
56
57
        # Create bezier6 and bezier
        a = bezier(waypoints, 0., 3.)
58
        # Test waypoints
Guilhem Saurel's avatar
Guilhem Saurel committed
59
        self.assertTrue(a.nbWaypoints == 2)
60
        for i in range(0, a.nbWaypoints):
Guilhem Saurel's avatar
Guilhem Saurel committed
61
            if i == 0:
62
                self.assertTrue((a.waypointAtIndex(0) == array([1., 2., 3.])).all())
Guilhem Saurel's avatar
Guilhem Saurel committed
63
            elif i == 1:
64
                self.assertTrue((a.waypointAtIndex(1) == array([4., 5., 6.])).all())
Guilhem Saurel's avatar
Guilhem Saurel committed
65
        # self.assertTrue((a.waypoints == waypoints).all())
66
        # Test : Degree, min, max, derivate
Guilhem Saurel's avatar
Guilhem Saurel committed
67
        # self.print_str(("test 1")
68
        self.assertEqual(a.degree, a.nbWaypoints - 1)
69
70
71
        a.min()
        a.max()
        a(0.4)
72
        self.assertTrue((a(a.min()) == array([1., 2., 3.])).all())
73
74
75
76
77
78
        a.derivate(0.4, 2)
        a = a.compute_derivate(100)
        prim = a.compute_primitive(1)
        # Check primitive and derivate - order 1
        for i in range(10):
            t = float(i) / 10.
79
            self.assertTrue((a(t) == prim.derivate(t, 1)).all())
80
        self.assertTrue((prim(0) == array([0., 0., 0.])).all())
81
82
83
84
        # Check primitive and derivate - order 2
        prim = a.compute_primitive(2)
        for i in range(10):
            t = float(i) / 10.
85
            self.assertTrue((a(t) == prim.derivate(t, 2)).all())
86
        self.assertTrue((prim(0) == array([0., 0., 0.])).all())
87
        # Create new bezier curve
88
        waypoints = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
89
90
        a0 = bezier(waypoints)
        a1 = bezier(waypoints, 0., 3.)
91
92
        prim0 = a0.compute_primitive(1)
        prim1 = a1.compute_primitive(1)
93
        # Check change in argument time_t of bezier
94
95
        for i in range(10):
            t = float(i) / 10.
96
97
98
99
            self.assertTrue(norm(a0(t) - a1(3 * t)) < __EPS)
            self.assertTrue(norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.) < __EPS)
            self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
            self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
100
        self.assertTrue((prim(0) == array([0., 0., 0.])).all())
101
102
103
104
105
106
107
108
        # testing accessor to waypoints :
        wp_getter = a0.waypoints()
        self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
        self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
        self.assertTrue(array_equal(wp_getter,waypoints))
        # check that it return a copy:
        a0.waypoints()[1,1] = -15.
        self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
109
        # testing bezier with constraints
stevet's avatar
stevet committed
110
        c = curve_constraints(3)
111
112
113
114
        c.init_vel = array([[0., 1., 1.]]).transpose()
        c.end_vel = array([[0., 1., 1.]]).transpose()
        c.init_acc = array([[0., 1., -1.]]).transpose()
        c.end_acc = array([[0., 100., 1.]]).transpose()
Guilhem Saurel's avatar
Guilhem Saurel committed
115
        # Check derivate with constraints
116
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
117
        a = bezier(waypoints, c)
118
119
        self.assertTrue(norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
        self.assertTrue(norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
120

121
        # Test serialization : bezier 3
122
        a.saveAsText("serialization_curve.test")
123
        # waypoints = array([[0,0,0,], [0,0,0,]]).transpose()
124
        b = bezier()
125
        b.loadFromText("serialization_curve.test")
126
        self.assertTrue((a(0.4) == b(0.4)).all())
127
        os.remove("serialization_curve.test")
128

129
        # Bezier dim 4
130
        waypoints = array([[1., 2., 3., 4.]]).T
131
132
133
        a = bezier(waypoints, 0., 2.)
        # Test serialization : bezier of dim 4
        a.saveAsText("serialization_curve.test")
134
        # waypoints = array([[0,0,0,], [0,0,0,]]).transpose()
135
        b = bezier()
136
        b.loadFromText("serialization_curve.test")
137
        self.assertTrue((a(0.4) == b(0.4)).all())
138
        os.remove("serialization_curve.test")
139
140
141
142

        a_pickled = pickle.dumps(a)
        a_from_pickle = pickle.loads(a_pickled)
        self.assertEqual(a_from_pickle, a)
143
144
        return

145
146
147
148
149
150
    def test_bezier3(self):
        print("test_bezier3")
        # To test :
        # - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
        # - Variables : degree, nbWayPoints
        __EPS = 1e-6
151
        waypoints = array([[1., 2., 3.]]).T
152
153
154
        a = bezier3(waypoints, 0., 2.)
        t = 0.
        while t < 2.:
155
            self.assertTrue(norm(a(t) - array([1., 2., 3.]).T) < __EPS)
156
            t += 0.1
157
158
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        # time_waypoints = array([[0., 1.]]).transpose()
159
160
161
162
163
164
        # Create bezier6 and bezier
        a = bezier3(waypoints, 0., 3.)
        # Test waypoints
        self.assertTrue(a.nbWaypoints == 2)
        for i in range(0, a.nbWaypoints):
            if i == 0:
165
                self.assertTrue((a.waypointAtIndex(0) == array([1., 2., 3.])).all())
166
            elif i == 1:
167
                self.assertTrue((a.waypointAtIndex(1) == array([4., 5., 6.])).all())
168
169
170
171
172
173
174
        # self.assertTrue((a.waypoints == waypoints).all())
        # Test : Degree, min, max, derivate
        # self.print_str(("test 1")
        self.assertEqual(a.degree, a.nbWaypoints - 1)
        a.min()
        a.max()
        a(0.4)
175
        self.assertTrue((a(a.min()) == array([1., 2., 3.])).all())
176
177
178
179
180
181
182
        a.derivate(0.4, 2)
        a = a.compute_derivate(100)
        prim = a.compute_primitive(1)
        # Check primitive and derivate - order 1
        for i in range(10):
            t = float(i) / 10.
            self.assertTrue((a(t) == prim.derivate(t, 1)).all())
183
        self.assertTrue((prim(0) == array([0., 0., 0.])).all())
184
185
186
187
188
        # Check primitive and derivate - order 2
        prim = a.compute_primitive(2)
        for i in range(10):
            t = float(i) / 10.
            self.assertTrue((a(t) == prim.derivate(t, 2)).all())
189
        self.assertTrue((prim(0) == array([0., 0., 0.])).all())
190
        # Create new bezier curve
191
        waypoints = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
192
193
194
195
196
197
198
199
200
201
202
        a0 = bezier3(waypoints)
        a1 = bezier3(waypoints, 0., 3.)
        prim0 = a0.compute_primitive(1)
        prim1 = a1.compute_primitive(1)
        # Check change in argument time_t of bezier
        for i in range(10):
            t = float(i) / 10.
            self.assertTrue(norm(a0(t) - a1(3 * t)) < __EPS)
            self.assertTrue(norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.) < __EPS)
            self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
            self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
203
        self.assertTrue((prim(0) == array([0., 0., 0.])).all())
204
205
206
207
208
209
210
211
        # testing accessor to waypoints :
        wp_getter = a0.waypoints()
        self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
        self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
        self.assertTrue(array_equal(wp_getter,waypoints))
        # check that it return a copy:
        a0.waypoints()[1,1] = -15.
        self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
212
213
        # testing bezier with constraints
        c = curve_constraints(3)
214
215
216
217
        c.init_vel = array([[0., 1., 1.]]).transpose()
        c.end_vel = array([[0., 1., 1.]]).transpose()
        c.init_acc = array([[0., 1., -1.]]).transpose()
        c.end_acc = array([[0., 100., 1.]]).transpose()
218
        # Check derivate with constraints
219
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
220
221
222
223
224
225
        a = bezier3(waypoints, c)
        self.assertTrue(norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
        self.assertTrue(norm(a.derivate(1, 2) - c.end_acc) < 1e-10)

        # Test serialization : bezier 3
        a.saveAsText("serialization_curve.test")
226
        # waypoints = array([[0,0,0,], [0,0,0,]]).transpose()
227
228
229
230
231
232
        b = bezier3()
        b.loadFromText("serialization_curve.test")
        self.assertTrue((a(0.4) == b(0.4)).all())
        os.remove("serialization_curve.test")

        # Bezier dim 4
233
        waypoints = array([[1., 2., 3., 4.]]).T
234
        a = bezier(waypoints, 0., 2.)
235
236
        # Test serialization : bezier of dim 4
        a.saveAsText("serialization_curve.test")
237
        # waypoints = array([[0,0,0,], [0,0,0,]]).transpose()
238
        b = bezier()
239
240
241
        b.loadFromText("serialization_curve.test")
        self.assertTrue((a(0.4) == b(0.4)).all())
        os.remove("serialization_curve.test")
242
243
244
        a_pickled = pickle.dumps(a)
        a_from_pickle = pickle.loads(a_pickled)
        self.assertEqual(a_from_pickle, a)
245
246
        return

247
    def test_polynomial(self):
248
        print("test_polynomial")
249
        # To test :
250
        # - Functions : constructor, min, max, derivate, serialize, deserialize
251
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
252
253
        a = polynomial(waypoints)  # Defined on [0.,1.]
        a = polynomial(waypoints, -1., 3.)  # Defined on [-1.,3.]
254
255
256
        a.min()
        a.max()
        a(0.4)
257
        # Test get coefficient at degree
Guilhem Saurel's avatar
Guilhem Saurel committed
258
        self.assertTrue((a.coeff() == waypoints).all())
259
260
        self.assertTrue((a.coeffAtDegree(0) == array([1., 2., 3.])).all())
        self.assertTrue((a.coeffAtDegree(1) == array([4., 5., 6.])).all())
261
        # Other tests
262
        self.assertTrue((a(a.min()) == array([1., 2., 3.])).all())
Guilhem Saurel's avatar
Guilhem Saurel committed
263
        self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
264
        a.derivate(0.4, 2)
265
266
        a_derivated = a.compute_derivate(1)
        self.assertTrue((a.derivate(0.4, 1) == a_derivated(0.4)).all())
267
        # Test serialization
268
269
270
271
        a.saveAsText("serialization_curve.test")
        b = polynomial()
        b.loadFromText("serialization_curve.test")
        self.assertTrue((a(0.4) == b(0.4)).all())
272
        os.remove("serialization_curve.test")
273
274
275
        a_pickled = pickle.dumps(a)
        a_from_pickle = pickle.loads(a_pickled)
        self.assertEqual(a_from_pickle, a)
276
277
        return

278
    def test_polynomial_from_boundary_condition(self):
279
280
281
282
283
284
        p0 = array([1., 3., -2.])
        p1 = array([0.6, 2., 2.5])
        dp0 = array([-6., 2., -1.])
        dp1 = array([10., 10., 10.])
        ddp0 = array([1., -7., 4.5])
        ddp1 = array([6., -1., -4])
285
286
        min = 1.
        max = 2.5
287
        polC0 = polynomial(p0, p1, min, max)
288
        self.assertEqual(polC0.min(), min)
Guilhem Saurel's avatar
Guilhem Saurel committed
289
        self.assertEqual(polC0.max(), max)
290
        # TODO: Why are thoso `.T[0]` needed ?
291
        self.assertTrue(array_equal(polC0((min + max) / 2.), 0.5 * p0 + 0.5 * p1))
292
        polC1 = polynomial(p0, dp0, p1, dp1, min, max)
293
        self.assertEqual(polC1.min(), min)
Guilhem Saurel's avatar
Guilhem Saurel committed
294
        self.assertEqual(polC1.max(), max)
295
296
297
298
        self.assertTrue(isclose(polC1(min), p0).all())
        self.assertTrue(isclose(polC1(max), p1).all())
        self.assertTrue(isclose(polC1.derivate(min, 1), dp0).all())
        self.assertTrue(isclose(polC1.derivate(max, 1), dp1).all())
299
        polC2 = polynomial(p0, dp0, ddp0, p1, dp1, ddp1, min, max)
300
        self.assertEqual(polC2.min(), min)
Guilhem Saurel's avatar
Guilhem Saurel committed
301
        self.assertEqual(polC2.max(), max)
302
303
304
305
306
307
        self.assertTrue(isclose(polC2(min), p0).all())
        self.assertTrue(isclose(polC2(max), p1).all())
        self.assertTrue(isclose(polC2.derivate(min, 1), dp0).all())
        self.assertTrue(isclose(polC2.derivate(max, 1), dp1).all())
        self.assertTrue(isclose(polC2.derivate(min, 2), ddp0).all())
        self.assertTrue(isclose(polC2.derivate(max, 2), ddp1).all())
308
        # check that the exception are correctly raised :
309
        with self.assertRaises(ValueError):
310
            polC0 = polynomial(p0, p1, max, min)
311

312
        with self.assertRaises(ValueError):
313
            polC1 = polynomial(p0, dp0, p1, dp1, max, min)
314

315
        with self.assertRaises(ValueError):
316
            polC2 = polynomial(p0, dp0, ddp0, p1, dp1, ddp1, max, min)
317

318
    def test_cubic_hermite_spline(self):
319
        print("test_cubic_hermite_spline")
320
321
322
        points = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        tangents = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        time_points = array([[0., 1.]]).transpose()
323
324
325
326
327
328
329
330
331
332
        a = cubic_hermite_spline(points, tangents, time_points)
        a.min()
        a.max()
        a(0.4)
        self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
        a.derivate(0.4, 2)
        # Test serialization
        a.saveAsText("serialization_curve.test")
        b = cubic_hermite_spline()
        b.loadFromText("serialization_curve.test")
333
        self.assertTrue((a(0.4) == b(0.4)).all())
334
        os.remove("serialization_curve.test")
335
        # test dim 4
336
337
338
        points = array([[1., 2., 3., 4.], [4., 5., 6., 7.]]).transpose()
        tangents = array([[1., 2., 3., 4.], [4., 5., 6., 7.]]).transpose()
        time_points = array([[0., 1.]]).transpose()
339
        a = cubic_hermite_spline(points, tangents, time_points)
340
341
342
343
344
345
346
        a.min()
        a.max()
        a(0.4)
        self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
        a.derivate(0.4, 2)
        # Test serialization
        a.saveAsText("serialization_curve.test")
347
        b = cubic_hermite_spline()
348
        b.loadFromText("serialization_curve.test")
349
        self.assertTrue((a(0.4) == b(0.4)).all())
350
        os.remove("serialization_curve.test")
351
352
353
        a_pickled = pickle.dumps(a)
        a_from_pickle = pickle.loads(a_pickled)
        self.assertEqual(a_from_pickle, a)
354
355
356
        return

    def test_piecewise_polynomial_curve(self):
357
        print("test_piecewise_polynomial_curve")
358
        # To test :
359
        # - Functions : constructor, min, max, derivate, add_curve, is_continuous, serialize, deserialize
360
361
362
        waypoints0 = array([[0., 0., 0.]]).transpose()
        waypoints1 = array([[1., 1., 1.]]).transpose()
        waypoints2 = array([[1., 1., 1.], [1., 1., 1.]]).transpose()
Guilhem Saurel's avatar
Guilhem Saurel committed
363
        polynomial(waypoints0, 0., 0.1)
364
        a = polynomial(waypoints1, 0., 1.)
365
        b = polynomial(waypoints2, 1., 3.)
366
        pc = piecewise(a)
367
        pc.append(b)
368
369
370
        pc.min()
        pc.max()
        pc(0.4)
371
        self.assertTrue((pc(pc.min()) == array([[1., 1., 1.]]).transpose()).all())
372
        self.assertTrue((pc.derivate(0.4, 0) == pc(0.4)).all())
373
374
375
        pc.derivate(0.4, 2)
        pc.is_continuous(0)
        pc.is_continuous(1)
376
377
378
379
380
        a0 = pc.curve_at_index(0)
        self.assertTrue(array_equal(a0(0.5), pc(0.5)))
        self.assertTrue(a0 == a)
        a0 = b # should not have any effect
        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
381
        # Test serialization
382
        pc.saveAsText("serialization_pc.test")
383
        pc_test = piecewise()
384
385
        pc_test.loadFromText("serialization_pc.test")
        self.assertTrue((pc(0.4) == pc_test(0.4)).all())
386
        os.remove("serialization_pc.test")
387
388
389
        pc_pickled = pickle.dumps(pc)
        pc_from_pickle = pickle.loads(pc_pickled)
        self.assertEqual(pc_from_pickle, pc)
390

Guilhem Saurel's avatar
Guilhem Saurel committed
391
        waypoints3 = array([[1., 2., 3., 0.6, -9.], [-1., 1.6, 1.7, 6.7, 14]]).transpose()
392
        c = polynomial(waypoints3, 3., 5.2)
Guilhem Saurel's avatar
Guilhem Saurel committed
393
394
        with self.assertRaises(ValueError):  # a and c doesn't have the same dimension
            pc.append(c)
395

Guilhem Saurel's avatar
Guilhem Saurel committed
396
        # Test the different append methods :
397
        pc = piecewise()
Guilhem Saurel's avatar
Guilhem Saurel committed
398
399
        self.assertEqual(pc.num_curves(), 0)
        end_point1 = array([1., 3., 5., 6.5, -2.])
400
        max1 = 2.5
Guilhem Saurel's avatar
Guilhem Saurel committed
401
402
403
404
405
        with self.assertRaises(RuntimeError):  # cannot add final point in an empty curve
            pc.append(end_point1, max1)
        with self.assertRaises(ValueError):  # a and end_point1 doesn't have the same dimension
            pc.append(a)
            pc.append(end_point1, max1)
406

407
        pc = piecewise()
408
        d = polynomial(waypoints3, 0., 1.2)
Guilhem Saurel's avatar
Guilhem Saurel committed
409
        self.assertEqual(pc.num_curves(), 0)
410
        pc.append(d)
Guilhem Saurel's avatar
Guilhem Saurel committed
411
412
413
414
415
        self.assertEqual(pc.num_curves(), 1)
        pc.append(end_point1, max1)
        self.assertEqual(pc.num_curves(), 2)
        self.assertEqual(pc.min(), 0.)
        self.assertEqual(pc.max(), max1)
416
        self.assertTrue(pc.is_continuous(0))
Guilhem Saurel's avatar
Guilhem Saurel committed
417
418
        self.assertTrue(isclose(pc(0.), d(0.)).all())
        self.assertTrue(isclose(pc(max1), end_point1).all())
419

420
421
        return

422
423
    def test_piecewise_from_points_list(self):
        N = 7
424
425
426
427
        points = array(random.rand(3, N))
        points_derivative = array(random.rand(3, N))
        points_second_derivative = array(random.rand(3, N))
        time_points = array(random.rand(1, N)).T
428
        time_points.sort(0)
429
        polC0 = piecewise.FromPointsList(points, time_points)
Guilhem Saurel's avatar
Guilhem Saurel committed
430
431
        self.assertEqual(polC0.min(), time_points[0, 0])
        self.assertEqual(polC0.max(), time_points[-1, 0])
432
433
434
        self.assertTrue(polC0.is_continuous(0))
        self.assertTrue(not polC0.is_continuous(1))
        for i in range(N):
Guilhem Saurel's avatar
Guilhem Saurel committed
435
            self.assertTrue(isclose(polC0(time_points[i, 0]), points[:, i]).all())
436

437
438
439
440
        c0 = polC0.curve_at_index(0)
        self.assertEqual(c0.min(), time_points[0])
        self.assertEqual(c0.max(), time_points[1])
        self.assertEqual(c0.dim(), 3)
441
442
        mid_t = (c0.max() + c0.min()) /2.
        self.assertTrue(array_equal(polC0(mid_t), c0(mid_t)))
443

444
        polC1 = piecewise.FromPointsList(points, points_derivative, time_points)
Guilhem Saurel's avatar
Guilhem Saurel committed
445
446
        self.assertEqual(polC1.min(), time_points[0, 0])
        self.assertEqual(polC1.max(), time_points[-1, 0])
447
448
449
450
        self.assertTrue(polC1.is_continuous(0))
        self.assertTrue(polC1.is_continuous(1))
        self.assertTrue(not polC1.is_continuous(2))
        for i in range(N):
Guilhem Saurel's avatar
Guilhem Saurel committed
451
452
            self.assertTrue(isclose(polC1(time_points[i, 0]), points[:, i]).all())
            self.assertTrue(isclose(polC1.derivate(time_points[i, 0], 1), points_derivative[:, i]).all())
453

Guilhem Saurel's avatar
Guilhem Saurel committed
454
        polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative, time_points)
Guilhem Saurel's avatar
Guilhem Saurel committed
455
456
        self.assertEqual(polC2.min(), time_points[0, 0])
        self.assertEqual(polC2.max(), time_points[-1, 0])
457
458
459
460
461
        self.assertTrue(polC2.is_continuous(0))
        self.assertTrue(polC2.is_continuous(1))
        self.assertTrue(polC2.is_continuous(2))
        self.assertTrue(not polC2.is_continuous(3))
        for i in range(N):
Guilhem Saurel's avatar
Guilhem Saurel committed
462
463
464
            self.assertTrue(isclose(polC2(time_points[i, 0]), points[:, i]).all())
            self.assertTrue(isclose(polC2.derivate(time_points[i, 0], 1), points_derivative[:, i]).all())
            self.assertTrue(isclose(polC2.derivate(time_points[i, 0], 2), points_second_derivative[:, i]).all())
465
466

        # check if exepetion are corectly raised when time_points are not in ascending values
Guilhem Saurel's avatar
Guilhem Saurel committed
467
468
        time_points[0, 0] = 1
        time_points[1, 0] = 0.5
469
        with self.assertRaises(ValueError):
470
            polC0 = piecewise.FromPointsList(points, time_points)
471
472

        with self.assertRaises(ValueError):
473
            polC1 = piecewise.FromPointsList(points, points_derivative, time_points)
474
475

        with self.assertRaises(ValueError):
Guilhem Saurel's avatar
Guilhem Saurel committed
476
            polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative, time_points)
477

478
    def test_piecewise_bezier_curve(self):
479
480
        # To test :
        # - Functions : constructor, min, max, derivate, add_curve, is_continuous
481
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
482
483
        a = bezier(waypoints, 0., 1.)
        b = bezier(waypoints, 1., 2.)
484
        pc = piecewise(a)
Guilhem Saurel's avatar
Guilhem Saurel committed
485
486
        pc.append(b)
        a.split(array([0.4, 0.8])).curve_at_index(0)
487
488
489
        pc.min()
        pc.max()
        pc(0.4)
490
        self.assertTrue((pc(pc.min()) == array([1., 2., 3.])).all())
491
        self.assertTrue((pc.derivate(0.4, 0) == pc(0.4)).all())
492
493
494
        pc.derivate(0.4, 2)
        pc.is_continuous(0)
        pc.is_continuous(1)
495
496
497
498
499
500
501
502
        # test access to curves :
        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
        waypoints = array([[3., 4., -3.], [5., 1., 2.]]).transpose()
        c = bezier(waypoints, 1.5, 2.)
        c0 = pc.curve_at_index(0)
        c0 = c # should not have any effect
        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))

503
504
        # Test serialization
        pc.saveAsText("serialization_pc.test")
505
        pc_test = piecewise()
506
507
        pc_test.loadFromText("serialization_pc.test")
        self.assertTrue((pc(0.4) == pc_test(0.4)).all())
508
        os.remove("serialization_pc.test")
509
510
511
        pc_pickled = pickle.dumps(pc)
        pc_from_pickle = pickle.loads(pc_pickled)
        self.assertEqual(pc_from_pickle, pc)
512
513
514
        return

    def test_piecewise_cubic_hermite_curve(self):
515
        print("test_piecewise_cubic_hermite_curve")
516
517
        # To test :
        # - Functions : constructor, min, max, derivate, add_curve, is_continuous
518
519
520
521
        points = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        tangents = array([[2., 2., 2.], [4., 4., 4.]]).transpose()
        time_points0 = array([[0., 1.]]).transpose()
        time_points1 = array([[1., 2.]]).transpose()
522
523
        a = cubic_hermite_spline(points, tangents, time_points0)
        b = cubic_hermite_spline(points, tangents, time_points1)
524
        pc = piecewise(a)
525
        pc.append(b)
526
527
528
        pc.min()
        pc.max()
        pc(0.4)
529
        self.assertTrue((pc(0.) == array([1., 2., 3.])).all())
530
        self.assertTrue((pc.derivate(0.4, 0) == pc(0.4)).all())
531
532
533
        pc.derivate(0.4, 2)
        pc.is_continuous(0)
        pc.is_continuous(1)
534
535
536
537
538
        a0 = pc.curve_at_index(0)
        self.assertTrue(array_equal(a0(0.5), pc(0.5)))
        self.assertTrue(a0 == a)
        a0 = b # should not have any effect
        self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
539
540
        # Test serialization
        pc.saveAsText("serialization_pc.test")
541
        pc_test = piecewise()
542
543
        pc_test.loadFromText("serialization_pc.test")
        self.assertTrue((pc(0.4) == pc_test(0.4)).all())
544
        os.remove("serialization_pc.test")
545
546
547
        pc_pickled = pickle.dumps(pc)
        pc_from_pickle = pickle.loads(pc_pickled)
        self.assertEqual(pc_from_pickle, pc)
548
549
550
        return

    def test_exact_cubic(self):
551
        print("test_exact_cubic")
552
553
        # To test :
        # - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt
554
555
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        time_waypoints = array([[0., 1.]]).transpose()
556
557
558
559
        a = exact_cubic(waypoints, time_waypoints)
        a.min()
        a.max()
        a(0.4)
560
        self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
561
562
563
        a.derivate(0.4, 2)
        a.getNumberSplines()
        a.getSplineAt(0)
564
565
566
567
568
        # Test serialization
        a.saveAsText("serialization_pc.test")
        b = exact_cubic()
        b.loadFromText("serialization_pc.test")
        self.assertTrue((a(0.4) == b(0.4)).all())
569
        os.remove("serialization_pc.test")
570
571
572
        a_pickled = pickle.dumps(a)
        a_from_pickle = pickle.loads(a_pickled)
        self.assertEqual(a_from_pickle, a)
573
574
575
        return

    def test_exact_cubic_constraint(self):
576
        print("test_exact_cubic_constraint")
577
578
        # To test :
        # - Functions : constructor, min, max, derivate
579
580
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        time_waypoints = array([[0., 1.]]).transpose()
stevet's avatar
stevet committed
581
        c = curve_constraints(3)
582
583
584
585
        c.init_vel = array([[0., 1., 1.]]).transpose()
        c.end_vel = array([[0., 1., 1.]]).transpose()
        c.init_acc = array([[0., 1., 1.]]).transpose()
        c.end_acc = array([[0., 1., 1.]]).transpose()
586
587
588
589
        c.init_vel
        c.end_vel
        c.init_acc
        c.end_acc
Guilhem Saurel's avatar
Guilhem Saurel committed
590
591
        exact_cubic(waypoints, time_waypoints)
        exact_cubic(waypoints, time_waypoints, c)
592
593
        return

Guilhem Saurel's avatar
Guilhem Saurel committed
594
    def test_cubic_hermite_spline_2(self):
595
596
597
        points = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
        tangents = array([[2., 2., 2.], [4., 4., 4.]]).transpose()
        time_points = array([[0., 1.]]).transpose()
598
599
600
601
        a = cubic_hermite_spline(points, tangents, time_points)
        a.min()
        a.max()
        a(0.4)
602
        self.assertTrue((a(0.) == array([1., 2., 3.])).all())
603
        self.assertTrue(isclose(a.derivate(0., 1), array([[2., 2., 2.]]).transpose()).all())
Guilhem Saurel's avatar
Guilhem Saurel committed
604
        self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
605
606
607
608
        a.derivate(0.4, 2)
        return

    def test_conversion_curves(self):
609
        print("test_conversion_curves")
610
        __EPS = 1e-6
611
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
612
        a = bezier(waypoints)
613
        # converting bezier to polynomial
614
        a_pol = convert_to_polynomial(a)
615
        self.assertTrue(norm(a(0.3) - a_pol(0.3)) < __EPS)
616
        # converting polynomial to hermite
617
        a_chs = convert_to_hermite(a_pol)
618
        self.assertTrue(norm(a_chs(0.3) - a_pol(0.3)) < __EPS)
619
        # converting hermite to bezier
620
        a_bc = convert_to_bezier(a_chs)
621
622
        self.assertTrue(norm(a_chs(0.3) - a_bc(0.3)) < __EPS)
        self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
623
        # converting bezier to hermite
624
        a_chs = convert_to_hermite(a)
625
        self.assertTrue(norm(a(0.3) - a_chs(0.3)) < __EPS)
626
        # converting hermite to polynomial
627
        a_pol = convert_to_polynomial(a_chs)
628
        self.assertTrue(norm(a_pol(0.3) - a_chs(0.3)) < __EPS)
629
        # converting polynomial to bezier
630
        a_bc = convert_to_bezier(a_pol)
631
632
        self.assertTrue(norm(a_bc(0.3) - a_pol(0.3)) < __EPS)
        self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
633
        return
634

635
    def test_piecewise_se3_curve(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
        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(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), end_pos2, 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)

        pc.saveAsText("serialization_curve.txt")
        pc_txt = piecewise_SE3()
        pc_txt.loadFromText("serialization_curve.txt")
        self.compareCurves(pc, pc_txt)

        pc.saveAsXML("serialization_curve.xml", "pc")
        pc_xml = piecewise_SE3()
        pc_xml.loadFromXML("serialization_curve.xml", "pc")
        self.compareCurves(pc, pc_xml)

        pc.saveAsBinary("serialization_curve")
        pc_bin = piecewise_SE3()
        pc_bin.loadFromBinary("serialization_curve")
        self.compareCurves(pc, pc_bin)

694
695
696
697
        pc_pickled = pickle.dumps(pc)
        pc_from_pickle = pickle.loads(pc_pickled)
        self.assertEqual(pc_from_pickle, pc)

Guilhem Saurel's avatar
Guilhem Saurel committed
698
        se3_3 = SE3Curve(se3(max), se3_2(max2 - 0.5), max2, max2 + 1.5)
699
        pc.append(se3_3)
Guilhem Saurel's avatar
Guilhem Saurel committed
700
        self.assertFalse(pc.is_continuous(0))
701

Guilhem Saurel's avatar
Guilhem Saurel committed
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
        #  test the different append methods :
        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()
        self.assertEqual(pc.num_curves(), 0)
        pc.append(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())
        # append a final tranform :
        end_quat = Quaternion(sqrt(2.) / 2., 0., sqrt(2.) / 2., 0)
727
        end_rot = end_quat.matrix()
Guilhem Saurel's avatar
Guilhem Saurel committed
728
729
730
731
732
733
734
        end_translation = array([1.7, -0.8, 3.]).T
        end_pose = array(np.identity(4))
        end_pose[:3, :3] = end_rot
        end_pose[:3, 3] = end_translation
        max2 = 3.8
        pc.append(end_pose, max2)
        self.assertEqual(pc.num_curves(), 2)
735
        self.assertEqual(pc.min(), min)
Guilhem Saurel's avatar
Guilhem Saurel committed
736
        self.assertEqual(pc.max(), max2)
737
        pmin = pc(min)
Guilhem Saurel's avatar
Guilhem Saurel committed
738
        pmax = pc(max2)
739
740
741
742
743
        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], end_translation).all())
        self.assertTrue(pc.is_continuous(0))
Guilhem Saurel's avatar
Guilhem Saurel committed
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
        if CURVES_WITH_PINOCCHIO_SUPPORT:
            end_quat = Quaternion(sqrt(2.) / 2., 0., 0, sqrt(2.) / 2.)
            end_rot = end_quat.matrix()
            end_translation = array([-17., 3.7, 1.])
            end_pose = SE3.Identity()
            end_pose.rotation = end_rot
            end_pose.translation = end_translation
            max3 = 6.5
            pc.append(end_pose, max3)
            self.assertEqual(pc.num_curves(), 3)
            self.assertEqual(pc.min(), min)
            self.assertEqual(pc.max(), max3)
            pmin = pc(min)
            pmax = pc(max3)
            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], end_translation).all())
            self.assertTrue(pc.is_continuous(0))
        pc = piecewise_SE3()
        with self.assertRaises(RuntimeError):
            pc.append(end_pose, max)
766

767
768
769
770
771
772
773
774
775
776
777
778
779
780
    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
781
782
            init_pose.translation = init_translation
            end_pose.translation = end_translation
783
784
785
            min = 0.7
            max = 12.
            se3 = SE3Curve(init_pose, end_pose, min, max)
786
            pc = piecewise_SE3(se3)
Guilhem Saurel's avatar
Guilhem Saurel committed
787
            self.assertEqual(pc.num_curves(), 1)
788
789
790
791
792
793
794
795
796
797
798
799
800
801
            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
802
            end_pose2.translation = end_translation2
803
804
805
            max2 = 23.9
            se3_2 = SE3Curve(end_pose, end_pose2, max, max2)
            pc.append(se3_2)
Guilhem Saurel's avatar
Guilhem Saurel committed
806
            self.assertEqual(pc.num_curves(), 2)
807
808
809
810
811
812
813
814
            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())

815
    def test_conversion_piecewise_curves(self):
816
        waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
817
818
        a = bezier(waypoints, 0., 1.)
        b = bezier(waypoints, 1., 2.)
819
        pc = piecewise(a)
820
        pc.append(b)
821
822
        # Convert to piecewise polynomial
        pc_pol = pc.convert_piecewise_curve_to_polynomial()
823
        self.compareCurves(pc_pol, pc)
824
825
        # Convert to piecewise cubic hermite
        pc_chs = pc.convert_piecewise_curve_to_cubic_hermite()
826
        self.compareCurves(pc_chs, pc)
827
828
        # Convert to piecewise bezier
        pc_bc = pc_chs.convert_piecewise_curve_to_bezier()
829
        self.compareCurves(pc_bc, pc)
830
831
        return

832
    def test_so3_linear(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
833
        print("test SO3 Linear")
834
        init_quat = Quaternion.Identity()
Guilhem Saurel's avatar
Guilhem Saurel committed
835
        end_quat = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0)
836
837
838
839
        init_rot = init_quat.matrix()
        end_rot = end_quat.matrix()
        min = 0.2
        max = 1.5
840

Guilhem Saurel's avatar
Guilhem Saurel committed
841
842
843
844
845
846
847
848
849
850
        so3Rot = SO3Linear(init_rot, end_rot, min, max)
        so3Quat = SO3Linear(init_quat, end_quat, min, max)
        self.assertEqual(so3Rot.min(), min)
        self.assertEqual(so3Rot.max(), max)
        self.assertEqual(so3Quat.min(), min)
        self.assertEqual(so3Quat.max(), max)
        self.assertTrue(isclose(so3Rot(min), init_rot).all())
        self.assertTrue(isclose(so3Rot(max), end_rot).all())
        self.assertTrue(isclose(so3Quat(min), init_rot).all())
        self.assertTrue(isclose(so3Quat(max), end_rot).all())
851
852
853
854
        self.assertTrue(so3Rot.computeAsQuaternion(min).isApprox(init_quat))
        self.assertTrue(so3Rot.computeAsQuaternion(max).isApprox(end_quat))
        self.assertTrue(so3Quat.computeAsQuaternion(min).isApprox(init_quat))
        self.assertTrue(so3Quat.computeAsQuaternion(max).isApprox(end_quat))
855
856
        t = min
        while t < max:
Guilhem Saurel's avatar
Guilhem Saurel committed
857
858
            self.assertTrue(isclose(so3Quat(t), so3Rot(t)).all())
            t += 0.01
859
        # check the derivatives :
860
861
        vel = array([1.20830487, 0., 0.])
        zeros3 = zeros((3, 1))
862
863
        t = min
        while t < max:
Guilhem Saurel's avatar
Guilhem Saurel committed
864
865
            self.assertTrue(isclose(so3Quat.derivate(t, 1), vel).all())
            self.assertTrue(isclose(so3Rot.derivate(t, 1), vel).all())
866
            t += 0.01
Guilhem Saurel's avatar
Guilhem Saurel committed
867
868
869
870
871
872
        for i in range(2, 5):
            t = min
            while t < max:
                self.assertTrue(isclose(so3Quat.derivate(t, i), zeros3).all())
                self.assertTrue(isclose(so3Rot.derivate(t, i), zeros3).all())
                t += 0.01
873
874

        # check that errors are correctly raised when necessary :
875
        with self.assertRaises(ValueError):
Guilhem Saurel's avatar
Guilhem Saurel committed
876
            so3Rot(0.)
877
        with self.assertRaises(ValueError):
Guilhem Saurel's avatar
Guilhem Saurel committed
878
            so3Rot(-0.1)
879
        with self.assertRaises(ValueError):
Guilhem Saurel's avatar
Guilhem Saurel committed
880
            so3Rot(3)