Skip to content

GitLab

Commit a976e461 authored by t steve's avatar t steve
Browse files

Merge branch 'packaging'

parents d270a50a 950e634c
Hide whitespace changes
Inline Side-by-side
include/spline/bezier_curve.h
View file @ a976e461
......@@ -46,21 +46,60 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
: minBound_(minBound)
, maxBound_(maxBound)
bezier_curve(In PointsBegin, In PointsEnd)
: T_(1.)
, mult_T_(1.)
, 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))
if(Safe && (size_<1 || T_ <= 0.))
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);
}
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
: T_(T)
, mult_T_(1.)
, 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 || T_ <= 0.))
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);
}
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
: T_(T)
, mult_T_(mult_T)
, 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 || T_ <= 0.))
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);
}
///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
......@@ -69,14 +108,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\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)
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T=1.)
: T_(T)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
if(Safe && (size_<1 || minBound >= maxBound))
if(Safe && (size_<1 || T_ <= 0.))
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)
......@@ -101,7 +140,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t)
virtual point_t operator()(const time_t t) const
{
num_t nT = (t - minBound_) / (maxBound_ - minBound_);
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
......@@ -112,22 +151,22 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
switch(size_)
{
case 1 :
return pts_[0];
return mult_T_ * pts_[0];
case 2 :
return pts_[0] * dt + nT * pts_[1];
return mult_T_ * (pts_[0] * dt + nT * pts_[1]);
break;
case 3 :
return pts_[0] * dt * dt
return mult_T_ * (pts_[0] * dt * dt
+ 2 * pts_[1] * nT * dt
+ pts_[2] * nT * nT;
+ pts_[2] * nT * nT);
break;
case 4 :
return pts_[0] * dt * dt * dt
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;
+ pts_[3] * nT * nT *nT);
default :
return evalHorner(nT);
return mult_T_ * evalHorner(nT);
break;
}
}
......@@ -144,7 +183,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
derived_wp.push_back(degree_ * (*(pit+1) - (*pit)));
if(derived_wp.empty())
derived_wp.push_back(point_t::Zero());
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_, mult_T_ * (1./T_) );
return deriv.compute_derivate(order-1);
}
......@@ -165,7 +204,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_);
bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_, mult_T_*T_);
return integ.compute_primitive(order-1);
}
......@@ -183,6 +222,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
/// \brief Evaluates all Bernstein polynomes for a certain degree
/// 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
{
......@@ -246,15 +287,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
virtual time_t min() const{return minBound_;}
virtual time_t max() const{return maxBound_;}
virtual time_t min() const{return 0.;}
virtual time_t max() const{return T_;}
/*Helpers*/
public:
const time_t minBound_, maxBound_;
const std::size_t size_;
const std::size_t degree_;
const std::vector<Bern<Numeric> > bernstein_;
/*const*/ time_t T_;
/*const*/ time_t mult_T_;
/*const*/ std::size_t size_;
/*const*/ std::size_t degree_;
/*const*/ std::vector<Bern<Numeric> > bernstein_;
private:
t_point_t pts_;
......
include/spline/bezier_polynom_conversion.h 0 → 100644
View file @ a976e461
/**
* \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 spline
{
/// \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 *= 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;
}*/
}
#endif //_BEZIER_POLY_CONVERSION
include/spline/polynom.h
View file @ a976e461
......@@ -35,6 +35,7 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
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;
......
python/spline_python.cpp
View file @ a976e461
......@@ -3,6 +3,9 @@
#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 <vector>
......@@ -35,12 +38,15 @@ typedef polynom_t::coeff_t coeff_t;
typedef std::pair<real, point_t> waypoint_t;
typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t;
typedef spline::Bern<double> bernstein_t;
typedef spline::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_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(bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t)
......@@ -61,17 +67,17 @@ T_Point vectorFromEigenArray(const PointList& array)
}
template <typename Bezier, typename PointList, typename T_Point>
Bezier* wrapBezierConstructorTemplate(const PointList& array, const real lb = 0., const real ub =1.)
Bezier* wrapBezierConstructorTemplate(const PointList& array, const real ub =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), lb, ub);
return new Bezier(asVector.begin(), asVector.end(), 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.)
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real ub =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, lb, ub);
return new Bezier(asVector.begin(), asVector.end(), constraints, ub);
}
/*3D constructors */
......@@ -79,17 +85,17 @@ 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)
bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real ub)
{
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, lb, ub) ;
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, 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)
bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real ub)
{
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, lb, ub) ;
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, ub) ;
}
/*END 3D constructors */
/*6D constructors */
......@@ -97,17 +103,17 @@ bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
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)
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real ub)
{
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, lb, ub) ;
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, 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)
bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real ub)
{
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, lb, ub) ;
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, ub) ;
}
/*END 6D constructors */
......@@ -300,6 +306,17 @@ BOOST_PYTHON_MODULE(spline)
;
/** END spline_deriv_constraints**/
/** BEGIN bernstein polynom**/
class_<bernstein_t>
("bernstein", init<const unsigned int, const unsigned int>())
.def("__call__", &bernstein_t::operator())
;
/** END bernstein polynom**/
/** BEGIN Bezier to polynom conversion**/
def("from_bezier", from_bezier<bezier_t,polynom_t>);
/** END Bezier to polynom conversion**/
}
......
python/test/test.py
View file @ a976e461
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
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, -1., 3.)
a = bezier(waypoints, 3.)
assert(a.degree == a.nbWaypoints -1)
a.min()
......@@ -21,6 +23,7 @@ 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()
......@@ -32,6 +35,20 @@ for i in range(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();
......@@ -40,7 +57,7 @@ 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
......@@ -78,3 +95,9 @@ 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
src/tests/spline_test/Main.cpp
View file @ a976e461
......@@ -5,6 +5,7 @@
#include "spline/spline_deriv_constraint.h"
#include "spline/helpers/effector_spline.h"
#include "spline/helpers/effector_spline_rotation.h"
#include "spline/bezier_polynom_conversion.h"
#include <string>
#include <iostream>
......@@ -174,9 +175,7 @@ void BezierCurveTest(bool& error)
//4d curve
params.push_back(d);
bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
res1 = cf4(0.4);
ComparePoints(a, res1, errMsg + "3(0) ", error);
bezier_curve_t cf4(params.begin(), params.end(), 2);
res1 = cf4(2);
ComparePoints(d, res1, errMsg + "3(1) ", error);
......@@ -344,6 +343,32 @@ void BezierDerivativeCurveTest(bool& error)
ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
}
void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
{
std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest ; unexpected result for x ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
point_t d(3,4,5);
point_t e(3,4,5);
point_t f(3,4,5);
std::vector<point_t> params;
params.push_back(a);
params.push_back(b);
params.push_back(c);
params.push_back(d);
params.push_back(e);
params.push_back(f);
double T = 2.;
bezier_curve_t cf(params.begin(), params.end());
bezier_curve_t cfT(params.begin(), params.end(),T);
ComparePoints(cf(0.5), cfT(1), errMsg, error);
ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * T, errMsg, error);
ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * T*T, errMsg, error);
}
void BezierDerivativeCurveConstraintTest(bool& error)
{
std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
......@@ -377,6 +402,39 @@ void BezierDerivativeCurveConstraintTest(bool& error)
}
void BezierToPolynomConversionTest(bool& error)
{
std::string errMsg("In test BezierToPolynomConversionTest ; unexpected result for x ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
point_t d(3,6,7);
point_t e(3,61,7);
point_t f(3,56,7);
point_t g(3,36,7);
point_t h(43,6,7);
point_t i(3,6,77);
std::vector<point_t> params;
params.push_back(a);
params.push_back(b);
params.push_back(c);
params.push_back(d);
params.push_back(e);
params.push_back(f);
params.push_back(g);
params.push_back(h);
params.push_back(i);
bezier_curve_t cf(params.begin(), params.end());
polynom_t pol =from_bezier<bezier_curve_t, polynom_t>(cf);
for(double i =0.; i<1.; i+=0.01)
{
ComparePoints(cf(i),pol(i),errMsg, error, true);
ComparePoints(cf(i),pol(i),errMsg, error, false);
}
}
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
......@@ -714,7 +772,7 @@ int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
/*CubicFunctionTest(error);
CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
ExactCubicTwoPointsTest(error);
......@@ -727,8 +785,10 @@ int main(int /*argc*/, char** /*argv[]*/)
EffectorSplineRotationWayPointRotationTest(error);
BezierCurveTest(error);
BezierDerivativeCurveTest(error);
BezierDerivativeCurveConstraintTest(error);*/
BezierDerivativeCurveConstraintTest(error);
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
BezierToPolynomConversionTest(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