Skip to content
Snippets Groups Projects
Commit c9cdc923 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[test][python] adapt SE3 tests to use of pinocchio objects

parent 386d89c3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 181 additions and 39 deletions
python/test/test.py
+ 181
− 39
View file @ c9cdc923
......@@ -13,6 +13,14 @@ from curves import (bezier_from_hermite, bezier_from_polynomial, hermite_from_po
piecewise_polynomial_curve, polynomial,SO3Linear,SE3Curve,Quaternion
)
try:
from pinocchio import SE3,Motion
CURVES_WITH_PINOCCHIO_SUPPORT=True
except ImportError:
CURVES_WITH_PINOCCHIO_SUPPORT=False
pass
class TestCurves(unittest.TestCase):
# def print_str(self, inStr):
# print inStr
......@@ -632,6 +640,7 @@ class TestCurves(unittest.TestCase):
pass
def test_se3_curve_linear(self):
print "test SE3 Linear"
init_quat = Quaternion.Identity()
end_quat = Quaternion(sqrt(2.)/2.,sqrt(2.)/2.,0,0)
init_rot = init_quat.matrix()
......@@ -648,27 +657,44 @@ class TestCurves(unittest.TestCase):
max = 1.5
se3 = SE3Curve(init_pose,end_pose,min,max)
p = se3(min)
self.assertEqual(p.shape[0],4)
self.assertEqual(p.shape[1],4)
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isinstance(p,SE3))
init_pose = SE3(init_pose)
end_pose = SE3(end_pose)
self.assertTrue(se3(min).isApprox(init_pose,1e-6))
self.assertTrue(se3(max).isApprox(end_pose,1e-6))
else:
self.assertEqual(p.shape[0],4)
self.assertEqual(p.shape[1],4)
self.assertTrue(isclose(se3(min),init_pose).all())
self.assertTrue(isclose(se3(max),end_pose).all())
self.assertEqual(se3.min(),min)
self.assertEqual(se3.max(),max)
self.assertTrue(isclose(se3(min),init_pose).all())
self.assertTrue(isclose(se3(max),end_pose).all())
self.assertTrue(isclose(se3.rotation(min),init_rot).all())
self.assertTrue(isclose(se3.translation(min),init_translation).all())
self.assertTrue(isclose(se3.rotation(max),end_rot).all())
self.assertTrue(isclose(se3.translation(max),end_translation).all())
# check value of derivative (should be constant here)
d = se3.derivate(min,1)
self.assertEqual(d.shape[0],6)
self.assertEqual(d.shape[1],1)
self.assertTrue(isclose(d[0:3],((end_translation-init_translation)/(max-min))).all())
self.assertTrue(isclose(d[3],1.20830487))
self.assertTrue(isclose(d[4:6],matrix([0,0]).T).all())
self.assertTrue(isclose(d,se3.derivate(0.5,1)).all())
self.assertTrue(isclose(d,se3.derivate(max,1)).all())
self.assertTrue(isclose(se3.derivate(min,2),matrix(zeros(6)).T).all())
self.assertTrue(isclose(se3.derivate(min,3),matrix(zeros(6)).T).all())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isinstance(d,Motion))
self.assertTrue(isclose(d.linear,((end_translation-init_translation)/(max-min))).all())
self.assertTrue(isclose(d.angular[0],1.20830487))
self.assertTrue(isclose(d.angular[1:3],matrix([0,0]).T).all())
self.assertTrue(d.isApprox(se3.derivate(0.5,1),1e-6))
self.assertTrue(d.isApprox(se3.derivate(max,1),1e-6))
self.assertTrue(se3.derivate(min,2).isApprox(Motion.Zero(),1e-6))
self.assertTrue(se3.derivate(min,3).isApprox(Motion.Zero(),1e-6))
else:
self.assertEqual(d.shape[0],6)
self.assertEqual(d.shape[1],1)
self.assertTrue(isclose(d[0:3],((end_translation-init_translation)/(max-min))).all())
self.assertTrue(isclose(d[3],1.20830487))
self.assertTrue(isclose(d[4:6],matrix([0,0]).T).all())
self.assertTrue(isclose(d,se3.derivate(0.5,1)).all())
self.assertTrue(isclose(d,se3.derivate(max,1)).all())
self.assertTrue(isclose(se3.derivate(min,2),matrix(zeros(6)).T).all())
self.assertTrue(isclose(se3.derivate(min,3),matrix(zeros(6)).T).all())
# check that errors are correctly raised when necessary :
try:
se3(0.)
......@@ -702,6 +728,7 @@ class TestCurves(unittest.TestCase):
pass
def test_se3_from_translation_curve(self):
print "test SE3 From translation curves"
init_quat = Quaternion.Identity()
end_quat = Quaternion(sqrt(2.)/2.,sqrt(2.)/2.,0,0)
init_rot = init_quat.matrix()
......@@ -716,14 +743,24 @@ class TestCurves(unittest.TestCase):
self.assertEqual(se3.max(),max)
pmin = se3(min)
pmax = se3(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())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(pmin.rotation,init_rot).all())
self.assertTrue(isclose(pmax.rotation,end_rot).all())
self.assertTrue(isclose(pmin.translation,translation(min)).all())
self.assertTrue(isclose(pmax.translation,translation(max)).all())
else :
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())
t = min
while t < max:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(se3(t).translation,translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1).linear,translation.derivate(t,1)).all())
else:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
t += 0.02
# test with bezier3
......@@ -733,14 +770,24 @@ class TestCurves(unittest.TestCase):
self.assertEqual(se3.max(),max)
pmin = se3(min)
pmax = se3(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())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(pmin.rotation,init_rot).all())
self.assertTrue(isclose(pmax.rotation,end_rot).all())
self.assertTrue(isclose(pmin.translation,translation(min)).all())
self.assertTrue(isclose(pmax.translation,translation(max)).all())
else :
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())
t = min
while t < max:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(se3(t).translation,translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1).linear,translation.derivate(t,1)).all())
else:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
t += 0.02
# test with piecewise polynomial
......@@ -758,17 +805,28 @@ class TestCurves(unittest.TestCase):
self.assertEqual(se3.max(),max)
pmin = se3(min)
pmax = se3(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())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(pmin.rotation,init_rot).all())
self.assertTrue(isclose(pmax.rotation,end_rot).all())
self.assertTrue(isclose(pmin.translation,translation(min)).all())
self.assertTrue(isclose(pmax.translation,translation(max)).all())
else :
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())
t = min
while t < max:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(se3(t).translation,translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1).linear,translation.derivate(t,1)).all())
else:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
t += 0.02
def test_se3_from_curves(self):
print "test SE3 from curves"
init_quat = Quaternion.Identity()
end_quat = Quaternion(sqrt(2.)/2.,sqrt(2.)/2.,0,0)
init_rot = init_quat.matrix()
......@@ -783,16 +841,28 @@ class TestCurves(unittest.TestCase):
self.assertEqual(se3.max(),max)
pmin = se3(min)
pmax = se3(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())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(pmin.rotation,init_rot).all())
self.assertTrue(isclose(pmax.rotation,end_rot).all())
self.assertTrue(isclose(pmin.translation,translation(min)).all())
self.assertTrue(isclose(pmax.translation,translation(max)).all())
else :
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())
t = min
while t < max:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3(t)[0:3,0:3],rotation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
self.assertTrue(isclose(se3.derivate(t,1)[3:6],rotation.derivate(t,1)).all())
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isclose(se3(t).translation,translation(t)).all())
self.assertTrue(isclose(se3(t).rotation,rotation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1).linear,translation.derivate(t,1)).all())
self.assertTrue(isclose(se3.derivate(t,1).angular,rotation.derivate(t,1)).all())
else:
self.assertTrue(isclose(se3(t)[0:3,3],translation(t)).all())
self.assertTrue(isclose(se3(t)[0:3,0:3],rotation(t)).all())
self.assertTrue(isclose(se3.derivate(t,1)[0:3],translation.derivate(t,1)).all())
self.assertTrue(isclose(se3.derivate(t,1)[3:6],rotation.derivate(t,1)).all())
t += 0.02
# check if errors are correctly raised :
......@@ -821,5 +891,77 @@ class TestCurves(unittest.TestCase):
except:
pass
if CURVES_WITH_PINOCCHIO_SUPPORT:
def test_se3_curve_linear_pinocchio(self):
print "test SE3 Linear 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 = matrix([0.2,-0.7,0.6]).T
end_translation = matrix([3.6,-2.2,-0.9]).T
init_pose = SE3.Identity()
end_pose = SE3.Identity()
init_pose.rotation = init_rot
end_pose.rotation = end_rot
init_pose.translation = init_translation
end_pose.translation = end_translation
min = 0.7
max = 12.
se3 = SE3Curve(init_pose,end_pose,min,max)
p = se3(min)
self.assertTrue(isinstance(p,SE3))
self.assertTrue(se3(min).isApprox(init_pose,1e-6))
self.assertTrue(se3(max).isApprox(end_pose,1e-6))
self.assertEqual(se3.min(),min)
self.assertEqual(se3.max(),max)
self.assertTrue(isclose(se3.rotation(min),init_rot).all())
self.assertTrue(isclose(se3.translation(min),init_translation).all())
self.assertTrue(isclose(se3.rotation(max),end_rot).all())
self.assertTrue(isclose(se3.translation(max),end_translation).all())
# check value of derivative (should be constant here)
d = se3.derivate(min,1)
self.assertTrue(isinstance(d,Motion))
self.assertTrue(isclose(d.linear,((end_translation-init_translation)/(max-min))).all())
self.assertTrue(isclose(d.angular[0],0.139009))
self.assertTrue(isclose(d.angular[1:3],matrix([0,0]).T).all())
self.assertTrue(d.isApprox(se3.derivate((min + max)/2.,1),1e-6))
self.assertTrue(d.isApprox(se3.derivate(max,1),1e-6))
self.assertTrue(se3.derivate(min,2).isApprox(Motion.Zero(),1e-6))
self.assertTrue(se3.derivate(min,3).isApprox(Motion.Zero(),1e-6))
# check that errors are correctly raised when necessary :
try:
se3(0.)
self.assertTrue(False)
except:
pass
try:
se3(-0.1)
self.assertTrue(False)
except:
pass
try:
se3(3)
self.assertTrue(False)
except:
pass
try:
se3.derivate(0,1)
self.assertTrue(False)
except:
pass
try:
se3.derivate(3.,1)
self.assertTrue(False)
except:
pass
try:
se3.derivate(1.,0)
self.assertTrue(False)
except:
pass
if __name__ == '__main__':
unittest.main()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment