From bc0c6e281a359b7a678a0a684ea6b5bca9c1c33b Mon Sep 17 00:00:00 2001
From: Valenza Florian <fvalenza@laas.fr>
Date: Fri, 29 Apr 2016 12:27:25 +0200
Subject: [PATCH] [Python] Added test of exposition of spatial classes
---
python/bindings_SE3.py | 96 ++++++++++++++++++++++++++++++
python/bindings_force.py | 74 +++++++++++++++++++++++
python/bindings_frame.py | 44 ++++++++++++++
python/bindings_geometry_object.py | 47 +++++++++++++++
python/bindings_inertia.py | 89 +++++++++++++++++++++++++++
python/bindings_motion.py | 72 ++++++++++++++++++++++
python/tests.py | 6 ++
7 files changed, 428 insertions(+)
create mode 100644 python/bindings_SE3.py
create mode 100644 python/bindings_force.py
create mode 100644 python/bindings_frame.py
create mode 100644 python/bindings_geometry_object.py
create mode 100644 python/bindings_inertia.py
create mode 100644 python/bindings_motion.py
diff --git a/python/bindings_SE3.py b/python/bindings_SE3.py
new file mode 100644
index 000000000..d8430ecfc
--- /dev/null
+++ b/python/bindings_SE3.py
@@ -0,0 +1,96 @@
+import unittest
+import pinocchio as se3
+import numpy as np
+from pinocchio.utils import eye,zero,rand
+
+ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
+
+class TestSE3Bindings(unittest.TestCase):
+
+ v3zero = zero(3)
+ v3ones = ones(3)
+ m3zero = zero([3,3])
+ m3ones = eye(3)
+ m4ones = eye(4)
+
+ def test_identity(self):
+ transform = se3.SE3.Identity()
+ self.assertTrue(np.allclose(self.v3zero,transform.translation))
+ self.assertTrue(np.allclose(self.m3ones, transform.rotation))
+ self.assertTrue(np.allclose(self.m4ones, transform.homogeneous))
+ transform.setRandom()
+ transform.setIdentity()
+ self.assertTrue(np.allclose(self.m4ones, transform.homogeneous))
+
+ def test_get_translation(self):
+ transform = se3.SE3.Identity()
+ self.assertTrue(np.allclose(transform.translation, self.v3zero))
+
+ def test_get_rotation(self):
+ transform = se3.SE3.Identity()
+ self.assertTrue(np.allclose(transform.rotation, self.m3ones))
+
+ def test_set_translation(self):
+ transform = se3.SE3.Identity()
+ transform.translation = self.v3ones
+ self.assertFalse(np.allclose(transform.translation, self.v3zero))
+ self.assertTrue(np.allclose(transform.translation, self.v3ones))
+
+ def test_set_rotation(self):
+ transform = se3.SE3.Identity()
+ transform.rotation = self.m3zero
+ self.assertFalse(np.allclose(transform.rotation, self.m3ones))
+ self.assertTrue(np.allclose(transform.rotation, self.m3zero))
+
+ def test_homogeneous(self):
+ amb = se3.SE3.Random()
+ aMb = amb.homogeneous
+ self.assertTrue(np.allclose(aMb[0:3,0:3], amb.rotation)) # bloc 33 = rotation de amb
+ self.assertTrue(np.allclose(aMb[0:3,3], amb.translation)) # derniere colonne = translation de amb
+
+ def test_inverse(self):
+ amb = se3.SE3.Random()
+ aMb = amb.homogeneous
+ bma = amb.inverse()
+ self.assertTrue(np.allclose(np.linalg.inv(aMb), bma.homogeneous))
+
+ def test_internal_product(self):
+ amb = se3.SE3.Random()
+ aMb = amb.homogeneous
+ bmc = se3.SE3.Random()
+ bMc = bmc.homogeneous
+ amc = amb*bmc
+ cma = amc.inverse()
+ aMc = amc.homogeneous
+
+ self.assertTrue(np.allclose(aMb*bMc, aMc))
+ self.assertTrue(np.allclose(cma.homogeneous,np.linalg.inv(aMc)))
+
+ def test_point_action(self):
+ amb = se3.SE3.Random()
+ aMb = amb.homogeneous
+ p_homogeneous = rand(4)
+ p_homogeneous[3] = 1
+ p = p_homogeneous[0:3].copy()
+
+ # act
+ self.assertTrue(np.allclose(amb.act_point(p),(aMb*p_homogeneous)[0:3]))
+
+ # actinv
+ bMa = np.linalg.inv(aMb)
+ bma = amb.inverse()
+ self.assertTrue(np.allclose(bma.act_point(p), (bMa * p_homogeneous)[0:3]))
+
+
+ def test_action_matrix(self):
+ amb = se3.SE3.Random()
+ bmc = se3.SE3.Random()
+ amc = amb * bmc
+ aXb = amb.action
+ bXc = bmc.action
+ aXc = aXb * bXc
+ self.assertTrue(np.allclose(amc.action,aXc))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/python/bindings_force.py b/python/bindings_force.py
new file mode 100644
index 000000000..6e8a75c81
--- /dev/null
+++ b/python/bindings_force.py
@@ -0,0 +1,74 @@
+import unittest
+import pinocchio as se3
+import numpy as np
+from pinocchio.utils import eye,zero,rand
+
+ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
+
+class TestForceBindings(unittest.TestCase):
+
+ v3zero = zero(3)
+ v6zero = zero(6)
+ v3ones = ones(3)
+ m3zero = zero([3,3])
+ m3ones = eye(3)
+ m4ones = eye(4)
+
+ def test_zero_getters(self):
+ f = se3.Force.Zero()
+ self.assertTrue(np.allclose(self.v3zero, f.linear))
+ self.assertTrue(np.allclose(self.v3zero, f.angular))
+ self.assertTrue(np.allclose(self.v6zero, f.vector))
+
+ def test_setRandom(self):
+ f = se3.Force.Zero()
+ f.setRandom()
+ self.assertFalse(np.allclose(self.v3zero, f.linear))
+ self.assertFalse(np.allclose(self.v3zero, f.angular))
+ self.assertFalse(np.allclose(self.v6zero, f.vector))
+
+ def test_setZero(self):
+ f = se3.Force.Zero()
+ f.setRandom()
+ f.setZero()
+ self.assertTrue(np.allclose(self.v3zero, f.linear))
+ self.assertTrue(np.allclose(self.v3zero, f.angular))
+ self.assertTrue(np.allclose(self.v6zero, f.vector))
+
+ def test_set_linear(self):
+ f = se3.Force.Zero()
+ lin = rand(3) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double)
+ f.linear = lin
+ self.assertTrue(np.allclose(f.linear, lin))
+
+ def test_set_angular(self):
+ f = se3.Force.Zero()
+ ang = rand(3)
+ f.angular = ang
+ self.assertTrue(np.allclose(f.angular, ang))
+
+ def test_set_vector(self):
+ f = se3.Force.Zero()
+ vec = rand(6)
+ f.vector = vec
+ self.assertTrue(np.allclose(f.vector, vec))
+
+ def test_internal_sums(self):
+ f1 = se3.Force.Random()
+ f2 = se3.Force.Random()
+ self.assertTrue(np.allclose((f1+f2).vector,f1.vector + f2.vector))
+ self.assertTrue(np.allclose((f1 - f2).vector, f1.vector - f2.vector))
+
+ def test_se3_action(self):
+ f = se3.Force.Random()
+ m = se3.SE3.Random()
+ self.assertTrue(np.allclose((m * f).vector, np.linalg.inv(m.action.T) * f.vector))
+ self.assertTrue(np.allclose((m.actInv(f)).vector, m.action.T * f.vector))
+ v = se3.Motion.Random()
+ f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
+ self.assertTrue(np.allclose((v ** f).vector, zero(6)))
+
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/python/bindings_frame.py b/python/bindings_frame.py
new file mode 100644
index 000000000..8367945e6
--- /dev/null
+++ b/python/bindings_frame.py
@@ -0,0 +1,44 @@
+import unittest
+import pinocchio as se3
+import numpy as np
+from pinocchio.utils import eye,zero,rand
+from pinocchio.robot_wrapper import RobotWrapper
+
+ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
+
+class TestFrameBindings(unittest.TestCase):
+
+ v3zero = zero(3)
+ v6zero = zero(6)
+ v3ones = ones(3)
+ m3zero = zero([3,3])
+ m6zero = zero([6,6])
+ m3ones = eye(3)
+ m4ones = eye(4)
+
+ hint_list = ["/local/fvalenza/devel/src/pinocchio/models", "wrong/hint"] # hint list
+ robot = RobotWrapper("/local/fvalenza/devel/src/pinocchio/models/romeo.urdf", hint_list, se3.JointModelFreeFlyer())
+
+ def test_name_get_set(self):
+ f = self.robot.model.operational_frames[1]
+ self.assertTrue(f.name == 'r_sole_joint')
+ f.name = 'new_sole_joint'
+ self.assertTrue(f.name == 'new_sole_joint')
+
+ def test_parent_get_set(self):
+ f = self.robot.model.operational_frames[1]
+ self.assertTrue(f.parent == 13)
+ f.parent = 5
+ self.assertTrue(f.parent == 5)
+
+ def test_placement_get_set(self):
+ m = se3.SE3(self.m3ones, np.array([0,0,-0.0684],np.double))
+ new_m = se3.SE3(rand([3,3]), rand(3))
+ f = self.robot.model.operational_frames[1]
+ self.assertTrue(np.allclose(f.placement.homogeneous,m.homogeneous))
+ f.placement = new_m
+ self.assertTrue(np.allclose(f.placement.homogeneous , new_m.homogeneous))
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/python/bindings_geometry_object.py b/python/bindings_geometry_object.py
new file mode 100644
index 000000000..9bbf74eca
--- /dev/null
+++ b/python/bindings_geometry_object.py
@@ -0,0 +1,47 @@
+import unittest
+import pinocchio as se3
+import numpy as np
+from pinocchio.utils import eye,zero,rand
+from pinocchio.robot_wrapper import RobotWrapper
+
+ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
+
+class TestGeometryObjectBindings(unittest.TestCase):
+
+ v3zero = zero(3)
+ v6zero = zero(6)
+ v3ones = ones(3)
+ m3zero = zero([3,3])
+ m6zero = zero([6,6])
+ m3ones = eye(3)
+ m4ones = eye(4)
+
+ hint_list = ["/local/fvalenza/devel/src/pinocchio/models", "wrong/hint"] # hint list
+ robot = RobotWrapper("/local/fvalenza/devel/src/pinocchio/models/romeo.urdf", hint_list, se3.JointModelFreeFlyer())
+
+ def test_name_get_set(self):
+ col = self.robot.geometry_model.collision_objects[1]
+ self.assertTrue(col.name == '') # See in parser why name is empty
+ col.name = 'new_collision_name'
+ self.assertTrue(col.name == 'new_collision_name')
+
+ def test_parent_get_set(self):
+ col = self.robot.geometry_model.collision_objects[1]
+ self.assertTrue(col.parent == 3)
+ col.parent = 5
+ self.assertTrue(col.parent == 5)
+
+ def test_placement_get_set(self):
+ m = se3.SE3(self.m3ones, self.v3zero)
+ new_m = se3.SE3(rand([3,3]), rand(3))
+ col = self.robot.geometry_model.collision_objects[1]
+ self.assertTrue(np.allclose(col.placement.homogeneous,m.homogeneous))
+ col.placement = new_m
+ self.assertTrue(np.allclose(col.placement.homogeneous , new_m.homogeneous))
+
+ def test_meshpath_get(self):
+ col = self.robot.geometry_model.collision_objects[1]
+ self.assertTrue(col.mesh_path == '/local/fvalenza/devel/src/pinocchio/models/meshes/romeo/collision/LHipPitch.dae')
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/python/bindings_inertia.py b/python/bindings_inertia.py
new file mode 100644
index 000000000..9c0062235
--- /dev/null
+++ b/python/bindings_inertia.py
@@ -0,0 +1,89 @@
+import unittest
+import pinocchio as se3
+import numpy as np
+from pinocchio.utils import eye,zero,rand
+
+ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
+
+class TestInertiaBindings(unittest.TestCase):
+
+ v3zero = zero(3)
+ v6zero = zero(6)
+ v3ones = ones(3)
+ m3zero = zero([3,3])
+ m6zero = zero([6,6])
+ m3ones = eye(3)
+ m4ones = eye(4)
+
+ def test_zero_getters(self):
+ Y = se3.Inertia.Zero()
+ self.assertTrue(Y.mass == 0)
+ self.assertTrue(np.allclose(self.v3zero, Y.lever))
+ self.assertTrue(np.allclose(self.m3zero, Y.inertia))
+
+ def test_identity_getters(self):
+ Y = se3.Inertia.Identity()
+ self.assertTrue(Y.mass == 1)
+ self.assertTrue(np.allclose(self.v3zero, Y.lever))
+ self.assertTrue(np.allclose(self.m3ones, Y.inertia))
+
+
+ def test_setRandom(self):
+ Y = se3.Inertia.Identity()
+ Y.setRandom()
+ self.assertFalse(Y.mass == 1)
+ self.assertFalse(np.allclose(self.v3zero, Y.lever))
+ self.assertFalse(np.allclose(self.m3ones, Y.inertia))
+
+ def test_setZero(self):
+ Y = se3.Inertia.Zero()
+ Y.setRandom()
+ Y.setZero()
+ self.assertTrue(Y.mass == 0)
+ self.assertTrue(np.allclose(self.v3zero, Y.lever))
+ self.assertTrue(np.allclose(self.m3zero, Y.inertia))
+
+ def test_setIdentity(self):
+ Y = se3.Inertia.Zero()
+ Y.setIdentity()
+ self.assertTrue(Y.mass == 1)
+ self.assertTrue(np.allclose(self.v3zero, Y.lever))
+ self.assertTrue(np.allclose(self.m3ones, Y.inertia))
+
+ def test_set_mass(self):
+ Y = se3.Inertia.Zero()
+ Y.mass = 10
+ self.assertTrue(np.allclose(Y.mass, 10))
+
+ def test_set_lever(self):
+ Y = se3.Inertia.Zero()
+ lev = rand(3)
+ Y.lever = lev
+ self.assertTrue(np.allclose(Y.lever, lev))
+
+ def test_set_inertia(self):
+ Y = se3.Inertia.Zero()
+ iner = rand([3,3])
+ iner = (iner + iner.T) / 2. # Symmetrize the matrix
+ vec6_iner = np.matrix([iner[0,0],iner[0,1], iner[1,1], iner[0,2], iner[1,2], iner[2,2]], np.double)
+ Y.inertia = vec6_iner
+ self.assertTrue(np.allclose(Y.inertia, iner))
+
+ def test_internal_sums(self):
+ Y1 = se3.Inertia.Random()
+ Y2 = se3.Inertia.Random()
+ Y = Y1 + Y2
+ self.assertTrue(np.allclose(Y1.matrix() + Y2.matrix(), Y.matrix()))
+
+ def test_se3_action(self):
+ m = se3.SE3.Random()
+ Y = se3.Inertia.Random()
+ v = se3.Motion.Random()
+ self.assertTrue(np.allclose((Y * v).vector, Y.matrix() * v.vector))
+ self.assertTrue(np.allclose((m * Y).matrix(), m.inverse().action.T * Y.matrix() * m.inverse().action))
+ self.assertTrue(np.allclose((m.actInv(Y)).matrix(), m.action.T * Y.matrix() * m.action))
+
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/python/bindings_motion.py b/python/bindings_motion.py
new file mode 100644
index 000000000..794fd5e41
--- /dev/null
+++ b/python/bindings_motion.py
@@ -0,0 +1,72 @@
+import unittest
+import pinocchio as se3
+import numpy as np
+from pinocchio.utils import eye,zero,rand
+
+ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
+
+class TestMotionBindings(unittest.TestCase):
+
+ v3zero = zero(3)
+ v6zero = zero(6)
+ v3ones = ones(3)
+ m3zero = zero([3,3])
+ m3ones = eye(3)
+ m4ones = eye(4)
+
+ def test_zero_getters(self):
+ v = se3.Motion.Zero()
+ self.assertTrue(np.allclose(self.v3zero, v.linear))
+ self.assertTrue(np.allclose(self.v3zero, v.angular))
+ self.assertTrue(np.allclose(self.v6zero, v.vector))
+
+ def test_setRandom(self):
+ v = se3.Motion.Zero()
+ v.setRandom()
+ self.assertFalse(np.allclose(self.v3zero, v.linear))
+ self.assertFalse(np.allclose(self.v3zero, v.angular))
+ self.assertFalse(np.allclose(self.v6zero, v.vector))
+
+ def test_setZero(self):
+ v = se3.Motion.Zero()
+ v.setRandom()
+ v.setZero()
+ self.assertTrue(np.allclose(self.v3zero, v.linear))
+ self.assertTrue(np.allclose(self.v3zero, v.angular))
+ self.assertTrue(np.allclose(self.v6zero, v.vector))
+
+ def test_set_linear(self):
+ v = se3.Motion.Zero()
+ lin = rand(3) # TODO np.matrix([1,2,3],np.double) OR np.matrix( np.array([1,2,3], np.double), np.double)
+ v.linear = lin
+ self.assertTrue(np.allclose(v.linear, lin))
+
+ def test_set_angular(self):
+ v = se3.Motion.Zero()
+ ang = rand(3)
+ v.angular = ang
+ self.assertTrue(np.allclose(v.angular, ang))
+
+ def test_set_vector(self):
+ v = se3.Motion.Zero()
+ vec = rand(6)
+ v.vector = vec
+ self.assertTrue(np.allclose(v.vector, vec))
+
+ def test_internal_sums(self):
+ v1 = se3.Motion.Random()
+ v2 = se3.Motion.Random()
+ self.assertTrue(np.allclose((v1+v2).vector,v1.vector + v2.vector))
+ self.assertTrue(np.allclose((v1 - v2).vector, v1.vector - v2.vector))
+
+ def test_se3_action(self):
+ m = se3.SE3.Random()
+ v = se3.Motion.Random()
+ self.assertTrue(np.allclose((m * v).vector, m.action * v.vector))
+ self.assertTrue(np.allclose((m.actInv(v)).vector, np.linalg.inv(m.action) * v.vector))
+ self.assertTrue(np.allclose((v ** v).vector, zero(6)))
+
+
+
+if __name__ == '__main__':
+ unittest.main()
diff --git a/python/tests.py b/python/tests.py
index 78173ce84..156c8f2aa 100755
--- a/python/tests.py
+++ b/python/tests.py
@@ -3,6 +3,12 @@
import unittest, sys
from bindings import TestSE3 # noqa
+from bindings_SE3 import TestSE3Bindings
+from bindings_force import TestForceBindings
+from bindings_motion import TestMotionBindings
+from bindings_inertia import TestInertiaBindings
+from bindings_frame import TestFrameBindings
+from bindings_geometry_object import TestGeometryObjectBindings
from explog import TestExpLog # noqa
from model import TestModel # noqa
from rpy import TestRPY # noqa
--
GitLab