[Python] Tests with unittests & coveralls

.coveragerc

+[run]
+source = src,python

.travis.yml

-language: python
+language: cpp
 python:
-- "2.7"
+  - "2.7"
 sudo: required
 compiler:
-- gcc
+  - gcc
 # - clang
 env:
   global:
@@ -17,22 +17,28 @@ env:
     - BUILDTYPE=Minsizerel
 notifications:
 email:
-- hpp-source@laas.fr
+  - hpp-source@laas.fr
 branches:
 only:
-- master
-- debian
-- devel
+  - master
+  - debian
+  - devel
 matrix:
 allow_failures:
-- compiler:
+  - compiler:
+before_install: ./travis_custom/custom_before_install
+install:
+  - pip install --user coveralls
 script:
   - export CMAKE_ADDITIONAL_OPTIONS="-DCMAKE_BUILD_TYPE=${BUILDTYPE}"
   - sudo free -m -t
   - travis_wait ./.travis/run ../travis_custom/custom_build
-  - export PYTHONPATH=/tmp/_ci/install/lib/python2.7/site-packages
-  - python ./python/bindings.py
-  - python ./python/test_model.py
-after_success: ./.travis/run after_success
 after_failure: ./.travis/run after_failure
-before_install: ./travis_custom/custom_before_install
+after_success:
+  - ./.travis/run after_success
+  #- export PYTHONPATH=$install_dir/lib/python2.7/site-packages
+  #- coveralls-lcov -v -n $build_dir/coverage.info > coverage.json
+  - export PYTHONPATH=/tmp/_ci/install/lib/python2.7/site-packages
+  - coveralls-lcov -v -n /tmp/_ci/build/coverage.info > coverage.json
+  - coverage run ./python/tests.py
+  - coveralls --merge=coverage.json

python/bindings.py

-#!/usr/bin/env python
-
 import pinocchio as se3
