Unverified Commit dc7ad399 authored by stonneau's avatar stonneau Committed by GitHub

Merge pull request #9 from humanoid-path-planner/master

sync with hpp-spline master
parents b357d665 af5fcda7
# Build and Release Folders
bin/
lib/
build/
build-rel/
......
include: http://rainboard.laas.fr/project/hpp-spline/.gitlab-ci.yml
cmake_minimum_required(VERSION 2.6)
project(spline)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/test.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/hpp.cmake)
SET(PROJECT_NAME spline)
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 "")
"template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics."
)
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/build/")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/")
set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib/")
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
SET(CMAKE_VERBOSE_MAKEFILE True)
......@@ -19,43 +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" OFF)
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 (src/tests/spline_test)
install(FILES
${CMAKE_SOURCE_DIR}/include/spline/bernstein.h
${CMAKE_SOURCE_DIR}/include/spline/bezier_polynom_conversion.h
${CMAKE_SOURCE_DIR}/include/spline/curve_abc.h
${CMAKE_SOURCE_DIR}/include/spline/exact_cubic.h
${CMAKE_SOURCE_DIR}/include/spline/MathDefs.h
${CMAKE_SOURCE_DIR}/include/spline/polynom.h
${CMAKE_SOURCE_DIR}/include/spline/spline_deriv_constraint.h
${CMAKE_SOURCE_DIR}/include/spline/bezier_curve.h
${CMAKE_SOURCE_DIR}/include/spline/cubic_spline.h
${CMAKE_SOURCE_DIR}/include/spline/curve_constraint.h
${CMAKE_SOURCE_DIR}/include/spline/quintic_spline.h
DESTINATION ${CMAKE_INSTALL_PREFIX}/include/spline
)
install(FILES
${CMAKE_SOURCE_DIR}/include/spline/helpers/effector_spline.h
${CMAKE_SOURCE_DIR}/include/spline/helpers/effector_spline_rotation.h
DESTINATION ${CMAKE_INSTALL_PREFIX}/include/spline/helpers
)
SETUP_PROJECT_FINALIZE()
ADD_SUBDIRECTORY(include/hpp/spline)
ADD_SUBDIRECTORY(tests)
SETUP_HPP_PROJECT_FINALIZE()
Spline
===================
[![Pipeline status](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-spline/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-spline/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-spline/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-spline/master/coverage/)
A template-based Library for creating curves of arbitrary order and dimension, eventually subject to derivative constraints. The main use of the library is the creation of end-effector trajectories for legged robots.
......@@ -12,7 +15,7 @@ To do so, tools are provided to:
The library is template-based, thus generic: the curves can be of any dimension, and can be implemented in double, float ...
While a Bezier curve implementation is provided, the main interest
While a Bezier curve implementation is provided, the main interest
of this library is to create spline curves of arbitrary order
----------
......@@ -59,6 +62,9 @@ Please refer to the Main.cpp files to see all the unit tests and possibilities o
Installation
-------------
This package is available as binary in [robotpkg/wip](http://robotpkg.openrobots.org/robotpkg-wip.html)
## Dependencies
* [Eigen (version >= 3.2.2)](http://eigen.tuxfamily.org/index.php?title=Main_Page)
......@@ -75,14 +81,14 @@ The library is header only, so the build only serves to build the tests and pyth
```
cd $SPLINE_DIR && mkdir build && cd build
cmake .. && make
cmake .. && make
../bin/tests
```
If everything went fine you should obtain the following output:
```
performing tests...
no errors found
performing tests...
no errors found
```
### Optional: Python bindings installation
To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option:
......
Subproject commit 7b0b47cae2b082521ad674c8ee575f594f483cd7
Subproject commit cea261e3da7d383844530070356bca76d20197a8
SET(${PROJECT_NAME}_HEADERS
bernstein.h
bezier_polynom_conversion.h
curve_abc.h
exact_cubic.h
MathDefs.h
polynom.h
spline_deriv_constraint.h
bezier_curve.h
cubic_spline.h
curve_constraint.h
quintic_spline.h
)
INSTALL(FILES
${${PROJECT_NAME}_HEADERS}
DESTINATION include/hpp/spline
)
ADD_SUBDIRECTORY(helpers)
......@@ -139,37 +139,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual point_t operator()(const time_t t) const
{
num_t nT = t / T_;
if(Safe &! (0 <= nT && nT <= 1))
{
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
}
else
{
num_t dt = (1 - nT);
switch(size_)
{
case 1 :
return mult_T_ * pts_[0];
case 2 :
return mult_T_ * (pts_[0] * dt + nT * pts_[1]);
break;
case 3 :
return mult_T_ * (pts_[0] * dt * dt
+ 2 * pts_[1] * nT * dt
+ pts_[2] * nT * nT);
break;
case 4 :
return mult_T_ * (pts_[0] * dt * dt * dt
+ 3 * pts_[1] * nT * dt * dt
+ 3 * pts_[2] * nT * nT * dt
+ pts_[3] * nT * nT *nT);
default :
return mult_T_ * evalHorner(nT);
break;
}
}
if(Safe &! (0 <= t && t <= T_))
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
return evalHorner(t);
}
/// \brief Computes the derivative curve at order N.
......@@ -225,14 +198,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// Warning: the horner scheme is about 100 times faster than this method.
/// This method will probably be removed in the future
///
point_t evalBernstein(const Numeric u) const
point_t evalBernstein(const Numeric t) const
{
const Numeric u = t/T_;
point_t res = point_t::Zero(Dim);
typename t_point_t::const_iterator pts_it = pts_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
cit !=bernstein_.end(); ++cit, ++pts_it)
res += cit->operator()(u) * (*pts_it);
return res;
return res*mult_T_;
}
......@@ -241,19 +215,20 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
point_t evalHorner(const Numeric t) const
{
const Numeric u = t/T_;
typename t_point_t::const_iterator pts_it = pts_.begin();
Numeric u, bc, tn;
u = 1.0 - t;
Numeric u_op, bc, tn;
u_op = 1.0 - u;
bc = 1;
tn = 1;
point_t tmp =(*pts_it)*u; ++pts_it;
point_t tmp =(*pts_it)*u_op; ++pts_it;
for(unsigned int i=1; i<degree_; i++, ++pts_it)
{
tn = tn*t;
tn = tn*u;
bc = bc*((num_t)(degree_-i+1))/i;
tmp = (tmp + tn*bc*(*pts_it))*u;
tmp = (tmp + tn*bc*(*pts_it))*u_op;
}
return (tmp + tn*t*(*pts_it));
return (tmp + tn*u*(*pts_it))*mult_T_;
}
const t_point_t& waypoints() const {return pts_;}
......@@ -264,12 +239,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
* @param t unNormalized time
* @return the point at time t
*/
point_t evalDeCasteljau(const Numeric T) const {
point_t evalDeCasteljau(const Numeric t) const {
// normalize time :
const Numeric t = T/T_;
t_point_t pts = deCasteljauReduction(waypoints(),t);
const Numeric u = t/T_;
t_point_t pts = deCasteljauReduction(waypoints(),u);
while(pts.size() > 1){
pts = deCasteljauReduction(pts,t);
pts = deCasteljauReduction(pts,u);
}
return pts[0]*mult_T_;
}
......@@ -281,18 +256,18 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/**
* @brief deCasteljauReduction compute the de Casteljau's reduction of the given list of points at time t
* @param pts the original list of points
* @param t the NORMALIZED time
* @param u the NORMALIZED time
* @return the reduced list of point (size of pts - 1)
*/
t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric t) const{
if(t < 0 || t > 1)
throw std::out_of_range("In deCasteljau reduction : t is not in [0;1]");
t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const{
if(u < 0 || u > 1)
throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
if(pts.size() == 1)
return pts;
t_point_t new_pts;
for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit){
new_pts.push_back((1-t) * (*cit) + t*(*(cit+1)));
new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
}
return new_pts;
}
......@@ -303,24 +278,24 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
* @param t
* @return
*/
std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric T){
if (T == T_)
std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
if (t == T_)
throw std::runtime_error("can't split curve, interval range is equal to original curve");
t_point_t wps_first(size_),wps_second(size_);
const double t = T/T_;
const double u = t/T_;
wps_first[0] = pts_.front();
wps_second[degree_] = pts_.back();
t_point_t casteljau_pts = waypoints();
size_t id = 1;
while(casteljau_pts.size() > 1){
casteljau_pts = deCasteljauReduction(casteljau_pts,t);
casteljau_pts = deCasteljauReduction(casteljau_pts,u);
wps_first[id] = casteljau_pts.front();
wps_second[degree_-id] = casteljau_pts.back();
++id;
}
bezier_curve_t c_first(wps_first.begin(), wps_first.end(), T,mult_T_);
bezier_curve_t c_second(wps_second.begin(), wps_second.end(), T_-T,mult_T_);
bezier_curve_t c_first(wps_first.begin(), wps_first.end(), t,mult_T_);
bezier_curve_t c_second(wps_second.begin(), wps_second.end(), T_-t,mult_T_);
return std::make_pair(c_first,c_second);
}
......
SET(${PROJECT_NAME}_HELPERS_HEADERS
effector_spline.h
effector_spline_rotation.h
)
INSTALL(FILES
${${PROJECT_NAME}_HELPERS_HEADERS}
DESTINATION include/hpp/spline/helpers
)
cmake_minimum_required( VERSION 2.8 )
include_directories("${EIGEN3_INCLUDE_DIR}")
STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
ADD_REQUIRED_DEPENDENCY("eigenpy")
include_directories("${PROJECT_SOURCE_DIR}/include")
FILE(GLOB_RECURSE HeaderFiles "${PROJECT_SOURCE_DIR}/include/spline/*.h")
# Define the wrapper library that wraps our library
add_library( spline SHARED spline_python.cpp )
add_library( ${PY_NAME} SHARED spline_python.cpp )
#~ target_link_libraries( centroidal_dynamics ${Boost_LIBRARIES} ${PROJECT_NAME} )
# don't prepend wrapper library name with lib
set_target_properties( spline PROPERTIES PREFIX "" )
set_target_properties( ${PY_NAME} PROPERTIES PREFIX "" )
IF(APPLE)
# We need to change the extension for python bindings
SET_TARGET_PROPERTIES(spline PROPERTIES SUFFIX ".so")
# We need to change the extension for python bindings
SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES SUFFIX ".so")
ENDIF(APPLE)
PKG_CONFIG_USE_DEPENDENCY(spline eigenpy)
PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} eigenpy)
INSTALL(
FILES ${LIBRARY_OUTPUT_PATH}spline.so DESTINATION ${PYTHON_SITELIB}
)
TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB}
)
ADD_PYTHON_UNIT_TEST("python-spline" "python/test/test.py" "python")
#include "spline/bezier_curve.h"
#include "spline/polynom.h"
#include "spline/exact_cubic.h"
#include "spline/spline_deriv_constraint.h"
#include "spline/curve_constraint.h"
#include "spline/bezier_polynom_conversion.h"
#include "spline/bernstein.h"
#include "hpp/spline/bezier_curve.h"
#include "hpp/spline/polynom.h"
#include "hpp/spline/exact_cubic.h"
#include "hpp/spline/spline_deriv_constraint.h"
#include "hpp/spline/curve_constraint.h"
#include "hpp/spline/bezier_polynom_conversion.h"
#include "hpp/spline/bernstein.h"
#include <vector>
......@@ -205,7 +205,7 @@ void set_end_acc(curve_constraints_t& c, const point_t& val)
BOOST_PYTHON_MODULE(spline)
BOOST_PYTHON_MODULE(hpp_spline)
{
/** BEGIN eigenpy init**/
eigenpy::enableEigenPy();
......
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
#!/usr/bin/env python
import unittest
from numpy import matrix
from numpy.linalg import norm
__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.])
#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.]);
c.end_vel = matrix([0.,1.,1.]);
c.init_acc = matrix([0.,1.,-1.]);
c.end_acc = matrix([0.,100.,1.]);
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.]);
c.end_vel = matrix([0.,1.,1.]);
c.init_acc = matrix([0.,1.,1.]);
c.end_acc = matrix([0.,1.,1.]);
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
from numpy.testing import assert_allclose
from hpp_spline import bezier, bezier6, curve_constraints, exact_cubic, from_bezier, polynom, spline_deriv_constraint
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), atol=1e-20)
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()
cmake_minimum_required(VERSION 2.6)
include_directories("${PROJECT_SOURCE_DIR}/include")
FILE(GLOB_RECURSE HeaderFiles "${PROJECT_SOURCE_DIR}/include/spline/*.h")
add_executable(