Commit 30bf4e90 authored by Guilhem Saurel's avatar Guilhem Saurel

[Python] Tests with unittests & coveralls

parent de18e056
[run]
source = src,python
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
#!/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))
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))
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])
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)
import unittest
from pinocchio.utils import isapprox
class TestCase(unittest.TestCase):
def assertApprox(self, a, b):
return self.assertTrue(isapprox(a, b))
#!/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])
#!/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()
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))
......@@ -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)
......@@ -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:
......
......@@ -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
......
Markdown is supported
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