-from pinocchio.utils import isapprox, np, npl, rand, skew, zero
-
-# --- SE3
-R = rand([3, 3])
-[R, tmp, tmp] = npl.svd(R)
-p = rand(3)
-m = se3.SE3(R, p)
-X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
-assert isapprox(m.action, X)
-M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
-assert isapprox(m.homogeneous, M)
-m2 = se3.SE3.Random()
-assert isapprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
-assert isapprox((~m).homogeneous, npl.inv(m.homogeneous))
-
-p = rand(3)
-assert isapprox(m * p, m.rotation * p + m.translation)
-assert isapprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
-
-p = np.vstack([p, 1])
-assert isapprox(m * p, m.homogeneous * p)
-assert isapprox(m.actInv(p), npl.inv(m.homogeneous) * p)
-
-p = rand(6)
-assert isapprox(m * p, m.action * p)
-assert isapprox(m.actInv(p), npl.inv(m.action) * p)
-
-# --- Motion
-assert isapprox(se3.Motion.Zero().vector, zero(6))
-v = se3.Motion.Random()
-assert isapprox((m * v).vector, m.action * v.vector)
-assert isapprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
-vv = v.linear
-vw = v.angular
-assert isapprox(v.vector, np.vstack([vv, vw]))
-assert isapprox((v ** v).vector, zero(6))
-
-# --- Force ---
-assert isapprox(se3.Force.Zero().vector, zero(6))
-f = se3.Force.Random()
-ff = f.linear
-ft = f.angular
-assert isapprox(f.vector, np.vstack([ff, ft]))
-
-assert isapprox((m * f).vector, npl.inv(m.action.T) * f.vector)
-assert isapprox((m.actInv(f)).vector, m.action.T * f.vector)
-f = se3.Force(np.vstack([v.vector[3:], v.vector[:3]]))
-assert isapprox((v ** f).vector, zero(6))
-
-# --- Inertia ---
-Y1 = se3.Inertia.Random()
-Y2 = se3.Inertia.Random()
-Y = Y1 + Y2
-assert isapprox(Y1.matrix() + Y2.matrix(), Y.matrix())
-assert isapprox((Y * v).vector, Y.matrix() * v.vector)
-assert isapprox((m * Y).matrix(), m.inverse().action.T * Y.matrix() * m.inverse().action)
-assert isapprox((m.actInv(Y)).matrix(), m.action.T * Y.matrix() * m.action)
+from pinocchio.utils import np, npl, rand, skew, zero
+
+from test_case import TestCase
+
+
+class TestSE3(TestCase):
+    def setUp(self):
+        self.R = rand([3, 3])
+        self.R, _, _ = npl.svd(self.R)
+        self.p = rand(3)
+        self.m = se3.SE3(self.R, self.p)
+
+    def test_se3(self):
+        R, p, m = self.R, self.p, self.m
+        X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
+        self.assertApprox(m.action, X)
+        M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
+        self.assertApprox(m.homogeneous, M)
+        m2 = se3.SE3.Random()
+        self.assertApprox((m * m2).homogeneous, m.homogeneous * m2.homogeneous)
+        self.assertApprox((~m).homogeneous, npl.inv(m.homogeneous))
+
+        p = rand(3)
+        self.assertApprox(m * p, m.rotation * p + m.translation)
+        self.assertApprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
+
+        p = np.vstack([p, 1])
+        self.assertApprox(m * p, m.homogeneous * p)
+        self.assertApprox(m.actInv(p), npl.inv(m.homogeneous) * p)
+
+        p = rand(6)
+        self.assertApprox(m * p, m.action * p)
+        self.assertApprox(m.actInv(p), npl.inv(m.action) * p)
+
+        p = rand(5)
+        with self.assertRaises(ValueError):
+            m * p
+        with self.assertRaises(ValueError):
+            m.actInv(p)
+        with self.assertRaises(ValueError):
+            m.actInv('42')
+
+    def test_motion(self):
+        m = self.m
+        self.assertApprox(se3.Motion.Zero().vector, zero(6))
+        v = se3.Motion.Random()
+        self.assertApprox((m * v).vector, m.action * v.vector)
+        self.assertApprox((m.actInv(v)).vector, npl.inv(m.action) * v.vector)
+        vv = v.linear
+        vw = v.angular
+        self.assertApprox(v.vector, np.vstack([vv, vw]))
+        self.assertApprox((v ** v).vector, zero(6))
+
+    def test_force(self):
+        m = self.m
+        self.assertApprox(se3.Force.Zero().vector, zero(6))
+        f = se3.Force.Random()
+        ff = f.linear
+        ft = f.angular
+        self.assertApprox(f.vector, np.vstack([ff, ft]))
+
+        self.assertApprox((m * f).vector, npl.inv(m.action.T) * f.vector)
+        self.assertApprox((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.assertApprox((v ** f).vector, zero(6))
+
+    def test_inertia(self):
+        m = self.m
+        Y1 = se3.Inertia.Random()
+        Y2 = se3.Inertia.Random()
+        Y = Y1 + Y2
+        self.assertApprox(Y1.matrix() + Y2.matrix(), Y.matrix())
+        v = se3.Motion.Random()
+        self.assertApprox((Y * v).vector, Y.matrix() * v.vector)
+        self.assertApprox((m * Y).matrix(), m.inverse().action.T * Y.matrix() * m.inverse().action)
+        self.assertApprox((m.actInv(Y)).matrix(), m.action.T * Y.matrix() * m.action)
+
+    def test_cross(self):
+        m = se3.Motion.Random()
+        f = se3.Force.Random()
+        self.assertApprox(m ** m, m.cross_motion(m))
+        self.assertApprox(m ** f, m.cross_force(f))
+        with self.assertRaises(ValueError):
+            m ** 2
+
+    def test_exp(self):
+        m = se3.Motion.Random()
+        self.assertApprox(se3.exp(m), se3.exp6FromMotion(m))

python/explog.py

+import math
+
+import numpy as np
+import pinocchio as se3
+from pinocchio.explog import exp, log
+
+from test_case import TestCase
+
+
+class TestExpLog(TestCase):
+    def test_explog(self):
+        self.assertApprox(exp(42), math.exp(42))
+        self.assertApprox(log(42), math.log(42))
+        self.assertApprox(exp(log(42)), 42)
+        self.assertApprox(log(exp(42)), 42)
+        m = np.matrix(range(1, 4), np.double).T
+        self.assertApprox(log(exp(m)), m)
+        m = se3.SE3.Random()
+        self.assertApprox(exp(log(m)), m)
+        m = np.matrix([float(i) / 10 for i in range(1, 7)]).T
+        self.assertApprox(log(exp(m)), m)
+        m = np.eye(4)
+        self.assertApprox(exp(log(m)).homogeneous, m)
+        with self.assertRaises(ValueError):
+            exp(np.eye(4))
+        with self.assertRaises(ValueError):
+            exp(range(3))
+        with self.assertRaises(ValueError):
+            log(range(3))
+        with self.assertRaises(ValueError):
+            log(np.zeros(5))

python/model.py

+import pinocchio as se3
+from pinocchio.utils import np, zero
+
+from test_case import TestCase
+
+
+class TestModel(TestCase):
+    def setUp(self):
+        self.model = se3.Model.BuildHumanoidSimple()
+
+    def test_empty_model_sizes(self):
+        model = se3.Model.BuildEmptyModel()
+        self.assertEqual(model.nbody, 1)
+        self.assertEqual(model.nq, 0)
+        self.assertEqual(model.nv, 0)
+
+    def test_model(self):
+        model = self.model
+        nb = 28  # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
+        self.assertEqual(model.nbody, nb)
+        self.assertEqual(model.nq, nb - 1 + 6)
+        self.assertEqual(model.nv, nb - 1 + 5)
+
+    def test_inertias(self):
+        model = self.model
+        model.inertias[1] = model.inertias[2]
+        self.assertApprox(model.inertias[1].np, model.inertias[2].np)
+
+    def test_placements(self):
+        model = self.model
+        model.jointPlacements[1] = model.jointPlacements[2]
+        self.assertApprox(model.jointPlacements[1].np, model.jointPlacements[2].np)
+        self.assertEqual(model.parents[0], 0)
+        self.assertEqual(model.parents[1], 0)
+        model.parents[2] = model.parents[1]
+        self.assertEqual(model.parents[2], model.parents[1])
+        self.assertEqual(model.names[0], "universe")
+
+    def test_gravity(self):
+        self.assertApprox(self.model.gravity.np, np.matrix('0 0 -9.81 0 0 0').T)
+
+    def test_rnea(self):
+        model = self.model
+        data = model.createData()
+
+        q = zero(model.nq)
+        qdot = zero(model.nv)
+        qddot = zero(model.nv)
+        for i in range(model.nbody):
+            data.a[i] = se3.Motion.Zero()
+
+        se3.rnea(model, data, q, qdot, qddot)
+        for i in range(model.nbody):
+            self.assertApprox(data.v[i].np, zero(6))
+        self.assertApprox(data.a_gf[0].np, -model.gravity.np)
+        self.assertApprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])

