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
......@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from .curves import *
from .curves import * # noqa
......@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from .curves.optimization import *
from .curves.optimization import * # noqa
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
......
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
class TestNotebook(unittest.TestCase):
# def print_str(self, inStr):
# print inStr
......@@ -32,64 +22,57 @@ 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
# 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
# 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])
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)
pD.degree = refDegree - 1
problem = setup_control_points(pD)
variableBezier = problem.bezier()
......@@ -97,32 +80,29 @@ class TestNotebook(unittest.TestCase):
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
# 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)
# 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,:]
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)) )
res = quadprog_solve_qp(A, b, G=ineqMatrix, h=ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
fitBezier
if __name__ == '__main__':
......
......@@ -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
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(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.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 :
......@@ -441,7 +439,7 @@ class TestCurves(unittest.TestCase):
b = bezier(waypoints, 1., 2.)
pc = piecewise(a)
pc.append(b)
a.split(array([0.4,0.8])).curve_at_index(0)
a.split(array([0.4, 0.8])).curve_at_index(0)
pc.min()
pc.max()
pc(0.4)
......@@ -580,7 +578,7 @@ class TestCurves(unittest.TestCase):
# test with bezier
se3 = SE3Curve(translation, init_rot, end_rot)
pc = piecewise_SE3(se3)
self.assertEqual(pc.num_curves(),1)
self.assertEqual(pc.num_curves(), 1)
self.assertEqual(pc.min(), min)
self.assertEqual(pc.max(), max)
pmin = pc(min)
......@@ -590,11 +588,11 @@ class TestCurves(unittest.TestCase):
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])
end_pos2 = array([-2, 0.2, 1.6])
max2 = 2.7
se3_2 = SE3Curve(translation(max),end_pos2,end_rot,end_rot,max,max2)
se3_2 = SE3Curve(translation(max), end_pos2, end_rot, end_rot, max, max2)
pc.append(se3_2)
self.assertEqual(pc.num_curves(),2)
self.assertEqual(pc.num_curves(), 2)
pmin2 = pc(max)
pmax2 = pc(max2)
self.assertTrue(isclose(pmin2[:3, :3], end_rot).all())
......@@ -606,33 +604,32 @@ class TestCurves(unittest.TestCase):
# 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)
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)
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)
self.compareCurves(pc, pc_txt)
pc.saveAsXML("serialization_curve.xml","pc")
pc.saveAsXML("serialization_curve.xml", "pc")
pc_xml = piecewise_SE3()
pc_xml.loadFromXML("serialization_curve.xml","pc")
self.compareCurves(pc,pc_xml)
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)
self.compareCurves(pc, pc_bin)
se3_3 = SE3Curve(se3(max),se3_2(max2-0.5),max2,max2+1.5)
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 :
# 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()
......@@ -644,9 +641,9 @@ class TestCurves(unittest.TestCase):
# test with bezier
se3 = SE3Curve(translation, init_rot, end_rot)
pc = piecewise_SE3()
self.assertEqual(pc.num_curves(),0)
self.assertEqual(pc.num_curves(), 0)
pc.append(se3)
self.assertEqual(pc.num_curves(),1)
self.assertEqual(pc.num_curves(), 1)
self.assertEqual(pc.min(), min)
self.assertEqual(pc.max(), max)
pmin = pc(min)
......@@ -663,8 +660,8 @@ class TestCurves(unittest.TestCase):
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)
pc.append(end_pose, max2)
self.assertEqual(pc.num_curves(), 2)
self.assertEqual(pc.min(), min)
self.assertEqual(pc.max(), max2)
pmin = pc(min)
......@@ -682,8 +679,8 @@ class TestCurves(unittest.TestCase):
end_pose.rotation = end_rot
end_pose.translation = end_translation
max3 = 6.5
pc.append(end_pose,max3)
self.assertEqual(pc.num_curves(),3)
pc.append(end_pose, max3)
self.assertEqual(pc.num_curves(), 3)
self.assertEqual(pc.min(), min)
self.assertEqual(pc.max(), max3)
pmin = pc(min)
......@@ -695,7 +692,7 @@ class TestCurves(unittest.TestCase):
self.assertTrue(pc.is_continuous(0))
pc = piecewise_SE3()
with self.assertRaises(RuntimeError):
pc.append(end_pose,max)
pc.append(end_pose, max)
if CURVES_WITH_PINOCCHIO_SUPPORT:
......@@ -717,7 +714,7 @@ class TestCurves(unittest.TestCase):
max = 12.
se3 = SE3Curve(init_pose, end_pose, min, max)
pc = piecewise_SE3(se3)
self.assertEqual(pc.num_curves(),1)
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))
......@@ -736,7 +733,7 @@ class TestCurves(unittest.TestCase):
max2 = 23.9
se3_2 = SE3Curve(end_pose, end_pose2, max, max2)
pc.append(se3_2)
self.assertEqual(pc.num_curves(),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))
......@@ -745,9 +742,7 @@ class TestCurves(unittest.TestCase):
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()
a = bezier(waypoints, 0., 1.)
b = bezier(waypoints, 1., 2.)
......@@ -821,11 +816,11 @@ class TestCurves(unittest.TestCase):
with self.assertRaises(ValueError):
so3Rot.derivate(1., 0)
with self.assertRaises(ValueError):
test = SO3Linear(init_rot,end_rot,max,min)
SO3Linear(init_rot, end_rot, max, min)
def test_so3_linear_serialization(self):
print("test SO3 Linear")
init_quat = Quaternion( 0.590,-0.002, -0.766, 0.255)
init_quat = Quaternion(0.590, -0.002, -0.766, 0.255)
end_quat = Quaternion(-0.820, 0.162, 0.381, 0.396)
init_quat.normalize()
end_quat.normalize()
......@@ -837,18 +832,17 @@ class TestCurves(unittest.TestCase):
so3Rot.saveAsText("serialization_curve.txt")
so3_txt = SO3Linear()
so3_txt.loadFromText("serialization_curve.txt")
self.compareCurves(so3Rot,so3_txt)
self.compareCurves(so3Rot, so3_txt)
so3Rot.saveAsXML("serialization_curve.xml","so3")
so3Rot.saveAsXML("serialization_curve.xml", "so3")
so3_xml = SO3Linear()
so3_xml.loadFromXML("serialization_curve.xml","so3")
self.compareCurves(so3Rot,so3_xml)
so3_xml.loadFromXML("serialization_curve.xml", "so3")
self.compareCurves(so3Rot, so3_xml)
so3Rot.saveAsBinary("serialization_curve")
so3_bin = SO3Linear()
so3_bin.loadFromBinary("serialization_curve")
self.compareCurves(so3Rot,so3_bin)
self.compareCurves(so3Rot, so3_bin)
def test_se3_curve_linear(self):
print("test SE3 Linear")
......@@ -917,7 +911,7 @@ class TestCurves(unittest.TestCase):
with self.assertRaises(ValueError):
se3.derivate(1., 0)
with self.assertRaises(ValueError):
test = SE3Curve(init_pose,end_pose,max,min)
SE3Curve(init_pose, end_pose, max, min)
def test_se3_from_translation_curve(self):
print("test SE3 From translation curves")
......@@ -1126,7 +1120,7 @@ class TestCurves(unittest.TestCase):
def test_se3_serialization(self):
print("test serialization SE3")
init_quat = Quaternion( 0.590,-0.002, -0.766, 0.255)
init_quat = Quaternion(0.590, -0.002, -0.766, 0.255)
end_quat = Quaternion(-0.820, 0.162, 0.381, 0.396)
init_quat.normalize()
end_quat.normalize()
......@@ -1147,19 +1141,19 @@ class TestCurves(unittest.TestCase):
se3_linear.saveAsText("serialization_curve.txt")
se3_txt = SE3Curve()
se3_txt.loadFromText("serialization_curve.txt")
self.compareCurves(se3_linear,se3_txt)
self.compareCurves(se3_linear, se3_txt)
se3_linear.saveAsXML("serialization_curve.xml","se3")
se3_linear.saveAsXML("serialization_curve.xml", "se3")
se3_xml = SE3Curve()
se3_xml.loadFromXML("serialization_curve.xml","se3")
self.compareCurves(se3_linear,se3_xml)
se3_xml.loadFromXML("serialization_curve.xml", "se3")
self.compareCurves(se3_linear, se3_xml)