Commit a6b9601a authored by Guilhem Saurel's avatar Guilhem Saurel

Merge branch 'devel' into 'master'

Devel

See merge request loco-3d/curves!20
parents d1ee66a4 8006bd7c
......@@ -5,3 +5,4 @@ build-rel/
# temp files
.*~
*.user
*.pyc
include: http://rainboard.laas.fr/project/hpp-spline/.gitlab-ci.yml
include: http://rainboard.laas.fr/project/curves/.gitlab-ci.yml
cmake_minimum_required(VERSION 2.6)
project(curve)
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/test.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/hpp.cmake)
SET(PROJECT_ORG loco-3d)
SET(PROJECT_NAME curves)
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."
......@@ -12,28 +14,35 @@ SET(PROJECT_DESCRIPTION
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
SET(CMAKE_VERBOSE_MAKEFILE True)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
SETUP_HPP_PROJECT()
ADD_REQUIRED_DEPENDENCY(eigen3)
SET(BOOST_COMPONENTS unit_test_framework serialization)
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} )
FINDPYTHON()
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_DIRS})
find_package( Boost COMPONENTS python REQUIRED )
include_directories( ${Boost_INCLUDE_DIR} )
STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
add_subdirectory (python)
ADD_REQUIRED_DEPENDENCY("eigenpy")
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
ENDIF(BUILD_PYTHON_INTERFACE)
#find_package(Boost 1.58 REQUIRED unit_test_framework system serialization)
#SET(BOOST_COMPONENTS unit_test_framework serialization)
SEARCH_FOR_BOOST()
INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS})
IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
ENDIF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(include/curve)
ADD_SUBDIRECTORY(include/curves)
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/)
[![Pipeline status](https://gepgitlab.laas.fr/loco-3d/curves/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/loco-3d/curves/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/loco-3d/curves/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/loco-3d/curves/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.
......
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _CLASS_BERNSTEIN
#define _CLASS_BERNSTEIN
#include "curve_abc.h"
#include "MathDefs.h"
#include <math.h>
#include <vector>
#include <stdexcept>
namespace curve
{
///
/// \brief Computes factorial of a number
///
inline unsigned int fact(const unsigned int n)
{
unsigned int res = 1;
for (unsigned int i=2 ; i <= n ; ++i)
res *= i;
return res;
}
///
/// \brief Computes a binomal coefficient
///
inline unsigned int bin(const unsigned int n, const unsigned int k)
{
return fact(n) / (fact(k) * fact(n - k));
}
/// \class Bernstein
/// \brief Computes a Bernstein polynome
///
template <typename Numeric = double>
struct Bern{
Bern(const unsigned int m, const unsigned int i)
:m_minus_i(m - i)
,i_(i)
,bin_m_i_(bin(m,i)) {}
~Bern(){}
Numeric operator()(const Numeric u) const
{
assert(u >= 0. && u <= 1.);
return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i);
}
Numeric m_minus_i;
Numeric i_;
Numeric bin_m_i_;
};
///
/// \brief Computes all Bernstein polynomes for a certain degree
///
template <typename Numeric>
std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
{
std::vector<Bern<Numeric> > res;
for(unsigned int i = 0; i<= n; ++i)
res.push_back(Bern<Numeric>(n, i));
return res;
}
} // namespace curve
#endif //_CLASS_BERNSTEIN
This diff is collapsed.
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _BEZIER_POLY_CONVERSION
#define _BEZIER_POLY_CONVERSION
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
#include <vector>
#include <stdexcept>
#include <iostream>
namespace curve
{
/// \brief Provides methods for converting a curve from a bernstein representation
/// to a polynom representation
///
///
///
///\brief Converts a Bezier curve to a polynom
///\param bezier: the Bezier curve to be converted from
///\return the equivalent polynom
template<typename Bezier, typename Polynom>
Polynom from_bezier(const Bezier& curve)
{
typedef typename Polynom::t_point_t t_point_t;
typedef typename Polynom::num_t num_t;
assert (curve.min() == 0.);
assert (curve.max() == 1.);
t_point_t coefficients;
Bezier current (curve);
coefficients.push_back(curve(0.));
num_t fact = 1;
for(std::size_t i = 1; i<= curve.degree_; ++i)
{
current = current.compute_derivate(1);
fact *= (num_t)i;
coefficients.push_back(current(0.)/fact);
}
return Polynom(coefficients,curve.min(),curve.max());
}
///\brief Converts a polynom to a Bezier curve
///\param polynom: the polynom to be converted from
///\return the equivalent Bezier curve
/*template<typename Bezier, typename Polynom>
Bezier from_polynom(const Polynom& polynom)
{
typedef Bezier::point_t point_t;
typedef Bezier::time_t time_t;
typedef Bezier::num_t num_t;
typedef Bezier::curve_constraints_t curve_constraints_t;
typedef Bezier::t_point_t t_point_t;
typedef Bezier::cit_point_t cit_point_t;
typedef Bezier::bezier_curve_t bezier_curve_t;
}*/
} // namespace curve
#endif //_BEZIER_POLY_CONVERSION
/**
* \file curve_abc.h
* \brief interface for a Curve of arbitrary dimension.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* Interface for a curve
*/
#ifndef _STRUCT_CURVE_ABC
#define _STRUCT_CURVE_ABC
#include "MathDefs.h"
#include <functional>
namespace curve
{
/// \struct curve_abc
/// \brief Represents a curve of dimension Dim
/// is Safe is false, no verification is made on the evaluation of the curve.
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct curve_abc : std::unary_function<Time, Point>
{
typedef Point point_t;
typedef Time time_t;
/* Constructors - destructors */
public:
///\brief Constructor
curve_abc(){}
///\brief Destructor
virtual ~curve_abc(){}
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual point_t operator()(const time_t t) const = 0;
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spline
/// \param order : order of the derivative
/// \param return : the value x(t)
virtual point_t derivate(const time_t t, const std::size_t order) const = 0;
/*Operations*/
/*Helpers*/
public:
/// \brief Returns the minimum time for wich curve is defined
virtual time_t min() const = 0;
/// \brief Returns the maximum time for wich curve is defined
virtual time_t max() const = 0;
std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
/*Helpers*/
};
} // namespace curve
#endif //_STRUCT_CURVE_ABC
/**
* \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_EXACTCUBIC
#define _CLASS_EXACTCUBIC
#include "curve_abc.h"
#include "cubic_spline.h"
#include "quintic_spline.h"
#include "MathDefs.h"
#include <functional>
#include <vector>
namespace curve
{
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >
, typename SplineBase=polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Time time_t;
typedef Numeric num_t;
typedef SplineBase spline_t;
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
typedef typename t_spline_t::const_iterator cit_spline_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
/* Constructors - destructors */
public:
///\brief Constructor
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
///\brief Constructor
///\param subSplines: vector of subsplines
exact_cubic(const t_spline_t& subSplines)
: curve_abc_t(), subSplines_(subSplines) {}
///\brief Copy Constructor
exact_cubic(const exact_cubic& other)
: curve_abc_t(), subSplines_(other.subSplines_) {}
///\brief Destructor
virtual ~exact_cubic(){}
private:
template<typename In>
t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
throw; // TODO
}
t_spline_t subSplines; subSplines.reserve(size);
// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
MatrixX h2 = MatrixX::Zero(size, size);
MatrixX h3 = MatrixX::Zero(size, size);
MatrixX h4 = MatrixX::Zero(size, size);
MatrixX h5 = MatrixX::Zero(size, size);
MatrixX h6 = MatrixX::Zero(size, size);
MatrixX a = MatrixX::Zero(size, Dim);
MatrixX b = MatrixX::Zero(size, Dim);
MatrixX c = MatrixX::Zero(size, Dim);
MatrixX d = MatrixX::Zero(size, Dim);
MatrixX x = MatrixX::Zero(size, Dim);
In it(wayPointsBegin), next(wayPointsBegin);
++next;
for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
{
num_t const dTi((*next).first - (*it).first);
num_t const dTi_sqr(dTi * dTi);
num_t const dTi_cube(dTi_sqr * dTi);
// filling matrices values
h3(i,i) = -3 / dTi_sqr;
h3(i,i+1) = 3 / dTi_sqr;
h4(i,i) = -2 / dTi;
h4(i,i+1) = -1 / dTi;
h5(i,i) = 2 / dTi_cube;
h5(i,i+1) = -2 / dTi_cube;
h6(i,i) = 1 / dTi_sqr;
h6(i,i+1) = 1 / dTi_sqr;
if( i+2 < size)
{
In it2(next); ++ it2;
num_t const dTi_1((*it2).first - (*next).first);
num_t const dTi_1sqr(dTi_1 * dTi_1);
// this can be optimized but let's focus on clarity as long as not needed
h1(i+1, i) = 2 / dTi;
h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
h1(i+1, i+2) = 2 / dTi_1;
h2(i+1, i) = -6 / dTi_sqr;
h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
h2(i+1, i+2) = 6 / dTi_1sqr;
}
x.row(i)= (*it).second.transpose();
}
// adding last x
x.row(size-1)= (*it).second.transpose();
a= x;
PseudoInverse(h1);
b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c = h3 * x + h4 * b;
d = h5 * x + h6 * b;
it= wayPointsBegin, next=wayPointsBegin; ++ next;
for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
{
subSplines.push_back(
create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first));
}
subSplines.push_back(
create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
return subSplines;
}
private:
//exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spline
/// \param return : the value x(t)
virtual point_t operator()(const time_t t) const
{
if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
return it->operator()(t);
}
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
}
/// \brief Evaluation of the derivative spline at time t.
/// \param t : the time when to evaluate the spline
/// \param order : order of the derivative
/// \param return : the value x(t)
virtual point_t derivate(const time_t t, const std::size_t order) const
{
if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if( (t >= (it->min()) && t <= (it->max())) || it+1 == subSplines_.end())
return it->derivate(t, order);
}
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
}
/*Operations*/
/*Helpers*/
public:
num_t virtual min() const{return subSplines_.front().min();}
num_t virtual max() const{return subSplines_.back().max();}
/*Helpers*/
/*Attributes*/
public:
t_spline_t subSplines_; // const
/*Attributes*/
};
} // namespace curve
#endif //_CLASS_EXACTCUBIC
/**
* \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 "curve/spline_deriv_constraint.h"
namespace curve
{
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
This diff is collapsed.
This diff is collapsed.
/**
* \file polynom.h
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the polynom struct.
* It allows the creation and evaluation of natural
* smooth splines of arbitrary dimension and order
*/
#ifndef _STRUCT_POLYNOM
#define _STRUCT_POLYNOM
#include "MathDefs.h"
#include "curve_abc.h"
#include <iostream>
#include <algorithm>
#include <functional>
#include <stdexcept>
namespace curve
{
/// \class polynom
/// \brief Represents a polynomf arbitrary order defined on the interval
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - t_min_) + ... + d(t - t_min_)^N, where N is the order
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
typedef Eigen::Ref<coeff_t> coeff_t_ref;
/* Constructors - destructors */
public:
///\brief Constructor
///\param coefficients : a reference to an Eigen matrix where each column is a coefficient,
/// from the zero order coefficient, up to the highest order. Spline order is given
/// by the number of the columns -1.
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
polynom(const coeff_t& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients), dim_(Dim), order_(coefficients_.cols()-1), t_min_