python/rpy.py

+from math import pi
+
+import numpy as np
+from pinocchio.rpy import matrixToRpy, npToTuple, rotate, rpyToMatrix
+
+from test_case import TestCase
+
+
+class TestRPY(TestCase):
+    def test_npToTuple(self):
+        m = np.matrix(range(9))
+        self.assertEqual(npToTuple(m), tuple(range(9)))
+        self.assertEqual(npToTuple(m.T), tuple(range(9)))
+        self.assertEqual(npToTuple(np.reshape(m, (3, 3))), ((0, 1, 2), (3, 4, 5), (6, 7, 8)))
+
+    def test_rotate(self):
+        self.assertApprox(rotate('x', pi / 2), np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.'))
+        self.assertApprox(rotate('x', pi) * rotate('y', pi), rotate('z', pi))
+        m = rotate('x', pi / 3) * rotate('y', pi / 5) * rotate('y', pi / 7)
+        self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
+        rpy = np.matrix(range(3)).T * pi / 2
+        self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)

python/test_case.py

+import unittest
+
+from pinocchio.utils import isapprox
+
+
+class TestCase(unittest.TestCase):
+    def assertApprox(self, a, b):
+        return self.assertTrue(isapprox(a, b))

python/test_model.py

-#!/usr/bin/env python
-
-import pinocchio as se3
-from pinocchio.utils import isapprox, np, zero
-
-model = se3.Model.BuildEmptyModel()
-assert model.nbody == 1 and model.nq == 0 and model.nv == 0
-
-model = se3.Model.BuildHumanoidSimple()
-nb = 28  # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
-assert model.nbody == nb and model.nq == nb - 1 + 6 and model.nv == nb - 1 + 5
-model.inertias[1] = model.inertias[2]
-assert isapprox(model.inertias[1].np, model.inertias[2].np)
-model.jointPlacements[1] = model.jointPlacements[2]
-assert isapprox(model.jointPlacements[1].np, model.jointPlacements[2].np)
-assert model.parents[0] == 0 and model.parents[1] == 0
-model.parents[2] = model.parents[1]
-assert model.parents[2] == model.parents[1]
-assert model.names[0] == "universe"
-assert isapprox(model.gravity.np, np.matrix('0 0 -9.81 0 0 0').T)
-
-data = model.createData()
-
-
-q = zero(model.nq)
-qdot = zero(model.nv)
-qddot = zero(model.nv)
-for i in range(model.nbody):
-    data.a[i] = se3.Motion.Zero()
-
-se3.rnea(model, data, q, qdot, qddot)
-for i in range(model.nbody):
-    assert isapprox(data.v[i].np, zero(6))
-assert isapprox(data.a_gf[0].np, -model.gravity.np)
-assert isapprox(data.f[-1], model.inertias[-1] * data.a_gf[-1])

python/tests.py

+#!/usr/bin/env python
+
+import unittest
+
+from bindings import TestSE3  # noqa
+from explog import TestExpLog  # noqa
+from model import TestModel  # noqa
+from rpy import TestRPY  # noqa
+from utils import TestUtils  # noqa
+
+if __name__ == '__main__':
+    unittest.main()

python/utils.py

