[pythontest] Fix python unit tests

unittest/python/bindings_SE3.py

@@ -7,40 +7,35 @@ ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.doub
 
 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))
+        self.assertTrue(np.allclose(zero(3),transform.translation))
+        self.assertTrue(np.allclose(eye(3), transform.rotation))
+        self.assertTrue(np.allclose(eye(4), transform.homogeneous))
+        self.assertTrue(np.allclose(eye(6), transform.action))
         transform.setRandom()
         transform.setIdentity()
-        self.assertTrue(np.allclose(self.m4ones, transform.homogeneous))
+        self.assertTrue(np.allclose(eye(4), transform.homogeneous))
 
     def test_get_translation(self):
         transform = se3.SE3.Identity()
-        self.assertTrue(np.allclose(transform.translation, self.v3zero))
+        self.assertTrue(np.allclose(transform.translation, zero(3)))
 
     def test_get_rotation(self):
         transform = se3.SE3.Identity()
-        self.assertTrue(np.allclose(transform.rotation, self.m3ones))
+        self.assertTrue(np.allclose(transform.rotation, eye(3)))
 
     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))
+        transform.translation = ones(3)
+        self.assertFalse(np.allclose(transform.translation, zero(3)))
+        self.assertTrue(np.allclose(transform.translation, ones(3)))
 
     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))
+        transform.rotation = zero([3,3])
+        self.assertFalse(np.allclose(transform.rotation, eye(3)))
+        self.assertTrue(np.allclose(transform.rotation, zero([3,3])))
 
     def test_homogeneous(self):
         amb = se3.SE3.Random()
@@ -56,7 +51,7 @@ class TestSE3Bindings(unittest.TestCase):
         self.assertTrue(np.allclose(aXb[3:,3:], amb.rotation)) # bottom right 33 corner = rotation of amb
         tblock = skew(amb.translation)*amb.rotation
         self.assertTrue(np.allclose(aXb[:3,3:], tblock))       # top right 33 corner = translation + rotation
-        self.assertTrue(np.allclose(aXb[3:,:3], self.m3zero))  # bottom left 33 corner = 0
+        self.assertTrue(np.allclose(aXb[3:,:3], zero([3,3])))  # bottom left 33 corner = 0
 
     def test_inverse(self):
         amb = se3.SE3.Random()

unittest/python/bindings_force.py

 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)
+from pinocchio.utils import zero,rand
 
 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))
+        self.assertTrue(np.allclose(zero(3), f.linear))
+        self.assertTrue(np.allclose(zero(3), f.angular))
+        self.assertTrue(np.allclose(zero(6), f.vector))
 
     # TODO: this is not nice, since a random vector can *in theory* be zero
     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))
+        self.assertFalse(np.allclose(zero(3), f.linear))
+        self.assertFalse(np.allclose(zero(3), f.angular))
+        self.assertFalse(np.allclose(zero(6), 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))
+        self.assertTrue(np.allclose(zero(3), f.linear))
+        self.assertTrue(np.allclose(zero(3), f.angular))
+        self.assertTrue(np.allclose(zero(6), f.vector))
 
     def test_set_linear(self):
         f = se3.Force.Zero()

unittest/python/bindings_frame.py

 import unittest
 import pinocchio as se3
 import numpy as np
-from pinocchio.utils import eye,zero,rand
-import os
-
-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)
-
     def setUp(self):
-        self.model = se3.Model.BuildHumanoidSimple()
+        self.model = se3.buildSampleModelHumanoidRandom()
         self.parent_idx = self.model.getJointId("rarm2_joint") if self.model.existJointName("rarm2_joint") else (self.model.njoints-1)
         self.frame_name = self.model.names[self.parent_idx] + "_frame"
         self.frame_placement = se3.SE3.Random()

unittest/python/bindings_geometry_object.py

 import unittest
 import pinocchio as se3
 import numpy as np
-from pinocchio.utils import eye,zero,rand
-import os
-
-ones = lambda n: np.matrix(np.ones([n, 1] if isinstance(n, int) else n), np.double)
-
-# TODO: remove these lines. Do not use RobotWrapper. Do not use URDF
-from pinocchio.robot_wrapper import RobotWrapper
-current_file =  os.path.dirname(os.path.abspath(__file__))
-romeo_model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
-romeo_model_path = os.path.abspath(os.path.join(romeo_model_dir, 'romeo_description/urdf/romeo.urdf'))
-hint_list = [romeo_model_dir, "wrong/hint"] # hint list
-robot = RobotWrapper(romeo_model_path, hint_list, se3.JointModelFreeFlyer())
-expected_mesh_path = os.path.join(romeo_model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
 
 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)
-
-    # WARNING: the collision model is the same object is the same for all the tests.
-    # This can cause problems if a test expects the collision model to be in a certain way but a previous test has changed it
-    # Still, at the moment this is not the case.
-    # Loading the URDF and related meshes before each test would make the whole unit test run too slowly
-    # 
-    # TODO: do not use URDF. Then, build self.collision_model from scratch for each test
     def setUp(self):
-        self.collision_model = robot.collision_model
-        self.expected_mesh_path = expected_mesh_path
+        model = se3.buildSampleModelHumanoid()
+        self.collision_model = se3.buildSampleGeometryModelHumanoid(model)
 
     def test_name_get_set(self):
