Skip to content
GitLab
Commit ea0f5767 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Merge branch 'topic/bindingPythonDynamicCurves' into devel

parents a9efbf46 cf72bc1b
Expand all Hide whitespace changes
Inline Side-by-side
include/curves/bezier_curve.h
View file @ ea0f5767
......@@ -28,7 +28,7 @@ namespace curves
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
template<typename Time= double, typename Numeric=Time, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
{
......@@ -38,14 +38,14 @@ namespace curves
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;
typedef bezier_curve<Time, Numeric, Safe, Point > bezier_curve_t;
/* Constructors - destructors */
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
bezier_curve()
: T_min_(0), T_max_(0)
: dim_(0), T_min_(0), T_max_(0)
{}
/// \brief Constructor.
......@@ -74,6 +74,11 @@ namespace curves
{
control_points_.push_back(*it);
}
// set dim
if (control_points_.size()!=0)
{
dim_ = PointsBegin->size();
}
}
/// \brief Constructor
......@@ -102,10 +107,15 @@ namespace curves
{
control_points_.push_back(*cit);
}
// set dim
if (control_points_.size()!=0)
{
dim_ = PointsBegin->size();
}
}
bezier_curve(const bezier_curve& other)
: T_min_(other.T_min_), T_max_(other.T_max_),
: dim_(other.dim_), T_min_(other.T_min_), T_max_(other.T_max_),
mult_T_(other.mult_T_), size_(other.size_),
degree_(other.degree_), bernstein_(other.bernstein_),
control_points_(other.control_points_)
......@@ -123,7 +133,7 @@ namespace curves
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
check_if_not_empty();
check_conditions();
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
......@@ -144,7 +154,7 @@ namespace curves
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t compute_derivate(const std::size_t order) const
{
check_if_not_empty();
check_conditions();
if(order == 0)
{
return *this;
......@@ -156,7 +166,7 @@ namespace curves
}
if(derived_wp.empty())
{
derived_wp.push_back(point_t::Zero(Dim));
derived_wp.push_back(point_t::Zero(dim_));
}
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) );
return deriv.compute_derivate(order-1);
......@@ -169,14 +179,14 @@ namespace curves
/// \return primitive at order N of x(t).
bezier_curve_t compute_primitive(const std::size_t order) const
{
check_if_not_empty();
check_conditions();
if(order == 0)
{
return *this;
}
num_t new_degree = (num_t)(degree_+1);
t_point_t n_wp;
point_t current_sum = point_t::Zero(Dim);
point_t current_sum = point_t::Zero(dim_);
// recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant.
// then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1
n_wp.push_back(current_sum);
......@@ -213,7 +223,7 @@ namespace curves
point_t evalBernstein(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
point_t res = point_t::Zero(Dim);
point_t res = point_t::Zero(dim_);
typename t_point_t::const_iterator control_points_it = control_points_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
cit !=bernstein_.end(); ++cit, ++control_points_it)
......@@ -316,7 +326,7 @@ namespace curves
///
std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t)
{
check_if_not_empty();
check_conditions();
if (fabs(t-T_max_)<MARGIN)
{
throw std::runtime_error("can't split curve, interval range is equal to original curve");
......@@ -388,17 +398,24 @@ namespace curves
return res;
}
void check_if_not_empty() const
void check_conditions() const
{
if (control_points_.size() == 0)
{
throw std::runtime_error("Error in beziercurve : there is no control points set / did you use empty constructor ?");
throw std::runtime_error("Error in bezier curve : there is no control points set / did you use empty constructor ?");
}
else if(dim_ == 0)
{
throw std::runtime_error("Error in bezier curve : Dimension of points is zero / did you use empty constructor ?");
}
}
/*Operations*/
public:
/*Helpers*/
/// \brief Get dimension of curve.
/// \return dimension of curve.
std::size_t virtual dim() const{return dim_;};
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
virtual time_t min() const{return T_min_;}
......@@ -408,6 +425,8 @@ namespace curves
/*Helpers*/
/* Attributes */
/// Dim of curve
std::size_t dim_;
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
/*const*/ time_t T_min_;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
......@@ -420,10 +439,10 @@ namespace curves
static const double MARGIN;
/* Attributes */
static bezier_curve_t zero(const time_t T=1.)
static bezier_curve_t zero(const std::size_t dim, const time_t T=1.)
{
std::vector<point_t> ts;
ts.push_back(point_t::Zero(Dim));
ts.push_back(point_t::Zero(dim));
return bezier_curve_t(ts.begin(), ts.end(),0.,T);
}
......@@ -435,6 +454,7 @@ namespace curves
if (version) {
// Do something depending on version ?
}
ar & boost::serialization::make_nvp("dim", dim_);
ar & boost::serialization::make_nvp("T_min", T_min_);
ar & boost::serialization::make_nvp("T_max", T_max_);
ar & boost::serialization::make_nvp("mult_T", mult_T_);
......@@ -445,8 +465,8 @@ namespace curves
}
}; // End struct bezier_curve
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
const double bezier_curve<Time, Numeric, Dim, Safe, Point>::MARGIN(0.001);
template<typename Time, typename Numeric, bool Safe, typename Point>
const double bezier_curve<Time, Numeric, Safe, Point>::MARGIN(0.001);
} // namespace curve
#endif //_CLASS_BEZIERCURVE
......
include/curves/cubic_hermite_spline.h
View file @ ea0f5767
......@@ -30,12 +30,11 @@ namespace curves
/// - crosses each of the waypoint given in its initialization (\f$P_0\f$, \f$P_1\f$,...,\f$P_N\f$).
/// - has its derivatives on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$p'(t_{P_i}) = m_i\f$ and \f$p'(t_{P_{i+1}}) = m_{i+1}\f$.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename Tangent= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
template<typename Time= double, typename Numeric=Time, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
{
typedef std::pair<Point, Tangent> pair_point_tangent_t;
typedef std::pair<Point, Point> pair_point_tangent_t;
typedef std::vector< pair_point_tangent_t ,Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
typedef std::vector<Time> vector_time_t;
typedef Numeric num_t;
......@@ -44,7 +43,7 @@ namespace curves
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
cubic_hermite_spline()
: T_min_(0), T_max_(0)
: dim_(0), T_min_(0), T_max_(0)
{}
/// \brief Constructor.
......@@ -67,12 +66,18 @@ namespace curves
{
control_points_.push_back(*it);
}
// Set dimension according to size of points
if (control_points_.size()!=0)
{
dim_ = control_points_[0].first.size();
}
// Set time
setTime(time_control_points);
}
cubic_hermite_spline(const cubic_hermite_spline& other)
: control_points_(other.control_points_), time_control_points_(other.time_control_points_),
: dim_(other.dim_), control_points_(other.control_points_), time_control_points_(other.time_control_points_),
duration_splines_(other.duration_splines_), T_min_(other.T_min_), T_max_(other.T_max_),
size_(other.size_), degree_(other.degree_)
{}
......@@ -89,7 +94,7 @@ namespace curves
///
virtual Point operator()(const Time t) const
{
check_if_not_empty();
check_conditions();
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
......@@ -111,7 +116,7 @@ namespace curves
///
virtual Point derivate(const Time t, const std::size_t order) const
{
check_if_not_empty();
check_conditions();
return evalCubicHermiteSpline(t, order);
}
......@@ -315,12 +320,16 @@ namespace curves
return left_id-1;
}
void check_if_not_empty() const
void check_conditions() const
{
if (control_points_.size() == 0)
{
throw std::runtime_error("Error in cubic hermite : there is no control points set / did you use empty constructor ?");
}
else if(dim_ == 0)
{
throw std::runtime_error("Error in cubic hermite : Dimension of points is zero / did you use empty constructor ?");
}
}
/// \brief compute duration of each spline.
......@@ -358,6 +367,9 @@ namespace curves
/*Helpers*/
public:
/// \brief Get dimension of curve.
/// \return dimension of curve.
std::size_t virtual dim() const{return dim_;};
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
Time virtual min() const{return time_control_points_.front();}
......@@ -367,12 +379,13 @@ namespace curves
/*Helpers*/
/*Attributes*/
/// Dim of curve
std::size_t dim_;
/// Vector of pair < Point, Tangent >.
t_pair_point_tangent_t control_points_;
/// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
/// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
vector_time_t time_control_points_;
/// Vector of Time corresponding to time duration of each subspline.<br>
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
/// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
......@@ -396,6 +409,7 @@ namespace curves
if (version) {
// Do something depending on version ?
}
ar & boost::serialization::make_nvp("dim", dim_);
ar & boost::serialization::make_nvp("control_points", control_points_);
ar & boost::serialization::make_nvp("time_control_points", time_control_points_);
ar & boost::serialization::make_nvp("duration_splines", duration_splines_);
......
include/curves/cubic_spline.h
View file @ ea0f5767
......@@ -35,13 +35,12 @@ namespace curves
return res;
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe,
typename Point, typename T_Point>
polynomial<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
const Time t_min, const Time t_max)
template<typename Time, typename Numeric, bool Safe, typename Point, typename T_Point>
polynomial<Time,Numeric,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
const Time t_min, const Time t_max)
{
T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d);
return polynomial<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
return polynomial<Time,Numeric,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
}
} // namespace curves
#endif //_STRUCT_CUBICSPLINE
......
include/curves/curve_abc.h
View file @ ea0f5767
......@@ -54,6 +54,9 @@ namespace curves
/*Operations*/
/*Helpers*/
/// \brief Get dimension of curve.
/// \return dimension of curve.
virtual std::size_t dim() const = 0;
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual time_t min() const = 0;
......
include/curves/curve_constraint.h
View file @ ea0f5767
......@@ -19,12 +19,11 @@
namespace curves
{
template <typename Point, std::size_t Dim=3>
template <typename Point>
struct curve_constraints
{
typedef Point point_t;
curve_constraints():
init_vel(point_t::Zero(Dim)),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
curve_constraints(){}
~curve_constraints(){}
......
include/curves/exact_cubic.h
View file @ ea0f5767
......@@ -37,11 +37,11 @@ namespace curves
/// \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,
template<typename Time= double, typename Numeric=Time, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >,
typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
typename SplineBase=polynomial<Time, Numeric, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase>
{
typedef Point point_t;
typedef T_Point t_point_t;
......@@ -53,10 +53,10 @@ namespace curves
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_constraints<Point, Dim> spline_constraints;
typedef curve_constraints<Point> spline_constraints;
typedef exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> exact_cubic_t;
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> piecewise_curve_t;
typedef exact_cubic<Time, Numeric, Safe, Point, T_Point, SplineBase> exact_cubic_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> piecewise_curve_t;
/* Constructors - destructors */
public:
......@@ -119,7 +119,8 @@ namespace curves
template<typename In>
t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
const std::size_t dim = wayPointsBegin->second.size();
const std::size_t size = std::distance(wayPointsBegin, wayPointsEnd);
if(Safe && size < 1)
{
throw std::length_error("size of waypoints must be superior to 0") ; // TODO
......@@ -132,11 +133,11 @@ namespace curves
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);
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;
// Fill the matrices H as specified in the paper.
......@@ -182,9 +183,9 @@ namespace curves
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));
create_cubic<Time,Numeric,Safe,Point,T_Point>(a.row(i), b.row(i),
c.row(i), d.row(i),
(*it).first, (*next).first));
}
return subSplines;
}
......@@ -218,7 +219,7 @@ namespace curves
const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a0,b0,c0,d0,init_t, end_t));
subSplines.push_back(create_cubic<Time,Numeric,Safe,Point,T_Point>(a0,b0,c0,d0,init_t, end_t));
constraints.init_vel = subSplines.back().derivate(end_t, 1);
constraints.init_acc = subSplines.back().derivate(end_t, 2);
}
......@@ -226,9 +227,10 @@ namespace curves
template<typename In>
void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
{
const std::size_t dim = wayPointsBegin->second.size();
const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel,
c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
//solving a system of four linear eq with 4 unknows: d0, e0
......@@ -239,7 +241,7 @@ namespace curves
const num_t x_e_0 = dt_4, x_e_1 = 4*dt_3, x_e_2 = 12*dt_2;
const num_t x_f_0 = dt_5, x_f_1 = 5*dt_4, x_f_2 = 20*dt_3;
point_t d, e, f;
MatrixX rhs = MatrixX::Zero(3,Dim);
MatrixX rhs = MatrixX::Zero(3,dim);
rhs.row(0) = alpha_0; rhs.row(1) = alpha_1; rhs.row(2) = alpha_2;
Matrix3 eq = Matrix3::Zero(3,3);
eq(0,0) = x_d_0; eq(0,1) = x_e_0; eq(0,2) = x_f_0;
......@@ -247,7 +249,7 @@ namespace curves
eq(2,0) = x_d_2; eq(2,1) = x_e_2; eq(2,2) = x_f_2;
rhs = eq.inverse().eval() * rhs;
d = rhs.row(0); e = rhs.row(1); f = rhs.row(2);
subSplines.push_back(create_quintic<Time,Numeric,Dim,Safe,Point,T_Point>(a0,b0,c0,d,e,f, init_t, end_t));
subSplines.push_back(create_quintic<Time,Numeric,Safe,Point,T_Point>(a0,b0,c0,d,e,f, init_t, end_t));
}
public:
......
include/curves/helpers/effector_spline.h
View file @ ea0f5767
......@@ -28,11 +28,11 @@ namespace curves
{
typedef double Numeric;
typedef double Time;
typedef Eigen::Matrix<Numeric, 3, 1> Point;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 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 exact_cubic<Time, Numeric, 3, true, Point, T_Point> exact_cubic_t;
typedef exact_cubic<Time, Numeric, true, Point, T_Point> exact_cubic_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef exact_cubic_t::t_spline_t t_spline_t;
typedef exact_cubic_t::spline_t spline_t;
......@@ -68,6 +68,8 @@ namespace curves
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/)
{
spline_constraints_t constraints;
constraints.init_acc = Point::Zero(end_spline.dim());
constraints.init_vel = Point::Zero(end_spline.dim());
constraints.end_acc = end_spline.derivate(end_spline.min(),2);
constraints.end_vel = end_spline.derivate(end_spline.min(),1);
return constraints;
......
include/curves/helpers/effector_spline_rotation.h
View file @ ea0f5767
......@@ -36,7 +36,7 @@ namespace curves
typedef std::pair<Numeric, quat_t > waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
typedef exact_cubic <Numeric, Numeric, 1, false, point_one_dim_t> exact_cubic_constraint_one_dim;
typedef exact_cubic <Numeric, Numeric, false, point_one_dim_t> exact_cubic_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t > waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
......@@ -87,10 +87,12 @@ namespace curves
return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
}
virtual std::size_t dim() const{return dim_;}
virtual time_t min() const{return min_;}
virtual time_t max() const{return max_;}
/*Attributes*/
std::size_t dim_; //const
Eigen::Quaterniond quat_from_; //const
Eigen::Quaterniond quat_to_; //const
double min_; //const
......@@ -100,7 +102,7 @@ namespace curves
}; // End class rotation_spline
typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
typedef exact_cubic<Time, Numeric, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
/// \class effector_spline_rotation.
/// \brief Represents a trajectory for and end effector.
......
include/curves/linear_variable.h
View file @ ea0f5767
......@@ -116,6 +116,12 @@ namespace curves
return *this;
}
std::size_t size()
{
variables_t w;
return w.size();
}
static variables_t Zero(size_t /*dim*/){
variables_t w;
return w;
......
include/curves/piecewise_curve.h
View file @ ea0f5767
......@@ -22,7 +22,7 @@ namespace curves
/// On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$,
/// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
template<typename Time= double, typename Numeric=Time, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> >,
typename Curve= curve_abc<Time, Numeric, Safe, Point> >
......@@ -40,7 +40,7 @@ namespace curves
/// \brief Empty constructor. Add at least one curve to call other class functions.
///
piecewise_curve()
: size_(0), T_min_(0), T_max_(0)
: dim_(0), size_(0), T_min_(0), T_max_(0)
{}
/// \brief Constructor.
......@@ -49,12 +49,17 @@ namespace curves
///
piecewise_curve(const curve_t& cf)
{
dim_ = cf(cf.min()).size();
size_ = 0;
add_curve(cf);
}
piecewise_curve(const t_curve_t list_curves)
{
if (list_curves.size()!=0)
{
dim_ = list_curves[0](list_curves[0].min()).size();
}
size_ = 0;
for( std::size_t i=0; i<list_curves.size(); i++ )
{
......@@ -63,7 +68,7 @@ namespace curves
}
piecewise_curve(const piecewise_curve& other)
: curves_(other.curves_), time_curves_(other.time_curves_), size_(other.size_),
: dim_(other.dim_), curves_(other.curves_), time_curves_(other.time_curves_), size_(other.size_),
T_min_(other.T_min_), T_max_(other.T_max_)
{}
......@@ -102,6 +107,10 @@ namespace curves
///
void add_curve(const curve_t& cf)
{
if (size_==0 && dim_==0)
{
dim_ = cf(cf.min()).size();
}
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
if (size_!=0 && !(fabs(cf.min()-T_max_)<MARGIN))
{
......@@ -153,10 +162,10 @@ namespace curves
}
template<typename Bezier>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
{
check_if_not_empty();
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Bezier first_curve_output = bezier_from_curve<Bezier, curve_t>(first_curve);
......@@ -171,10 +180,10 @@ namespace curves
}
template<typename Hermite>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
{
check_if_not_empty();
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Hermite first_curve_output = hermite_from_curve<Hermite, curve_t>(first_curve);
......@@ -189,10 +198,10 @@ namespace curves
}
template<typename Polynomial>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
{