+from math import sqrt
+
+import numpy as np
+import pinocchio as se3
+from pinocchio.utils import (ViewerConfigurationToXYZQUAT, XYZQUATToSe3,
+                             XYZQUATToViewerConfiguration, cross, isapprox, se3ToXYZQUAT)
+
+from test_case import TestCase
+
+
+class TestUtils(TestCase):
+    def test_cross(self):
+        a = np.matrix('2. 0. 0.').T
+        b = np.matrix('0. 3. 0.').T
+        c = np.matrix('0. 0. 6.').T
+        self.assertApprox(cross(a, b), c)
+
+    def test_se3ToXYZQUAT_XYZQUATToSe3(self):
+        m = se3.SE3.Identity()
+        m.translation = np.matrix('1. 2. 3.').T
+        m.rotation = np.matrix('1. 0. 0.;0. 0. -1.;0. 1. 0.')  # rotate('x', pi / 2)
+        self.assertApprox(se3ToXYZQUAT(m), [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
+        self.assertApprox(XYZQUATToSe3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
+
+    # def test_XYZQUATToViewerConfiguration_ViewerConfigurationToXYZQUAT(self):
+        # TODO
+        # v = [float(i + 1) for i in range(7)]
+        # self.assertEqual(XYZQUATToViewerConfiguration(ViewerConfigurationToXYZQUAT(v)), v)
+        # self.assertEqual(ViewerConfigurationToXYZQUAT(XYZQUATToViewerConfiguration(v)), v)
+
+    def test_isapprox(self):
+        self.assertFalse(isapprox(1, 2))
+        self.assertTrue(isapprox(1, 2, 10))
+        self.assertFalse(isapprox([1e10, 1e-7], [1.00001e10, 1e-8]))
+        self.assertTrue(isapprox([1e10, 1e-8], [1.00001e10, 1e-9], 1e-5))

src/python/__init__.py

@@ -37,7 +37,7 @@ def SE3act(m, x):
             return m.homogeneous * x
         if x.shape[0] == 6:
             return m.action * x
-        raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
+        raise ValueError('m can only act on linear object of size 3, 4 and 6.')
     if 'se3Action' in x.__class__.__dict__:
         return x.se3Action(m)
     return m.oldmult(x)
@@ -56,10 +56,10 @@ def SE3actinv(m, x):
             return m.inverse().homogeneous * x
         if x.shape[0] == 6:
             return m.inverse().action * x
-        raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
+        raise ValueError('m can only act on linear object of size 3, 4 and 6.')
     if 'se3Action' in x.__class__.__dict__:
         return x.se3ActionInverse(m)
-    raise ValueError('Error: SE3 cannot act on the given object')
+    raise ValueError('SE3 cannot act on the given object')
 
 setattr(se3.SE3, 'actInv', SE3actinv)
 
@@ -71,7 +71,7 @@ def SE3cross(self, y):
         return self.cross_motion(y)
     if isinstance(y, se3.Force):
         return self.cross_force(y)
-    raise ValueException('Error: SE3 cross product only apply on M6xM6 or M6xF6.')
+    raise ValueError('SE3 cross product only apply on M6xM6 or M6xF6.')
 
 setattr(se3.Motion, '__pow__', SE3cross)
 setattr(se3.Motion, 'cross', SE3cross)

src/python/explog.py

@@ -15,20 +15,22 @@
 # Pinocchio If not, see
 # <http://www.gnu.org/licenses/>.
 
-import libpinocchio_pywrap as se3
-import numpy as np
 import math
 
+import numpy as np
+
+import libpinocchio_pywrap as se3
+
 
 def exp(x):
     if isinstance(x, se3.Motion):
         return se3.exp6FromMotion(x)
     if np.isscalar(x):
         return math.exp(x)
-    if isinstance(x, np.matrix):
-        if len(x) == 6:
+    if isinstance(x, np.ndarray):
+        if x.shape == (6, 1):
             return se3.exp6FromVector(x)
-        if len(x) == 3:
+        if x.shape == (3, 1):
             return se3.exp3(x)
         raise ValueError('Error only 3 and 6 vectors are allowed.')
     raise ValueError('Error exp is only defined for real, vector3, vector6 and se3.Motion objects.')
@@ -39,7 +41,7 @@ def log(x):
         return se3.log6FromSE3(x)
     if np.isscalar(x):
         return math.log(x)
-    if isinstance(x, np.matrix):
+    if isinstance(x, np.ndarray):
         if len(x) == 4:
             return se3.log6FromMatrix(x)
         if len(x) == 3:

src/python/utils.py

@@ -80,7 +80,7 @@ def isapprox(a, b, epsilon=1e-6):
         a = a.np
     if "np" in b.__class__.__dict__:
         b = b.np
-    if isinstance(a, np.ndarray) and isinstance(b, np.ndarray):
+    if isinstance(a, (np.ndarray, list)) and isinstance(b, (np.ndarray, list)):
         return np.allclose(a, b, epsilon)
     return abs(a - b) < epsilon