-        col = self.collision_model.geometryObjects[1]
-        self.assertTrue(col.name == 'LHipPitchLink_0')
+        col = self.collision_model.geometryObjects[0]
+        self.assertTrue(col.name == 'rlegshoulder_object')
         col.name = 'new_collision_name'
         self.assertTrue(col.name == 'new_collision_name')
 
     def test_parent_get_set(self):
-        col = self.collision_model.geometryObjects[1]
-        self.assertTrue(col.parentJoint == 4)
-        col.parentJoint = 5
-        self.assertTrue(col.parentJoint == 5)
+        col = self.collision_model.geometryObjects[0]
+        self.assertTrue(col.parentJoint == 2)
+        col.parentJoint = 3
+        self.assertTrue(col.parentJoint == 3)
 
     def test_placement_get_set(self):
-        m = se3.SE3(self.m3ones, self.v3zero)
-        new_m = se3.SE3(rand([3,3]), rand(3))
-        col = self.collision_model.geometryObjects[1]
+        m = se3.SE3.Identity()
+        new_m = se3.SE3.Random()
+        col = self.collision_model.geometryObjects[0]
         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.collision_model.geometryObjects[1]
-        self.assertTrue(col.meshPath == self.expected_mesh_path)
+        col = self.collision_model.geometryObjects[0]
+        self.assertTrue(col.meshPath == "")
 
 if __name__ == '__main__':
     unittest.main()

unittest/python/bindings_geometry_object_urdf.py

+import unittest
+import pinocchio as se3
+import os
+
+class TestGeometryObjectUrdfBindings(unittest.TestCase):
+
+    def test_load(self):
+        current_file =  os.path.dirname(os.path.abspath(__file__))
+        model_dir = os.path.abspath(os.path.join(current_file, '../../models/romeo'))
+        model_path = os.path.abspath(os.path.join(model_dir, 'romeo_description/urdf/romeo.urdf'))
+        expected_mesh_path = os.path.join(model_dir,'romeo_description/meshes/V1/collision/LHipPitch.dae')
+
+        hint_list = [model_dir, "wrong/hint"]
+        model = se3.buildModelFromUrdf(model_path, se3.JointModelFreeFlyer())
+        collision_model = se3.buildGeomFromUrdf(model, model_path, se3.utils.fromListToVectorOfString(hint_list), se3.GeometryType.COLLISION)
+
+        col = collision_model.geometryObjects[1]
+        self.assertTrue(col.meshPath == expected_mesh_path)
+
+if __name__ == '__main__':
+    unittest.main()

unittest/python/bindings_inertia.py

@@ -3,52 +3,42 @@ 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))
+        self.assertTrue(np.allclose(zero(3), Y.lever))
+        self.assertTrue(np.allclose(zero([3,3]), 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))
+        self.assertTrue(np.allclose(zero(3), Y.lever))
+        self.assertTrue(np.allclose(eye(3), Y.inertia))
 
     # TODO: this is not nice, since a random matrix can *in theory* be unity
     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))
+        self.assertFalse(np.allclose(zero(3), Y.lever))
+        self.assertFalse(np.allclose(eye(3), 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))
+        self.assertTrue(np.allclose(zero(3), Y.lever))
+        self.assertTrue(np.allclose(zero([3,3]), 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))
+        self.assertTrue(np.allclose(zero(3), Y.lever))
+        self.assertTrue(np.allclose(eye(3), Y.inertia))
 
     def test_set_mass(self):
         Y = se3.Inertia.Zero()

unittest/python/bindings_motion.py

 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)
+from pinocchio.utils import zero,rand
 
 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))
+        self.assertTrue(np.allclose(zero(3), v.linear))
+        self.assertTrue(np.allclose(zero(3), v.angular))
+        self.assertTrue(np.allclose(zero(6), v.vector))
 
     # TODO: this is not nice, since a random vector can *in theory* be zero
     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))
+        self.assertFalse(np.allclose(zero(3), v.linear))
+        self.assertFalse(np.allclose(zero(3), v.angular))
+        self.assertFalse(np.allclose(zero(6), 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))
+        self.assertTrue(np.allclose(zero(3), v.linear))
+        self.assertTrue(np.allclose(zero(3), v.angular))
+        self.assertTrue(np.allclose(zero(6), v.vector))
 
     def test_set_linear(self):
         v = se3.Motion.Zero()

unittest/python/model.py

@@ -6,10 +6,10 @@ from test_case import TestCase
 
 class TestModel(TestCase):
     def setUp(self):
-        self.model = se3.Model.BuildHumanoidSimple()
+        self.model = se3.buildSampleModelHumanoidRandom()
 
     def test_empty_model_sizes(self):
-        model = se3.Model.BuildEmptyModel()
+        model = se3.Model()
         self.assertEqual(model.nbodies, 1)
         self.assertEqual(model.nq, 0)
         self.assertEqual(model.nv, 0)

unittest/python/tests.py

@@ -10,7 +10,8 @@ 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  # noqa. Needs to remove romeo
+from bindings_geometry_object import TestGeometryObjectBindings
+from bindings_geometry_object_urdf import TestGeometryObjectUrdfBindings
 from explog import TestExpLog
 from model import TestModel
 from rpy import TestRPY