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

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
)
/**
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
#include "spline/spline_deriv_constraint.h"
namespace spline
{
namespace helpers
{
typedef double Numeric;
typedef double Time;
typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point> spline_deriv_constraint_t;
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
typedef spline_deriv_constraint_t::spline_t spline_t;
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
{
//compute time such that the equation from source to offsetpoint is necessarily a line
Numeric norm = normal.norm();
assert(norm>0.);
return std::make_pair(source.first + time_offset,(source.second + normal / norm* offset));
}
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset,
const Time init_time, const Time time_offset)
{
// compute spline from land way point to end point
// constraints are null velocity and acceleration
Numeric norm = normal.norm();
assert(norm>0.);
Point n = normal / norm;
Point d = offset / (time_offset*time_offset*time_offset) * -n;
Point c = -3 * d * time_offset;
Point b = -c * time_offset;
T_Point points;
points.push_back(from);
points.push_back(b);
points.push_back(c);
points.push_back(d);
return spline_t(points.begin(), points.end(), init_time, init_time+time_offset);
}
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/)
{
//computing end velocity: along landing normal and respecting time
spline_constraints_t constraints;
constraints.end_acc = end_spline.derivate(end_spline.min(),2);
constraints.end_vel = end_spline.derivate(end_spline.min(),1);
return constraints;
}
/// \brief Helper method to create a spline typically used to
/// guide the 3d trajectory of a robot end effector.
/// Given a set of waypoints, and the normal vector of the start and
/// ending positions, automatically create the spline such that:
/// + init and end velocities / accelerations are 0.
/// + the effector lifts and lands exactly in the direction of the specified normals
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container
/// \param wayPointsEnd : an iterator pointing to the end of a waypoint container
/// \param lift_normal : normal to be followed by end effector at take-off
/// \param land_normal : normal to be followed by end effector at landing
/// \param lift_offset : length of the straight line along normal at take-off
/// \param land_offset : length of the straight line along normal at landing
/// \param lift_offset_duration : time travelled along straight line at take-off
/// \param land_offset_duration : time travelled along straight line at landing
///
template<typename In>
exact_cubic_t* effector_spline(
In wayPointsBegin, In wayPointsEnd, const Point& lift_normal=Eigen::Vector3d::UnitZ(), const Point& land_normal=Eigen::Vector3d::UnitZ(),
const Numeric lift_offset=0.02, const Numeric land_offset=0.02,
const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
{
T_Waypoint waypoints;
const Waypoint& inPoint=*wayPointsBegin, endPoint =*(wayPointsEnd-1);
waypoints.push_back(inPoint);
//adding initial offset
waypoints.push_back(compute_offset(inPoint, lift_normal ,lift_offset, lift_offset_duration));
//inserting all waypoints but last
waypoints.insert(waypoints.end(),wayPointsBegin+1,wayPointsEnd-1);
//inserting waypoint to start landing
const Waypoint& landWaypoint=compute_offset(endPoint, land_normal ,land_offset, -land_offset_duration);
waypoints.push_back(landWaypoint);
//specifying end velocity constraint such that landing will be in straight line
spline_t end_spline=make_end_spline(land_normal,landWaypoint.second,land_offset,landWaypoint.first,land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration);
spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
t_spline_t splines = all_but_end.subSplines_;
splines.push_back(end_spline);
return new exact_cubic_t(splines);
}
}
}
#endif //_CLASS_EFFECTORSPLINE
/**
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/
#ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE
#include "hpp/spline/spline_deriv_constraint.h"
namespace spline
{
namespace helpers
{
typedef double Numeric;
typedef double Time;
typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point> spline_deriv_constraint_t;
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
typedef spline_deriv_constraint_t::exact_cubic_t exact_cubic_t;
typedef spline_deriv_constraint_t::t_spline_t t_spline_t;
typedef spline_deriv_constraint_t::spline_t spline_t;
Waypoint compute_offset(const Waypoint& source, const Point& normal, const Numeric offset, const Time time_offset)
{
//compute time such that the equation from source to offsetpoint is necessarily a line
Numeric norm = normal.norm();
assert(norm>0.);
return std::make_pair(source.first + time_offset,(source.second + normal / norm* offset));
}
spline_t make_end_spline(const Point& normal, const Point& from, const Numeric offset,
const Time init_time, const Time time_offset)
{
// compute spline from land way point to end point
// constraints are null velocity and acceleration
Numeric norm = normal.norm();
assert(norm>0.);
Point n = normal / norm;
Point d = offset / (time_offset*time_offset*time_offset) * -n;
Point c = -3 * d * time_offset;
Point b = -c * time_offset;
T_Point points;
points.push_back(from);
points.push_back(b);
points.push_back(c);
points.push_back(d);
return spline_t(points.begin(), points.end(), init_time, init_time+time_offset);
}
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/)
{
//computing end velocity: along landing normal and respecting time
spline_constraints_t constraints;
constraints.end_acc = end_spline.derivate(end_spline.min(),2);
constraints.end_vel = end_spline.derivate(end_spline.min(),1);
return constraints;
}
/// \brief Helper method to create a spline typically used to
/// guide the 3d trajectory of a robot end effector.
/// Given a set of waypoints, and the normal vector of the start and
/// ending positions, automatically create the spline such that:
/// + init and end velocities / accelerations are 0.
/// + the effector lifts and lands exactly in the direction of the specified normals
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container
/// \param wayPointsEnd : an iterator pointing to the end of a waypoint container
/// \param lift_normal : normal to be followed by end effector at take-off
/// \param land_normal : normal to be followed by end effector at landing
/// \param lift_offset : length of the straight line along normal at take-off
/// \param land_offset : length of the straight line along normal at landing
/// \param lift_offset_duration : time travelled along straight line at take-off
/// \param land_offset_duration : time travelled along straight line at landing
///
template<typename In>
exact_cubic_t* effector_spline(
In wayPointsBegin, In wayPointsEnd, const Point& lift_normal=Eigen::Vector3d::UnitZ(), const Point& land_normal=Eigen::Vector3d::UnitZ(),
const Numeric lift_offset=0.02, const Numeric land_offset=0.02,
const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
{
T_Waypoint waypoints;
const Waypoint& inPoint=*wayPointsBegin, endPoint =*(wayPointsEnd-1);
waypoints.push_back(inPoint);
//adding initial offset
waypoints.push_back(compute_offset(inPoint, lift_normal ,lift_offset, lift_offset_duration));
//inserting all waypoints but last
waypoints.insert(waypoints.end(),wayPointsBegin+1,wayPointsEnd-1);
//inserting waypoint to start landing
const Waypoint& landWaypoint=compute_offset(endPoint, land_normal ,land_offset, -land_offset_duration);
waypoints.push_back(landWaypoint);
//specifying end velocity constraint such that landing will be in straight line
spline_t end_spline=make_end_spline(land_normal,landWaypoint.second,land_offset,landWaypoint.first,land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline,land_offset_duration);
spline_deriv_constraint_t all_but_end(waypoints.begin(), waypoints.end(),constraints);
t_spline_t splines = all_but_end.subSplines_;
splines.push_back(end_spline);
return new exact_cubic_t(splines);
}
}
}
#endif //_CLASS_EFFECTORSPLINE
Markdown is supported
0% or .