Commit 9ddb2d4f authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub

Merge pull request #10 from nim65s/devel

fix python tests
parents 68fc845b 6616a103
......@@ -3,13 +3,12 @@ project(spline)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/test.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/hpp.cmake)
SET(PROJECT_ORG humanoid-path-planner)
SET(PROJECT_NAME hpp-spline)
SET(PROJECT_DESCRIPTION
"template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics."
)
SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
"template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics."
)
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
......@@ -18,24 +17,23 @@ SET(CMAKE_VERBOSE_MAKEFILE True)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
SETUP_PROJECT()
SETUP_HPP_PROJECT()
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
# search for python
FINDPYTHON(2.7 REQUIRED)
find_package( PythonLibs 2.7 REQUIRED )
include_directories( ${PYTHON_INCLUDE_DIRS} )
# search for python
FINDPYTHON(2.7 REQUIRED)
find_package( PythonLibs 2.7 REQUIRED )
include_directories( ${PYTHON_INCLUDE_DIRS} )
find_package( Boost COMPONENTS python REQUIRED )
include_directories( ${Boost_INCLUDE_DIR} )
add_subdirectory (python)
find_package( Boost COMPONENTS python REQUIRED )
include_directories( ${Boost_INCLUDE_DIR} )
add_subdirectory (python)
ENDIF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(include/hpp/spline)
ADD_SUBDIRECTORY(tests)
SETUP_PROJECT_FINALIZE()
SETUP_HPP_PROJECT_FINALIZE()
Subproject commit 77876c449c4a8b20a3e534a6db9eaa47c2b76a0c
Subproject commit cea261e3da7d383844530070356bca76d20197a8
#!/usr/bin/env python
import unittest
from numpy import matrix
from numpy.linalg import norm
from numpy.testing import assert_allclose
from hpp_spline import bezier, bezier6, curve_constraints, exact_cubic, from_bezier, polynom, spline_deriv_constraint
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose()
# testing bezier curve
a = bezier6(waypoints6)
a = bezier(waypoints, 3.)
assert (a.degree == a.nbWaypoints - 1)
a.min()
a.max()
a(0.4)
assert ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert (a(t) == prim.derivate(t, 1)).all()
assert (prim(0) == matrix([0., 0., 0.])).all()
prim = a.compute_primitive(2)
for i in range(10):
t = float(i) / 10.
assert (a(t) == prim.derivate(t, 2)).all()
assert (prim(0) == matrix([0., 0., 0.])).all()
waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
a0 = bezier(waypoints)
a1 = bezier(waypoints, 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert norm(a0(t) - a1(3 * t)) < __EPS
assert norm(a0.derivate(t, 1) - a1.derivate(3 * t, 1) * 3.) < __EPS
assert norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS
assert norm(prim0(t) - prim1(t * 3) / 3.) < __EPS
assert (prim(0) == matrix([0., 0., 0.])).all()
# testing bezier with constraints
c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., -1.]).transpose()
c.end_acc = matrix([0., 100., 1.]).transpose()
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints, c)
assert norm(a.derivate(0, 1) - c.init_vel) < 1e-10
assert norm(a.derivate(1, 2) - c.end_acc) < 1e-10
# testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
assert ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
# testing exact_cubic function
a = exact_cubic(waypoints, time_waypoints)
a.min()
a.max()
a(0.4)
assert ((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
# testing spline_deriv_constraints
c = curve_constraints()
c.init_vel
c.end_vel
c.init_acc
c.end_acc
c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., 1.]).transpose()
c.end_acc = matrix([0., 1., 1.]).transpose()
a = spline_deriv_constraint(waypoints, time_waypoints)
a = spline_deriv_constraint(waypoints, time_waypoints, c)
# converting bezier to polynom
a = bezier(waypoints)
a_pol = from_bezier(a)
assert norm(a(0.3) - a_pol(0.3)) < __EPS
class TestSpline(unittest.TestCase):
def test_spline(self):
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).T
time_waypoints = matrix([0., 1.]).T
# testing bezier curve
a = bezier6(waypoints6)
a = bezier(waypoints, 3.)
self.assertEqual(a.degree, a.nbWaypoints - 1)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert_allclose(a(t), prim.derivate(t, 1))
assert_allclose(prim(0), matrix([0., 0., 0.]).T)
prim = a.compute_primitive(2)
for i in range(10):
t = float(i) / 10.
assert_allclose(a(t), prim.derivate(t, 2))
assert_allclose(prim(0), matrix([0., 0., 0.]).T)
waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).T
a0 = bezier(waypoints)
a1 = bezier(waypoints, 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert_allclose(a0(t), a1(3 * t))
assert_allclose(a0.derivate(t, 1), a1.derivate(3 * t, 1) * 3.)
assert_allclose(a0.derivate(t, 2), a1.derivate(3 * t, 2) * 9.)
assert_allclose(prim0(t), prim1(t * 3) / 3.)
assert_allclose(prim(0), matrix([0., 0., 0.]).T)
with self.assertRaises(AssertionError):
assert_allclose(prim(0), matrix([0., 0., 0.]))
# testing bezier with constraints
c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).T
c.end_vel = matrix([0., 1., 1.]).T
c.init_acc = matrix([0., 1., -1.]).T
c.end_acc = matrix([0., 100., 1.]).T
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T
a = bezier(waypoints, c)
assert_allclose(a.derivate(0, 1), c.init_vel)
assert_allclose(a.derivate(1, 2), c.end_acc)
# testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
# testing exact_cubic function
a = exact_cubic(waypoints, time_waypoints)
a.min()
a.max()
a(0.4)
assert_allclose(a.derivate(0.4, 0), a(0.4))
a.derivate(0.4, 2)
# testing spline_deriv_constraints
c = curve_constraints()
c.init_vel
c.end_vel
c.init_acc
c.end_acc
c.init_vel = matrix([0., 1., 1.]).T
c.end_vel = matrix([0., 1., 1.]).T
c.init_acc = matrix([0., 1., 1.]).T
c.end_acc = matrix([0., 1., 1.]).T
a = spline_deriv_constraint(waypoints, time_waypoints)
a = spline_deriv_constraint(waypoints, time_waypoints, c)
# converting bezier to polynom
a = bezier(waypoints)
a_pol = from_bezier(a)
assert_allclose(a(0.3), a_pol(0.3))
if __name__ == '__main__':
unittest.main()
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