Skip to content

GitLab

Commit e4131820 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

Merge branch 'packaging'

parents efa985d6 4b1c4e8e
Hide whitespace changes
Inline Side-by-side
include/spline/bezier_curve.h
View file @ e4131820
......@@ -12,6 +12,7 @@
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
......@@ -34,7 +35,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_constraints<point_t> curve_constraints_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
/* Constructors - destructors */
......@@ -49,18 +52,36 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || minBound >= maxBound))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
}
for(; it != PointsEnd; ++it)
{
pts_.push_back(*it);
}
}
for(; it != PointsEnd; ++it)
pts_.push_back(*it);
}
///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///\param constraints : constraints applying on start / end velocities and acceleration
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound=0, const time_t maxBound=1)
: minBound_(minBound)
, maxBound_(maxBound)
, size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
if(Safe && (size_<1 || minBound >= maxBound))
throw std::out_of_range("can't create bezier min bound is higher than max bound");
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
pts_.push_back(*cit);
}
///\brief Destructor
~bezier_curve()
......@@ -86,8 +107,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
}
else
{
num_t dt = (1 - nT);
{
num_t dt = (1 - nT);
switch(size_)
{
case 1 :
......@@ -106,7 +127,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
+ 3 * pts_[2] * nT * nT * dt
+ pts_[3] * nT * nT *nT;
default :
return evalBernstein(dt);
return evalBernstein(nT);
break;
}
}
......@@ -154,7 +175,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param order : order of the derivative
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual point_t derivate(const time_t t, const std::size_t order) const
virtual point_t derivate(const time_t t, const std::size_t order) const
{
bezier_curve_t deriv =compute_derivate(order);
return deriv(t);
......@@ -169,17 +190,41 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
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;
}
const t_point_t& waypoints() const {return pts_;}
private:
template<typename In>
t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
{
t_point_t res;
point_t P0, P1, P2, P_n_2, P_n_1, PN;
P0 = *PointsBegin; PN = *(PointsEnd-1);
P1 = P0+ constraints.init_vel / degree_;
P_n_1 = PN -constraints.end_vel / degree_;
P2 = constraints.init_acc / (degree_ * (degree_-1)) + 2* P1 - P0;
P_n_2 = constraints.end_acc / (degree_ * (degree_-1)) + 2* P_n_1 - PN;
res.push_back(P0);
res.push_back(P1);
res.push_back(P2);
for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
res.push_back(*it);
res.push_back(P_n_2);
res.push_back(P_n_1);
res.push_back(PN);
return res;
}
/*Operations*/
/*Helpers*/
public:
virtual time_t min() const{return minBound_;}
virtual time_t max() const{return maxBound_;}
/*Helpers*/
......
include/spline/curve_constraint.h 0 → 100644
View file @ e4131820
/**
* \file curve_constraint.h
* \brief struct to define constraints on start / end velocities and acceleration
* on a curve
* \author Steve T.
* \version 0.1
* \date 04/05/2017
*
*/
#ifndef _CLASS_CURVE_CONSTRAINT
#define _CLASS_CURVE_CONSTRAINT
#include "MathDefs.h"
#include <functional>
#include <vector>
namespace spline
{
template <typename Point>
struct curve_constraints
{
typedef Point point_t;
curve_constraints():
init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
~curve_constraints(){}
point_t init_vel;
point_t init_acc;
point_t end_vel;
point_t end_acc;
};
}
#endif //_CLASS_CUBICZEROVELACC
include/spline/spline_deriv_constraint.h
View file @ e4131820
......@@ -21,6 +21,7 @@
#define _CLASS_CUBICZEROVELACC
#include "exact_cubic.h"
#include "curve_constraint.h"
#include "MathDefs.h"
......@@ -53,19 +54,7 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
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;
public:
struct spline_constraints
{
spline_constraints():
init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
~spline_constraints(){}
point_t init_vel;
point_t init_acc;
point_t end_vel;
point_t end_acc;
};
typedef curve_constraints<point_t> spline_constraints;
/* Constructors - destructors */
public:
......
python/spline_python.cpp
View file @ e4131820
......@@ -2,6 +2,7 @@
#include "spline/spline_curve.h"
#include "spline/exact_cubic.h"
#include "spline/spline_deriv_constraint.h"
#include "spline/curve_constraint.h"
#include <vector>
......@@ -36,14 +37,15 @@ typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t
typedef spline::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t;
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
typedef spline::curve_constraints<point_t> curve_constraints_t;
typedef spline::curve_constraints<point6_t> curve_constraints6_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
namespace spline
......@@ -58,31 +60,56 @@ T_Point vectorFromEigenArray(const PointList& array)
return res;
}
bezier_t* wrapBezierConstructor(const point_list_t& array)
template <typename Bezier, typename PointList, typename T_Point>
Bezier* wrapBezierConstructorTemplate(const PointList& array, const real lb = 0., const real ub =1.)
{
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end());
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), lb, ub);
}
template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real lb = 0., const real ub =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, lb, ub);
}
/*3D constructors */
bezier_t* wrapBezierConstructor(const point_list_t& array)
{
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array) ;
}
bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb, const real ub)
{
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end(), lb, ub);
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, lb, ub) ;
}
bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints)
{
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ;
}
bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real lb, const real ub)
{
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, lb, ub) ;
}
/*END 3D constructors */
/*6D constructors */
bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end());
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array) ;
}
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real lb, const real ub)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end(), lb, ub);
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, lb, ub) ;
}
bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints)
{
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints) ;
}
bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real lb, const real ub)
{
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, lb, ub) ;
}
/*END 6D constructors */
spline_curve_t* wrapSplineConstructor(const coeff_t& array)
{
......@@ -118,7 +145,7 @@ exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoi
}
spline_deriv_constraint_t* wrapSplineDerivConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const spline_constraints_t& constraints)
spline_deriv_constraint_t* wrapSplineDerivConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const curve_constraints_t& constraints)
{
t_waypoint_t wps = getWayPoints(array, time_wp);
return new spline_deriv_constraint_t(wps.begin(), wps.end(),constraints);
......@@ -130,42 +157,42 @@ spline_deriv_constraint_t* wrapSplineDerivConstraintNoConstraints(const coeff_t&
return new spline_deriv_constraint_t(wps.begin(), wps.end());
}
point_t get_init_vel(const spline_constraints_t& c)
point_t get_init_vel(const curve_constraints_t& c)
{
return c.init_vel;
}
point_t get_init_acc(const spline_constraints_t& c)
point_t get_init_acc(const curve_constraints_t& c)
{
return c.init_acc;
}
point_t get_end_vel(const spline_constraints_t& c)
point_t get_end_vel(const curve_constraints_t& c)
{
return c.end_vel;
}
point_t get_end_acc(const spline_constraints_t& c)
point_t get_end_acc(const curve_constraints_t& c)
{
return c.end_acc;
}
void set_init_vel(spline_constraints_t& c, const point_t& val)
void set_init_vel(curve_constraints_t& c, const point_t& val)
{
c.init_vel = val;
}
void set_init_acc(spline_constraints_t& c, const point_t& val)
void set_init_acc(curve_constraints_t& c, const point_t& val)
{
c.init_acc = val;
}
void set_end_vel(spline_constraints_t& c, const point_t& val)
void set_end_vel(curve_constraints_t& c, const point_t& val)
{
c.end_vel = val;
}
void set_end_acc(spline_constraints_t& c, const point_t& val)
void set_end_acc(curve_constraints_t& c, const point_t& val)
{
c.end_acc = val;
}
......@@ -193,6 +220,8 @@ BOOST_PYTHON_MODULE(spline)
("bezier6", no_init)
.def("__init__", make_constructor(&wrapBezierConstructor6))
.def("__init__", make_constructor(&wrapBezierConstructorBounds6))
//.def("__init__", make_constructor(&wrapBezierConstructor6Constraints))
//.def("__init__", make_constructor(&wrapBezierConstructorBounds6Constraints))
.def("min", &bezier6_t::min)
.def("max", &bezier6_t::max)
.def("__call__", &bezier6_t::operator())
......@@ -210,6 +239,8 @@ BOOST_PYTHON_MODULE(spline)
("bezier", no_init)
.def("__init__", make_constructor(&wrapBezierConstructor))
.def("__init__", make_constructor(&wrapBezierConstructorBounds))
.def("__init__", make_constructor(&wrapBezierConstructorConstraints))
.def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
.def("min", &bezier_t::min)
.def("max", &bezier_t::max)
.def("__call__", &bezier_t::operator())
......@@ -246,15 +277,15 @@ BOOST_PYTHON_MODULE(spline)
/** END bezier curve**/
/** BEGIN spline constraints**/
class_<spline_constraints_t>
("spline_constraints", init<>())
/** BEGIN curve constraints**/
class_<curve_constraints_t>
("curve_constraints", init<>())
.add_property("init_vel", &get_init_vel, &set_init_vel)
.add_property("init_acc", &get_init_acc, &set_init_acc)
.add_property("end_vel", &get_end_vel, &set_end_vel)
.add_property("end_acc", &get_end_acc, &set_end_acc)
;
/** END spline constraints**/
/** END curve constraints**/
/** BEGIN spline_deriv_constraints**/
......
python/test/test.py
View file @ e4131820
from spline import bezier, spline, exact_cubic, spline_constraints, spline_constraints, spline_deriv_constraint
from spline import bezier, bezier6, spline, exact_cubic, curve_constraints, spline_deriv_constraint
from numpy import matrix
from numpy.linalg import norm
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, -1., 3.)
a = bezier(waypoints)
assert(a.degree == a.nbWaypoints -1)
a.min()
a.max()
......@@ -31,6 +33,18 @@ for i in range(10):
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.]);
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 spline function
a = spline(waypoints)
......@@ -50,7 +64,7 @@ assert((a.derivate(0.4,0) == a(0.4)).all())
a.derivate(0.4,2)
#testing spline_deriv_constraints
c = spline_constraints();
c = curve_constraints();
c.init_vel;
c.end_vel;
c.init_acc;
......
src/tests/spline_test/Main.cpp
View file @ e4131820
......@@ -60,7 +60,7 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
if((pt1-pt2).norm() > margin && !notequal)
{
error = true;
std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
}
}
......@@ -247,6 +247,37 @@ void BezierDerivativeCurveTest(bool& error)
ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
}
void BezierDerivativeCurveConstraintTest(bool& error)
{
std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
bezier_curve_t::curve_constraints_t constraints;
constraints.init_vel = point_t(-1,-1,-1);
constraints.init_acc = point_t(-2,-2,-2);
constraints.end_vel = point_t(-10,-10,-10);
constraints.end_acc = point_t(-20,-20,-20);
std::vector<point_t> params;
params.push_back(a);
params.push_back(b);
params.push_back(c);
bezier_curve_t cf3(params.begin(), params.end(), constraints);
assert(cf3.degree_ == params.size() + 3);
assert(cf3.size_ == params.size() + 4);
ComparePoints(a, cf3(0), errMsg, error);
ComparePoints(c, cf3(1), errMsg, error);
ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error);
ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error);
ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error);
ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error);
ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error);
}
/*Exact Cubic Function tests*/
......@@ -599,6 +630,7 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorSplineRotationWayPointRotationTest(error);
BezierCurveTest(error);
BezierDerivativeCurveTest(error);
BezierDerivativeCurveConstraintTest(error);
if(error)
{
std::cout << "There were some errors\n";
......
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