Skip to content
GitLab
Commit 11c34d91 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Python Format

parent ee941bdc
Pipeline #7884 failed with stage
in 4 minutes and 36 seconds
Hide whitespace changes
Inline Side-by-side
python/curves/__init__.py
View file @ 11c34d91
......@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from .curves import *
from .curves import * # noqa
python/curves/optimization.py
View file @ 11c34d91
......@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from .curves.optimization import *
from .curves.optimization import * # noqa
python/curves/plot.py
View file @ 11c34d91
import eigenpy
import matplotlib.pyplot as plt
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
from numpy import array
from .curves import bezier
......
python/test/notebook.py
View file @ 11c34d91
import os
import unittest
from math import sqrt
import eigenpy
import numpy as np
from numpy import array, array_equal, isclose, random, zeros, identity, dot, hstack, vstack
from numpy.linalg import norm
from numpy import array, dot, identity, zeros
from curves import (CURVES_WITH_PINOCCHIO_SUPPORT, Quaternion, SE3Curve, SO3Linear, bezier, bezier3,
cubic_hermite_spline, curve_constraints, exact_cubic,polynomial,
piecewise, piecewise_SE3, convert_to_polynomial,convert_to_bezier,convert_to_hermite)
# importing the bezier curve class
from curves import bezier
eigenpy.switchToNumpyArray()
#importing the bezier curve class
from curves import (bezier)
#dummy methods
# dummy methods
def plot(*karrgs):
pass
pass
class TestNotebook(unittest.TestCase):
......@@ -31,98 +21,88 @@ class TestNotebook(unittest.TestCase):
def test_notebook(self):
print("test_notebook")
#We describe a degree 3 curve as a Bezier curve with 4 control points
# We describe a degree 3 curve as a Bezier curve with 4 control points
waypoints = array([[1., 2., 3.], [-4., -5., -6.], [4., 5., 6.], [7., 8., 9.]]).transpose()
ref = bezier(waypoints)
numSamples = 10; fNumSamples = float(numSamples)
ptsTime = [ (ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples+1)]
numSamples = 10
fNumSamples = float(numSamples)
ptsTime = [(ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples + 1)]
from curves.optimization import (problem_definition, setup_control_points)
#dimension of our problem (here 3 as our curve is 3D)
# dimension of our problem (here 3 as our curve is 3D)
dim = 3
refDegree = 3
pD = problem_definition(dim)
pD.degree = refDegree #we want to fit a curve of the same degree as the reference curve for the sanity check
pD.degree = refDegree # we want to fit a curve of the same degree as the reference curve for the sanity check
#generates the variable bezier curve with the parameters of problemDefinition
# generates the variable bezier curve with the parameters of problemDefinition
problem = setup_control_points(pD)
#for now we only care about the curve itself
# for now we only care about the curve itself
variableBezier = problem.bezier()
linearVariable = variableBezier(0.)
variableBezier(0.)
#least square form of ||Ax-b||**2
# least square form of ||Ax-b||**2
def to_least_square(A, b):
return dot(A.T, A), - dot(A.T, b)
return dot(A.T, A), -dot(A.T, b)
def genCost(variableBezier, ptsTime):
#first evaluate variableBezier for each time sampled
allsEvals = [(variableBezier(time), pt) for (pt,time) in ptsTime]
#then compute the least square form of the cost for each points
allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals]
#and finally sum the costs
# first evaluate variableBezier for each time sampled
allsEvals = [(variableBezier(time), pt) for (pt, time) in ptsTime]
# then compute the least square form of the cost for each points
allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals]
# and finally sum the costs
Ab = [sum(x) for x in zip(*allLeastSquares)]
return Ab[0], Ab[1]
A, b = genCost(variableBezier, ptsTime)
A, b = genCost(variableBezier, ptsTime)
def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False):
return zeros(P.shape[0])
return zeros(P.shape[0])
res = quadprog_solve_qp(A, b)
def evalAndPlot(variableBezier, res):
fitBezier = variableBezier.evaluate(res.reshape((-1,1)) )
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
return fitBezier
fitBezier = evalAndPlot(variableBezier, res)
fitBezier = evalAndPlot(variableBezier, res)
pD.degree = refDegree - 1
problem = setup_control_points(pD)
variableBezier = problem.bezier()
A, b = genCost(variableBezier, ptsTime)
res = quadprog_solve_qp(A, b)
fitBezier = evalAndPlot(variableBezier, res)
from curves.optimization import constraint_flag
pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS
#set initial position
pD.init_pos = array([ptsTime[ 0][0]]).T
#set end position
pD.end_pos = array([ptsTime[-1][0]]).T
# set initial position
pD.init_pos = array([ptsTime[0][0]]).T
# set end position
pD.end_pos = array([ptsTime[-1][0]]).T
problem = setup_control_points(pD)
variableBezier = problem.bezier()
prob = setup_control_points(pD)
variableBezier = prob.bezier()
A, b = genCost(variableBezier, ptsTime)
res = quadprog_solve_qp(A, b)
_ = evalAndPlot(variableBezier, res)
evalAndPlot(variableBezier, res)
#values are 0 by default, so if the constraint is zero this can be skipped
# values are 0 by default, so if the constraint is zero this can be skipped
pD.init_vel = array([[0., 0., 0.]]).T
pD.init_acc = array([[0., 0., 0.]]).T
pD.end_vel = array([[0., 0., 0.]]).T
pD.end_acc = array([[0., 0., 0.]]).T
pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS | constraint_flag.INIT_VEL | constraint_flag.END_VEL | constraint_flag.INIT_ACC | constraint_flag.END_ACC
pD.flag = (constraint_flag.END_POS | constraint_flag.INIT_POS | constraint_flag.INIT_VEL
| constraint_flag.END_VEL | constraint_flag.INIT_ACC | constraint_flag.END_ACC)
err = False
try:
......@@ -131,7 +111,6 @@ class TestNotebook(unittest.TestCase):
err = True
assert err
pD.degree = refDegree + 4
prob = setup_control_points(pD)
variableBezier = prob.bezier()
......@@ -139,47 +118,44 @@ class TestNotebook(unittest.TestCase):
res = quadprog_solve_qp(A, b)
fitBezier = evalAndPlot(variableBezier, res)
pD.degree = refDegree + 60
prob = setup_control_points(pD)
variableBezier = prob.bezier()
A, b = genCost(variableBezier, ptsTime)
#regularization matrix
# regularization matrix
reg = identity(A.shape[1]) * 0.001
res = quadprog_solve_qp(A + reg, b)
fitBezier = evalAndPlot(variableBezier, res)
#set initial / terminal constraints
# set initial / terminal constraints
pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS
pD.degree = refDegree
prob = setup_control_points(pD)
variableBezier = prob.bezier()
#get value of the curve first order derivative at t = 0.8
t08Constraint = variableBezier.derivate(0.8,1)
target = zeros(3)
# get value of the curve first order derivative at t = 0.8
t08Constraint = variableBezier.derivate(0.8, 1)
target = zeros(3)
A, b = genCost(variableBezier, ptsTime)
#solve optimization problem with quadprog
# solve optimization problem with quadprog
res = quadprog_solve_qp(A, b, C=t08Constraint.B(), d=target - t08Constraint.c())
fitBezier = evalAndPlot(variableBezier, res)
#returns a curve composed of the split curves, 2 in our case
# returns a curve composed of the split curves, 2 in our case
piecewiseCurve = ref.split(array([[0.6]]).T)
#displaying the obtained curves
# displaying the obtained curves
#first, split the variable curve
# first, split the variable curve
piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)
constrainedCurve = piecewiseCurve.curve_at_index(1)
#find the number of variables
# find the number of variables
problemSize = prob.numVariables * dim
#find the number of constraints, as many as waypoints
# find the number of constraints, as many as waypoints
nConstraints = constrainedCurve.nbWaypoints
waypoints = constrainedCurve.waypoints()
......@@ -187,17 +163,15 @@ class TestNotebook(unittest.TestCase):
ineqMatrix = zeros((nConstraints, problemSize))
ineqVector = zeros(nConstraints)
#finding the z equation of each control point
# finding the z equation of each control point
for i in range(nConstraints):
wayPoint = constrainedCurve.waypointAtIndex(i)
ineqMatrix[i,:] = wayPoint.B()[2,:]
ineqVector[i] = -wayPoint.c()[2]
res = quadprog_solve_qp(A, b, G=ineqMatrix, h = ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1,1)) )
ineqMatrix[i, :] = wayPoint.B()[2, :]
ineqVector[i] = -wayPoint.c()[2]
res = quadprog_solve_qp(A, b, G=ineqMatrix, h=ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
fitBezier
if __name__ == '__main__':
......
python/test/test.py
View file @ 11c34d91
......@@ -7,9 +7,9 @@ import numpy as np
from numpy import array, array_equal, isclose, random, zeros
from numpy.linalg import norm
from curves import (CURVES_WITH_PINOCCHIO_SUPPORT, Quaternion, SE3Curve, SO3Linear, bezier, bezier3,
cubic_hermite_spline, curve_constraints, exact_cubic,polynomial,
piecewise, piecewise_SE3, convert_to_polynomial,convert_to_bezier,convert_to_hermite)
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)
eigenpy.switchToNumpyArray()
......@@ -17,26 +17,27 @@ if CURVES_WITH_PINOCCHIO_SUPPORT:
from pinocchio import SE3, Motion
class TestCurves(unittest.TestCase):
# def print_str(self, inStr):
# print inStr
# return
def compareCurves(self,c1,c2):
def compareCurves(self, c1, c2):
t_min = c1.min()
t_max = c1.max()
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)))
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)))
t = t_min
while t < t_max:
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
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
def test_bezier(self):
print("test_bezier")
......@@ -350,35 +351,34 @@ class TestCurves(unittest.TestCase):
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
waypoints3 = array([[1., 2., 3.,0.6,-9.], [-1., 1.6, 1.7,6.7,14]]).transpose()
waypoints3 = array([[1., 2., 3., 0.6, -9.], [-1., 1.6, 1.7, 6.7, 14]]).transpose()
c = polynomial(waypoints3, 3., 5.2)
with self.assertRaises(ValueError):# a and c doesn't have the same dimension
pc.append(c)
with self.assertRaises(ValueError): # a and c doesn't have the same dimension
pc.append(c)
### Test the different append methods :
# Test the different append methods :
pc = piecewise()
self.assertEqual(pc.num_curves(),0)
end_point1 = array([1.,3.,5.,6.5,-2.])
self.assertEqual(pc.num_curves(), 0)
end_point1 = array([1., 3., 5., 6.5, -2.])
max1 = 2.5
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)
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)
pc = piecewise()
d = polynomial(waypoints3, 0., 1.2)
self.assertEqual(pc.num_curves(),0)
self.assertEqual(pc.num_curves(), 0)
pc.append(d)
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)
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)
self.assertTrue(pc.is_continuous(0))
self.assertTrue(isclose(pc(0.),d(0.)).all())
self.assertTrue(isclose(pc(max1),end_point1).all())
self.assertTrue(isclose(pc(0.), d(0.)).all())
self.assertTrue(isclose(pc(max1), end_point1).all())
return
......@@ -407,8 +407,7 @@ class TestCurves(unittest.TestCase):
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())
polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative,
time_points)
polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative, time_points)
self.assertEqual(polC2.min(), time_points[0, 0])
self.assertEqual(polC2.max(), time_points[-1, 0])
self.assertTrue(polC2.is_continuous(0))
......@@ -430,8 +429,7 @@ class TestCurves(unittest.TestCase):
polC1 = piecewise.FromPointsList(points, points_derivative, time_points)
with self.assertRaises(ValueError):
polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative,
time_points)
polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative, time_points)
def test_piecewise_bezier_curve(self):
# To test :
......@@ -440,8 +438,8 @@ class TestCurves(unittest.TestCase):
a = bezier(waypoints, 0., 1.)
b = bezier(waypoints, 1., 2.)
pc = piecewise(a)
pc.append(b)
a.split(array([0.4,0.8])).curve_at_index(0)
pc.append(b)
a.split(array([0.4, 0.8])).curve_at_index(0)
pc.min()
pc.max()
pc(0.4)
......@@ -569,133 +567,132 @@ class TestCurves(unittest.TestCase):
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(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)
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)
se3_3 = SE3Curve(se3(max), se3_2(max2 - 0.5), max2, max2 + 1.5)
pc.append(se3_3)
self.assertFalse(pc.is_continuous(0))
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)
se3_3 = SE3Curve(se3(max),se3_2(max2-0.5),max2,max2+1.5)
pc.append(se3_3)
self.assertFalse(pc.is_continuous(0))
### 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)
end_rot = end_quat.matrix()
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)
self.assertEqual(pc.min(), min)
self.assertEqual(pc.max(), max2)
pmin = pc(min)
pmax = pc(max2)
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))
if CURVES_WITH_PINOCCHIO_SUPPORT:
end_quat = Quaternion(sqrt(2.) / 2., 0., 0, sqrt(2.) / 2.)
# 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