Commit d0f0841d authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Merge branch 'topic/piecewise_polynomial' into 'devel'

Add new methods in polynomial and piecewise polynomial

See merge request loco-3d/curves!19
parents 70c4ecba 4eb2b375
......@@ -100,6 +100,19 @@ namespace curves
return (curves_.at(find_interval(t))).derivate(t, order);
}
/**
* @brief compute_derivate return a piecewise_curve which is the derivative of this at given order
* @param order order of derivative
* @return
*/
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve> compute_derivate(const std::size_t order) const{
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve> res;
for(typename t_curve_t::const_iterator itc = curves_.begin() ; itc < curves_.end() ; ++itc){
res.add_curve(itc->compute_derivate(order));
}
return res;
}
/// \brief Add a new curve to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
/// is equal to \f$T_{max}\f$ of the actual piecewise curve. The curve added should be of type Curve as defined
/// in the template.
......@@ -179,6 +192,7 @@ namespace curves
return pc_res;
}
template<typename Hermite>
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
{
......@@ -216,53 +230,75 @@ namespace curves
}
template<typename Polynomial>
static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max)
static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
convert_discrete_points_to_polynomial(T_Point points, t_time_t time_points)
{
if(Safe &! (points.size()>1))
{
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::invalid_argument("piecewise_curve -> convert_discrete_points_to_polynomial, Error, less than 2 discrete points");
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
}
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
Time discretization_step = (T_max-T_min)/Time(points.size()-1);
Time time_actual = T_min;
// Initialization at first points
point_t actual_point = points[0];
point_t next_point = points[1];
point_t coeff_order_zero(actual_point);
point_t coeff_order_one((next_point-actual_point)/discretization_step);
t_point_t coeffs;
coeffs.push_back(coeff_order_zero);
coeffs.push_back(coeff_order_one);
Polynomial pol(coeffs,time_actual,time_actual+discretization_step);
piecewise_curve_out_t ppc(pol);
time_actual += discretization_step;
// Other points
for (std::size_t i=1; i<points.size()-2; i++)
if(points.size() != time_points.size()){
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same size.");
}
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
for(size_t i = 1 ; i < points.size() ; ++i){
piecewise_res.add_curve(Polynomial(points[i-1],points[i],time_points[i-1],time_points[i]));
}
return piecewise_res;
}
template<typename Polynomial>
static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
convert_discrete_points_to_polynomial(T_Point points,T_Point points_derivative, t_time_t time_points)
{
if(Safe &! (points.size()>1))
{
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
}
if(points.size() != time_points.size()){
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same size.");
}
if(points.size() != points_derivative.size()){
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the same size.");
}
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
for(size_t i = 1 ; i < points.size() ; ++i){
piecewise_res.add_curve(Polynomial(points[i-1],points_derivative[i-1],points[i],points_derivative[i],time_points[i-1],time_points[i]));
}
return piecewise_res;
}
template<typename Polynomial>
static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
convert_discrete_points_to_polynomial(T_Point points,T_Point points_derivative, T_Point points_second_derivative, t_time_t time_points)
{
if(Safe &! (points.size()>1))
{
coeffs.clear();
actual_point = points[i];
next_point = points[i+1];
coeff_order_zero = actual_point;
coeff_order_one = (next_point-actual_point)/discretization_step;
coeffs.push_back(coeff_order_zero);
coeffs.push_back(coeff_order_one);
ppc.add_curve(Polynomial(coeffs,time_actual,time_actual+discretization_step));
time_actual += discretization_step;
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
}
if(points.size() != time_points.size()){
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same size.");
}
if(points.size() != points_derivative.size()){
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the same size.");
}
// Last points
coeffs.clear();
actual_point = points[points.size()-2];
next_point = points[points.size()-1];
coeff_order_zero = actual_point;
coeff_order_one = (next_point-actual_point)/discretization_step;
coeffs.push_back(coeff_order_zero);
coeffs.push_back(coeff_order_one);
ppc.add_curve(Polynomial(coeffs,time_actual,T_max));
return ppc;
if(points.size() != points_second_derivative.size()){
throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_second_derivative must have the same size.");
}
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
for(size_t i = 1 ; i < points.size() ; ++i){
piecewise_res.add_curve(Polynomial(points[i-1],points_derivative[i-1],points_second_derivative[i-1],points[i],points_derivative[i],points_second_derivative[i],time_points[i-1],time_points[i]));
}
return piecewise_res;
}
private:
/// \brief Get index of the interval corresponding to time t for the interpolation.
......@@ -357,4 +393,4 @@ namespace curves
} // end namespace
#endif // _CLASS_PIECEWISE_CURVE
\ No newline at end of file
#endif // _CLASS_PIECEWISE_CURVE
......@@ -84,6 +84,7 @@ namespace curves
safe_check();
}
/// \brief Constructor.
/// \param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
/// it corresponds to the zero degree coefficient.
......@@ -101,6 +102,134 @@ namespace curves
safe_check();
}
///
/// \brief Constructor from boundary condition with C0 : create a polynomial that connect exactly init and end (order 1)
/// \param init the initial point of the curve
/// \param end the final point of the curve
/// \param min : LOWER bound on interval definition of the spline.
/// \param max : UPPER bound on interval definition of the spline.
///
polynomial(const Point& init, const Point& end, const time_t min, const time_t max ):
dim_(init.size()), degree_(1),
T_min_(min), T_max_(max)
{
if(init.size() != end.size())
throw std::invalid_argument("init and end points must have the same dimensions.");
t_point_t coeffs;
coeffs.push_back(init);
coeffs.push_back((end-init)/(max-min));
coefficients_ = init_coeffs(coeffs.begin(), coeffs.end());
safe_check();
}
///
/// \brief Constructor from boundary condition with C1 :
/// create a polynomial that connect exactly init and end and thier first order derivatives(order 3)
/// \param init the initial point of the curve
/// \param d_init the initial value of the derivative of the curve
/// \param end the final point of the curve
/// \param d_end the final value of the derivative of the curve
/// \param min : LOWER bound on interval definition of the spline.
/// \param max : UPPER bound on interval definition of the spline.
///
polynomial(const Point& init,const Point& d_init, const Point& end, const Point& d_end,const time_t min, const time_t max ):
dim_(init.size()), degree_(3),
T_min_(min), T_max_(max)
{
if(init.size() != end.size())
throw std::invalid_argument("init and end points must have the same dimensions.");
if(init.size() != d_init.size())
throw std::invalid_argument("init and d_init points must have the same dimensions.");
if(init.size() != d_end.size())
throw std::invalid_argument("init and d_end points must have the same dimensions.");
/* the coefficients [c0 c1 c2 c3] are found by solving the following system of equation
(found from the boundary conditions) :
[1 0 0 0 ] [c0] [ init ]
[1 T T^2 T^3 ] x [c1] = [ end ]
[0 1 0 0 ] [c2] [d_init]
[0 1 2T 3T^2] [c3] [d_end ]
*/
double T = max-min;
Eigen::Matrix<double, 4, 4> m;
m << 1.,0,0,0,
1.,T,T*T,T*T*T,
0,1.,0,0,
0,1.,2.*T,3.*T*T;
Eigen::Matrix<double, 4, 4> m_inv = m.inverse();
Eigen::Matrix<double,4,1> bc; // boundary condition vector
coefficients_ = coeff_t::Zero(dim_,degree_+1); // init coefficient matrix with the right size
for(size_t i = 0 ;i < dim_ ; ++i){ // for each dimension, solve the boundary condition problem :
bc[0] = init[i];
bc[1] = end[i];
bc[2] = d_init[i];
bc[3] = d_end[i];
coefficients_.row(i) = (m_inv*bc).transpose();
}
safe_check();
}
///
/// \brief Constructor from boundary condition with C2 :
/// create a polynomial that connect exactly init and end and thier first and second order derivatives(order 5)
/// \param init the initial point of the curve
/// \param d_init the initial value of the derivative of the curve
/// \param d_init the initial value of the second derivative of the curve
/// \param end the final point of the curve
/// \param d_end the final value of the derivative of the curve
/// \param d_end the final value of the second derivative of the curve
/// \param min : LOWER bound on interval definition of the spline.
/// \param max : UPPER bound on interval definition of the spline.
///
polynomial(const Point& init,const Point& d_init,const Point& dd_init, const Point& end, const Point& d_end,const Point& dd_end,const time_t min, const time_t max ):
dim_(init.size()), degree_(5),
T_min_(min), T_max_(max)
{
if(init.size() != end.size())
throw std::invalid_argument("init and end points must have the same dimensions.");
if(init.size() != d_init.size())
throw std::invalid_argument("init and d_init points must have the same dimensions.");
if(init.size() != d_end.size())
throw std::invalid_argument("init and d_end points must have the same dimensions.");
if(init.size() != dd_init.size())
throw std::invalid_argument("init and dd_init points must have the same dimensions.");
if(init.size() != dd_end.size())
throw std::invalid_argument("init and dd_end points must have the same dimensions.");
/* the coefficients [c0 c1 c2 c3 c4 c5] are found by solving the following system of equation
(found from the boundary conditions) :
[1 0 0 0 0 0 ] [c0] [ init ]
[1 T T^2 T^3 T^4 T^5 ] [c1] [ end ]
[0 1 0 0 0 0 ] [c2] [d_init ]
[0 1 2T 3T^2 4T^3 5T^4 ] x [c3] = [d_end ]
[0 0 2 0 0 0 ] [c4] [dd_init]
[0 0 2 6T 12T^2 20T^3] [c5] [dd_end ]
*/
double T = max-min;
Eigen::Matrix<double, 6, 6> m;
m << 1.,0,0,0,0,0,
1.,T,T*T,pow(T,3),pow(T,4),pow(T,5),
0,1.,0,0,0,0,
0,1.,2.*T,3.*T*T,4.*pow(T,3),5.*pow(T,4),
0,0,2,0,0,0,
0,0,2,6.*T,12.*T*T,20.*pow(T,3);
Eigen::Matrix<double, 6, 6> m_inv = m.inverse();
Eigen::Matrix<double,6,1> bc; // boundary condition vector
coefficients_ = coeff_t::Zero(dim_,degree_+1); // init coefficient matrix with the right size
for(size_t i = 0 ;i < dim_ ; ++i){ // for each dimension, solve the boundary condition problem :
bc[0] = init[i];
bc[1] = end[i];
bc[2] = d_init[i];
bc[3] = d_end[i];
bc[4] = dd_init[i];
bc[5] = dd_end[i];
coefficients_.row(i) = (m_inv*bc).transpose();
}
safe_check();
}
/// \brief Destructor
~polynomial()
{
......@@ -123,11 +252,11 @@ namespace curves
{
if(T_min_ > T_max_)
{
std::invalid_argument("Tmin should be inferior to Tmax");
throw std::invalid_argument("Tmin should be inferior to Tmax");
}
if(coefficients_.size() != int(degree_+1))
if(coefficients_.cols() != int(degree_+1))
{
std::runtime_error("Spline order and coefficients do not match");
throw std::runtime_error("Spline order and coefficients do not match");
}
}
}
......@@ -203,6 +332,8 @@ namespace curves
coeff_t deriv_coeff(coeff_t coeff) const
{
if(coeff.cols() == 1) // only the constant part is left, fill with 0
return coeff_t::Zero(coeff.rows(),1);
coeff_t coeff_derivated(coeff.rows(), coeff.cols()-1);
for (std::size_t i=0; i<std::size_t(coeff_derivated.cols()); i++) {
coeff_derivated.col(i) = coeff.col(i+1)*(num_t)(i+1);
......
......@@ -140,6 +140,18 @@ namespace curves
{
return new polynomial_t(array, 0., 1.);
}
polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree1(const pointX_t& init,const pointX_t& end,const real min, const real max)
{
return new polynomial_t(init,end,min,max);
}
polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree3(const pointX_t& init,const pointX_t& d_init,const pointX_t& end,const pointX_t& d_end,const real min, const real max)
{
return new polynomial_t(init,d_init,end,d_end,min,max);
}
polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree5(const pointX_t& init,const pointX_t& d_init,const pointX_t& dd_init,const pointX_t& end,const point_t& d_end,const point_t& dd_end,const real min, const real max)
{
return new polynomial_t(init,d_init,dd_init,end,d_end,dd_end,min,max);
}
/* End wrap polynomial */
/* Wrap piecewise curve */
......@@ -167,6 +179,49 @@ namespace curves
{
return new piecewise_cubic_hermite_curve_t();
}
static piecewise_polynomial_curve_t discretPointToPolynomialC0(const pointX_list_t& points, const time_waypoints_t& time_points){
t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,time_points_list);
}
static piecewise_polynomial_curve_t discretPointToPolynomialC1(const pointX_list_t& points,const pointX_list_t& points_derivative, const time_waypoints_t& time_points){
t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,time_points_list);
}
static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list_t& points,const pointX_list_t& points_derivative,const pointX_list_t& points_second_derivative, const time_waypoints_t& time_points){
t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
t_pointX_t points_second_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_second_derivative);
t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,points_second_derivative_list,time_points_list);
}
void addFinalPointC0(piecewise_polynomial_curve_t self,const pointX_t& end,const real time){
if(self.is_continuous(1))
std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only guarantee C0 continuity."<<std::endl;
polynomial_t pol(self(self.max()),end,self.max(),time);
self.add_curve(pol);
}
void addFinalPointC1(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const real time){
if(self.is_continuous(2))
std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C2 continuity and only guarantee C1 continuity."<<std::endl;
if(!self.is_continuous(1))
std::cout<<"Warning: the current piecewise curve is not C1 continuous."<<std::endl;
polynomial_t pol(self(self.max()),self.derivate(self.max(),1),end,d_end,self.max(),time);
self.add_curve(pol);
}
void addFinalPointC2(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const pointX_t& dd_end,const real time){
if(self.is_continuous(3))
std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C3 continuity and only guarantee C2 continuity."<<std::endl;
if(!self.is_continuous(2))
std::cout<<"Warning: the current piecewise curve is not C2 continuous."<<std::endl;
polynomial_t pol(self(self.max()),self.derivate(self.max(),1),self.derivate(self.max(),2),end,d_end,dd_end,self.max(),time);
self.add_curve(pol);
}
/* end wrap piecewise polynomial curve */
/* Wrap exact cubic spline */
......@@ -325,14 +380,30 @@ namespace curves
/** END variable points bezier curve**/
/** BEGIN polynomial curve function**/
class_<polynomial_t>("polynomial", init<>())
.def("__init__", make_constructor(&wrapPolynomialConstructor1),
.def("__init__", make_constructor(&wrapPolynomialConstructor1,default_call_policies(),args("coeffs","min","max")),
"Create polynomial spline from an Eigen matrix of coefficient defined for t \in [min,max]."
" The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
" Spline order is given by the number of the columns -1.")
.def("__init__", make_constructor(&wrapPolynomialConstructor2),
.def("__init__", make_constructor(&wrapPolynomialConstructor2,default_call_policies(),arg("coeffs")),
"Create polynomial spline from an Eigen matrix of coefficient defined for t \in [0,1]."
" The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
" Spline order is given by the number of the columns -1.")
.def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree1,
default_call_policies(),args("init","end","min","max")),
"Create a polynomial of degree 1 defined for t \in [min,max], "
"such that c(min) == init and c(max) == end.")
.def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree3,
default_call_policies(),args("init","d_init","end","d_end","min","max")),
"Create a polynomial of degree 3 defined for t \in [min,max], "
"such that c(min) == init and c(max) == end"
" dc(min) == d_init and dc(max) == d_end")
.def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree5,
default_call_policies(),
args("init","d_init","dd_init","end","d_end","dd_end","min","max")),
"Create a polynomial of degree 5 defined for t \in [min,max], "
"such that c(min) == init and c(max) == end"
" dc(min) == d_init and dc(max) == d_end"
" ddc(min) == dd_init and ddc(max) == dd_end")
.def("min", &polynomial_t::min, "Get the LOWER bound on interval definition of the curve.")
.def("max", &polynomial_t::max,"Get the HIGHER bound on interval definition of the curve.")
.def("dim", &polynomial_t::dim)
......@@ -352,14 +423,28 @@ namespace curves
/** BEGIN piecewise curve function **/
class_<piecewise_polynomial_curve_t>
("piecewise_polynomial_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor),
.def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor,default_call_policies(),arg("curve")),
"Create a peicewise-polynomial curve containing the given polynomial curve.")
.def("FromPointsList",&discretPointToPolynomialC0,
"Create a piecewise-polynomial connecting exactly all the given points at the given time. The created piecewise is C0 continuous.",args("points","time_points"))
.def("FromPointsList",&discretPointToPolynomialC1,
"Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative values. The created piecewise is C1 continuous.",args("points","points_derivative","time_points"))
.def("FromPointsList",&discretPointToPolynomialC2,
"Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative and second derivative values. The created piecewise is C2 continuous.",args("points","points_derivative","points_second_derivative","time_points"))
.staticmethod("FromPointsList")
.def("append",&addFinalPointC0,
"Append a new polynomial curve of degree 1 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end",args("self","end","time"))
.def("append",&addFinalPointC1,
"Append a new polynomial curve of degree 3 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C1 continuity and guarantee that self.derivate(time,1) == d_end",args("self","end","d_end","time"))
.def("append",&addFinalPointC2,
"Append a new polynomial curve of degree 5 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C2 continuity and guarantee that self.derivate(time,1) == d_end and self.derivate(time,2) == dd_end",args("self","end","d_end","d_end","time"))
.def("min", &piecewise_polynomial_curve_t::min,"Set the LOWER bound on interval definition of the curve.")
.def("max", &piecewise_polynomial_curve_t::max,"Set the HIGHER bound on interval definition of the curve.")
.def("dim", &piecewise_polynomial_curve_t::dim)
.def("__call__", &piecewise_polynomial_curve_t::operator(),"Evaluate the curve at the given time.")
.def("derivate", &piecewise_polynomial_curve_t::derivate,"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
.def("add_curve", &piecewise_polynomial_curve_t::add_curve,
.def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
.def("append", &piecewise_polynomial_curve_t::add_curve,
"Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
"where T_{min} is equal toT_{max} of the actual piecewise curve.")
.def("is_continuous", &piecewise_polynomial_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
......@@ -379,6 +464,7 @@ namespace curves
.def("dim", &piecewise_bezier_curve_t::dim)
.def("__call__", &piecewise_bezier_curve_t::operator())
.def("derivate", &piecewise_bezier_curve_t::derivate)
.def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
.def("add_curve", &piecewise_bezier_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_curve_t::is_continuous)
.def("saveAsText", &piecewise_bezier_curve_t::saveAsText<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
......
......@@ -19,6 +19,7 @@ namespace curves
typedef Eigen::VectorXd time_waypoints_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
typedef std::vector<real> t_time_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef std::vector<point6_t,Eigen::aligned_allocator<point6_t> > t_point6_t;
typedef std::pair<real, point_t> Waypoint;
......@@ -36,5 +37,15 @@ namespace curves
}
return res;
}
template <typename PointList, typename T_Point>
T_Point vectorFromEigenVector(const PointList& vector)
{
T_Point res;
for(int i =0;i<vector.rows();++i)
{
res.push_back(vector[i]);
}
return res;
}
} //namespace curves
#endif //_DEFINITION_PYTHON_BINDINGS
import unittest
import os
from numpy import matrix
from numpy import matrix, array_equal, isclose,random
from numpy.linalg import norm
#from curves import ( serialize_polynomial, deserialize_polynomial, serialize_piecewise_polynomial_curve, deserialize_piecewise_polynomial_curve )
......@@ -128,6 +128,58 @@ class TestCurves(unittest.TestCase):
os.remove("serialization_curve.test")
return
def test_polynomial_from_boundary_condition(self):
p0 = matrix([1.,3.,-2.]).T
p1 = matrix([0.6,2.,2.5]).T
dp0 = matrix([-6.,2.,-1.]).T
dp1 = matrix([10.,10.,10.]).T
ddp0 = matrix([1.,-7.,4.5]).T
ddp1 = matrix([6.,-1.,-4]).T
min = 1.
max = 2.5
polC0 = polynomial(p0,p1,min,max)
self.assertEqual(polC0.min(), min)
self.assertEqual(polC0.max(),max)
self.assertTrue(array_equal(polC0(min), p0))
self.assertTrue(array_equal(polC0(max), p1))
self.assertTrue(array_equal(polC0((min+max)/2.),0.5*p0+0.5*p1))
polC1 = polynomial(p0,dp0,p1,dp1,min,max)
self.assertEqual(polC1.min(), min)
self.assertEqual(polC1.max(),max)
self.assertTrue(isclose(polC1(min), p0).all())
self.assertTrue(isclose(polC1(max), p1).all())
self.assertTrue(isclose(polC1.derivate(min,1), dp0).all())
self.assertTrue(isclose(polC1.derivate(max,1), dp1).all())
polC2 = polynomial(p0,dp0,ddp0,p1,dp1,ddp1,min,max)
self.assertEqual(polC2.min(), min)
self.assertEqual(polC2.max(),max)
self.assertTrue(isclose(polC2(min), p0).all())
self.assertTrue(isclose(polC2(max), p1).all())
self.assertTrue(isclose(polC2.derivate(min,1), dp0).all())
self.assertTrue(isclose(polC2.derivate(max,1), dp1).all())
self.assertTrue(isclose(polC2.derivate(min,2), ddp0).all())
self.assertTrue(isclose(polC2.derivate(max,2), ddp1).all())
# check that the exception are correctly raised :
try:
polC0 = polynomial(p0,p1,max,min)
self.assertTrue(False) # should never get there
except ValueError:
pass
try:
polC1 = polynomial(p0,dp0,p1,dp1,max,min)
self.assertTrue(False) # should never get there
except ValueError:
pass
try:
polC2 = polynomial(p0,dp0,ddp0,p1,dp1,ddp1,max,min)
self.assertTrue(False) # should never get there
except ValueError:
pass
return
def test_cubic_hermite_spline(self):
print("test_cubic_hermite_spline")
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
......@@ -174,7 +226,7 @@ class TestCurves(unittest.TestCase):
a = polynomial(waypoints1, 0., 1.)
b = polynomial(waypoints2, 1., 3.)
pc = piecewise_polynomial_curve(a)
pc.add_curve(b)
pc.append(b)
pc.min()
pc.max()
pc(0.4)
......@@ -191,8 +243,64 @@ class TestCurves(unittest.TestCase):
os.remove("serialization_pc.test")
return
def test_piecewise_bezier_curve(self):
print("test_piecewise_bezier_curve")
def test_piecewise_from_points_list(self):
N = 7
points = matrix(random.rand(3,N))
points_derivative = matrix(random.rand(3,N))
points_second_derivative = matrix(random.rand(3,N))
time_points = matrix(random.rand(N)).T
time_points.sort(0)
polC0 =piecewise_polynomial_curve.FromPointsList(points,time_points)
self.assertEqual(polC0.min(),time_points[0,0])
self.assertEqual(polC0.max(),time_points[-1,0])
self.assertTrue(polC0.is_continuous(0))
self.assertTrue(not polC0.is_continuous(1))
for i in range(N):
self.assertTrue(isclose(polC0(time_points[i,0]),points[:,i]).all())
polC1 =piecewise_polynomial_curve.FromPointsList(points,points_derivative,time_points)
self.assertEqual(polC1.min(),time_points[0,0])
self.assertEqual(polC1.max(),time_points[-1,0])
self.assertTrue(polC1.is_continuous(0))
self.assertTrue(polC1.is_continuous(1))
self.assertTrue(not polC1.is_continuous(2))
for i in range(N):
self.assertTrue(isclose(polC1(time_points[i,0]),points[:,i]).all())
self.assertTrue(isclose(polC1.derivate(time_points[i,0],1),points_derivative[:,i]).all())