diff --git a/include/curves/MathDefs.h b/include/curves/MathDefs.h
index 251daa63e98d40f0582743cfe67c76c2379b8255..3c3c99f18476d41042c9d09950afee41bcc55449 100644
--- a/include/curves/MathDefs.h
+++ b/include/curves/MathDefs.h
@@ -21,26 +21,22 @@
#include <vector>
#include <utility>
namespace curves{
-
-//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
-template<typename _Matrix_Type_>
-void PseudoInverse(_Matrix_Type_& pinvmat)
-{
+ //REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
+ template<typename _Matrix_Type_>
+ void PseudoInverse(_Matrix_Type_& pinvmat)
+ {
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues();
-
double pinvtoler= 1.e-6; // choose your tolerance widely!
-
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
- if (m_sigma(i) > pinvtoler)
- {
- m_sigma_inv(i,i)=1.0/m_sigma(i);
- }
+ if (m_sigma(i) > pinvtoler)
+ {
+ m_sigma_inv(i,i)=1.0/m_sigma(i);
+ }
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
-}
-
+ }
} // namespace curves
#endif //_SPLINEMATH
diff --git a/include/curves/bernstein.h b/include/curves/bernstein.h
index 745a7dc858453f59089304fd8b440d19362a3662..3d2184597b3e89c3c45c6483dd8aa5080c81d887 100644
--- a/include/curves/bernstein.h
+++ b/include/curves/bernstein.h
@@ -22,71 +22,71 @@
namespace curves
{
-/// \brief Computes a binomial coefficient.
-/// \param n : an unsigned integer.
-/// \param k : an unsigned integer.
-/// \return \f$\binom{n}{k}f$
-///
-inline unsigned int bin(const unsigned int n, const unsigned int k)
-{
- if(k > n)
- throw std::runtime_error("binomial coefficient higher than degree");
- if(k == 0)
- return 1;
- if(k > n/2)
- return bin(n,n-k);
+ /// \brief Computes a binomial coefficient .
+ /// \param n : an unsigned integer.
+ /// \param k : an unsigned integer.
+ /// \return \f$\binom{n}{k}f$
+ ///
+ inline unsigned int bin(const unsigned int n, const unsigned int k)
+ {
+ if(k > n) throw std::runtime_error("binomial coefficient higher than degree");
+ if(k == 0) return 1;
+ if(k > n/2) return bin(n,n-k);
return n * bin(n-1,k-1) / k;
-}
-
-/// \class Bernstein.
-/// \brief Computes a Bernstein polynome.
-///
-template <typename Numeric = double>
-struct Bern : public serialization::Serializable< Bern<Numeric> > {
-
-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_;
-
-// Serialization of the class
-friend class boost::serialization::access;
+ }
+
+ /// \class Bernstein.
+ /// \brief Computes a Bernstein polynome.
+ ///
+ template <typename Numeric = double>
+ struct Bern : public serialization::Serializable< Bern<Numeric> > {
+ 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);
+ }
-template<class Archive>
-void serialize(Archive& ar, const unsigned int version){
- if (version) {
- // Do something depending on version ?
+ /* Attributes */
+ Numeric m_minus_i;
+ Numeric i_;
+ Numeric bin_m_i_;
+ /* Attributes */
+
+ // Serialization of the class
+ friend class boost::serialization::access;
+
+ template<class Archive>
+ void serialize(Archive& ar, const unsigned int version){
+ if (version) {
+ // Do something depending on version ?
+ }
+ ar & boost::serialization::make_nvp("m_minus_i", m_minus_i);
+ ar & boost::serialization::make_nvp("i", i_);
+ ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_);
}
- ar & boost::serialization::make_nvp("m_minus_i", m_minus_i);
- ar & boost::serialization::make_nvp("i", i_);
- ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_);
-}
-};
+ }; // End struct Bern
-/// \brief Computes all Bernstein polynomes for a certain degree.
-///
-template <typename Numeric>
-std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
-{
+ /// \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));
+ {
+ res.push_back(Bern<Numeric>(n, i));
+ }
return res;
-}
+ }
} // namespace curves
#endif //_CLASS_BERNSTEIN
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index 3b508c1bd5049479c1fbee85f2a00c7c90f1a3ed..3f3c4cdd49ccd25051f50c3512fcd9ec5709bf21 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -26,16 +26,16 @@
namespace curves
{
-/// \class BezierCurve.
-/// \brief Represents a Bezier curve of arbitrary dimension and order.
-/// 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
-, typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
-struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
- public serialization::Serializable< bezier_curve<Time, Numeric, Dim, Safe, Point> >
-{
+ /// \class BezierCurve.
+ /// \brief Represents a Bezier curve of arbitrary dimension and order.
+ /// 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
+ , typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
+ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
+ public serialization::Serializable< bezier_curve<Time, Numeric, Dim, Safe, Point> >
+ {
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
@@ -44,132 +44,133 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
-/* Constructors - destructors */
+ /* Constructors - destructors */
public:
- bezier_curve(){}
-
- /// \brief Constructor.
- /// Given the first and last point of a control points set, create the bezier curve.
- /// \param PointsBegin : an iterator pointing to the first element of a control point container.
- /// \param PointsEnd : an iterator pointing to the last element of a control point container.
- /// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
- /// \param mult_T : ... (default value is 1.0).
- ///
- template<typename In>
- bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
- : T_min_(T_min)
- , T_max_(T_max)
- , mult_T_(mult_T)
- , size_(std::distance(PointsBegin, PointsEnd))
- , degree_(size_-1)
- , bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
- {
+ bezier_curve(){}
+
+ /// \brief Constructor.
+ /// Given the first and last point of a control points set, create the bezier curve.
+ /// \param PointsBegin : an iterator pointing to the first element of a control point container.
+ /// \param PointsEnd : an iterator pointing to the last element of a control point container.
+ /// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
+ /// \param mult_T : ... (default value is 1.0).
+ ///
+ template<typename In>
+ bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
+ : T_min_(T_min),
+ T_max_(T_max),
+ mult_T_(mult_T),
+ size_(std::distance(PointsBegin, PointsEnd)),
+ degree_(size_-1),
+ bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
+ {
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || T_max_ <= T_min_))
{
- throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
+ throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
}
for(; it != PointsEnd; ++it)
{
- control_points_.push_back(*it);
+ control_points_.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 : an iterator pointing to the first element of a control point container.
- /// \param PointsEnd : an iterator pointing to the last element of a control point container.
- /// \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 T_min=0., const time_t T_max=1., const time_t mult_T=1.)
- : T_min_(T_min)
- , T_max_(T_max)
- , mult_T_(mult_T)
- , size_(std::distance(PointsBegin, PointsEnd)+4)
- , degree_(size_-1)
- , bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
- {
+ }
+
+ /// \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 : an iterator pointing to the first element of a control point container.
+ /// \param PointsEnd : an iterator pointing to the last element of a control point container.
+ /// \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 T_min=0., const time_t T_max=1., const time_t mult_T=1.)
+ : T_min_(T_min),
+ T_max_(T_max),
+ mult_T_(mult_T),
+ size_(std::distance(PointsBegin, PointsEnd)+4),
+ degree_(size_-1),
+ bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
+ {
if(Safe && (size_<1 || T_max_ <= T_min_))
{
- throw std::invalid_argument("can't create bezier min bound is higher than max bound");
+ throw std::invalid_argument("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)
{
- control_points_.push_back(*cit);
+ control_points_.push_back(*cit);
}
- }
-
- bezier_curve(const bezier_curve& other)
- : 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_)
- {}
-
- ///\brief Destructor
- ~bezier_curve()
- {
+ }
+
+ bezier_curve(const bezier_curve& other)
+ : 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_)
+ {}
+
+ ///\brief Destructor
+ ~bezier_curve()
+ {
// NOTHING
- }
-
- private:
-// bezier_curve(const bezier_curve&);
-// bezier_curve& operator=(const bezier_curve&);
-/* Constructors - destructors */
-
-/*Operations*/
- public:
- /// \brief Evaluation of the bezier curve at time t.
- /// \param t : time when to evaluate the curve.
- /// \return \f$x(t)\f$ point corresponding on curve at time t.
- virtual point_t operator()(const time_t t) const
- {
+ }
+
+ /*Operations*/
+ /// \brief Evaluation of the bezier curve at time t.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
+ virtual point_t operator()(const time_t t) const
+ {
if(Safe &! (T_min_ <= t && t <= T_max_))
{
- throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
+ throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
}
if (size_ == 1)
{
return mult_T_*control_points_[0];
- }else
+ }
+ else
{
return evalHorner(t);
}
- }
-
- /// \brief Compute the derived curve at order N.
- /// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
- /// \param order : order of derivative.
- /// \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
- {
+ }
+
+ /// \brief Compute the derived curve at order N.
+ /// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
+ /// \param order : order of derivative.
+ /// \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
+ {
if(order == 0)
{
- return *this;
+ return *this;
}
t_point_t derived_wp;
for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end()-1; ++pit)
- derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
+ {
+ derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
+ }
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);
- }
-
- /// \brief Compute the primitive of the curve at order N.
- /// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
- /// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
- /// \param order : order of the primitive.
- /// \return primitive at order N of x(t).
- bezier_curve_t compute_primitive(const std::size_t order) const
- {
+ }
+
+ /// \brief Compute the primitive of the curve at order N.
+ /// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
+ /// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
+ /// \param order : order of the primitive.
+ /// \return primitive at order N of x(t).
+ bezier_curve_t compute_primitive(const std::size_t order) const
+ {
if(order == 0)
{
- return *this;
+ return *this;
}
num_t new_degree = (num_t)(degree_+1);
t_point_t n_wp;
@@ -179,61 +180,61 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
n_wp.push_back(current_sum);
for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit)
{
- current_sum += *pit;
- n_wp.push_back(current_sum / new_degree);
+ current_sum += *pit;
+ n_wp.push_back(current_sum / new_degree);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_min_, T_max_, mult_T_*(T_max_-T_min_));
return integ.compute_primitive(order-1);
- }
-
- /// \brief Evaluate the derivative order N of curve at time t.
- /// If derivative is to be evaluated several times, it is
- /// rather recommended to compute derived curve using compute_derivate.
- /// \param order : order of derivative.
- /// \param t : time when to evaluate the curve.
- /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
- ///
- virtual point_t derivate(const time_t t, const std::size_t order) const
- {
+ }
+
+ /// \brief Evaluate the derivative order N of curve at time t.
+ /// If derivative is to be evaluated several times, it is
+ /// rather recommended to compute derived curve using compute_derivate.
+ /// \param order : order of derivative.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
+ ///
+ virtual point_t derivate(const time_t t, const std::size_t order) const
+ {
bezier_curve_t deriv = compute_derivate(order);
return deriv(t);
- }
-
- /// \brief Evaluate all Bernstein polynomes for a certain degree.
- /// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
- /// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
- /// Warning: the horner scheme is about 100 times faster than this method.<br>
- /// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
- /// \param t : time when to evaluate the curve.
- /// \return \f$x(t)\f$ point corresponding on curve at time t.
- ///
- point_t evalBernstein(const Numeric t) const
- {
+ }
+
+ /// \brief Evaluate all Bernstein polynomes for a certain degree.
+ /// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
+ /// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
+ /// Warning: the horner scheme is about 100 times faster than this method.<br>
+ /// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
+ ///
+ point_t evalBernstein(const Numeric t) const
+ {
const Numeric u = (t-T_min_)/(T_max_-T_min_);
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)
+ cit !=bernstein_.end(); ++cit, ++control_points_it)
{
- res += cit->operator()(u) * (*control_points_it);
+ res += cit->operator()(u) * (*control_points_it);
}
return res*mult_T_;
- }
-
- /// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
- /// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
- /// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
- /// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
- /// Horner's scheme : for a polynom of degree N expressed by : <br>
- /// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
- /// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
- /// Using Horner's method, the polynom is transformed into : <br>
- /// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
- /// \param t : time when to evaluate the curve.
- /// \return \f$x(t)\f$ point corresponding on curve at time t.
- ///
- point_t evalHorner(const Numeric t) const
- {
+ }
+
+ /// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
+ /// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
+ /// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
+ /// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
+ /// Horner's scheme : for a polynom of degree N expressed by : <br>
+ /// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
+ /// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
+ /// Using Horner's method, the polynom is transformed into : <br>
+ /// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
+ ///
+ point_t evalHorner(const Numeric t) const
+ {
const Numeric u = (t-T_min_)/(T_max_-T_min_);
typename t_point_t::const_iterator control_points_it = control_points_.begin();
Numeric u_op, bc, tn;
@@ -243,75 +244,79 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
point_t tmp =(*control_points_it)*u_op; ++control_points_it;
for(unsigned int i=1; i<degree_; i++, ++control_points_it)
{
- tn = tn*u;
- bc = bc*((num_t)(degree_-i+1))/i;
- tmp = (tmp + tn*bc*(*control_points_it))*u_op;
+ tn = tn*u;
+ bc = bc*((num_t)(degree_-i+1))/i;
+ tmp = (tmp + tn*bc*(*control_points_it))*u_op;
}
return (tmp + tn*u*(*control_points_it))*mult_T_;
- }
-
- const t_point_t& waypoints() const {return control_points_;}
-
- /// \brief Evaluate the curve value at time t using deCasteljau algorithm.
- /// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
- /// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
- /// curve at time \f$t\f$.
- /// \param t : time when to evaluate the curve.
- /// \return \f$x(t)\f$ point corresponding on curve at time t.
- ///
- point_t evalDeCasteljau(const Numeric t) const {
+ }
+
+ const t_point_t& waypoints() const {return control_points_;}
+
+ /// \brief Evaluate the curve value at time t using deCasteljau algorithm.
+ /// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
+ /// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
+ /// curve at time \f$t\f$.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
+ ///
+ point_t evalDeCasteljau(const Numeric t) const
+ {
// normalize time :
const Numeric u = (t-T_min_)/(T_max_-T_min_);
t_point_t pts = deCasteljauReduction(waypoints(),u);
while(pts.size() > 1)
{
- pts = deCasteljauReduction(pts,u);
+ pts = deCasteljauReduction(pts,u);
}
return pts[0]*mult_T_;
- }
+ }
- t_point_t deCasteljauReduction(const Numeric t) const{
+ t_point_t deCasteljauReduction(const Numeric t) const
+ {
const Numeric u = (t-T_min_)/(T_max_-T_min_);
return deCasteljauReduction(waypoints(),u);
- }
-
- /// \brief Compute de Casteljau's reduction of the given list of points at time t.
- /// For the list \f$pts\f$ of N points, compute a new list of points of size N-1 :<br>
- /// \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$<br>
- /// with t the time when to evaluate bezier curve.<br>\ The new list contains centroid of
- /// parameters \f${t,1-t}\f$ of consecutive points in the list.
- /// \param pts : list of points.
- /// \param u : NORMALIZED time when to evaluate the curve.
- /// \return reduced list of point (size of pts - 1).
- ///
- t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const{
+ }
+
+ /// \brief Compute de Casteljau's reduction of the given list of points at time t.
+ /// For the list \f$pts\f$ of N points, compute a new list of points of size N-1 :<br>
+ /// \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$<br>
+ /// with t the time when to evaluate bezier curve.<br>\ The new list contains centroid of
+ /// parameters \f${t,1-t}\f$ of consecutive points in the list.
+ /// \param pts : list of points.
+ /// \param u : NORMALIZED time when to evaluate the curve.
+ /// \return reduced list of point (size of pts - 1).
+ ///
+ t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const
+ {
if(u < 0 || u > 1)
{
- throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
+ throw std::out_of_range("In deCasteljau reduction : u is not in [0;1]");
}
if(pts.size() == 1)
{
- return pts;
+ return pts;
}
t_point_t new_pts;
for(cit_point_t cit = pts.begin() ; cit != (pts.end() - 1) ; ++cit)
{
- new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
+ new_pts.push_back((1-u) * (*cit) + u*(*(cit+1)));
}
return new_pts;
- }
-
- /// \brief Split the bezier curve in 2 at time t.
- /// \param t : list of points.
- /// \param u : unNormalized time.
- /// \return pair containing the first element of both bezier curve obtained.
- ///
- std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
+ }
+
+ /// \brief Split the bezier curve in 2 at time t.
+ /// \param t : list of points.
+ /// \param u : unNormalized time.
+ /// \return pair containing the first element of both bezier curve obtained.
+ ///
+ std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t)
+ {
if (fabs(t-T_max_)<MARGIN)
{
- throw std::runtime_error("can't split curve, interval range is equal to original curve");
+ throw std::runtime_error("can't split curve, interval range is equal to original curve");
}
t_point_t wps_first(size_),wps_second(size_);
const Numeric u = (t-T_min_)/(T_max_-T_min_);
@@ -321,43 +326,43 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
size_t id = 1;
while(casteljau_pts.size() > 1)
{
- casteljau_pts = deCasteljauReduction(casteljau_pts,u);
- wps_first[id] = casteljau_pts.front();
- wps_second[degree_-id] = casteljau_pts.back();
- ++id;
+ casteljau_pts = deCasteljauReduction(casteljau_pts,u);
+ wps_first[id] = casteljau_pts.front();
+ wps_second[degree_-id] = casteljau_pts.back();
+ ++id;
}
bezier_curve_t c_first(wps_first.begin(), wps_first.end(),T_min_,t,mult_T_);
bezier_curve_t c_second(wps_second.begin(), wps_second.end(),t, T_max_,mult_T_);
return std::make_pair(c_first,c_second);
- }
+ }
- bezier_curve_t extract(const Numeric t1, const Numeric t2){
+ bezier_curve_t extract(const Numeric t1, const Numeric t2){
if(t1 < T_min_ || t1 > T_max_ || t2 < T_min_ || t2 > T_max_)
{
- throw std::out_of_range("In Extract curve : times out of bounds");
+ throw std::out_of_range("In Extract curve : times out of bounds");
}
if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN)
{
- return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
+ return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
}
if (fabs(t1-T_min_)<MARGIN)
{
- return split(t2).first;
+ return split(t2).first;
}
if (fabs(t2-T_max_)<MARGIN)
{
- return split(t1).second;
+ return split(t1).second;
}
-
std::pair<bezier_curve_t,bezier_curve_t> c_split = this->split(t1);
return c_split.second.split(t2-t1).first;
- }
+ }
private:
- template<typename In>
- t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
- {
+
+ template<typename In>
+ t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
+ {
t_point_t res;
num_t T = T_max_ - T_min_;
num_t T_square = T*T;
@@ -367,60 +372,57 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
P_n_1 = PN- constraints.end_vel * T / (num_t)degree_;
P2 = constraints.init_acc * T_square / (num_t)(degree_ * (degree_-1)) + 2* P1 - P0;
P_n_2 = constraints.end_acc * T_square / (num_t)(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(*it);
}
-
res.push_back(P_n_2);
res.push_back(P_n_1);
res.push_back(PN);
return res;
- }
-
-/*Operations*/
+ }
+ /*Operations*/
-/*Helpers*/
public:
- /// \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_;}
- /// \brief Get the maximum time for which the curve is defined.
- /// \return \f$t_{max}\f$, upper bound of time range.
- virtual time_t max() const{return T_max_;}
-/*Helpers*/
-
- public:
- /// 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.
- /*const*/ time_t T_max_;
- /*const*/ time_t mult_T_;
- /*const*/ std::size_t size_;
- /*const*/ std::size_t degree_;
- /*const*/ std::vector<Bern<Numeric> > bernstein_;
- /*const*/ t_point_t control_points_;
- static const double MARGIN;
-
- static bezier_curve_t zero(const time_t T=1.)
- {
+ /*Helpers*/
+ /// \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_;}
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$, upper bound of time range.
+ virtual time_t max() const{return T_max_;}
+ /*Helpers*/
+
+ /* Attributes */
+ /// 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.
+ /*const*/ time_t T_max_;
+ /*const*/ time_t mult_T_;
+ /*const*/ std::size_t size_;
+ /*const*/ std::size_t degree_;
+ /*const*/ std::vector<Bern<Numeric> > bernstein_;
+ /*const*/ t_point_t control_points_;
+ static const double MARGIN;
+ /* Attributes */
+
+ static bezier_curve_t zero(const time_t T=1.)
+ {
std::vector<point_t> ts;
ts.push_back(point_t::Zero(Dim));
return bezier_curve_t(ts.begin(), ts.end(),0.,T);
- }
+ }
- // Serialization of the class
- friend class boost::serialization::access;
+ // Serialization of the class
+ friend class boost::serialization::access;
- template<class Archive>
- void serialize(Archive& ar, const unsigned int version){
+ template<class Archive>
+ void serialize(Archive& ar, const unsigned int version){
if (version) {
- // Do something depending on version ?
+ // Do something depending on version ?
}
ar & boost::serialization::make_nvp("T_min", T_min_);
ar & boost::serialization::make_nvp("T_max", T_max_);
@@ -429,12 +431,11 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
ar & boost::serialization::make_nvp("degree", degree_);
ar & boost::serialization::make_nvp("bernstein", bernstein_);
ar & boost::serialization::make_nvp("control_points", control_points_);
- }
-
-};
+ }
+ }; // 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, std::size_t Dim, bool Safe, typename Point>
+ const double bezier_curve<Time, Numeric, Dim, Safe, Point>::MARGIN(0.001);
} // namespace curve
#endif //_CLASS_BEZIERCURVE
diff --git a/include/curves/bezier_polynomial_conversion.h b/include/curves/bezier_polynomial_conversion.h
deleted file mode 100644
index 062c376ed6025b953c2f2eb361cacd6ac3061830..0000000000000000000000000000000000000000
--- a/include/curves/bezier_polynomial_conversion.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/**
-* \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 curves
-{
-
-/// \brief Converts a Bezier curve to a polynomial.
-/// \param bezier : the Bezier curve to convert.
-/// \return the equivalent polynomial.
-template<typename Bezier, typename Polynomial>
-Polynomial from_bezier(const Bezier& curve)
-{
- typedef typename Polynomial::t_point_t t_point_t;
- typedef typename Polynomial::num_t num_t;
- 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 Polynomial(coefficients,curve.min(),curve.max());
-}
-
-/*
-/// \brief Converts a polynomial to a Bezier curve.
-/// \param polynomial : the polynomial to convert.
-/// \return the equivalent Bezier curve.
-template<typename Bezier, typename Polynomial>
-Bezier from_polynomial(const Polynomial& polynomial)
-{
- 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 curves
-#endif //_BEZIER_POLY_CONVERSION
-
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index e27a797f05685e51394c6754e89895ba6fc0af7a..594b6c6f533d709b15cc7f826cb343aa8acba3f0 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -25,174 +25,174 @@
namespace curves
{
-/// \class CubicHermiteSpline.
-/// \brief Represents a set of cubic hermite splines defining a continuous function \f$p(t)\f$.
-/// A hermite cubic spline is a minimal degree polynom interpolating a function in two
-/// points \f$P_i\f$ and \f$P_{i+1}\f$ with its tangent \f$m_i\f$ and \f$m_{i+1}\f$.<br>
-/// A hermite cubic spline :
-/// - 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>
->
-struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>,
- public serialization::Serializable< cubic_hermite_spline<Time, Numeric, Dim, Safe, Point, Tangent> >
-{
+ /// \class CubicHermiteSpline.
+ /// \brief Represents a set of cubic hermite splines defining a continuous function \f$p(t)\f$.
+ /// A hermite cubic spline is a minimal degree polynom interpolating a function in two
+ /// points \f$P_i\f$ and \f$P_{i+1}\f$ with its tangent \f$m_i\f$ and \f$m_{i+1}\f$.<br>
+ /// A hermite cubic spline :
+ /// - 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>
+ >
+ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>,
+ public serialization::Serializable< cubic_hermite_spline<Time, Numeric, Dim, Safe, Point, Tangent> >
+ {
typedef std::pair<Point, Tangent> 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;
-
+
public:
- cubic_hermite_spline(){}
-
- /// \brief Constructor.
- /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
- /// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
- /// \param time_control_points : vector containing time for each waypoint.
- ///
- template<typename In>
- cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
- : size_(std::distance(PairsBegin, PairsEnd)), degree_(3)
- {
+ cubic_hermite_spline(){}
+
+ /// \brief Constructor.
+ /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
+ /// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
+ /// \param time_control_points : vector containing time for each waypoint.
+ ///
+ template<typename In>
+ cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
+ : size_(std::distance(PairsBegin, PairsEnd)), degree_(3)
+ {
// Check size of pairs container.
if(Safe && size_ < 1)
{
- throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
+ throw std::length_error("can not create cubic_hermite_spline, number of pairs is inferior to 2.");
}
// Push all pairs in controlPoints
In it(PairsBegin);
for(; it != PairsEnd; ++it)
{
- control_points_.push_back(*it);
+ control_points_.push_back(*it);
}
setTime(time_control_points);
- }
+ }
+
-
- cubic_hermite_spline(const cubic_hermite_spline& other)
+ cubic_hermite_spline(const cubic_hermite_spline& other)
: 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_)
- {}
-
+ {}
- /// \brief Destructor.
- virtual ~cubic_hermite_spline(){}
- /*Operations*/
- public:
- /// \brief Evaluation of the cubic hermite spline at time t.
- /// \param t : time when to evaluate the spline.
- /// \return \f$p(t)\f$ point corresponding on spline at time t.
- ///
- virtual Point operator()(const Time t) const
- {
+ /// \brief Destructor.
+ virtual ~cubic_hermite_spline(){}
+
+ /*Operations*/
+ public:
+ /// \brief Evaluation of the cubic hermite spline at time t.
+ /// \param t : time when to evaluate the spline.
+ /// \return \f$p(t)\f$ point corresponding on spline at time t.
+ ///
+ virtual Point operator()(const Time t) const
+ {
if(Safe &! (T_min_ <= t && t <= T_max_))
{
- throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
+ throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
}
if (size_ == 1)
{
- return control_points_.front().first;
+ return control_points_.front().first;
}
else
{
- return evalCubicHermiteSpline(t, 0);
+ return evalCubicHermiteSpline(t, 0);
}
- }
-
- /// \brief Evaluate the derivative of order N of spline at time t.
- /// \param t : time when to evaluate the spline.
- /// \param order : order of derivative.
- /// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
- ///
- virtual Point derivate(const Time t, const std::size_t order) const
- {
+ }
+
+ /// \brief Evaluate the derivative of order N of spline at time t.
+ /// \param t : time when to evaluate the spline.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
+ ///
+ virtual Point derivate(const Time t, const std::size_t order) const
+ {
return evalCubicHermiteSpline(t, order);
- }
-
- /// \brief Set time of each control point of cubic hermite spline.
- /// Set duration of each spline, 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.<br>
- /// \param time_control_points : Vector containing time for each control point.
- ///
- void setTime(const vector_time_t & time_control_points)
- {
+ }
+
+ /// \brief Set time of each control point of cubic hermite spline.
+ /// Set duration of each spline, 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.<br>
+ /// \param time_control_points : Vector containing time for each control point.
+ ///
+ void setTime(const vector_time_t & time_control_points)
+ {
time_control_points_ = time_control_points;
T_min_ = time_control_points_.front();
T_max_ = time_control_points_.back();
if (time_control_points.size() != size())
{
- throw std::length_error("size of time control points should be equal to number of control points");
+ throw std::length_error("size of time control points should be equal to number of control points");
}
computeDurationSplines();
if (!checkDurationSplines())
{
- throw std::invalid_argument("time_splines not monotonous, all spline duration should be superior to 0");
+ throw std::invalid_argument("time_splines not monotonous, all spline duration should be superior to 0");
}
- }
+ }
- /// \brief Get vector of pair (positition, derivative) corresponding to control points.
- /// \return vector containing control points.
- ///
- t_pair_point_tangent_t getControlPoints()
- {
+ /// \brief Get vector of pair (positition, derivative) corresponding to control points.
+ /// \return vector containing control points.
+ ///
+ t_pair_point_tangent_t getControlPoints()
+ {
return control_points_;
- }
+ }
- /// \brief Get vector of Time corresponding to Time for each control point.
- /// \return vector containing time of each control point.
- ///
- vector_time_t getTime()
- {
+ /// \brief Get vector of Time corresponding to Time for each control point.
+ /// \return vector containing time of each control point.
+ ///
+ vector_time_t getTime()
+ {
return time_control_points_;
- }
-
- /// \brief Get number of control points contained in the trajectory.
- /// \return number of control points.
- ///
- std::size_t size() const { return size_; }
-
- /// \brief Get number of intervals (subsplines) contained in the trajectory.
- /// \return number of intervals (subsplines).
- ///
- std::size_t numIntervals() const { return size()-1; }
-
-
- /// \brief Evaluate value of cubic hermite spline or its derivate at specified order at time \f$t\f$.
- /// A cubic hermite spline on unit interval \f$[0,1]\f$ and given two control points defined by
- /// their position and derivative \f$\{p_0,m_0\}\f$ and \f$\{p_1,m_1\}\f$, is defined by the polynom : <br>
- /// \f$p(t)=h_{00}(t)P_0 + h_{10}(t)m_0 + h_{01}(t)p_1 + h_{11}(t)m_1\f$<br>
- /// To extend this formula to a cubic hermite spline on any arbitrary interval,
- /// we define \f$\alpha=(t-t_0)/(t_1-t_0) \in [0, 1]\f$ where \f$t \in [t_0, t_1]\f$.<br>
- /// Polynom \f$p(t) \in [t_0, t_1]\f$ becomes \f$p(\alpha) \in [0, 1]\f$
- /// and \f$p(\alpha) = p((t-t_0)/(t_1-t_0))\f$.
- /// \param t : time when to evaluate the curve.
- /// \param degree_derivative : Order of derivate of cubic hermite spline (set value to 0 if you do not want derivate)
- /// \return point corresponding \f$p(t)\f$ on spline at time t or its derivate order N \f$\frac{d^Np(t)}{dt^N}\f$.
- ///
- Point evalCubicHermiteSpline(const Numeric t, std::size_t degree_derivative) const
- {
+ }
+
+ /// \brief Get number of control points contained in the trajectory.
+ /// \return number of control points.
+ ///
+ std::size_t size() const { return size_; }
+
+ /// \brief Get number of intervals (subsplines) contained in the trajectory.
+ /// \return number of intervals (subsplines).
+ ///
+ std::size_t numIntervals() const { return size()-1; }
+
+
+ /// \brief Evaluate value of cubic hermite spline or its derivate at specified order at time \f$t\f$.
+ /// A cubic hermite spline on unit interval \f$[0,1]\f$ and given two control points defined by
+ /// their position and derivative \f$\{p_0,m_0\}\f$ and \f$\{p_1,m_1\}\f$, is defined by the polynom : <br>
+ /// \f$p(t)=h_{00}(t)P_0 + h_{10}(t)m_0 + h_{01}(t)p_1 + h_{11}(t)m_1\f$<br>
+ /// To extend this formula to a cubic hermite spline on any arbitrary interval,
+ /// we define \f$\alpha=(t-t_0)/(t_1-t_0) \in [0, 1]\f$ where \f$t \in [t_0, t_1]\f$.<br>
+ /// Polynom \f$p(t) \in [t_0, t_1]\f$ becomes \f$p(\alpha) \in [0, 1]\f$
+ /// and \f$p(\alpha) = p((t-t_0)/(t_1-t_0))\f$.
+ /// \param t : time when to evaluate the curve.
+ /// \param degree_derivative : Order of derivate of cubic hermite spline (set value to 0 if you do not want derivate)
+ /// \return point corresponding \f$p(t)\f$ on spline at time t or its derivate order N \f$\frac{d^Np(t)}{dt^N}\f$.
+ ///
+ Point evalCubicHermiteSpline(const Numeric t, std::size_t degree_derivative) const
+ {
const std::size_t id = findInterval(t);
// ID is on the last control point
if(id == size_-1)
{
- if (degree_derivative==0)
- {
- return control_points_.back().first;
- }
- else if (degree_derivative==1)
- {
- return control_points_.back().second;
- }
- else
- {
- return control_points_.back().first*0.; // To modify, create a new Tangent ininitialized with 0.
- }
+ if (degree_derivative==0)
+ {
+ return control_points_.back().first;
+ }
+ else if (degree_derivative==1)
+ {
+ return control_points_.back().second;
+ }
+ else
+ {
+ return control_points_.back().first*0.; // To modify, create a new Tangent ininitialized with 0.
+ }
}
const pair_point_tangent_t Pair0 = control_points_.at(id);
const pair_point_tangent_t Pair1 = control_points_.at(id+1);
@@ -215,178 +215,178 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>,
// if derivative, divide by dt^degree_derivative
for (std::size_t i=0; i<degree_derivative; i++)
{
- p_ /= dt;
+ p_ /= dt;
}
return p_;
- }
-
- /// \brief Evaluate coefficient for polynom of cubic hermite spline.
- /// Coefficients of polynom :<br>
- /// - \f$h00(t)=2t^3-3t^2+1\f$;
- /// - \f$h10(t)=t^3-2t^2+t\f$;
- /// - \f$h01(t)=-2t^3+3t^2\f$;
- /// - \f$h11(t)=t^3-t^2\f$.<br>
- /// From it, we can calculate their derivate order N :
- /// \f$\frac{d^Nh00(t)}{dt^N}\f$, \f$\frac{d^Nh10(t)}{dt^N}\f$,\f$\frac{d^Nh01(t)}{dt^N}\f$, \f$\frac{d^Nh11(t)}{dt^N}\f$.
- /// \param t : time to calculate coefficients.
- /// \param h00 : variable to store value of coefficient.
- /// \param h10 : variable to store value of coefficient.
- /// \param h01 : variable to store value of coefficient.
- /// \param h11 : variable to store value of coefficient.
- /// \param degree_derivative : order of derivative.
- ///
- static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t degree_derivative)
- {
+ }
+
+ /// \brief Evaluate coefficient for polynom of cubic hermite spline.
+ /// Coefficients of polynom :<br>
+ /// - \f$h00(t)=2t^3-3t^2+1\f$;
+ /// - \f$h10(t)=t^3-2t^2+t\f$;
+ /// - \f$h01(t)=-2t^3+3t^2\f$;
+ /// - \f$h11(t)=t^3-t^2\f$.<br>
+ /// From it, we can calculate their derivate order N :
+ /// \f$\frac{d^Nh00(t)}{dt^N}\f$, \f$\frac{d^Nh10(t)}{dt^N}\f$,\f$\frac{d^Nh01(t)}{dt^N}\f$, \f$\frac{d^Nh11(t)}{dt^N}\f$.
+ /// \param t : time to calculate coefficients.
+ /// \param h00 : variable to store value of coefficient.
+ /// \param h10 : variable to store value of coefficient.
+ /// \param h01 : variable to store value of coefficient.
+ /// \param h11 : variable to store value of coefficient.
+ /// \param degree_derivative : order of derivative.
+ ///
+ static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t degree_derivative)
+ {
Numeric t_square = t*t;
Numeric t_cube = t_square*t;
if (degree_derivative==0)
{
- h00 = 2*t_cube -3*t_square +1.;
- h10 = t_cube -2*t_square +t;
- h01 = -2*t_cube +3*t_square;
- h11 = t_cube -t_square;
+ h00 = 2*t_cube -3*t_square +1.;
+ h10 = t_cube -2*t_square +t;
+ h01 = -2*t_cube +3*t_square;
+ h11 = t_cube -t_square;
}
else if (degree_derivative==1)
{
- h00 = 6*t_square -6*t;
- h10 = 3*t_square -4*t +1.;
- h01 = -6*t_square +6*t;
- h11 = 3*t_square -2*t;
+ h00 = 6*t_square -6*t;
+ h10 = 3*t_square -4*t +1.;
+ h01 = -6*t_square +6*t;
+ h11 = 3*t_square -2*t;
}
else if (degree_derivative==2)
{
- h00 = 12*t -6.;
- h10 = 6*t -4.;
- h01 = -12*t +6.;
- h11 = 6*t -2.;
+ h00 = 12*t -6.;
+ h10 = 6*t -4.;
+ h01 = -12*t +6.;
+ h11 = 6*t -2.;
}
else if (degree_derivative==3)
{
- h00 = 12.;
- h10 = 6.;
- h01 = -12.;
- h11 = 6.;
+ h00 = 12.;
+ h10 = 6.;
+ h01 = -12.;
+ h11 = 6.;
}
else
{
- h00 = 0.;
- h10 = 0.;
- h01 = 0.;
- h11 = 0.;
+ h00 = 0.;
+ h10 = 0.;
+ h01 = 0.;
+ h11 = 0.;
}
- }
+ }
private:
- /// \brief Get index of the interval (subspline) corresponding to time t for the interpolation.
- /// \param t : time where to look for interval.
- /// \return Index of interval for time t.
- ///
- std::size_t findInterval(const Numeric t) const
- {
+ /// \brief Get index of the interval (subspline) corresponding to time t for the interpolation.
+ /// \param t : time where to look for interval.
+ /// \return Index of interval for time t.
+ ///
+ std::size_t findInterval(const Numeric t) const
+ {
// time before first control point time.
if(t < time_control_points_[0])
{
- return 0;
+ return 0;
}
// time is after last control point time
if(t > time_control_points_[size_-1])
{
- return size_-1;
+ return size_-1;
}
std::size_t left_id = 0;
std::size_t right_id = size_-1;
while(left_id <= right_id)
{
- const std::size_t middle_id = left_id + (right_id - left_id)/2;
- if(time_control_points_.at(middle_id) < t)
- {
- left_id = middle_id+1;
- }
- else if(time_control_points_.at(middle_id) > t)
- {
- right_id = middle_id-1;
- }
- else
- {
- return middle_id;
- }
+ const std::size_t middle_id = left_id + (right_id - left_id)/2;
+ if(time_control_points_.at(middle_id) < t)
+ {
+ left_id = middle_id+1;
+ }
+ else if(time_control_points_.at(middle_id) > t)
+ {
+ right_id = middle_id-1;
+ }
+ else
+ {
+ return middle_id;
+ }
}
return left_id-1;
- }
+ }
- /// \brief compute duration of each spline.
- /// 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} ).
- ///
- void computeDurationSplines() {
+ /// \brief compute duration of each spline.
+ /// 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} ).
+ ///
+ void computeDurationSplines() {
duration_splines_.clear();
Time actual_time;
Time prev_time = *(time_control_points_.begin());
std::size_t i = 0;
for (i=0; i<size()-1; i++)
{
- actual_time = time_control_points_.at(i+1);
- duration_splines_.push_back(actual_time-prev_time);
- prev_time = actual_time;
+ actual_time = time_control_points_.at(i+1);
+ duration_splines_.push_back(actual_time-prev_time);
+ prev_time = actual_time;
}
- }
+ }
- /// \brief Check if duration of each subspline is strictly positive.
- /// \return true if all duration of strictly positive, false otherwise.
- ///
- bool checkDurationSplines() const
- {
+ /// \brief Check if duration of each subspline is strictly positive.
+ /// \return true if all duration of strictly positive, false otherwise.
+ ///
+ bool checkDurationSplines() const
+ {
std::size_t i = 0;
bool is_positive = true;
while (is_positive && i<duration_splines_.size())
{
- is_positive = (duration_splines_.at(i)>0.);
- i++;
+ is_positive = (duration_splines_.at(i)>0.);
+ i++;
}
return is_positive;
- }
- /*Operations*/
+ }
+ /*Operations*/
/*Helpers*/
public:
- /// \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();}
- /// \brief Get the maximum time for which the curve is defined.
- /// \return \f$t_{max}\f$, upper bound of time range.
- Time virtual max() const{return time_control_points_.back();}
- /*Helpers*/
-
- /*Attributes*/
- /// 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>
- /// It contains \f$N-1\f$ durations.
- vector_time_t duration_splines_;
- /// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
- /*const*/ Time T_min_;
- /// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
- /*const*/ Time T_max_;
- /// Number of control points (pairs).
- std::size_t size_;
- /// Degree (Cubic so degree 3)
- std::size_t degree_;
- /*Attributes*/
-
- // Serialization of the class
- friend class boost::serialization::access;
-
- template<class Archive>
- void serialize(Archive& ar, const unsigned int version){
+ /// \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();}
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$, upper bound of time range.
+ Time virtual max() const{return time_control_points_.back();}
+ /*Helpers*/
+
+ /*Attributes*/
+ /// 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>
+ /// It contains \f$N-1\f$ durations.
+ vector_time_t duration_splines_;
+ /// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
+ /*const*/ Time T_min_;
+ /// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
+ /*const*/ Time T_max_;
+ /// Number of control points (pairs).
+ std::size_t size_;
+ /// Degree (Cubic so degree 3)
+ std::size_t degree_;
+ /*Attributes*/
+
+ // Serialization of the class
+ friend class boost::serialization::access;
+
+ template<class Archive>
+ void serialize(Archive& ar, const unsigned int version){
if (version) {
- // Do something depending on version ?
+ // Do something depending on version ?
}
ar & boost::serialization::make_nvp("control_points", control_points_);
ar & boost::serialization::make_nvp("time_control_points", time_control_points_);
@@ -395,9 +395,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>,
ar & boost::serialization::make_nvp("T_max", T_max_);
ar & boost::serialization::make_nvp("size", size_);
ar & boost::serialization::make_nvp("degree", degree_);
- }
-
-};
-
+ }
+ }; // End struct Cubic hermite spline
} // namespace curve
#endif //_CLASS_CUBICHERMITESPLINE
\ No newline at end of file
diff --git a/include/curves/cubic_spline.h b/include/curves/cubic_spline.h
index 20def8256486cfd0f4cb893c95e2b0934c64896c..f8233987db571cda9c4057348c2254f37d0d73d7 100644
--- a/include/curves/cubic_spline.h
+++ b/include/curves/cubic_spline.h
@@ -22,26 +22,27 @@
namespace curves
{
-/// \brief Creates coefficient vector of a cubic spline defined on the interval
-/// \f$[t_{min}, t_{max}]\f$. It follows the equation : <br>
-/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
-/// with a, b, c and d the control points.
-///
-template<typename Point, typename T_Point>
-T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d)
-{
- T_Point res;
- res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d);
- 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)
-{
- 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);
-}
+ /// \brief Creates coefficient vector of a cubic spline defined on the interval
+ /// \f$[t_{min}, t_{max}]\f$. It follows the equation : <br>
+ /// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
+ /// with a, b, c and d the control points.
+ ///
+ template<typename Point, typename T_Point>
+ T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d)
+ {
+ T_Point res;
+ res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d);
+ 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)
+ {
+ 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);
+ }
} // namespace curves
#endif //_STRUCT_CUBICSPLINE
diff --git a/include/curves/curve_abc.h b/include/curves/curve_abc.h
index 5020ebe034c0a933f38193c2d3011ef69d190281..8c50e809f38d21e690c2f52c78176249010a4445 100644
--- a/include/curves/curve_abc.h
+++ b/include/curves/curve_abc.h
@@ -20,53 +20,49 @@
namespace curves
{
-/// \struct curve_abc.
-/// \brief Represents a curve of dimension Dim.
-/// If value of parameter Safe is false, no verification is made on the evaluation of the curve.
-template<typename Time= double, typename Numeric=Time, bool Safe=false
-, typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
-struct curve_abc : std::unary_function<Time, Point>
-{
+ /// \struct curve_abc.
+ /// \brief Represents a curve of dimension Dim.
+ /// If value of parameter Safe is false, no verification is made on the evaluation of the curve.
+ template<typename Time= double, typename Numeric=Time, bool Safe=false
+ , typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
+ struct curve_abc : std::unary_function<Time, Point>
+ {
typedef Point point_t;
typedef Time time_t;
-/* Constructors - destructors */
+ /* Constructors - destructors */
public:
- /// \brief Constructor.
- curve_abc(){}
-
- /// \brief Destructor.
- virtual ~curve_abc(){}
-/* Constructors - destructors */
+ /// \brief Constructor.
+ curve_abc(){}
-/*Operations*/
- public:
- /// \brief Evaluation of the cubic spline at time t.
- /// \param t : time when to evaluate the spine
- /// \return \f$x(t)\f$, point corresponding on curve at time t.
- virtual point_t operator()(const time_t t) const = 0;
+ /// \brief Destructor.
+ virtual ~curve_abc(){}
+ /* Constructors - destructors */
+ /*Operations*/
+ /// \brief Evaluation of the cubic spline at time t.
+ /// \param t : time when to evaluate the spine
+ /// \return \f$x(t)\f$, point corresponding on curve at time t.
+ virtual point_t operator()(const time_t t) const = 0;
- /// \brief Evaluate the derivative of order N of curve at time t.
- /// \param t : time when to evaluate the spline.
- /// \param order : order of derivative.
- /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
- virtual point_t derivate(const time_t t, const std::size_t order) const = 0;
-/*Operations*/
-
-/*Helpers*/
- public:
- /// \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;
- /// \brief Get the maximum time for which the curve is defined.
- /// \return \f$t_{max}\f$, upper bound of time range.
- virtual time_t max() const = 0;
+ /// \brief Evaluate the derivative of order N of curve at time t.
+ /// \param t : time when to evaluate the spline.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
+ virtual point_t derivate(const time_t t, const std::size_t order) const = 0;
+ /*Operations*/
- std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
-/*Helpers*/
+ /*Helpers*/
+ /// \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;
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$, upper bound of time range.
+ virtual time_t max() const = 0;
+ std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
+ /*Helpers*/
- };
+ };
} // namespace curves
#endif //_STRUCT_CURVE_ABC
diff --git a/include/curves/curve_constraint.h b/include/curves/curve_constraint.h
index c7cc96c20c61ea8da83f1e8a56111ecebca58c75..26f9c3e986965b1e22bf6f5aad0526d52753efa0 100644
--- a/include/curves/curve_constraint.h
+++ b/include/curves/curve_constraint.h
@@ -19,19 +19,19 @@
namespace curves
{
-template <typename Point, std::size_t Dim=3>
-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){}
+ template <typename Point, std::size_t Dim=3>
+ 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(){}
- point_t init_vel;
- point_t init_acc;
- point_t end_vel;
- point_t end_acc;
-};
+ point_t init_vel;
+ point_t init_acc;
+ point_t end_vel;
+ point_t end_acc;
+ };
} // namespace curves
#endif //_CLASS_CUBICZEROVELACC
diff --git a/include/curves/curve_conversion.h b/include/curves/curve_conversion.h
index 880a5eb704d974364e0264742ff65410a6eda84f..e49d97a586f8e0468783496cb5b102b83bbf1a04 100644
--- a/include/curves/curve_conversion.h
+++ b/include/curves/curve_conversion.h
@@ -14,13 +14,12 @@
namespace curves
{
-
-/// \brief Converts a cubic hermite spline or a bezier curve to a polynomial.
-/// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert.
-/// \return the equivalent polynomial.
-template<typename Polynomial, typename curveTypeToConvert>
-Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
-{
+ /// \brief Converts a cubic hermite spline or a bezier curve to a polynomial.
+ /// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert.
+ /// \return the equivalent polynomial.
+ template<typename Polynomial, typename curveTypeToConvert>
+ Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
+ {
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t;
t_point_t coefficients;
@@ -31,34 +30,30 @@ Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
num_t fact = 1;
for(std::size_t i = 1; i<= curve.degree_; ++i)
{
- fact *= (num_t)i;
- coefficients.push_back(current.derivate(current.min(),i)/fact);
+ fact *= (num_t)i;
+ coefficients.push_back(current.derivate(current.min(),i)/fact);
}
return Polynomial(coefficients,curve.min(),curve.max());
-}
-
-
-/// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
-/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
-/// \return the equivalent cubic bezier curve.
-template<typename Bezier, typename curveTypeToConvert>
-Bezier bezier_from_curve(const curveTypeToConvert& curve)
-{
+ }
+
+ /// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
+ /// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
+ /// \return the equivalent cubic bezier curve.
+ template<typename Bezier, typename curveTypeToConvert>
+ Bezier bezier_from_curve(const curveTypeToConvert& curve)
+ {
typedef typename Bezier::point_t point_t;
typedef typename Bezier::t_point_t t_point_t;
typedef typename Bezier::num_t num_t;
curveTypeToConvert current (curve);
-
num_t T_min = current.min();
num_t T_max = current.max();
num_t T = T_max-T_min;
-
// Positions and derivatives
point_t p0 = current(T_min);
point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min,1);
point_t m1 = current.derivate(T_max,1);
-
// Convert to bezier control points
// for t in [Tmin,Tmax] and T=Tmax-Tmin : x'(0)=3(b_p1-b_p0)/T and x'(1)=3(b_p3-b_p2)/T
// so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T
@@ -67,51 +62,43 @@ Bezier bezier_from_curve(const curveTypeToConvert& curve)
point_t b_p3 = p1;
point_t b_p1 = T*m0/3+b_p0;
point_t b_p2 = -T*m1/3+b_p3;
-
-
t_point_t control_points;
control_points.push_back(b_p0);
control_points.push_back(b_p1);
control_points.push_back(b_p2);
control_points.push_back(b_p3);
-
return Bezier(control_points.begin(), control_points.end(), current.min(), current.max());
-}
-
-/// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline.
-/// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert.
-/// \return the equivalent cubic hermite spline.
-template<typename Hermite, typename curveTypeToConvert>
-Hermite hermite_from_curve(const curveTypeToConvert& curve)
-{
+ }
+
+ /// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline.
+ /// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert.
+ /// \return the equivalent cubic hermite spline.
+ template<typename Hermite, typename curveTypeToConvert>
+ Hermite hermite_from_curve(const curveTypeToConvert& curve)
+ {
typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t;
typedef typename Hermite::t_pair_point_tangent_t t_pair_point_tangent_t;
typedef typename Hermite::point_t point_t;
typedef typename Hermite::num_t num_t;
curveTypeToConvert current (curve);
-
num_t T_min = current.min();
num_t T_max = current.max();
num_t T = T_max-T_min;
-
// Positions and derivatives
point_t p0 = current(T_min);
point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min,1);
point_t m1 = current.derivate(T_max,1);
-
+ // Create pairs pos/vel
pair_point_tangent_t pair0(p0,m0);
pair_point_tangent_t pair1(p1,m1);
t_pair_point_tangent_t control_points;
control_points.push_back(pair0);
control_points.push_back(pair1);
-
std::vector< double > time_control_points;
time_control_points.push_back(T_min);
time_control_points.push_back(T_max);
-
return Hermite(control_points.begin(), control_points.end(), time_control_points);
-}
-
+ }
} // namespace curve
#endif //_CLASS_CURVE_CONVERSION
\ No newline at end of file
diff --git a/include/curves/exact_cubic.h b/include/curves/exact_cubic.h
index 927c19aef406a045dd68b2782680fa10e5c36e2b..27fbbaecb522ae20d1df9de15905b6e466dfab09 100644
--- a/include/curves/exact_cubic.h
+++ b/include/curves/exact_cubic.h
@@ -37,16 +37,17 @@
namespace curves
{
-/// \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, 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>//,
- //public serialization::Serializable< exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> >
-{
+ /// \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, 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>//,
+ //public serialization::Serializable< exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> >
+ {
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
@@ -65,68 +66,67 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_P
/* Constructors - destructors */
public:
- exact_cubic(){}
+ exact_cubic(){}
- /// \brief Constructor.
- /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
- /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
- ///
- template<typename In>
- exact_cubic(In wayPointsBegin, In wayPointsEnd)
+ /// \brief Constructor.
+ /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
+ /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
+ ///
+ template<typename In>
+ exact_cubic(In wayPointsBegin, In wayPointsEnd)
: piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
- {}
-
- /// \brief Constructor.
- /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
- /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
- /// \param constraints : constraints on the init and end velocity / accelerations of the spline.
- ///
- template<typename In>
- exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
+ {}
+
+ /// \brief Constructor.
+ /// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
+ /// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
+ /// \param constraints : constraints on the init and end velocity / accelerations of the spline.
+ ///
+ template<typename In>
+ exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
: piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints))
- {}
+ {}
- /// \brief Constructor.
- /// \param subSplines: vector of subSplines.
- exact_cubic(const t_spline_t& subSplines)
+ /// \brief Constructor.
+ /// \param subSplines: vector of subSplines.
+ exact_cubic(const t_spline_t& subSplines)
: piecewise_curve_t(subSplines)
- {}
+ {}
- /// \brief Copy Constructor.
- exact_cubic(const exact_cubic& other)
+ /// \brief Copy Constructor.
+ exact_cubic(const exact_cubic& other)
: piecewise_curve_t(other)
- {}
+ {}
- /// \brief Destructor.
- virtual ~exact_cubic(){}
+ /// \brief Destructor.
+ virtual ~exact_cubic(){}
- std::size_t getNumberSplines()
- {
+ std::size_t getNumberSplines()
+ {
return this->getNumberCurves();
- }
+ }
- spline_t getSplineAt(std::size_t index)
- {
+ spline_t getSplineAt(std::size_t index)
+ {
return this->curves_.at(index);
- }
+ }
private:
- /// \brief Compute polynom of exact cubic spline from waypoints.
- /// Compute the coefficients of polynom as in paper : "Task-Space Trajectories via Cubic Spline Optimization".<br>
- /// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$<br>
- /// with \f$a=x\f$, \f$H_1b=H_2x\f$, \f$c=H_3x+H_4b\f$, \f$d=H_5x+H_6b\f$.<br>
- /// The matrices \f$H\f$ are defined as in the paper in Appendix A.
- ///
- template<typename In>
- t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
- {
+ /// \brief Compute polynom of exact cubic spline from waypoints.
+ /// Compute the coefficients of polynom as in paper : "Task-Space Trajectories via Cubic Spline Optimization".<br>
+ /// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$<br>
+ /// with \f$a=x\f$, \f$H_1b=H_2x\f$, \f$c=H_3x+H_4b\f$, \f$d=H_5x+H_6b\f$.<br>
+ /// The matrices \f$H\f$ are defined as in the paper in Appendix A.
+ ///
+ 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 std::length_error("size of waypoints must be superior to 0") ; // TODO
+ throw std::length_error("size of waypoints must be superior to 0") ; // 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);
@@ -134,46 +134,42 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_P
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;
-
// Fill the matrices H as specified in the paper.
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();
+ 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();
@@ -187,23 +183,21 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_P
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(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;
- }
+ }
- template<typename In>
- t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
- {
+ template<typename In>
+ t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
+ {
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
- throw std::length_error("number of waypoints should be superior to one"); // TODO
+ throw std::length_error("number of waypoints should be superior to one"); // TODO
}
t_spline_t subSplines;
subSplines.reserve(size-1);
@@ -212,32 +206,31 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_P
++next;
for(std::size_t i(0); next != end; ++next, ++it, ++i)
{
- compute_one_spline<In>(it, next, cons, subSplines);
+ compute_one_spline<In>(it, next, cons, subSplines);
}
compute_end_spline<In>(it, next,cons, subSplines);
return subSplines;
- }
+ }
- template<typename In>
- void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
- {
+ template<typename In>
+ void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
+ {
const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
const point_t& b0 = constraints.init_vel , c0 = constraints.init_acc / 2.;
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,Dim,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);
- }
+ }
- template<typename In>
- void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
- {
+ template<typename In>
+ void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
+ {
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
@@ -247,7 +240,6 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_P
const num_t x_d_0 = dt_3, x_d_1 = 3*dt_2, x_d_2 = 6*dt;
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);
rhs.row(0) = alpha_0; rhs.row(1) = alpha_1; rhs.row(2) = alpha_2;
@@ -257,23 +249,21 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_P
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,Dim,Safe,Point,T_Point>(a0,b0,c0,d,e,f, init_t, end_t));
+ }
public:
- // Serialization of the class
- friend class boost::serialization::access;
+ // Serialization of the class
+ friend class boost::serialization::access;
- template<class Archive>
- void serialize(Archive& ar, const unsigned int version){
+ template<class Archive>
+ void serialize(Archive& ar, const unsigned int version){
if (version) {
- // Do something depending on version ?
+ // Do something depending on version ?
}
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(exact_cubic_t);
- }
-};
+ }
+ };
} // namespace curves
#endif //_CLASS_EXACTCUBIC
diff --git a/include/curves/linear_variable.h b/include/curves/linear_variable.h
index ff1537c09209f17bfaf6c0c16d1623b539ac5f38..c8a087b242120d00ad0eab44fd46b889a5f8ced1 100644
--- a/include/curves/linear_variable.h
+++ b/include/curves/linear_variable.h
@@ -21,46 +21,54 @@
namespace curves
{
-template <int Dim, typename Numeric=double>
-struct linear_variable{
+ template <int Dim, typename Numeric=double>
+ struct linear_variable{
typedef Numeric num_t;
typedef Eigen::Matrix<num_t, Dim, Dim> matrix_t;
typedef Eigen::Matrix<num_t, Dim, 1> point_t;
-
typedef linear_variable<Dim, Numeric> linear_variable_t;
+ /* Attributes */
matrix_t A_;
point_t b_;
-
- linear_variable(): A_(matrix_t::Identity()), b_(point_t::Zero()){}
- linear_variable(const matrix_t& A, const point_t& b):A_(A),b_(b) {}
- linear_variable(const point_t& b):A_(matrix_t::Zero()),b_(b) {} // constant
-
+ /* Attributes */
+
+ linear_variable()
+ : A_(matrix_t::Identity()), b_(point_t::Zero())
+ {}
+ linear_variable(const matrix_t& A, const point_t& b)
+ : A_(A),b_(b)
+ {}
+ linear_variable(const point_t& b)
+ : A_(matrix_t::Zero()),b_(b)
+ {} // constant
linear_variable& operator+=(const linear_variable& w1)
{
- this->A_ += w1.A_;
- this->b_ += w1.b_;
- return *this;
+ this->A_ += w1.A_;
+ this->b_ += w1.b_;
+ return *this;
}
+
linear_variable& operator-=(const linear_variable& w1)
{
- this->A_ -= w1.A_;
- this->b_ -= w1.b_;
- return *this;
+ this->A_ -= w1.A_;
+ this->b_ -= w1.b_;
+ return *this;
}
static linear_variable_t Zero(){
- linear_variable_t w;
- w.A_ = matrix_t::Zero();
- w.b_ = point_t::Zero();
- return w;
+ linear_variable_t w;
+ w.A_ = matrix_t::Zero();
+ w.b_ = point_t::Zero();
+ return w;
}
-};
+ }; // End struct linear_variable
-template<typename Var>
-struct variables{
+ template<typename Var>
+ struct variables
+ {
typedef Var var_t;
typedef variables<Var> variables_t;
@@ -74,158 +82,162 @@ struct variables{
variables& operator+=(const variables& w1)
{
- if(variables_.size() == 0)
+ if(variables_.size() == 0)
+ {
+ variables_ = w1.variables_;
+ }
+ else if (w1.variables_.size() !=0)
+ {
+ assert(variables_.size() == w1.variables_.size());
+ CIT_var_t cit = w1.variables_.begin();
+ for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
{
- variables_ = w1.variables_;
- } else if (w1.variables_.size() !=0)
- {
- assert(variables_.size() == w1.variables_.size());
- CIT_var_t cit = w1.variables_.begin();
- for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
- {
- (*it)+=(*cit);
- }
+ (*it)+=(*cit);
}
- return *this;
+ }
+ return *this;
}
variables& operator-=(const variables& w1)
{
- if(variables_.size() == 0)
- {
- variables_ = w1.variables_;
- } else if (w1.variables_.size() !=0)
+ if(variables_.size() == 0)
+ {
+ variables_ = w1.variables_;
+ }
+ else if (w1.variables_.size() !=0)
+ {
+ assert(variables_.size() == w1.variables_.size());
+ CIT_var_t cit = w1.variables_.begin();
+ for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
{
- assert(variables_.size() == w1.variables_.size());
- CIT_var_t cit = w1.variables_.begin();
- for(IT_var_t it = variables_.begin(); it != variables_.end(); ++it, ++cit)
- {
- (*it)-=(*cit);
- }
+ (*it)-=(*cit);
}
- return *this;
+ }
+ return *this;
}
static variables_t Zero(size_t /*dim*/){
- variables_t w;
- return w;
+ variables_t w;
+ return w;
}
-};
+ }; // End struct variables
-template <int D, typename N>
-inline linear_variable<D,N> operator+(const linear_variable<D,N>& w1, const linear_variable<D,N>& w2)
-{
+ template <int D, typename N>
+ inline linear_variable<D,N> operator+(const linear_variable<D,N>& w1, const linear_variable<D,N>& w2)
+ {
return linear_variable<D,N>(w1.A_ + w2.A_, w1.b_ + w2.b_);
-}
+ }
-template <int D, typename N>
-linear_variable<D,N> operator-(const linear_variable<D,N>& w1, const linear_variable<D,N>& w2)
-{
+ template <int D, typename N>
+ linear_variable<D,N> operator-(const linear_variable<D,N>& w1, const linear_variable<D,N>& w2)
+ {
return linear_variable<D,N>(w1.A_ - w2.A_, w1.b_ - w2.b_);
-}
+ }
-template <int D, typename N>
-linear_variable<D,N> operator*(const double k, const linear_variable<D,N>& w){
+ template <int D, typename N>
+ linear_variable<D,N> operator*(const double k, const linear_variable<D,N>& w)
+ {
return linear_variable<D,N>(k*w.A_,k*w.b_);
-}
+ }
-template <int D, typename N>
-linear_variable<D,N> operator*(const linear_variable<D,N>& w,const double k){
+ template <int D, typename N>
+ linear_variable<D,N> operator*(const linear_variable<D,N>& w,const double k)
+ {
return linear_variable<D,N>(k*w.A_,k*w.b_);
-}
+ }
-template <int D, typename N>
-linear_variable<D,N> operator/(const linear_variable<D,N>& w,const double k){
+ template <int D, typename N>
+ linear_variable<D,N> operator/(const linear_variable<D,N>& w,const double k)
+ {
return linear_variable<D,N>(w.A_/k,w.b_/k);
-}
+ }
-template<typename V>
-variables<V> operator+(const variables<V>& w1, const variables<V>& w2)
-{
+ template<typename V>
+ variables<V> operator+(const variables<V>& w1, const variables<V>& w2)
+ {
if(w2.variables_.size() == 0)
{
- return w1;
+ return w1;
}
if(w1.variables_.size() == 0)
{
- return w2;
+ return w2;
}
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2)
{
- res.variables_.push_back((*cit)+(*cit2));
+ res.variables_.push_back((*cit)+(*cit2));
}
return res;
-}
+ }
-template<typename V>
-variables<V> operator-(const variables<V>& w1, const variables<V>& w2)
-{
+ template<typename V>
+ variables<V> operator-(const variables<V>& w1, const variables<V>& w2)
+ {
if(w2.variables_.size() == 0)
{
- return w1;
+ return w1;
}
if(w1.variables_.size() == 0)
{
- return w2;
+ return w2;
}
variables<V> res;
assert(w2.variables_.size() == w1.variables_.size());
typename variables<V>::CIT_var_t cit = w1.variables_.begin();
for(typename variables<V>::CIT_var_t cit2 = w2.variables_.begin(); cit2 != w2.variables_.end(); ++cit, ++cit2)
{
- res.variables_.push_back((*cit)-(*cit2));
+ res.variables_.push_back((*cit)-(*cit2));
}
return res;
-}
+ }
-template<typename V>
-variables<V> operator*(const double k, const variables<V>& w)
-{
+ template<typename V>
+ variables<V> operator*(const double k, const variables<V>& w)
+ {
if(w.variables_.size() == 0)
{
- return w;
+ return w;
}
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
{
- res.variables_.push_back(k*(*cit));
+ res.variables_.push_back(k*(*cit));
}
return res;
-}
+ }
-template<typename V>
-variables<V> operator*(const variables<V>& w,const double k)
-{
+ template<typename V>
+ variables<V> operator*(const variables<V>& w,const double k)
+ {
if(w.variables_.size() == 0)
{
- return w;
+ return w;
}
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
{
- res.variables_.push_back((*cit)*k);
+ res.variables_.push_back((*cit)*k);
}
return res;
-}
+ }
-template<typename V>
-variables<V> operator/(const variables<V>& w,const double k)
-{
+ template<typename V>
+ variables<V> operator/(const variables<V>& w,const double k)
+ {
if(w.variables_.size() == 0)
{
- return w;
+ return w;
}
variables<V> res;
for(typename variables<V>::CIT_var_t cit = w.variables_.begin(); cit != w.variables_.end(); ++cit)
{
- res.variables_.push_back((*cit)/k);
+ res.variables_.push_back((*cit)/k);
}
return res;
-}
-
+ }
} // namespace curves
#endif //_CLASS_LINEAR_VARIABLE
diff --git a/include/curves/optimization/OptimizeSpline.h b/include/curves/optimization/OptimizeSpline.h
index b8c4e2075d393e42188d18af77ac202b8073d01f..ed6e1d21c9d2349d81491b8357f059f2b88e924b 100644
--- a/include/curves/optimization/OptimizeSpline.h
+++ b/include/curves/optimization/OptimizeSpline.h
@@ -418,113 +418,113 @@ namespace curves
bux[i] = +MSK_INFINITY;
}
- MSKrescodee r;
- MSKtask_t task = NULL;
- /* Create the optimization task. */
- r = MSK_maketask(env_,numcon,numvar,&task);
-
- /* Directs the log task stream to the 'printstr' function. */
- /*if ( r==MSK_RES_OK )
- r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
-
- /* Append 'numcon' empty constraints.
- The constraints will initially have no bounds. */
- if ( r == MSK_RES_OK )
- {
- r = MSK_appendcons(task,numcon);
- }
+ MSKrescodee r;
+ MSKtask_t task = NULL;
+ /* Create the optimization task. */
+ r = MSK_maketask(env_,numcon,numvar,&task);
+
+ /* Directs the log task stream to the 'printstr' function. */
+ /*if ( r==MSK_RES_OK )
+ r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
+
+ /* Append 'numcon' empty constraints.
+ The constraints will initially have no bounds. */
+ if ( r == MSK_RES_OK )
+ {
+ r = MSK_appendcons(task,numcon);
+ }
- /* Append 'numvar' variables.
- The variables will initially be fixed at zero (x=0). */
- if ( r == MSK_RES_OK )
- {
- r = MSK_appendvars(task,numvar);
- }
+ /* Append 'numvar' variables.
+ The variables will initially be fixed at zero (x=0). */
+ if ( r == MSK_RES_OK )
+ {
+ r = MSK_appendvars(task,numvar);
+ }
- for(int j=0; j<numvar && r == MSK_RES_OK; ++j)
- {
- /* Set the linear term c_j in the objective.*/
- if(r == MSK_RES_OK)
- {
- r = MSK_putcj(task,j,0);
- }
- /* Set the bounds on variable j.
- blx[j] <= x_j <= bux[j] */
- if(r == MSK_RES_OK)
- {
- r = MSK_putvarbound(task,
- j, /* Index of variable.*/
- bkx[j], /* Bound key.*/
- blx[j], /* Numerical value of lower bound.*/
- bux[j]); /* Numerical value of upper bound.*/
- }
- /* Input column j of A */
- if(r == MSK_RES_OK)
- {
- r = MSK_putacol(task,
- j, /* Variable (column) index.*/
- aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
- asub+aptrb[j], /* Pointer to row indexes of column j.*/
- aval+aptrb[j]); /* Pointer to Values of column j.*/
- }
- }
+ for(int j=0; j<numvar && r == MSK_RES_OK; ++j)
+ {
+ /* Set the linear term c_j in the objective.*/
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putcj(task,j,0);
+ }
+ /* Set the bounds on variable j.
+ blx[j] <= x_j <= bux[j] */
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putvarbound(task,
+ j, /* Index of variable.*/
+ bkx[j], /* Bound key.*/
+ blx[j], /* Numerical value of lower bound.*/
+ bux[j]); /* Numerical value of upper bound.*/
+ }
+ /* Input column j of A */
+ if(r == MSK_RES_OK)
+ {
+ r = MSK_putacol(task,
+ j, /* Variable (column) index.*/
+ aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
+ asub+aptrb[j], /* Pointer to row indexes of column j.*/
+ aval+aptrb[j]); /* Pointer to Values of column j.*/
+ }
+ }
- /* Set the bounds on constraints.
- for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
- for(int i=0; i<numcon && r == MSK_RES_OK; ++i)
- {
- r = MSK_putconbound(task,
- i, /* Index of constraint.*/
- bkc[i], /* Bound key.*/
- blc[i], /* Numerical value of lower bound.*/
- buc[i]); /* Numerical value of upper bound.*/
- }
+ /* Set the bounds on constraints.
+ for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
+ for(int i=0; i<numcon && r == MSK_RES_OK; ++i)
+ {
+ r = MSK_putconbound(task,
+ i, /* Index of constraint.*/
+ bkc[i], /* Bound key.*/
+ blc[i], /* Numerical value of lower bound.*/
+ buc[i]); /* Numerical value of upper bound.*/
+ }
- /* Maximize objective function. */
- if (r == MSK_RES_OK)
- {
- r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
- }
+ /* Maximize objective function. */
+ if (r == MSK_RES_OK)
+ {
+ r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
+ }
- if ( r==MSK_RES_OK )
- {
- /* Input the Q for the objective. */
- r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
- }
+ if ( r==MSK_RES_OK )
+ {
+ /* Input the Q for the objective. */
+ r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
+ }
- if ( r==MSK_RES_OK )
- {
- MSKrescodee trmcode;
+ if ( r==MSK_RES_OK )
+ {
+ MSKrescodee trmcode;
- /* Run optimizer */
- r = MSK_optimizetrm(task,&trmcode);
- if ( r==MSK_RES_OK )
- {
- double *xx = (double*) calloc(numvar,sizeof(double));
- if ( xx )
- {
- /* Request the interior point solution. */
- MSK_getxx(task, MSK_SOL_ITR, xx);
- T_waypoints_t nwaypoints;
- In begin(wayPointsBegin);
- for(int i=0; i< size; i = ++i, ++begin)
- {
- point_t target;
- for(int j=0; j< Dim; ++ j)
- {
- target(j) = xx[i + j*size];
- }
- nwaypoints.push_back(std::make_pair(begin->first, target));
- }
- res = new exact_cubic_t(nwaypoints.begin(), nwaypoints.end());
- free(xx);
- }
- }
- }
- /* Delete the task and the associated data. */
- MSK_deletetask(&task);
- return res;
- }
+ /* Run optimizer */
+ r = MSK_optimizetrm(task,&trmcode);
+ if ( r==MSK_RES_OK )
+ {
+ double *xx = (double*) calloc(numvar,sizeof(double));
+ if ( xx )
+ {
+ /* Request the interior point solution. */
+ MSK_getxx(task, MSK_SOL_ITR, xx);
+ T_waypoints_t nwaypoints;
+ In begin(wayPointsBegin);
+ for(int i=0; i< size; i = ++i, ++begin)
+ {
+ point_t target;
+ for(int j=0; j< Dim; ++ j)
+ {
+ target(j) = xx[i + j*size];
+ }
+ nwaypoints.push_back(std::make_pair(begin->first, target));
+ }
+ res = new exact_cubic_t(nwaypoints.begin(), nwaypoints.end());
+ free(xx);
+ }
+ }
+ }
+ /* Delete the task and the associated data. */
+ MSK_deletetask(&task);
+ return res;
+ } // End SplineOptimizer
} // namespace curves
#endif //_CLASS_SPLINEOPTIMIZER
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index d230f93c871194082575493a37150adf56183124..7a88ff424b76a83f4c8e0bb94b5b764158493930 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -16,23 +16,23 @@
namespace curves
{
-/// \class PiecewiseCurve.
-/// \brief Represent a piecewise curve. We can add some new curve,
-/// but the starting time of the curve to add should be equal to the ending time of the actual
-/// piecewise_curve.<br>\ Example : A piecewise curve composed of three curves cf0,
-/// cf1 and cf2 where cf0 is defined between \f$[T0_{min},T0_{max}]\f$, cf1 between
-/// \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
-/// 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,
- 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>
- >
-struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
- public serialization::Serializable< piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Curve> >
-{
+ /// \class PiecewiseCurve.
+ /// \brief Represent a piecewise curve. We can add some new curve,
+ /// but the starting time of the curve to add should be equal to the ending time of the actual
+ /// piecewise_curve.<br>\ Example : A piecewise curve composed of three curves cf0,
+ /// cf1 and cf2 where cf0 is defined between \f$[T0_{min},T0_{max}]\f$, cf1 between
+ /// \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
+ /// 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,
+ 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> >
+ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
+ public serialization::Serializable< piecewise_curve<Time, Numeric, Dim, Safe,
+ Point, T_Point, Curve> >
+ {
typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t;
@@ -43,71 +43,71 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
public:
- piecewise_curve()
+ piecewise_curve()
: size_(0)
- {}
-
- /// \brief Constructor.
- /// Initialize a piecewise curve by giving the first curve.
- /// \param pol : a polynomial curve.
- ///
- piecewise_curve(const curve_t& cf)
- {
+ {}
+
+ /// \brief Constructor.
+ /// Initialize a piecewise curve by giving the first curve.
+ /// \param pol : a polynomial curve.
+ ///
+ piecewise_curve(const curve_t& cf)
+ {
size_ = 0;
add_curve(cf);
- }
+ }
- piecewise_curve(const t_curve_t list_curves)
- {
+ piecewise_curve(const t_curve_t list_curves)
+ {
size_ = 0;
for( std::size_t i=0; i<list_curves.size(); i++ )
{
- add_curve(list_curves[i]);
+ add_curve(list_curves[i]);
}
- }
+ }
- piecewise_curve(const piecewise_curve& other)
+ piecewise_curve(const piecewise_curve& other)
: curves_(other.curves_), time_curves_(other.time_curves_), size_(other.size_),
- T_min_(other.T_min_), T_max_(other.T_max_)
- {}
+ T_min_(other.T_min_), T_max_(other.T_max_)
+ {}
- virtual ~piecewise_curve(){}
+ virtual ~piecewise_curve(){}
- virtual Point operator()(const Time t) const
- {
+ virtual Point operator()(const Time t) const
+ {
if(Safe &! (T_min_ <= t && t <= T_max_))
{
- //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
- throw std::out_of_range("can't evaluate piecewise curve, out of range");
+ //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
+ throw std::out_of_range("can't evaluate piecewise curve, out of range");
}
return curves_.at(find_interval(t))(t);
- }
-
- /// \brief Evaluate the derivative of order N of curve at time t.
- /// \param t : time when to evaluate the spline.
- /// \param order : order of derivative.
- /// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
- ///
- virtual Point derivate(const Time t, const std::size_t order) const
- {
+ }
+
+ /// \brief Evaluate the derivative of order N of curve at time t.
+ /// \param t : time when to evaluate the spline.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
+ ///
+ virtual Point derivate(const Time t, const std::size_t order) const
+ {
if(Safe &! (T_min_ <= t && t <= T_max_))
{
- throw std::invalid_argument("can't evaluate piecewise curve, out of range");
+ throw std::invalid_argument("can't evaluate piecewise curve, out of range");
}
return (curves_.at(find_interval(t))).derivate(t, order);
- }
-
- /// \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.
- /// \param cf : curve to add.
- ///
- void add_curve(const curve_t& cf)
- {
+ }
+
+ /// \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.
+ /// \param cf : curve to add.
+ ///
+ void add_curve(const curve_t& cf)
+ {
// 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))
{
- throw std::invalid_argument("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
+ throw std::invalid_argument("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
}
curves_.push_back(cf);
size_ = curves_.size();
@@ -115,49 +115,47 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
time_curves_.push_back(T_max_);
if (size_ == 1)
{
- // First curve added
- time_curves_.push_back(cf.min());
- T_min_ = cf.min();
+ // First curve added
+ time_curves_.push_back(cf.min());
+ T_min_ = cf.min();
}
- }
-
- /// \brief Check if the curve is continuous of order given.
- /// \param order : order of continuity we want to check.
- /// \return True if the curve is continuous of order given.
- ///
- bool is_continuous(const std::size_t order)
- {
+ }
+
+ /// \brief Check if the curve is continuous of order given.
+ /// \param order : order of continuity we want to check.
+ /// \return True if the curve is continuous of order given.
+ ///
+ bool is_continuous(const std::size_t order)
+ {
bool isContinuous = true;
std::size_t i=0;
point_t value_end, value_start;
while (isContinuous && i<(size_-1))
{
- curve_t current = curves_.at(i);
- curve_t next = curves_.at(i+1);
-
- if (order == 0)
- {
- value_end = current(current.max());
- value_start = next(next.min());
- }
- else
- {
- value_end = current.derivate(current.max(),order);
- value_start = next.derivate(next.min(),order);
- }
-
- if ((value_end-value_start).norm() > MARGIN)
- {
- isContinuous = false;
- }
- i++;
+ curve_t current = curves_.at(i);
+ curve_t next = curves_.at(i+1);
+ if (order == 0)
+ {
+ value_end = current(current.max());
+ value_start = next(next.min());
+ }
+ else
+ {
+ value_end = current.derivate(current.max(),order);
+ value_start = next.derivate(next.min(),order);
+ }
+ if ((value_end-value_start).norm() > MARGIN)
+ {
+ isContinuous = false;
+ }
+ i++;
}
return isContinuous;
- }
+ }
- template<typename Bezier>
- piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
- {
+ template<typename Bezier>
+ piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
+ {
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
@@ -167,14 +165,14 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
// Convert and add all other curves (segments)
for (std::size_t i=1; i<size_; i++)
{
- pc_res.add_curve(bezier_from_curve<Bezier, curve_t>(curves_.at(i)));
+ pc_res.add_curve(bezier_from_curve<Bezier, curve_t>(curves_.at(i)));
}
return pc_res;
- }
+ }
- template<typename Hermite>
- piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
- {
+ template<typename Hermite>
+ piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
+ {
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
@@ -184,14 +182,14 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
// Convert and add all other curves (segments)
for (std::size_t i=1; i<size_; i++)
{
- pc_res.add_curve(hermite_from_curve<Hermite, curve_t>(curves_.at(i)));
+ pc_res.add_curve(hermite_from_curve<Hermite, curve_t>(curves_.at(i)));
}
return pc_res;
- }
+ }
- template<typename Polynomial>
- piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
- {
+ template<typename Polynomial>
+ piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
+ {
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
@@ -201,19 +199,19 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
// Convert and add all other curves (segments)
for (std::size_t i=1; i<size_; i++)
{
- pc_res.add_curve(polynomial_from_curve<Polynomial, curve_t>(curves_.at(i)));
+ pc_res.add_curve(polynomial_from_curve<Polynomial, curve_t>(curves_.at(i)));
}
return pc_res;
- }
+ }
- template<typename Polynomial>
- static piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial>
- convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max)
- {
+ template<typename Polynomial>
+ static piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial>
+ convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max)
+ {
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");
+ //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");
}
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
Time discretization_step = (T_max-T_min)/Time(points.size()-1);
@@ -232,15 +230,15 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
// Other points
for (std::size_t i=1; i<points.size()-2; i++)
{
- 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;
+ 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;
}
// Last points
coeffs.clear();
@@ -252,87 +250,86 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point>,
coeffs.push_back(coeff_order_one);
ppc.add_curve(Polynomial(coeffs,time_actual,T_max));
return ppc;
- }
+ }
private:
- /// \brief Get index of the interval corresponding to time t for the interpolation.
- /// \param t : time where to look for interval.
- /// \return Index of interval for time t.
- ///
- std::size_t find_interval(const Numeric t) const
- {
+ /// \brief Get index of the interval corresponding to time t for the interpolation.
+ /// \param t : time where to look for interval.
+ /// \return Index of interval for time t.
+ ///
+ std::size_t find_interval(const Numeric t) const
+ {
// time before first control point time.
if(t < time_curves_[0])
{
- return 0;
+ return 0;
}
// time is after last control point time
if(t > time_curves_[size_-1])
{
- return size_-1;
+ return size_-1;
}
std::size_t left_id = 0;
std::size_t right_id = size_-1;
while(left_id <= right_id)
{
- const std::size_t middle_id = left_id + (right_id - left_id)/2;
- if(time_curves_.at(middle_id) < t)
- {
- left_id = middle_id+1;
- }
- else if(time_curves_.at(middle_id) > t)
- {
- right_id = middle_id-1;
- }
- else
- {
- return middle_id;
- }
+ const std::size_t middle_id = left_id + (right_id - left_id)/2;
+ if(time_curves_.at(middle_id) < t)
+ {
+ left_id = middle_id+1;
+ }
+ else if(time_curves_.at(middle_id) > t)
+ {
+ right_id = middle_id-1;
+ }
+ else
+ {
+ return middle_id;
+ }
}
return left_id-1;
- }
+ }
/*Helpers*/
public:
- /// \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 T_min_;}
- /// \brief Get the maximum time for which the curve is defined.
- /// \return \f$t_{max}\f$, upper bound of time range.
- Time virtual max() const{return T_max_;}
- std::size_t getNumberCurves() { return curves_.size(); }
- /*Helpers*/
-
- /* Variables */
- t_curve_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
- t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
- std::size_t size_; // Number of segments in piecewise curve = size of curves_
- Time T_min_, T_max_;
- static const double MARGIN;
-
- public:
- // Serialization of the class
- friend class boost::serialization::access;
-
- template<class Archive>
- void serialize(Archive& ar, const unsigned int version){
+ /// \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 T_min_;}
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$, upper bound of time range.
+ Time virtual max() const{return T_max_;}
+ std::size_t getNumberCurves() { return curves_.size(); }
+ /*Helpers*/
+
+ /* Attributes */
+ t_curve_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
+ t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
+ std::size_t size_; // Number of segments in piecewise curve = size of curves_
+ Time T_min_, T_max_;
+ static const double MARGIN;
+ /* Attributes */
+
+ // Serialization of the class
+ friend class boost::serialization::access;
+
+ template<class Archive>
+ void serialize(Archive& ar, const unsigned int version){
if (version) {
- // Do something depending on version ?
+ // Do something depending on version ?
}
ar & boost::serialization::make_nvp("curves", curves_);
ar & boost::serialization::make_nvp("time_curves", time_curves_);
ar & boost::serialization::make_nvp("size", size_);
ar & boost::serialization::make_nvp("T_min", T_min_);
ar & boost::serialization::make_nvp("T_max", T_max_);
- }
-
-};
+ }
+ }; // End struct piecewise curve
-template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point, typename Curve>
-const double piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Curve>::MARGIN(0.001);
+ template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point, typename Curve>
+ const double piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Curve>::MARGIN(0.001);
} // end namespace
diff --git a/include/curves/polynomial.h b/include/curves/polynomial.h
index 59ea329483bfd56dd5887b15d7307c97e6324907..bda91b365cc38136729695fff012e84c6eedc978 100644
--- a/include/curves/polynomial.h
+++ b/include/curves/polynomial.h
@@ -28,17 +28,17 @@
namespace curves
{
-/// \class polynomial.
-/// \brief Represents a polynomial of an arbitrary order defined on the interval
-/// \f$[t_{min}, t_{max}]\f$. It follows the equation :<br>
-/// \f$ x(t) = a + b(t - t_{min}) + ... + d(t - t_{min})^N \f$<br>
-/// where N is the order and \f$ t \in [t_{min}, t_{max}] \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 T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
-struct polynomial : public curve_abc<Time, Numeric, Safe, Point>,
- public serialization::Serializable< polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
-{
+ /// \class polynomial.
+ /// \brief Represents a polynomial of an arbitrary order defined on the interval
+ /// \f$[t_{min}, t_{max}]\f$. It follows the equation :<br>
+ /// \f$ x(t) = a + b(t - t_{min}) + ... + d(t - t_{min})^N \f$<br>
+ /// where N is the order and \f$ t \in [t_{min}, t_{max}] \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 T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
+ struct polynomial : public curve_abc<Time, Numeric, Safe, Point>,
+ public serialization::Serializable< polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
+ {
typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t;
@@ -47,209 +47,191 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point>,
typedef Eigen::Matrix<double, Dim, Eigen::Dynamic> coeff_t;
typedef Eigen::Ref<coeff_t> coeff_t_ref;
-/* Constructors - destructors */
+ /* Constructors - destructors */
public:
- polynomial(){}
+ polynomial(){}
- /// \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 curve.
- /// \param max : UPPER bound on interval definition of the curve.
- polynomial(const coeff_t& coefficients, const time_t min, const time_t max)
+ /// \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 curve.
+ /// \param max : UPPER bound on interval definition of the curve.
+ polynomial(const coeff_t& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients),
dim_(Dim), degree_(coefficients_.cols()-1),
T_min_(min), T_max_(max)
- {
+ {
safe_check();
- }
-
- /// \brief Constructor
- /// \param coefficients : a container containing all coefficients of the spline, starting
- /// with the zero order coefficient, up to the highest order. Spline order is given
- /// by the size of the coefficients.
- /// \param min : LOWER bound on interval definition of the spline.
- /// \param max : UPPER bound on interval definition of the spline.
- polynomial(const T_Point& coefficients, const time_t min, const time_t max)
+ }
+
+ /// \brief Constructor
+ /// \param coefficients : a container containing all coefficients of the spline, starting
+ /// with the zero order coefficient, up to the highest order. Spline order is given
+ /// by the size of the coefficients.
+ /// \param min : LOWER bound on interval definition of the spline.
+ /// \param max : UPPER bound on interval definition of the spline.
+ polynomial(const T_Point& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(init_coeffs(coefficients.begin(), coefficients.end())),
dim_(Dim), degree_(coefficients_.cols()-1),
T_min_(min), T_max_(max)
- {
+ {
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.
- /// \param out : an iterator pointing to the last element of a structure ofcoefficients.
- /// \param min : LOWER bound on interval definition of the spline.
- /// \param max : UPPER bound on interval definition of the spline.
- template<typename In>
- polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
+ }
+
+ /// \brief Constructor.
+ /// \param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
+ /// it corresponds to the zero degree coefficient.
+ /// \param out : an iterator pointing to the last element of a structure ofcoefficients.
+ /// \param min : LOWER bound on interval definition of the spline.
+ /// \param max : UPPER bound on interval definition of the spline.
+ template<typename In>
+ polynomial(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
: coefficients_(init_coeffs(zeroOrderCoefficient, out)),
dim_(Dim), degree_(coefficients_.cols()-1),
T_min_(min), T_max_(max)
- {
+ {
safe_check();
- }
+ }
- /// \brief Destructor
- ~polynomial()
- {
- // NOTHING
- }
+ /// \brief Destructor
+ ~polynomial()
+ {
+ // NOTHING
+ }
- polynomial(const polynomial& other)
+ polynomial(const polynomial& other)
: coefficients_(other.coefficients_),
dim_(other.dim_), degree_(other.degree_), T_min_(other.T_min_), T_max_(other.T_max_)
- {}
+ {}
//polynomial& operator=(const polynomial& other);
private:
- void safe_check()
- {
+ void safe_check()
+ {
if(Safe)
{
- if(T_min_ > T_max_)
- {
- std::invalid_argument("Tmin should be inferior to Tmax");
- }
- if(coefficients_.size() != int(degree_+1))
- {
- std::runtime_error("Spline order and coefficients do not match");
- }
+ if(T_min_ > T_max_)
+ {
+ std::invalid_argument("Tmin should be inferior to Tmax");
+ }
+ if(coefficients_.size() != int(degree_+1))
+ {
+ std::runtime_error("Spline order and coefficients do not match");
+ }
}
- }
+ }
-/* Constructors - destructors */
+ /* Constructors - destructors */
-/*Operations*/
+ /*Operations*/
public:
- /*/// \brief Evaluation of the cubic spline at time t.
- /// \param t : the time when to evaluate the spine
- /// \param return \f$x(t)\f$, point corresponding on curve at time t.
- virtual point_t operator()(const time_t t) const
- {
- if((t < T_min_ || t > T_max_) && Safe){ throw std::out_of_range("TODO");}
- time_t const dt (t-T_min_);
- time_t cdt(1);
- point_t currentPoint_ = point_t::Zero();
- for(int i = 0; i < degree_+1; ++i, cdt*=dt)
- currentPoint_ += cdt *coefficients_.col(i);
- return currentPoint_;
- }*/
-
- /// \brief Evaluation of the cubic spline at time t using horner's scheme.
- /// \param t : time when to evaluate the spline.
- /// \return \f$x(t)\f$ point corresponding on spline at time t.
- virtual point_t operator()(const time_t t) const
- {
+ /// \brief Evaluation of the cubic spline at time t using horner's scheme.
+ /// \param t : time when to evaluate the spline.
+ /// \return \f$x(t)\f$ point corresponding on spline at time t.
+ virtual point_t operator()(const time_t t) const
+ {
if((t < T_min_ || t > T_max_) && Safe)
{
- throw std::invalid_argument("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
+ throw std::invalid_argument("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
}
time_t const dt (t-T_min_);
point_t h = coefficients_.col(degree_);
for(int i=(int)(degree_-1); i>=0; i--)
{
- h = dt*h + coefficients_.col(i);
+ h = dt*h + coefficients_.col(i);
}
return h;
- }
+ }
- /// \brief Evaluation of the derivative of order N of spline at time t.
- /// \param t : the time when to evaluate the spline.
- /// \param order : order of derivative.
- /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
- virtual point_t derivate(const time_t t, const std::size_t order) const
- {
+ /// \brief Evaluation of the derivative of order N of spline at time t.
+ /// \param t : the time when to evaluate the spline.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
+ virtual point_t derivate(const time_t t, const std::size_t order) const
+ {
if((t < T_min_ || t > T_max_) && Safe)
{
- throw std::invalid_argument("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
+ throw std::invalid_argument("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
}
time_t const dt (t-T_min_);
time_t cdt(1);
point_t currentPoint_ = point_t::Zero(dim_);
for(int i = (int)(order); i < (int)(degree_+1); ++i, cdt*=dt)
{
- currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
+ currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
}
return currentPoint_;
- }
+ }
private:
- num_t fact(const std::size_t n, const std::size_t order) const
- {
+ num_t fact(const std::size_t n, const std::size_t order) const
+ {
num_t res(1);
for(std::size_t i = 0; i < order; ++i)
{
- res *= (num_t)(n-i);
+ res *= (num_t)(n-i);
}
return res;
- }
+ }
+ /*Operations*/
-/*Operations*/
-
-/*Helpers*/
- public:
- /// \brief Get the minimum time for which the curve is defined
- /// \return \f$t_{min}\f$ lower bound of time range.
- num_t virtual min() const {return T_min_;}
- /// \brief Get the maximum time for which the curve is defined.
- /// \return \f$t_{max}\f$ upper bound of time range.
- num_t virtual max() const {return T_max_;}
-/*Helpers*/
-
-/*Attributes*/
public:
- coeff_t coefficients_; //const
- std::size_t dim_; //const
- std::size_t degree_; //const
-
- private:
- time_t T_min_, T_max_;
-/*Attributes*/
+ /*Helpers*/
+ /// \brief Get the minimum time for which the curve is defined
+ /// \return \f$t_{min}\f$ lower bound of time range.
+ num_t virtual min() const {return T_min_;}
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$ upper bound of time range.
+ num_t virtual max() const {return T_max_;}
+ /*Helpers*/
+
+ /*Attributes*/
+ coeff_t coefficients_; //const
+ std::size_t dim_; //const
+ std::size_t degree_; //const
+ time_t T_min_, T_max_;
+ /*Attributes*/
private:
- template<typename In>
- coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
- {
+ template<typename In>
+ coeff_t init_coeffs(In zeroOrderCoefficient, In highestOrderCoefficient)
+ {
std::size_t size = std::distance(zeroOrderCoefficient, highestOrderCoefficient);
coeff_t res = coeff_t(Dim, size); int i = 0;
for(In cit = zeroOrderCoefficient; cit != highestOrderCoefficient; ++cit, ++i)
{
- res.col(i) = *cit;
+ res.col(i) = *cit;
}
return res;
- }
+ }
public:
- // Serialization of the class
- friend class boost::serialization::access;
+ // Serialization of the class
+ friend class boost::serialization::access;
- template<class Archive>
- void serialize(Archive& ar, const unsigned int version){
+ template<class Archive>
+ void serialize(Archive& ar, const unsigned int version){
if (version) {
- // Do something depending on version ?
+ // Do something depending on version ?
}
ar & boost::serialization::make_nvp("coefficients", coefficients_);
ar & boost::serialization::make_nvp("dim", dim_);
ar & boost::serialization::make_nvp("degree", degree_);
ar & boost::serialization::make_nvp("T_min", T_min_);
ar & boost::serialization::make_nvp("T_max", T_max_);
- }
+ }
-}; //class polynomial
+ }; //class polynomial
} // namespace curves
#endif //_STRUCT_POLYNOMIAL
diff --git a/include/curves/quintic_spline.h b/include/curves/quintic_spline.h
index 4bfa35a48a3f0bcef05dd3cb38d6f341b61bd96c..e6f6356fa32fd9216424d20e141f332878658876 100644
--- a/include/curves/quintic_spline.h
+++ b/include/curves/quintic_spline.h
@@ -22,28 +22,28 @@
namespace curves
{
-/// \brief Creates coefficient vector of a quintic spline defined on the interval
-/// \f$[t_{min}, t_{max}]\f$. It follows the equation :<br>
-/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 + e(t - t_{min})^4 + f(t - t_{min})^5 \f$ <br>
-/// where \f$ t \in [t_{min}, t_{max}] \f$.
-///
-template<typename Point, typename T_Point>
-T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
- Point const &d, Point const& e, Point const& f)
-{
- T_Point res;
- res.push_back(a);res.push_back(b);res.push_back(c);
- res.push_back(d);res.push_back(e);res.push_back(f);
- 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_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
- const Time t_min, const Time t_max)
-{
- T_Point coeffs = make_quintic_vector<Point, T_Point>(a,b,c,d,e,f);
- return polynomial<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
-}
+ /// \brief Creates coefficient vector of a quintic spline defined on the interval
+ /// \f$[t_{min}, t_{max}]\f$. It follows the equation :<br>
+ /// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 + e(t - t_{min})^4 + f(t - t_{min})^5 \f$ <br>
+ /// where \f$ t \in [t_{min}, t_{max}] \f$.
+ ///
+ template<typename Point, typename T_Point>
+ T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
+ Point const &d, Point const& e, Point const& f)
+ {
+ T_Point res;
+ res.push_back(a);res.push_back(b);res.push_back(c);
+ res.push_back(d);res.push_back(e);res.push_back(f);
+ 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_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
+ const Time t_min, const Time t_max)
+ {
+ T_Point coeffs = make_quintic_vector<Point, T_Point>(a,b,c,d,e,f);
+ return polynomial<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), t_min, t_max);
+ }
} // namespace curves
#endif //_STRUCT_QUINTIC_SPLINE
diff --git a/include/curves/serialization/archive.hpp b/include/curves/serialization/archive.hpp
index 13171ba68bfa30f04305ec54d16187cab8a6ddbe..63594ba33a194db11f965dd4c70f613ea15f4651 100644
--- a/include/curves/serialization/archive.hpp
+++ b/include/curves/serialization/archive.hpp
@@ -21,113 +21,112 @@ namespace curves
{
namespace serialization
{
-
template<class Derived>
struct Serializable
{
- private:
- Derived & derived() { return *static_cast<Derived*>(this); }
- const Derived & derived() const { return *static_cast<const Derived*>(this); }
+ private:
+ Derived & derived() { return *static_cast<Derived*>(this); }
+ const Derived & derived() const { return *static_cast<const Derived*>(this); }
- public:
- /// \brief Loads a Derived object from a text file.
- void loadFromText(const std::string & filename) throw (std::invalid_argument)
- {
- std::ifstream ifs(filename.c_str());
- if(ifs)
- {
- boost::archive::text_iarchive ia(ifs);
- ia >> derived();
- }
- else
+ public:
+ /// \brief Loads a Derived object from a text file.
+ void loadFromText(const std::string & filename) throw (std::invalid_argument)
{
- const std::string exception_message(filename + " does not seem to be a valid file.");
- throw std::invalid_argument(exception_message);
+ std::ifstream ifs(filename.c_str());
+ if(ifs)
+ {
+ boost::archive::text_iarchive ia(ifs);
+ ia >> derived();
+ }
+ else
+ {
+ const std::string exception_message(filename + " does not seem to be a valid file.");
+ throw std::invalid_argument(exception_message);
+ }
}
- }
- /// \brief Saved a Derived object as a text file.
- void saveAsText(const std::string & filename) const throw (std::invalid_argument)
- {
- std::ofstream ofs(filename.c_str());
- if(ofs)
+ /// \brief Saved a Derived object as a text file.
+ void saveAsText(const std::string & filename) const throw (std::invalid_argument)
{
- boost::archive::text_oarchive oa(ofs);
- oa << derived();
+ std::ofstream ofs(filename.c_str());
+ if(ofs)
+ {
+ boost::archive::text_oarchive oa(ofs);
+ oa << derived();
+ }
+ else
+ {
+ const std::string exception_message(filename + " does not seem to be a valid file.");
+ throw std::invalid_argument(exception_message);
+ }
}
- else
- {
- const std::string exception_message(filename + " does not seem to be a valid file.");
- throw std::invalid_argument(exception_message);
- }
- }
- /// \brief Loads a Derived object from an XML file.
- void loadFromXML(const std::string & filename, const std::string & tag_name) throw (std::invalid_argument)
- {
- assert(!tag_name.empty());
- std::ifstream ifs(filename.c_str());
- if(ifs)
- {
- boost::archive::xml_iarchive ia(ifs);
- ia >> boost::serialization::make_nvp(tag_name.c_str(),derived());
- }
- else
+ /// \brief Loads a Derived object from an XML file.
+ void loadFromXML(const std::string & filename, const std::string & tag_name) throw (std::invalid_argument)
{
- const std::string exception_message(filename + " does not seem to be a valid file.");
- throw std::invalid_argument(exception_message);
+ assert(!tag_name.empty());
+ std::ifstream ifs(filename.c_str());
+ if(ifs)
+ {
+ boost::archive::xml_iarchive ia(ifs);
+ ia >> boost::serialization::make_nvp(tag_name.c_str(),derived());
+ }
+ else
+ {
+ const std::string exception_message(filename + " does not seem to be a valid file.");
+ throw std::invalid_argument(exception_message);
+ }
}
- }
- /// \brief Saved a Derived object as an XML file.
- void saveAsXML(const std::string & filename, const std::string & tag_name) const throw (std::invalid_argument)
- {
- assert(!tag_name.empty());
- std::ofstream ofs(filename.c_str());
- if(ofs)
+ /// \brief Saved a Derived object as an XML file.
+ void saveAsXML(const std::string & filename, const std::string & tag_name) const throw (std::invalid_argument)
{
- boost::archive::xml_oarchive oa(ofs);
- oa << boost::serialization::make_nvp(tag_name.c_str(),derived());
+ assert(!tag_name.empty());
+ std::ofstream ofs(filename.c_str());
+ if(ofs)
+ {
+ boost::archive::xml_oarchive oa(ofs);
+ oa << boost::serialization::make_nvp(tag_name.c_str(),derived());
+ }
+ else
+ {
+ const std::string exception_message(filename + " does not seem to be a valid file.");
+ throw std::invalid_argument(exception_message);
+ }
}
- else
- {
- const std::string exception_message(filename + " does not seem to be a valid file.");
- throw std::invalid_argument(exception_message);
- }
- }
- /// \brief Loads a Derived object from an binary file.
- void loadFromBinary(const std::string & filename) throw (std::invalid_argument)
- {
- std::ifstream ifs(filename.c_str());
- if(ifs)
- {
- boost::archive::binary_iarchive ia(ifs);
- ia >> derived();
- }
- else
+ /// \brief Loads a Derived object from an binary file.
+ void loadFromBinary(const std::string & filename) throw (std::invalid_argument)
{
- const std::string exception_message(filename + " does not seem to be a valid file.");
- throw std::invalid_argument(exception_message);
+ std::ifstream ifs(filename.c_str());
+ if(ifs)
+ {
+ boost::archive::binary_iarchive ia(ifs);
+ ia >> derived();
+ }
+ else
+ {
+ const std::string exception_message(filename + " does not seem to be a valid file.");
+ throw std::invalid_argument(exception_message);
+ }
}
- }
- /// \brief Saved a Derived object as an binary file.
- void saveAsBinary(const std::string & filename) const throw (std::invalid_argument)
- {
- std::ofstream ofs(filename.c_str());
- if(ofs)
- {
- boost::archive::binary_oarchive oa(ofs);
- oa << derived();
- }
- else
+ /// \brief Saved a Derived object as an binary file.
+ void saveAsBinary(const std::string & filename) const throw (std::invalid_argument)
{
- const std::string exception_message(filename + " does not seem to be a valid file.");
- throw std::invalid_argument(exception_message);
+ std::ofstream ofs(filename.c_str());
+ if(ofs)
+ {
+ boost::archive::binary_oarchive oa(ofs);
+ oa << derived();
+ }
+ else
+ {
+ const std::string exception_message(filename + " does not seem to be a valid file.");
+ throw std::invalid_argument(exception_message);
+ }
}
- }
- };
+ }; // End struct Serializable
}
diff --git a/include/curves/serialization/eigen-matrix.hpp b/include/curves/serialization/eigen-matrix.hpp
index e9422a44bb6b13bc6b3b3f5ca7a71bd9963a7f6b..80d53a441ee6a4bdd07944b300360ab4e6d88d93 100644
--- a/include/curves/serialization/eigen-matrix.hpp
+++ b/include/curves/serialization/eigen-matrix.hpp
@@ -33,7 +33,6 @@
#include <boost/serialization/vector.hpp>
namespace boost{
-
namespace serialization{
template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
void save(Archive & ar, const Eigen::Matrix<_Scalar,_Rows,_Cols,_Options,_MaxRows,_MaxCols> & m, const unsigned int) // version
diff --git a/python/archive_python_binding.h b/python/archive_python_binding.h
index 6bfddf66cb448ae791d60778756e76e698dc9c37..fa8a2966c1ef1290959274dcebc91b22139f75c4 100644
--- a/python/archive_python_binding.h
+++ b/python/archive_python_binding.h
@@ -8,14 +8,11 @@
namespace curves
{
-
namespace bp = boost::python;
-
template<typename Derived>
struct SerializableVisitor
- : public boost::python::def_visitor< SerializableVisitor<Derived> >
+ : public boost::python::def_visitor< SerializableVisitor<Derived> >
{
-
template<class PyClass>
void visit(PyClass& cl) const
{
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index e35510ca15dfdc1728de6e01accbf590f47b45ae..20824b188180a4ed0fdec795622b117dcf8e44ed 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -68,134 +68,136 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
namespace curves
{
-using namespace boost::python;
+ using namespace boost::python;
-/* Template constructor bezier */
-template <typename Bezier, typename PointList, typename T_Point>
-Bezier* wrapBezierConstructorTemplate(const PointList& array, const real T_min =0., const real T_max =1.)
-{
+ /* Template constructor bezier */
+ template <typename Bezier, typename PointList, typename T_Point>
+ Bezier* wrapBezierConstructorTemplate(const PointList& array, const real T_min =0., const real T_max =1.)
+ {
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), T_min, T_max);
-}
+ }
-template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
-Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints,
- const real T_min =0., const real T_max =1.)
-{
+ template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
+ Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints,
+ const real T_min =0., const real T_max =1.)
+ {
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, T_min, T_max);
-}
-/* End Template constructor bezier */
+ }
+ /* End Template constructor bezier */
-/*3D constructors bezier */
-bezier3_t* wrapBezierConstructor3(const point_list_t& array)
-{
+ /*3D constructors bezier */
+ bezier3_t* wrapBezierConstructor3(const point_list_t& array)
+ {
return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array) ;
-}
-bezier3_t* wrapBezierConstructorBounds3(const point_list_t& array, const real T_min, const real T_max)
-{
+ }
+ bezier3_t* wrapBezierConstructorBounds3(const point_list_t& array, const real T_min, const real T_max)
+ {
return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array, T_min, T_max) ;
-}
-bezier3_t* wrapBezierConstructor3Constraints(const point_list_t& array, const curve_constraints_t& constraints)
-{
+ }
+ bezier3_t* wrapBezierConstructor3Constraints(const point_list_t& array, const curve_constraints_t& constraints)
+ {
return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ;
-}
-bezier3_t* wrapBezierConstructorBounds3Constraints(const point_list_t& array, const curve_constraints_t& constraints,
- const real T_min, const real T_max)
-{
- return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, T_min, T_max) ;
-}
-/*END 3D constructors bezier */
-/*6D constructors bezier */
-bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
-{
+ }
+ bezier3_t* wrapBezierConstructorBounds3Constraints(const point_list_t& array, const curve_constraints_t& constraints,
+ const real T_min, const real T_max)
+ {
+ return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, T_min, T_max) ;
+ }
+ /*END 3D constructors bezier */
+ /*6D constructors bezier */
+ 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 T_min, const real T_max)
-{
+ }
+ bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real T_min, const real T_max)
+ {
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, T_min, T_max) ;
-}
-bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints)
-{
+ }
+ 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 T_min, const real T_max)
-{
- return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, T_min, T_max) ;
-}
-/*END 6D constructors bezier */
-
-/* Wrap Cubic hermite spline */
-t_pair_point_tangent_t getPairsPointTangent(const point_list_t& points, const point_list_t& tangents)
-{
+ }
+ bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints,
+ const real T_min, const real T_max)
+ {
+ return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints,
+ T_min, T_max) ;
+ }
+ /*END 6D constructors bezier */
+
+ /* Wrap Cubic hermite spline */
+ t_pair_point_tangent_t getPairsPointTangent(const point_list_t& points, const point_list_t& tangents)
+ {
t_pair_point_tangent_t res;
if (points.size() != tangents.size())
{
- throw std::length_error("size of points and tangents must be the same");
+ throw std::length_error("size of points and tangents must be the same");
}
for(int i =0;i<points.cols();++i)
{
- res.push_back(pair_point_tangent_t(tangents.col(i), tangents.col(i)));
+ res.push_back(pair_point_tangent_t(tangents.col(i), tangents.col(i)));
}
return res;
-}
+ }
-cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const point_list_t& points, const point_list_t& tangents, const time_waypoints_t& time_pts)
-{
+ cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const point_list_t& points, const point_list_t& tangents, const time_waypoints_t& time_pts)
+ {
t_pair_point_tangent_t ppt = getPairsPointTangent(points, tangents);
std::vector<real> time_control_pts;
for( int i =0; i<time_pts.size(); ++i)
{
- time_control_pts.push_back(time_pts[i]);
+ time_control_pts.push_back(time_pts[i]);
}
return new cubic_hermite_spline_t(ppt.begin(), ppt.end(), time_control_pts);
-}
-/* End wrap Cubic hermite spline */
+ }
+ /* End wrap Cubic hermite spline */
-/* Wrap polynomial */
-polynomial_t* wrapSplineConstructor1(const coeff_t& array, const real min, const real max)
-{
+ /* Wrap polynomial */
+ polynomial_t* wrapSplineConstructor1(const coeff_t& array, const real min, const real max)
+ {
return new polynomial_t(array, min, max);
-}
-polynomial_t* wrapSplineConstructor2(const coeff_t& array)
-{
+ }
+ polynomial_t* wrapSplineConstructor2(const coeff_t& array)
+ {
return new polynomial_t(array, 0., 1.);
-}
-/* End wrap polynomial */
+ }
+ /* End wrap polynomial */
-/* Wrap piecewise curve */
-piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol)
-{
+ /* Wrap piecewise curve */
+ piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol)
+ {
return new piecewise_polynomial_curve_t(pol);
-}
-piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier3_t& bc)
-{
+ }
+ piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier3_t& bc)
+ {
return new piecewise_bezier3_curve_t(bc);
-}
-piecewise_bezier6_curve_t* wrapPiecewiseBezier6CurveConstructor(const bezier6_t& bc)
-{
+ }
+ piecewise_bezier6_curve_t* wrapPiecewiseBezier6CurveConstructor(const bezier6_t& bc)
+ {
return new piecewise_bezier6_curve_t(bc);
-}
-piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch)
-{
+ }
+ piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch)
+ {
return new piecewise_cubic_hermite_curve_t(ch);
-}
-/* end wrap piecewise polynomial curve */
+ }
+ /* end wrap piecewise polynomial curve */
-/* Wrap exact cubic spline */
-t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
-{
+ /* Wrap exact cubic spline */
+ t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
+ {
t_waypoint_t res;
for(int i =0;i<array.cols();++i)
{
- res.push_back(std::make_pair(time_wp(i), array.col(i)));
+ res.push_back(std::make_pair(time_wp(i), array.col(i)));
}
return res;
-}
+ }
-template <typename BezierType, int dim>
-Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> wayPointsToList(const BezierType& self)
-{
+ template <typename BezierType, int dim>
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> wayPointsToList(const BezierType& self)
+ {
typedef typename BezierType::t_point_t t_point;
typedef typename BezierType::t_point_t::const_iterator cit_point;
const t_point& wps = self.waypoints();
@@ -203,70 +205,69 @@ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> wayPointsToList(const Bezier
int col = 0;
for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col)
{
- res.block<dim,1>(0,col) = *cit;
+ res.block<dim,1>(0,col) = *cit;
}
return res;
-}
+ }
-exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoints_t& time_wp)
-{
+ exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoints_t& time_wp)
+ {
t_waypoint_t wps = getWayPoints(array, time_wp);
return new exact_cubic_t(wps.begin(), wps.end());
-}
+ }
-exact_cubic_t* wrapExactCubicConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const curve_constraints_t& constraints)
-{
+ exact_cubic_t* wrapExactCubicConstructorConstraint(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 exact_cubic_t(wps.begin(), wps.end(), constraints);
-}
+ }
-point_t get_init_vel(const curve_constraints_t& c)
-{
+ point_t get_init_vel(const curve_constraints_t& c)
+ {
return c.init_vel;
-}
+ }
-point_t get_init_acc(const curve_constraints_t& c)
-{
+ point_t get_init_acc(const curve_constraints_t& c)
+ {
return c.init_acc;
-}
+ }
-point_t get_end_vel(const curve_constraints_t& c)
-{
+ point_t get_end_vel(const curve_constraints_t& c)
+ {
return c.end_vel;
-}
+ }
-point_t get_end_acc(const curve_constraints_t& c)
-{
+ point_t get_end_acc(const curve_constraints_t& c)
+ {
return c.end_acc;
-}
+ }
-void set_init_vel(curve_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(curve_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(curve_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(curve_constraints_t& c, const point_t& val)
-{
+ void set_end_acc(curve_constraints_t& c, const point_t& val)
+ {
c.end_acc = val;
-}
-/* End wrap exact cubic spline */
+ }
+ /* End wrap exact cubic spline */
-BOOST_PYTHON_MODULE(curves)
-{
+ BOOST_PYTHON_MODULE(curves)
+ {
/** BEGIN eigenpy init**/
eigenpy::enableEigenPy();
-
eigenpy::enableEigenPySpecific<point_t,point_t>();
eigenpy::enableEigenPySpecific<ret_point_t,ret_point_t>();
eigenpy::enableEigenPySpecific<point_list_t,point_list_t>();
@@ -277,181 +278,165 @@ BOOST_PYTHON_MODULE(curves)
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
-
/** BEGIN bezier curve 6**/
class_<bezier6_t>("bezier6", 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())
- .def("derivate", &bezier6_t::derivate)
- .def("compute_derivate", &bezier6_t::compute_derivate)
- .def("compute_primitive", &bezier6_t::compute_primitive)
- .def("waypoints", &wayPointsToList<bezier6_t,6>)
- .def_readonly("degree", &bezier6_t::degree_)
- .def_readonly("nbWaypoints", &bezier6_t::size_)
- .def(SerializableVisitor<bezier6_t>())
- ;
+ .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())
+ .def("derivate", &bezier6_t::derivate)
+ .def("compute_derivate", &bezier6_t::compute_derivate)
+ .def("compute_primitive", &bezier6_t::compute_primitive)
+ .def("waypoints", &wayPointsToList<bezier6_t,6>)
+ .def_readonly("degree", &bezier6_t::degree_)
+ .def_readonly("nbWaypoints", &bezier6_t::size_)
+ .def(SerializableVisitor<bezier6_t>())
+ ;
/** END bezier curve**/
-
/** BEGIN bezier curve**/
class_<bezier3_t>("bezier3", init<>())
- .def("__init__", make_constructor(&wrapBezierConstructor3))
- .def("__init__", make_constructor(&wrapBezierConstructorBounds3))
- .def("__init__", make_constructor(&wrapBezierConstructor3Constraints))
- .def("__init__", make_constructor(&wrapBezierConstructorBounds3Constraints))
- .def("min", &bezier3_t::min)
- .def("max", &bezier3_t::max)
- .def("__call__", &bezier3_t::operator())
- .def("derivate", &bezier3_t::derivate)
- .def("compute_derivate", &bezier3_t::compute_derivate)
- .def("compute_primitive", &bezier3_t::compute_primitive)
- .def("waypoints", &wayPointsToList<bezier3_t,3>)
- .def_readonly("degree", &bezier3_t::degree_)
- .def_readonly("nbWaypoints", &bezier3_t::size_)
- .def(SerializableVisitor<bezier3_t>())
- ;
+ .def("__init__", make_constructor(&wrapBezierConstructor3))
+ .def("__init__", make_constructor(&wrapBezierConstructorBounds3))
+ .def("__init__", make_constructor(&wrapBezierConstructor3Constraints))
+ .def("__init__", make_constructor(&wrapBezierConstructorBounds3Constraints))
+ .def("min", &bezier3_t::min)
+ .def("max", &bezier3_t::max)
+ .def("__call__", &bezier3_t::operator())
+ .def("derivate", &bezier3_t::derivate)
+ .def("compute_derivate", &bezier3_t::compute_derivate)
+ .def("compute_primitive", &bezier3_t::compute_primitive)
+ .def("waypoints", &wayPointsToList<bezier3_t,3>)
+ .def_readonly("degree", &bezier3_t::degree_)
+ .def_readonly("nbWaypoints", &bezier3_t::size_)
+ .def(SerializableVisitor<bezier3_t>())
+ ;
/** END bezier curve**/
-
-
/** BEGIN variable points bezier curve**/
class_<LinearControlPointsHolder>
- ("LinearWaypoint", no_init)
- .def_readonly("A", &LinearControlPointsHolder::A)
- .def_readonly("b", &LinearControlPointsHolder::b)
- ;
-
+ ("LinearWaypoint", no_init)
+ .def_readonly("A", &LinearControlPointsHolder::A)
+ .def_readonly("b", &LinearControlPointsHolder::b)
+ ;
class_<LinearBezierVector>
- ("bezierVarVector", no_init)
- .def_readonly("size", &LinearBezierVector::size)
- .def("at", &LinearBezierVector::at, return_value_policy<manage_new_object>())
- ;
-
+ ("bezierVarVector", no_init)
+ .def_readonly("size", &LinearBezierVector::size)
+ .def("at", &LinearBezierVector::at, return_value_policy<manage_new_object>())
+ ;
class_<bezier_linear_variable_t>("bezierVar", no_init)
- .def("__init__", make_constructor(&wrapBezierLinearConstructor))
- .def("__init__", make_constructor(&wrapBezierLinearConstructorBounds))
- .def("min", &bezier_linear_variable_t::min)
- .def("max", &bezier_linear_variable_t::max)
- //.def("__call__", &bezier_linear_control_t::operator())
- .def("derivate", &bezier_linear_variable_t::derivate)
- .def("compute_derivate", &bezier_linear_variable_t::compute_derivate)
- .def("compute_primitive", &bezier_linear_variable_t::compute_primitive)
- .def("split", &split, return_value_policy<manage_new_object>())
- .def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
- .def_readonly("degree", &bezier_linear_variable_t::degree_)
- .def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
- ;
+ .def("__init__", make_constructor(&wrapBezierLinearConstructor))
+ .def("__init__", make_constructor(&wrapBezierLinearConstructorBounds))
+ .def("min", &bezier_linear_variable_t::min)
+ .def("max", &bezier_linear_variable_t::max)
+ //.def("__call__", &bezier_linear_control_t::operator())
+ .def("derivate", &bezier_linear_variable_t::derivate)
+ .def("compute_derivate", &bezier_linear_variable_t::compute_derivate)
+ .def("compute_primitive", &bezier_linear_variable_t::compute_primitive)
+ .def("split", &split, return_value_policy<manage_new_object>())
+ .def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
+ .def_readonly("degree", &bezier_linear_variable_t::degree_)
+ .def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
+ ;
/** END variable points bezier curve**/
-
/** BEGIN polynomial curve function**/
class_<polynomial_t>("polynomial", init<>())
- .def("__init__", make_constructor(&wrapSplineConstructor1))
- .def("__init__", make_constructor(&wrapSplineConstructor2))
- .def("min", &polynomial_t::min)
- .def("max", &polynomial_t::max)
- .def("__call__", &polynomial_t::operator())
- .def("derivate", &polynomial_t::derivate)
- .def(SerializableVisitor<polynomial_t>())
- ;
+ .def("__init__", make_constructor(&wrapSplineConstructor1))
+ .def("__init__", make_constructor(&wrapSplineConstructor2))
+ .def("min", &polynomial_t::min)
+ .def("max", &polynomial_t::max)
+ .def("__call__", &polynomial_t::operator())
+ .def("derivate", &polynomial_t::derivate)
+ .def(SerializableVisitor<polynomial_t>())
+ ;
/** END polynomial function**/
-
/** BEGIN piecewise curve function **/
class_<piecewise_polynomial_curve_t>
- ("piecewise_polynomial_curve", init<>())
- .def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor))
- .def("min", &piecewise_polynomial_curve_t::min)
- .def("max", &piecewise_polynomial_curve_t::max)
- .def("__call__", &piecewise_polynomial_curve_t::operator())
- .def("derivate", &piecewise_polynomial_curve_t::derivate)
- .def("add_curve", &piecewise_polynomial_curve_t::add_curve)
- .def("is_continuous", &piecewise_polynomial_curve_t::is_continuous)
- .def(SerializableVisitor<piecewise_polynomial_curve_t>())
- ;
+ ("piecewise_polynomial_curve", init<>())
+ .def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor))
+ .def("min", &piecewise_polynomial_curve_t::min)
+ .def("max", &piecewise_polynomial_curve_t::max)
+ .def("__call__", &piecewise_polynomial_curve_t::operator())
+ .def("derivate", &piecewise_polynomial_curve_t::derivate)
+ .def("add_curve", &piecewise_polynomial_curve_t::add_curve)
+ .def("is_continuous", &piecewise_polynomial_curve_t::is_continuous)
+ .def(SerializableVisitor<piecewise_polynomial_curve_t>())
+ ;
class_<piecewise_bezier3_curve_t>
- ("piecewise_bezier3_curve", init<>())
- .def("__init__", make_constructor(&wrapPiecewiseBezier3CurveConstructor))
- .def("min", &piecewise_bezier3_curve_t::min)
- .def("max", &piecewise_bezier3_curve_t::max)
- .def("__call__", &piecewise_bezier3_curve_t::operator())
- .def("derivate", &piecewise_bezier3_curve_t::derivate)
- .def("add_curve", &piecewise_bezier3_curve_t::add_curve)
- .def("is_continuous", &piecewise_bezier3_curve_t::is_continuous)
- .def(SerializableVisitor<piecewise_bezier3_curve_t>())
- ;
+ ("piecewise_bezier3_curve", init<>())
+ .def("__init__", make_constructor(&wrapPiecewiseBezier3CurveConstructor))
+ .def("min", &piecewise_bezier3_curve_t::min)
+ .def("max", &piecewise_bezier3_curve_t::max)
+ .def("__call__", &piecewise_bezier3_curve_t::operator())
+ .def("derivate", &piecewise_bezier3_curve_t::derivate)
+ .def("add_curve", &piecewise_bezier3_curve_t::add_curve)
+ .def("is_continuous", &piecewise_bezier3_curve_t::is_continuous)
+ .def(SerializableVisitor<piecewise_bezier3_curve_t>())
+ ;
class_<piecewise_bezier6_curve_t>
- ("piecewise_bezier6_curve", init<>())
- .def("__init__", make_constructor(&wrapPiecewiseBezier6CurveConstructor))
- .def("min", &piecewise_bezier6_curve_t::min)
- .def("max", &piecewise_bezier6_curve_t::max)
- .def("__call__", &piecewise_bezier6_curve_t::operator())
- .def("derivate", &piecewise_bezier6_curve_t::derivate)
- .def("add_curve", &piecewise_bezier6_curve_t::add_curve)
- .def("is_continuous", &piecewise_bezier6_curve_t::is_continuous)
- .def(SerializableVisitor<piecewise_bezier6_curve_t>())
- ;
+ ("piecewise_bezier6_curve", init<>())
+ .def("__init__", make_constructor(&wrapPiecewiseBezier6CurveConstructor))
+ .def("min", &piecewise_bezier6_curve_t::min)
+ .def("max", &piecewise_bezier6_curve_t::max)
+ .def("__call__", &piecewise_bezier6_curve_t::operator())
+ .def("derivate", &piecewise_bezier6_curve_t::derivate)
+ .def("add_curve", &piecewise_bezier6_curve_t::add_curve)
+ .def("is_continuous", &piecewise_bezier6_curve_t::is_continuous)
+ .def(SerializableVisitor<piecewise_bezier6_curve_t>())
+ ;
class_<piecewise_cubic_hermite_curve_t>
- ("piecewise_cubic_hermite_curve", init<>())
- .def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
- .def("min", &piecewise_cubic_hermite_curve_t::min)
- .def("max", &piecewise_cubic_hermite_curve_t::max)
- .def("__call__", &piecewise_cubic_hermite_curve_t::operator())
- .def("derivate", &piecewise_cubic_hermite_curve_t::derivate)
- .def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
- .def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
- .def(SerializableVisitor<piecewise_cubic_hermite_curve_t>())
- ;
+ ("piecewise_cubic_hermite_curve", init<>())
+ .def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
+ .def("min", &piecewise_cubic_hermite_curve_t::min)
+ .def("max", &piecewise_cubic_hermite_curve_t::max)
+ .def("__call__", &piecewise_cubic_hermite_curve_t::operator())
+ .def("derivate", &piecewise_cubic_hermite_curve_t::derivate)
+ .def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
+ .def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
+ .def(SerializableVisitor<piecewise_cubic_hermite_curve_t>())
+ ;
/** END piecewise curve function **/
-
-
/** BEGIN exact_cubic curve**/
class_<exact_cubic_t>
- ("exact_cubic", init<>())
- .def("__init__", make_constructor(&wrapExactCubicConstructor))
- .def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
- .def("min", &exact_cubic_t::min)
- .def("max", &exact_cubic_t::max)
- .def("__call__", &exact_cubic_t::operator())
- .def("derivate", &exact_cubic_t::derivate)
- .def("getNumberSplines", &exact_cubic_t::getNumberSplines)
- .def("getSplineAt", &exact_cubic_t::getSplineAt)
- .def(SerializableVisitor<exact_cubic_t>())
- ;
+ ("exact_cubic", init<>())
+ .def("__init__", make_constructor(&wrapExactCubicConstructor))
+ .def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
+ .def("min", &exact_cubic_t::min)
+ .def("max", &exact_cubic_t::max)
+ .def("__call__", &exact_cubic_t::operator())
+ .def("derivate", &exact_cubic_t::derivate)
+ .def("getNumberSplines", &exact_cubic_t::getNumberSplines)
+ .def("getSplineAt", &exact_cubic_t::getSplineAt)
+ .def(SerializableVisitor<exact_cubic_t>())
+ ;
/** END exact_cubic curve**/
-
-
/** BEGIN cubic_hermite_spline **/
class_<cubic_hermite_spline_t>
- ("cubic_hermite_spline", init<>())
- .def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
- .def("min", &cubic_hermite_spline_t::min)
- .def("max", &cubic_hermite_spline_t::max)
- .def("__call__", &cubic_hermite_spline_t::operator())
- .def("derivate", &cubic_hermite_spline_t::derivate)
- .def(SerializableVisitor<cubic_hermite_spline_t>())
- ;
+ ("cubic_hermite_spline", init<>())
+ .def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
+ .def("min", &cubic_hermite_spline_t::min)
+ .def("max", &cubic_hermite_spline_t::max)
+ .def("__call__", &cubic_hermite_spline_t::operator())
+ .def("derivate", &cubic_hermite_spline_t::derivate)
+ .def(SerializableVisitor<cubic_hermite_spline_t>())
+ ;
/** END cubic_hermite_spline **/
-
-
/** 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)
- ;
+ ("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 curve constraints**/
-
/** BEGIN bernstein polynomial**/
class_<bernstein_t>
- ("bernstein", init<const unsigned int, const unsigned int>())
- .def("__call__", &bernstein_t::operator())
- ;
+ ("bernstein", init<const unsigned int, const unsigned int>())
+ .def("__call__", &bernstein_t::operator())
+ ;
/** END bernstein polynomial**/
-
/** BEGIN curves conversion**/
def("polynomial_from_bezier", polynomial_from_curve<polynomial_t,bezier3_t>);
def("polynomial_from_hermite", polynomial_from_curve<polynomial_t,cubic_hermite_spline_t>);
@@ -460,7 +445,5 @@ BOOST_PYTHON_MODULE(curves)
def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier3_t>);
def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial_t>);
/** END curves conversion**/
-
-}
-
+ } // End BOOST_PYTHON_MODULE
} // namespace curves
diff --git a/python/python_definitions.h b/python/python_definitions.h
index 8c4b3d674e6e965191f70b74243c187a356a379c..b65a3b49a02ddd29f3c4bf7be6c0e5f9c218b529 100644
--- a/python/python_definitions.h
+++ b/python/python_definitions.h
@@ -6,42 +6,35 @@
#ifndef _DEFINITION_PYTHON_BINDINGS
#define _DEFINITION_PYTHON_BINDINGS
-
namespace curves
{
-
-typedef double real;
-typedef Eigen::Vector3d point_t;
-typedef Eigen::Vector3d tangent_t;
-typedef Eigen::VectorXd vectorX_t;
-typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
-typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
-typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
-typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
-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<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;
-typedef std::vector<Waypoint> T_Waypoint;
-typedef std::pair<real, point6_t> Waypoint6;
-typedef std::vector<Waypoint6> T_Waypoint6;
-
-
-
-template <typename PointList, typename T_Point>
-T_Point vectorFromEigenArray(const PointList& array)
-{
- T_Point res;
- for(int i =0;i<array.cols();++i)
- {
- res.push_back(array.col(i));
- }
- return res;
-}
-
+ typedef double real;
+ typedef Eigen::Vector3d point_t;
+ typedef Eigen::Vector3d tangent_t;
+ typedef Eigen::VectorXd vectorX_t;
+ typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
+ typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
+ typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
+ typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
+ 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<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;
+ typedef std::vector<Waypoint> T_Waypoint;
+ typedef std::pair<real, point6_t> Waypoint6;
+ typedef std::vector<Waypoint6> T_Waypoint6;
+
+ template <typename PointList, typename T_Point>
+ T_Point vectorFromEigenArray(const PointList& array)
+ {
+ T_Point res;
+ for(int i =0;i<array.cols();++i)
+ {
+ res.push_back(array.col(i));
+ }
+ return res;
+ }
} //namespace curves
-
-
#endif //_DEFINITION_PYTHON_BINDINGS
diff --git a/python/python_variables.cpp b/python/python_variables.cpp
index 8306be298b6c831cb0cd2f3037350c8647e1d88e..3fa6f66a1cc8e76a94959416ae6536e0fc45f478 100644
--- a/python/python_variables.cpp
+++ b/python/python_variables.cpp
@@ -3,66 +3,63 @@
#include <Eigen/Core>
-
namespace curves
{
-
-std::vector<linear_variable_3_t> matrix3DFromEigenArray(const point_list_t& matrices, const point_list_t& vectors)
-{
+ std::vector<linear_variable_3_t> matrix3DFromEigenArray(const point_list_t& matrices, const point_list_t& vectors)
+ {
assert(vectors.cols() * 3 == matrices.cols() ) ;
std::vector<linear_variable_3_t> res;
for(int i =0;i<vectors.cols();++i)
{
- res.push_back(linear_variable_3_t(matrices.block<3,3>(0,i*3), vectors.col(i)));
+ res.push_back(linear_variable_3_t(matrices.block<3,3>(0,i*3), vectors.col(i)));
}
return res;
-}
+ }
-variables_3_t fillWithZeros(const linear_variable_3_t& var, const std::size_t totalvar, const std::size_t i)
-{
+ variables_3_t fillWithZeros(const linear_variable_3_t& var, const std::size_t totalvar, const std::size_t i)
+ {
variables_3_t res;
std::vector<linear_variable_3_t>& vars = res.variables_;
for (std::size_t idx = 0; idx < i; ++idx)
{
- vars.push_back(linear_variable_3_t::Zero());
+ vars.push_back(linear_variable_3_t::Zero());
}
vars.push_back(var);
for (std::size_t idx = i+1; idx < totalvar; ++idx)
{
- vars.push_back(linear_variable_3_t::Zero());
+ vars.push_back(linear_variable_3_t::Zero());
}
return res;
-}
+ }
-std::vector<variables_3_t> computeLinearControlPoints(const point_list_t& matrices, const point_list_t& vectors)
-{
+ std::vector<variables_3_t> computeLinearControlPoints(const point_list_t& matrices, const point_list_t& vectors)
+ {
std::vector<variables_3_t> res;
std::vector<linear_variable_3_t> variables = matrix3DFromEigenArray(matrices, vectors);
// now need to fill all this with zeros...
std::size_t totalvar = variables.size();
for (std::size_t i = 0; i < totalvar; ++i)
{
- res.push_back( fillWithZeros(variables[i],totalvar,i));
+ res.push_back( fillWithZeros(variables[i],totalvar,i));
}
return res;
-}
+ }
-/*linear variable control points*/
-bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors)
-{
+ /*linear variable control points*/
+ bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors)
+ {
std::vector<variables_3_t> asVector = computeLinearControlPoints(matrices, vectors);
return new bezier_linear_variable_t(asVector.begin(), asVector.end(), 0., 1.) ;
-}
+ }
-bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list_t& matrices, const point_list_t& vectors, const real T_min, const real T_max)
-{
+ bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list_t& matrices, const point_list_t& vectors, const real T_min, const real T_max)
+ {
std::vector<variables_3_t> asVector = computeLinearControlPoints(matrices, vectors);
return new bezier_linear_variable_t(asVector.begin(), asVector.end(), T_min, T_max) ;
-}
-
+ }
-LinearControlPointsHolder* wayPointsToLists(const bezier_linear_variable_t& self)
-{
+ LinearControlPointsHolder* wayPointsToLists(const bezier_linear_variable_t& self)
+ {
typedef typename bezier_linear_variable_t::t_point_t t_point;
typedef typename bezier_linear_variable_t::t_point_t::const_iterator cit_point;
const t_point& wps = self.waypoints();
@@ -73,39 +70,36 @@ LinearControlPointsHolder* wayPointsToLists(const bezier_linear_variable_t& self
int col = 0;
for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col)
{
- const std::vector<linear_variable_3_t>& variables = cit->variables_;
- int i = 0;
- for(std::vector<linear_variable_3_t>::const_iterator varit = variables.begin();
- varit != variables.end(); ++varit, i+=3)
- {
- vectors.block<3,1>(i,col) = varit->b_;
- matrices.block<3,3>(i,col*3) = varit->A_;
- }
+ const std::vector<linear_variable_3_t>& variables = cit->variables_;
+ int i = 0;
+ for(std::vector<linear_variable_3_t>::const_iterator varit = variables.begin();
+ varit != variables.end(); ++varit, i+=3)
+ {
+ vectors.block<3,1>(i,col) = varit->b_;
+ matrices.block<3,3>(i,col*3) = varit->A_;
+ }
}
LinearControlPointsHolder* res (new LinearControlPointsHolder);
res->res = std::make_pair(matrices, vectors);
return res;
-}
+ }
-
-// does not include end time
-LinearBezierVector* split(const bezier_linear_variable_t& self, const vectorX_t& times)
-{
+ // does not include end time
+ LinearBezierVector* split(const bezier_linear_variable_t& self, const vectorX_t& times)
+ {
LinearBezierVector* res (new LinearBezierVector);
bezier_linear_variable_t current = self;
real current_time = 0.;
real tmp;
for(int i = 0; i < times.rows(); ++i)
{
- tmp =times[i];
- std::pair<bezier_linear_variable_t, bezier_linear_variable_t> pairsplit = current.split(tmp-current_time);
- res->beziers_.push_back(pairsplit.first);
- current = pairsplit.second;
- current_time += tmp-current_time;
+ tmp =times[i];
+ std::pair<bezier_linear_variable_t, bezier_linear_variable_t> pairsplit = current.split(tmp-current_time);
+ res->beziers_.push_back(pairsplit.first);
+ current = pairsplit.second;
+ current_time += tmp-current_time;
}
res->beziers_.push_back(current);
return res;
-}
-
-}
- // namespace curves
+ }
+} // namespace curves
diff --git a/python/python_variables.h b/python/python_variables.h
index d369c20fb9a4dab6ac885d575fb7c175cc84afcc..91c6520a375a8c050c368682ce457ee028c4c2b5 100644
--- a/python/python_variables.h
+++ b/python/python_variables.h
@@ -11,43 +11,42 @@
namespace curves
{
-static const int dim = 3;
-typedef curves::linear_variable<dim, real> linear_variable_3_t;
-typedef curves::variables<linear_variable_3_t> variables_3_t;
-typedef curves::bezier_curve <real, real, dim, true, variables_3_t> bezier_linear_variable_t;
+ static const int dim = 3;
+ typedef curves::linear_variable<dim, real> linear_variable_3_t;
+ typedef curves::variables<linear_variable_3_t> variables_3_t;
+ typedef curves::bezier_curve <real, real, dim, true, variables_3_t> bezier_linear_variable_t;
+ /*linear variable control points*/
+ bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors);
-/*linear variable control points*/
-bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors);
+ bezier_linear_variable_t* wrapBezierLinearConstructorBounds
+ (const point_list_t& matrices, const point_list_t& vectors, const real T_min, const real T_max);
-bezier_linear_variable_t* wrapBezierLinearConstructorBounds
- (const point_list_t& matrices, const point_list_t& vectors, const real T_min, const real T_max);
+ typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
-typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
-
-struct LinearControlPointsHolder
-{
+ struct LinearControlPointsHolder
+ {
linear_points_t res;
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A() {return res.first;}
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> b() {return res.second;}
-};
+ };
-LinearControlPointsHolder* wayPointsToLists(const bezier_linear_variable_t& self);
+ LinearControlPointsHolder* wayPointsToLists(const bezier_linear_variable_t& self);
-struct LinearBezierVector
-{
+ struct LinearBezierVector
+ {
std::vector<bezier_linear_variable_t> beziers_;
std::size_t size() {return beziers_.size();}
bezier_linear_variable_t* at(std::size_t i)
{
- assert (i<size());
- return new bezier_linear_variable_t(beziers_[i]);
+ assert (i<size());
+ return new bezier_linear_variable_t(beziers_[i]);
}
-};
+ };
-// does not include end time
-LinearBezierVector* split(const bezier_linear_variable_t& self, const vectorX_t& times);
+ // does not include end time
+ LinearBezierVector* split(const bezier_linear_variable_t& self, const vectorX_t& times);
} //namespace curve.
diff --git a/python/test/variable/qp_traj/convex_hull.py b/python/test/variable/qp_traj/convex_hull.py
index 06c62d18cf96141f3ccf3c653f009fa5949aac31..897d53056895969cb44211edfcaed94bec0238ce 100644
--- a/python/test/variable/qp_traj/convex_hull.py
+++ b/python/test/variable/qp_traj/convex_hull.py
@@ -1,44 +1,45 @@
import quadprog
from numpy import array, dot, vstack, hstack, asmatrix, identity, cross
from numpy.linalg import norm
+import numpy as np
from scipy.spatial import ConvexHull
-import numpy as np
+
+import matplotlib.pyplot as plt
def genConvexHullLines(points):
- hull = ConvexHull(points)
- lineList = [points[el] for el in hull.vertices] + [points[hull.vertices[0]]]
- lineList = [array(el[:2].tolist() + [0.]) for el in lineList]
- return [[lineList[i],lineList[i+1]] for i in range(len(hull.vertices))], lineList[:-1]
- #now compute lines
+ hull = ConvexHull(points)
+ lineList = [points[el] for el in hull.vertices] + [points[hull.vertices[0]]]
+ lineList = [array(el[:2].tolist() + [0.]) for el in lineList]
+ return [[lineList[i],lineList[i+1]] for i in range(len(hull.vertices))], lineList[:-1]
+ #now compute lines
def getLineFromSegment(line):
- a = line[0]; b = line[1]; c = a.copy() ; c[2] = 1.
- normal = cross((b-a),(c-a))
- normal /= norm(normal)
- # get inequality
- dire = b - a
- coeff = normal
- rhs = a.dot(normal)
- return (coeff, array([rhs]))
+ a = line[0]; b = line[1]; c = a.copy() ; c[2] = 1.
+ normal = cross((b-a),(c-a))
+ normal /= norm(normal)
+ # get inequality
+ dire = b - a
+ coeff = normal
+ rhs = a.dot(normal)
+ return (coeff, array([rhs]))
+
-import numpy as np
-import matplotlib.pyplot as plt
#generate right of the line
def genFromLine(line, num_points, ranges, existing_points = []):
- coeff, rhs = getLineFromSegment(line)
- num_gen = 0
- gen = existing_points + [line[0][:2], line[1][:2]]
- while(len(gen) < num_points):
- pt = array([np.random.uniform(ranges[0][0], ranges[0][1]), np.random.uniform(ranges[1][0], ranges[1][1])])
- if coeff[:2].dot(pt) <= rhs :
- gen += [pt]
- return genConvexHullLines(gen)
+ coeff, rhs = getLineFromSegment(line)
+ num_gen = 0
+ gen = existing_points + [line[0][:2], line[1][:2]]
+ while(len(gen) < num_points):
+ pt = array([np.random.uniform(ranges[0][0], ranges[0][1]), np.random.uniform(ranges[1][0], ranges[1][1])])
+ if coeff[:2].dot(pt) <= rhs :
+ gen += [pt]
+ return genConvexHullLines(gen)
if __name__ == '__main__':
- genFromLine([array([0.5,0.,0.]),array([0.5,0.5,0.])],5)
- genFromLine([array([0.5,0.,0.]),array([0.5,-0.5,0.])],5)
+ genFromLine([array([0.5,0.,0.]),array([0.5,0.5,0.])],5)
+ genFromLine([array([0.5,0.,0.]),array([0.5,-0.5,0.])],5)
diff --git a/python/test/variable/qp_traj/plot_cord.py b/python/test/variable/qp_traj/plot_cord.py
index 490af3cef297c5a170eb2a88c780738f2c52b92a..c8289998132a500d30db5a064b6ee19f28e044c4 100644
--- a/python/test/variable/qp_traj/plot_cord.py
+++ b/python/test/variable/qp_traj/plot_cord.py
@@ -3,25 +3,25 @@ import numpy as np
def plotBezier(bez, color):
- step = 100.
- points1 = np.array([(bez(i/step*bez.max())[0][0],bez(i/step*bez.max())[1][0]) for i in range(int(step))])
- x = points1[:,0]
- y = points1[:,1]
- plt.plot(x,y,color,linewidth=2.0)
-
+ step = 100.
+ points1 = np.array([(bez(i/step*bez.max())[0][0],bez(i/step*bez.max())[1][0]) for i in range(int(step))])
+ x = points1[:,0]
+ y = points1[:,1]
+ plt.plot(x,y,color,linewidth=2.0)
+
def plotControlPoints(bez, color):
- wps = bez.waypoints()
- wps = np.array([wps[:2,i] for i in range(wps.shape[1]) ])
- x = wps[:,0]
- y = wps[:,1]
- plt.scatter(x,y,color=color)
-
+ wps = bez.waypoints()
+ wps = np.array([wps[:2,i] for i in range(wps.shape[1]) ])
+ x = wps[:,0]
+ y = wps[:,1]
+ plt.scatter(x,y,color=color)
+
def plotPoly(lines, color):
- step = 1000.
- for line in lines:
- a_0 = line[0]
- b_0 = line[1]
- pointsline = np.array([ a_0 * i / step + b_0 * (1. - i / step) for i in range(int(step))])
- xl = pointsline[:,0]
- yl = pointsline[:,1]
- plt.plot(xl,yl,color,linewidth=0.5)
+ step = 1000.
+ for line in lines:
+ a_0 = line[0]
+ b_0 = line[1]
+ pointsline = np.array([ a_0 * i / step + b_0 * (1. - i / step) for i in range(int(step))])
+ xl = pointsline[:,0]
+ yl = pointsline[:,1]
+ plt.plot(xl,yl,color,linewidth=0.5)
diff --git a/python/test/variable/qp_traj/qp.py b/python/test/variable/qp_traj/qp.py
index ac2761c19aa9f8df043b327e849c088ee0413d6c..655dfe654d216dccd047d9b5b32eeb19be5b51dd 100644
--- a/python/test/variable/qp_traj/qp.py
+++ b/python/test/variable/qp_traj/qp.py
@@ -9,11 +9,11 @@ def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None):
qp_a = -q
if C is not None:
if G is not None:
- qp_C = -vstack([C, G]).T
- qp_b = -hstack([d, h])
+ qp_C = -vstack([C, G]).T
+ qp_b = -hstack([d, h])
else:
- qp_C = -C.transpose()
- qp_b = -d
+ qp_C = -C.transpose()
+ qp_b = -d
meq = C.shape[0]
else: # no equality constraint
qp_C = -G.T
@@ -26,10 +26,10 @@ def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None):
#subject to G x <= h
#subject to C x = d
def solve_least_square(A,b,G=None, h=None, C=None, d=None):
- P = dot(A.T, A)
- #~ q = 2*dot(b, A).reshape(b.shape[0])
- q = 2*dot(b, A)
- return quadprog_solve_qp(P, q, G, h)
+ P = dot(A.T, A)
+ #~ q = 2*dot(b, A).reshape(b.shape[0])
+ q = 2*dot(b, A)
+ return quadprog_solve_qp(P, q, G, h)
#min q' x
@@ -40,11 +40,11 @@ def solve_lp(q, G=None, h=None, C=None, d=None):
qp_a = -q
if C is not None:
if G is not None:
- qp_C = -vstack([C, G]).T
- qp_b = -hstack([d, h])
+ qp_C = -vstack([C, G]).T
+ qp_b = -hstack([d, h])
else:
- qp_C = -C.transpose()
- qp_b = -d
+ qp_C = -C.transpose()
+ qp_b = -d
meq = C.shape[0]
else: # no equality constraint
qp_C = -G.T
@@ -54,17 +54,14 @@ def solve_lp(q, G=None, h=None, C=None, d=None):
if __name__ == '__main__':
-
- from numpy.linalg import norm
-
- A = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]])
- b = array([3., 2., 3.])
- P = dot(A.T, A)
- q = 2*dot(b, A).reshape((3,))
- G = array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]])
- h = array([3., 2., -2.]).reshape((3,))
-
- res2 = solve_least_square(A, b, G, h)
- res1 = quadprog_solve_qp(P, q, G, h)
- print res1
- print res2
+ from numpy.linalg import norm
+ A = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]])
+ b = array([3., 2., 3.])
+ P = dot(A.T, A)
+ q = 2*dot(b, A).reshape((3,))
+ G = array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]])
+ h = array([3., 2., -2.]).reshape((3,))
+ res2 = solve_least_square(A, b, G, h)
+ res1 = quadprog_solve_qp(P, q, G, h)
+ print res1
+ print res2
diff --git a/python/test/variable/qp_traj/qp_cord.py b/python/test/variable/qp_traj/qp_cord.py
index 55064920824c86e98403a9374accfa764079c8bb..b1d1df741b9e46bcf330ab61e7db01ba20523fdd 100644
--- a/python/test/variable/qp_traj/qp_cord.py
+++ b/python/test/variable/qp_traj/qp_cord.py
@@ -12,72 +12,70 @@ __EPS = 1e-6
#### helpers for stacking matrices ####
def concat(m1,m2):
- if (type(m1) == type(None)):
- return m2
-
- return vstack([m1,m2]).reshape([-1,m2.shape[-1]])
+ if (type(m1) == type(None)):
+ return m2
+ return vstack([m1,m2]).reshape([-1,m2.shape[-1]])
def concatvec(m1,m2):
- if (type(m1) == type(None)):
- return m2
- return array(m1.tolist()+m2.tolist())
+ if (type(m1) == type(None)):
+ return m2
+ return array(m1.tolist()+m2.tolist())
#### helpers for stacking matrices ####
#constraint to lie between 2 extremities of a segment
def segmentConstraint(varBez, a, b, wpIndex, constraintVarIndex, totalAddVarConstraints):
- mat, vec = varBez.matrixFromWaypoints(wpIndex)
- vec = b - vec
- resmat = zeros([mat.shape[0],mat.shape[1]+totalAddVarConstraints])
- resmat[:,:mat.shape[1]]=mat
- resmat[:, mat.shape[1]+constraintVarIndex-1]= b-a
- return (resmat, vec)
+ mat, vec = varBez.matrixFromWaypoints(wpIndex)
+ vec = b - vec
+ resmat = zeros([mat.shape[0],mat.shape[1]+totalAddVarConstraints])
+ resmat[:,:mat.shape[1]]=mat
+ resmat[:, mat.shape[1]+constraintVarIndex-1]= b-a
+ return (resmat, vec)
#constraint to lie one side of a line
def lineConstraint(varBez, C, d, totalAddVarConstraints):
- resmat = None
- resvec = None
- for wpIndex in range(varBez.bezier.nbWaypoints):
- mat, vec = varBez.matrixFromWaypoints(wpIndex)
- mat = C.dot(mat)
- vec = d - C.dot(vec)
- resmat = concat(resmat, mat)
- resvec = concatvec(resvec, vec)
- augmented = zeros([resmat.shape[0],resmat.shape[1]+totalAddVarConstraints])
- augmented[:,:resmat.shape[1]]=resmat
- return (augmented, resvec)
+ resmat = None
+ resvec = None
+ for wpIndex in range(varBez.bezier.nbWaypoints):
+ mat, vec = varBez.matrixFromWaypoints(wpIndex)
+ mat = C.dot(mat)
+ vec = d - C.dot(vec)
+ resmat = concat(resmat, mat)
+ resvec = concatvec(resvec, vec)
+ augmented = zeros([resmat.shape[0],resmat.shape[1]+totalAddVarConstraints])
+ augmented[:,:resmat.shape[1]]=resmat
+ return (augmented, resvec)
##### cost function integrals #####
#compute bezier primitive of variable waypoints given these waypoints
def compute_primitive(wps):
- new_degree = (len(wps)-1+1)
- current_sum = [zeros(wps[0][0].shape),0]
- n_wp = [(current_sum[0],0.)]
- for wp in wps:
- current_sum[0] = current_sum[0] + wp[0]
- current_sum[1] = current_sum[1] + wp[1]
- n_wp +=[(current_sum[0]/new_degree, current_sum[1]/new_degree)]
- return n_wp
+ new_degree = (len(wps)-1+1)
+ current_sum = [zeros(wps[0][0].shape),0]
+ n_wp = [(current_sum[0],0.)]
+ for wp in wps:
+ current_sum[0] = current_sum[0] + wp[0]
+ current_sum[1] = current_sum[1] + wp[1]
+ n_wp +=[(current_sum[0]/new_degree, current_sum[1]/new_degree)]
+ return n_wp
#cost function for analytical integral cost of squared acceleration
def accelerationcost(bezVar):
- acc = bezVar.bezier.compute_derivate(2)
-
- hacc = varBezier(); hacc.bezier = acc
- wps_i=[[],[],[]]
- for i in range(3): #integrate for each axis
- for j in range(acc.nbWaypoints):
- A_i = hacc.matrixFromWaypoints(j)[0][i,:].reshape([1,-1])
- b_i = hacc.matrixFromWaypoints(j)[1][i]
- wps_i[i] += [(A_i.transpose().dot(A_i), b_i*b_i) ]
- # now integrate each bezier curve
- for i, wps in enumerate(wps_i):
- wps_i[i] = compute_primitive(wps)
-
- resmat = wps_i[0][-1][0]; resvec = wps_i[0][-1][1]
- for i in range(1,3):
- resmat = resmat + wps_i[i][-1][0]
- resvec = resvec + wps_i[i][-1][1]
- return (resmat, resvec)
+ acc = bezVar.bezier.compute_derivate(2)
+ hacc = varBezier(); hacc.bezier = acc
+ wps_i=[[],[],[]]
+ for i in range(3): #integrate for each axis
+ for j in range(acc.nbWaypoints):
+ A_i = hacc.matrixFromWaypoints(j)[0][i,:].reshape([1,-1])
+ b_i = hacc.matrixFromWaypoints(j)[1][i]
+ wps_i[i] += [(A_i.transpose().dot(A_i), b_i*b_i) ]
+ # now integrate each bezier curve
+ for i, wps in enumerate(wps_i):
+ wps_i[i] = compute_primitive(wps)
+
+ resmat = wps_i[0][-1][0]; resvec = wps_i[0][-1][1]
+ for i in range(1,3):
+ resmat = resmat + wps_i[i][-1][0]
+ resvec = resvec + wps_i[i][-1][1]
+ return (resmat, resvec)
##### cost function integrals #####
diff --git a/python/test/variable/qp_traj/test_cord.py b/python/test/variable/qp_traj/test_cord.py
index 880e5b199966e436b1f616837b16e3867b266cb8..bef340ce955dab3f5ff521db2caf7a4e52faee05 100644
--- a/python/test/variable/qp_traj/test_cord.py
+++ b/python/test/variable/qp_traj/test_cord.py
@@ -17,127 +17,123 @@ import uuid; uuid.uuid4().hex.upper()[0:6]
######################## generate problems ########################
def genBezierInput(numvars = 3):
- valDep = array([np.random.uniform(0., 1.), np.random.uniform(0.,5.), 0.])
- valEnd = array([np.random.uniform(5., 10.), np.random.uniform(0.,5.), 0.])
- return varBezier([valDep]+["" for _ in range(numvars)]+[valEnd], 1.)
+ valDep = array([np.random.uniform(0., 1.), np.random.uniform(0.,5.), 0.])
+ valEnd = array([np.random.uniform(5., 10.), np.random.uniform(0.,5.), 0.])
+ return varBezier([valDep]+["" for _ in range(numvars)]+[valEnd], 1.)
def genSplit(numCurves):
- splits = []
- lastval = np.random.uniform(0., 1.)
- for i in range(numCurves):
- splits += [lastval]
- lastval += np.random.uniform(0., 1.)
- return [el / lastval for el in splits[:-1]]
+ splits = []
+ lastval = np.random.uniform(0., 1.)
+ for i in range(numCurves):
+ splits += [lastval]
+ lastval += np.random.uniform(0., 1.)
+ return [el / lastval for el in splits[:-1]]
def getRightMostLine(ptList):
- pt1 = array([0.,0.,0.])
- pt2 = array([0.,0.,0.])
- for pt in ptList:
- if pt[0] > pt1[0]:
- pt1 = pt
- elif pt[0] > pt2[0]:
- pt2 = pt
- if pt1[1] < pt2[1]:
- return [pt2,pt1]
- else:
- return [pt1,pt2]
+ pt1 = array([0.,0.,0.])
+ pt2 = array([0.,0.,0.])
+ for pt in ptList:
+ if pt[0] > pt1[0]:
+ pt1 = pt
+ elif pt[0] > pt2[0]:
+ pt2 = pt
+ if pt1[1] < pt2[1]:
+ return [pt2,pt1]
+ else:
+ return [pt1,pt2]
######################## generate problems ########################
######################## solve a given problem ########################
#inequality constraint from line
def getLineFromSegment(line):
- a = line[0]; b = line[1]; c = a.copy() ; c[2] = 1.
- normal = cross((b-a),(c-a))
- normal /= norm(normal)
- # get inequality
- dire = b - a
- coeff = normal
- rhs = a.dot(normal)
- return (coeff, array([rhs]))
+ a = line[0]; b = line[1]; c = a.copy() ; c[2] = 1.
+ normal = cross((b-a),(c-a))
+ normal /= norm(normal)
+ # get inequality
+ dire = b - a
+ coeff = normal
+ rhs = a.dot(normal)
+ return (coeff, array([rhs]))
def computeTrajectory(bezVar, splits, save, filename = uuid.uuid4().hex.upper()[0:6]):
- global idxFile
- colors=['b', 'g', 'r', 'c', 'm', 'y', 'k', 'w']
- subs = bezVar.split(splits)
- #generate random constraints for each line
- line_current = [array([1.,1.,0.]), array([0.,1.,0.])]
-
- #qp vars
- P = accelerationcost(bezVar)[0]; P = P + identity(P.shape[0]) * 0.0001
- q = zeros(3*bezVar.bezier.nbWaypoints)
- q[-1] = -1
- G = zeros([2,q.shape[0]])
- h = zeros(2)
- G[0,-1] = 1 ; h[0]=1.
- G[1,-1] = -1 ; h[1]=0.
-
- dimExtra = 0
-
+ global idxFile
+ colors=['b', 'g', 'r', 'c', 'm', 'y', 'k', 'w']
+ subs = bezVar.split(splits)
+ #generate random constraints for each line
+ line_current = [array([1.,1.,0.]), array([0.,1.,0.])]
+ #qp vars
+ P = accelerationcost(bezVar)[0]; P = P + identity(P.shape[0]) * 0.0001
+ q = zeros(3*bezVar.bezier.nbWaypoints)
+ q[-1] = -1
+ G = zeros([2,q.shape[0]])
+ h = zeros(2)
+ G[0,-1] = 1 ; h[0]=1.
+ G[1,-1] = -1 ; h[1]=0.
+ dimExtra = 0
+ for i, bez in enumerate(subs):
+ color = colors[i]
+ init_points = []
+ if i == 0:
+ init_points = [bezVar.waypoints()[1][:3,0][:2]]
+ if i == len(subs)-1:
+ init_points = [bezVar.waypoints()[1][-3:,-1][:2]]
+ lines, ptList = genFromLine(line_current, 5, [[0,5],[0,5]],init_points)
+ matineq0 = None; vecineq0 = None
+ for line in lines:
+ (mat,vec) = getLineFromSegment(line)
+ (mat,vec) = lineConstraint(bez, mat,vec,dimExtra)
+ (matineq0, vecineq0) = (concat(matineq0,mat), concatvec(vecineq0,vec))
+ ineq = (matineq0, vecineq0)
+ G = vstack([G,ineq[0]])
+ h = concatvec(h,ineq[1])
+ line_current = getRightMostLine(ptList)
+ plotPoly (lines, color)
+ C = None; d = None
+ try:
+ res = quadprog_solve_qp(P, q, G=G, h=h, C=C, d=d)
+ #plot bezier
for i, bez in enumerate(subs):
color = colors[i]
- init_points = []
- if i == 0:
- init_points = [bezVar.waypoints()[1][:3,0][:2]]
- if i == len(subs)-1:
- init_points = [bezVar.waypoints()[1][-3:,-1][:2]]
- lines, ptList = genFromLine(line_current, 5, [[0,5],[0,5]],init_points)
- matineq0 = None; vecineq0 = None
- for line in lines:
- (mat,vec) = getLineFromSegment(line)
- (mat,vec) = lineConstraint(bez, mat,vec,dimExtra)
- (matineq0, vecineq0) = (concat(matineq0,mat), concatvec(vecineq0,vec))
- ineq = (matineq0, vecineq0)
- G = vstack([G,ineq[0]])
- h = concatvec(h,ineq[1])
- line_current = getRightMostLine(ptList)
- plotPoly (lines, color)
- C = None; d = None
- try:
- res = quadprog_solve_qp(P, q, G=G, h=h, C=C, d=d)
- #plot bezier
- for i, bez in enumerate(subs):
- color = colors[i]
- test = bez.toBezier3(res[:])
- plotBezier(test, color)
- if save:
- plt.savefig(filename+str(idxFile))
- #plot subs control points
- for i, bez in enumerate(subs):
- color = colors[i]
- test = bez.toBezier3(res[:])
- plotBezier(test, color)
- plotControlPoints(test, color)
- if save:
- plt.savefig("subcp"+filename+str(idxFile))
- final = bezVar.toBezier3(res[:])
- plotControlPoints(final, "black")
- if save:
- plt.savefig("cp"+filename+str(idxFile))
- idxFile += 1
- if save:
- plt.close()
- else:
- plt.show()
-
- return final
- except ValueError:
- plt.close()
+ test = bez.toBezier3(res[:])
+ plotBezier(test, color)
+ if save:
+ plt.savefig(filename+str(idxFile))
+ #plot subs control points
+ for i, bez in enumerate(subs):
+ color = colors[i]
+ test = bez.toBezier3(res[:])
+ plotBezier(test, color)
+ plotControlPoints(test, color)
+ if save:
+ plt.savefig("subcp"+filename+str(idxFile))
+ final = bezVar.toBezier3(res[:])
+ plotControlPoints(final, "black")
+ if save:
+ plt.savefig("cp"+filename+str(idxFile))
+ idxFile += 1
+ if save:
+ plt.close()
+ else:
+ plt.show()
+ return final
+ except ValueError:
+ plt.close()
######################## solve a given problem ########################
#solve and gen problem
def gen(save = False):
- testConstant = genBezierInput(20)
- splits = genSplit(4)
- return computeTrajectory(testConstant,splits, save), testConstant
+ testConstant = genBezierInput(20)
+ splits = genSplit(4)
+ return computeTrajectory(testConstant,splits, save), testConstant
res = None
for i in range(1):
- res = gen(False)
- #~ if res[0] != None:
- #~ break
+ res = gen(False)
+ #~ if res[0] != None:
+ #~ break
diff --git a/python/test/variable/qp_traj/varBezier.py b/python/test/variable/qp_traj/varBezier.py
index df7719282c14d0bdd9339926d2f217f3e8bcec85..ecc8aa0b04a9b40cc3c2d6662c9e4b0810b0f767 100644
--- a/python/test/variable/qp_traj/varBezier.py
+++ b/python/test/variable/qp_traj/varBezier.py
@@ -15,63 +15,63 @@ _zeroVec = array([[0.,0.,0.]]).transpose()
def createControlPoint(val):
- if type(val) == str:
- return (_I3, _zeroVec)
- else:
- return (_zeroMat, val.reshape([3,1]))
+ if type(val) == str:
+ return (_I3, _zeroVec)
+ else:
+ return (_zeroMat, val.reshape([3,1]))
def createWaypointList(waypoints):
- mat = zeros([3, len(waypoints)*3])
- vec = zeros([3, len(waypoints)])
- for i, val in enumerate(waypoints):
- mvar, vvar = createControlPoint(val)
- mat [:,i*3:i*3+3] = mvar
- vec [:,i:i+1] = vvar
- return mat, vec
+ mat = zeros([3, len(waypoints)*3])
+ vec = zeros([3, len(waypoints)])
+ for i, val in enumerate(waypoints):
+ mvar, vvar = createControlPoint(val)
+ mat [:,i*3:i*3+3] = mvar
+ vec [:,i:i+1] = vvar
+ return mat, vec
class varBezier:
- #waypoints is a list that contains either 3d arrays (constants), or a string "variable"
- def __init__(self, waypoints=None, time=1.):
- if(waypoints != None):
- mat, vec = createWaypointList(waypoints)
- self.bezier = bezierVar(mat, vec, time)
-
- def fromBezier(self, bez):
- var = varBezier ()
- var.bezier = bez
- return var
-
- #splits into n+1 continuous curves, with n the number of time values
- def split(self, times):
- timearray = array(times).reshape([-1,1])
- subBeziers = self.bezier.split(timearray)
- dim = subBeziers.size
- return [self.fromBezier(subBeziers.at(i)) for i in range(dim)]
-
- # for each control point of the curve, gives the linear depency
- # of a given variable
- def waypoints(self, varId=-1):
- if varId < 0:
- return self.bezier.waypoints().A, self.bezier.waypoints().b
- assert self.bezier.nbWaypoints > varId
- mat = self.bezier.waypoints().A[:, varId*3:varId*3+3]
- vec = self.bezier.waypoints().b[:, varId]
- return mat, vec
-
- def matrixFromWaypoints (self, varId):
- assert(varId >= 0)
- mat, vec = self.waypoints(varId)
- resvec = zeros(3)
- for i in range(0, mat.shape[0]/3, 1):
- resvec += vec[i*3:i*3+3]
- return mat.transpose(), resvec
-
- def toBezier3(self, x):
- wps = []
- for i in range(self.bezier.nbWaypoints):
- mat, vec = self.matrixFromWaypoints(i)
- wps += [mat.dot(x) + vec]
- return bezier(array(wps).transpose(),self.bezier.max())
+ #waypoints is a list that contains either 3d arrays (constants), or a string "variable"
+ def __init__(self, waypoints=None, time=1.):
+ if(waypoints != None):
+ mat, vec = createWaypointList(waypoints)
+ self.bezier = bezierVar(mat, vec, time)
+
+ def fromBezier(self, bez):
+ var = varBezier ()
+ var.bezier = bez
+ return var
+
+ #splits into n+1 continuous curves, with n the number of time values
+ def split(self, times):
+ timearray = array(times).reshape([-1,1])
+ subBeziers = self.bezier.split(timearray)
+ dim = subBeziers.size
+ return [self.fromBezier(subBeziers.at(i)) for i in range(dim)]
+
+ # for each control point of the curve, gives the linear depency
+ # of a given variable
+ def waypoints(self, varId=-1):
+ if varId < 0:
+ return self.bezier.waypoints().A, self.bezier.waypoints().b
+ assert self.bezier.nbWaypoints > varId
+ mat = self.bezier.waypoints().A[:, varId*3:varId*3+3]
+ vec = self.bezier.waypoints().b[:, varId]
+ return mat, vec
+
+ def matrixFromWaypoints (self, varId):
+ assert(varId >= 0)
+ mat, vec = self.waypoints(varId)
+ resvec = zeros(3)
+ for i in range(0, mat.shape[0]/3, 1):
+ resvec += vec[i*3:i*3+3]
+ return mat.transpose(), resvec
+
+ def toBezier3(self, x):
+ wps = []
+ for i in range(self.bezier.nbWaypoints):
+ mat, vec = self.matrixFromWaypoints(i)
+ wps += [mat.dot(x) + vec]
+ return bezier(array(wps).transpose(),self.bezier.max())
diff --git a/python/test/variable/varBezier.py b/python/test/variable/varBezier.py
index df7719282c14d0bdd9339926d2f217f3e8bcec85..ecc8aa0b04a9b40cc3c2d6662c9e4b0810b0f767 100644
--- a/python/test/variable/varBezier.py
+++ b/python/test/variable/varBezier.py
@@ -15,63 +15,63 @@ _zeroVec = array([[0.,0.,0.]]).transpose()
def createControlPoint(val):
- if type(val) == str:
- return (_I3, _zeroVec)
- else:
- return (_zeroMat, val.reshape([3,1]))
+ if type(val) == str:
+ return (_I3, _zeroVec)
+ else:
+ return (_zeroMat, val.reshape([3,1]))
def createWaypointList(waypoints):
- mat = zeros([3, len(waypoints)*3])
- vec = zeros([3, len(waypoints)])
- for i, val in enumerate(waypoints):
- mvar, vvar = createControlPoint(val)
- mat [:,i*3:i*3+3] = mvar
- vec [:,i:i+1] = vvar
- return mat, vec
+ mat = zeros([3, len(waypoints)*3])
+ vec = zeros([3, len(waypoints)])
+ for i, val in enumerate(waypoints):
+ mvar, vvar = createControlPoint(val)
+ mat [:,i*3:i*3+3] = mvar
+ vec [:,i:i+1] = vvar
+ return mat, vec
class varBezier:
- #waypoints is a list that contains either 3d arrays (constants), or a string "variable"
- def __init__(self, waypoints=None, time=1.):
- if(waypoints != None):
- mat, vec = createWaypointList(waypoints)
- self.bezier = bezierVar(mat, vec, time)
-
- def fromBezier(self, bez):
- var = varBezier ()
- var.bezier = bez
- return var
-
- #splits into n+1 continuous curves, with n the number of time values
- def split(self, times):
- timearray = array(times).reshape([-1,1])
- subBeziers = self.bezier.split(timearray)
- dim = subBeziers.size
- return [self.fromBezier(subBeziers.at(i)) for i in range(dim)]
-
- # for each control point of the curve, gives the linear depency
- # of a given variable
- def waypoints(self, varId=-1):
- if varId < 0:
- return self.bezier.waypoints().A, self.bezier.waypoints().b
- assert self.bezier.nbWaypoints > varId
- mat = self.bezier.waypoints().A[:, varId*3:varId*3+3]
- vec = self.bezier.waypoints().b[:, varId]
- return mat, vec
-
- def matrixFromWaypoints (self, varId):
- assert(varId >= 0)
- mat, vec = self.waypoints(varId)
- resvec = zeros(3)
- for i in range(0, mat.shape[0]/3, 1):
- resvec += vec[i*3:i*3+3]
- return mat.transpose(), resvec
-
- def toBezier3(self, x):
- wps = []
- for i in range(self.bezier.nbWaypoints):
- mat, vec = self.matrixFromWaypoints(i)
- wps += [mat.dot(x) + vec]
- return bezier(array(wps).transpose(),self.bezier.max())
+ #waypoints is a list that contains either 3d arrays (constants), or a string "variable"
+ def __init__(self, waypoints=None, time=1.):
+ if(waypoints != None):
+ mat, vec = createWaypointList(waypoints)
+ self.bezier = bezierVar(mat, vec, time)
+
+ def fromBezier(self, bez):
+ var = varBezier ()
+ var.bezier = bez
+ return var
+
+ #splits into n+1 continuous curves, with n the number of time values
+ def split(self, times):
+ timearray = array(times).reshape([-1,1])
+ subBeziers = self.bezier.split(timearray)
+ dim = subBeziers.size
+ return [self.fromBezier(subBeziers.at(i)) for i in range(dim)]
+
+ # for each control point of the curve, gives the linear depency
+ # of a given variable
+ def waypoints(self, varId=-1):
+ if varId < 0:
+ return self.bezier.waypoints().A, self.bezier.waypoints().b
+ assert self.bezier.nbWaypoints > varId
+ mat = self.bezier.waypoints().A[:, varId*3:varId*3+3]
+ vec = self.bezier.waypoints().b[:, varId]
+ return mat, vec
+
+ def matrixFromWaypoints (self, varId):
+ assert(varId >= 0)
+ mat, vec = self.waypoints(varId)
+ resvec = zeros(3)
+ for i in range(0, mat.shape[0]/3, 1):
+ resvec += vec[i*3:i*3+3]
+ return mat.transpose(), resvec
+
+ def toBezier3(self, x):
+ wps = []
+ for i in range(self.bezier.nbWaypoints):
+ mat, vec = self.matrixFromWaypoints(i)
+ wps += [mat.dot(x) + vec]
+ return bezier(array(wps).transpose(),self.bezier.max())
diff --git a/tests/Main.cpp b/tests/Main.cpp
index aeb038db8d9caf9fa1447feefdcac12280d1bb79..8a1eae9035125c7903f9de467b131dd5c5a4400f 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -11,1546 +11,1452 @@
#include <string>
#include <iostream>
#include <cmath>
+#include <ctime>
using namespace std;
namespace curves
{
-typedef Eigen::Vector3d point_t;
-typedef Eigen::Vector3d tangent_t;
-typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
-typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
-typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
-typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
-typedef exact_cubic_t::spline_constraints spline_constraints_t;
-typedef std::pair<double, point_t> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-
-
-typedef Eigen::Matrix<double,1,1> point_one;
-typedef polynomial<double, double, 1, true, point_one> polynom_one;
-typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
-typedef std::pair<double, point_one> WaypointOne;
-typedef std::vector<WaypointOne> T_WaypointOne;
-
-typedef cubic_hermite_spline <double, double, 3, true, point_t> cubic_hermite_spline_t;
-typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
-typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
-
-typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
-typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
-typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
-
-const double margin = 1e-3;
-
-bool QuasiEqual(const double a, const double b)
-{
+ typedef Eigen::Vector3d point_t;
+ typedef Eigen::Vector3d tangent_t;
+ typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
+ typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
+ typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
+ typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
+ typedef exact_cubic_t::spline_constraints spline_constraints_t;
+ typedef std::pair<double, point_t> Waypoint;
+ typedef std::vector<Waypoint> T_Waypoint;
+ typedef Eigen::Matrix<double,1,1> point_one;
+ typedef polynomial<double, double, 1, true, point_one> polynom_one;
+ typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
+ typedef std::pair<double, point_one> WaypointOne;
+ typedef std::vector<WaypointOne> T_WaypointOne;
+ typedef cubic_hermite_spline <double, double, 3, true, point_t> cubic_hermite_spline_t;
+ typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
+ typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
+ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
+ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
+ typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
+ const double margin = 1e-3;
+ bool QuasiEqual(const double a, const double b)
+ {
return std::fabs(a-b)<margin;
-}
-
-} // namespace curves
+ }
+} // End namespace curves
using namespace curves;
ostream& operator<<(ostream& os, const point_t& pt)
{
- os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
- return os;
+ os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
+ return os;
}
void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error, bool notequal = false)
{
- if(!QuasiEqual((pt1-pt2).norm(), 0.0) && !notequal)
- {
- error = true;
- std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
- }
+ if(!QuasiEqual((pt1-pt2).norm(), 0.0) && !notequal)
+ {
+ error = true;
+ std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
+ }
}
template<typename curve1, typename curve2>
void CompareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error)
{
- double T_min = c1.min();
- double T_max = c1.max();
- if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max()))
- {
- std::cout << "CompareCurves, ERROR, time min and max of curves do not match ["<<T_min<<","<<T_max<<"] "
- << " and ["<<c2.min()<<","<<c2.max()<<"] "<<std::endl;
- error = true;
- }
- else
- {
- // derivative in T_min and T_max
- ComparePoints(c1.derivate(T_min,1),c2.derivate(T_min,1),errMsg, error, false);
- ComparePoints(c1.derivate(T_max,1),c2.derivate(T_max,1),errMsg, error, false);
- // Test values on curves
- for(double i =T_min; i<T_max; i+=0.02)
- {
- ComparePoints(c1(i),c2(i),errMsg, error, false);
- ComparePoints(c1(i),c2(i),errMsg, error, false);
- }
- }
+ double T_min = c1.min();
+ double T_max = c1.max();
+ if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max()))
+ {
+ std::cout << "CompareCurves, ERROR, time min and max of curves do not match ["<<T_min<<","<<T_max<<"] "
+ << " and ["<<c2.min()<<","<<c2.max()<<"] "<<std::endl;
+ error = true;
+ }
+ else
+ {
+ // derivative in T_min and T_max
+ ComparePoints(c1.derivate(T_min,1),c2.derivate(T_min,1),errMsg, error, false);
+ ComparePoints(c1.derivate(T_max,1),c2.derivate(T_max,1),errMsg, error, false);
+ // Test values on curves
+ for(double i =T_min; i<T_max; i+=0.02)
+ {
+ ComparePoints(c1(i),c2(i),errMsg, error, false);
+ ComparePoints(c1(i),c2(i),errMsg, error, false);
+ }
+ }
}
/*Cubic Function tests*/
void CubicFunctionTest(bool& error)
{
- std::string errMsg("In test CubicFunctionTest ; 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);
- t_point_t vec;
- vec.push_back(a);
- vec.push_back(b);
- vec.push_back(c);
- vec.push_back(d);
- polynomial_t cf(vec.begin(), vec.end(), 0, 1);
- point_t res1;
- res1 =cf(0);
- point_t x0(1,2,3);
- ComparePoints(x0, res1, errMsg + "(0) ", error);
-
- point_t x1(9,15,19);
- res1 =cf(1);
- ComparePoints(x1, res1, errMsg + "(1) ", error);
-
- point_t x2(3.125,5.25,7.125);
- res1 =cf(0.5);
- ComparePoints(x2, res1, errMsg + "(0.5) ", error);
-
- vec.clear();
- vec.push_back(a);
- vec.push_back(b);
- vec.push_back(c);
- vec.push_back(d);
- polynomial_t cf2(vec, 0.5, 1);
- res1 = cf2(0.5);
- ComparePoints(x0, res1, errMsg + "x3 ", error);
+ std::string errMsg("In test CubicFunctionTest ; 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);
+ t_point_t vec;
+ vec.push_back(a);
+ vec.push_back(b);
+ vec.push_back(c);
+ vec.push_back(d);
+ polynomial_t cf(vec.begin(), vec.end(), 0, 1);
+ point_t res1;
+ res1 =cf(0);
+ point_t x0(1,2,3);
+ ComparePoints(x0, res1, errMsg + "(0) ", error);
+ point_t x1(9,15,19);
+ res1 =cf(1);
+ ComparePoints(x1, res1, errMsg + "(1) ", error);
+ point_t x2(3.125,5.25,7.125);
+ res1 =cf(0.5);
+ ComparePoints(x2, res1, errMsg + "(0.5) ", error);
+ vec.clear();
+ vec.push_back(a);
+ vec.push_back(b);
+ vec.push_back(c);
+ vec.push_back(d);
+ polynomial_t cf2(vec, 0.5, 1);
+ res1 = cf2(0.5);
+ ComparePoints(x0, res1, errMsg + "x3 ", error);
+ error = true;
+ try
+ {
+ cf2(0.4);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
+ }
+ error = true;
+ try
+ {
+ cf2(1.1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
+ }
+ if (!QuasiEqual(cf.max(), 1.0))
+ {
error = true;
- try
- {
- cf2(0.4);
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
- }
+ std::cout << "Evaluation of cubic cf error, MaxBound should be equal to 1\n";
+ }
+ if (!QuasiEqual(cf.min(), 0.0))
+ {
error = true;
- try
- {
- cf2(1.1);
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
- }
- if (!QuasiEqual(cf.max(), 1.0))
- {
- error = true;
- std::cout << "Evaluation of cubic cf error, MaxBound should be equal to 1\n";
- }
- if (!QuasiEqual(cf.min(), 0.0))
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0\n";
- }
+ std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
+ }
}
/*bezier_curve Function tests*/
void BezierCurveTest(bool& error)
{
- std::string errMsg("In test BezierCurveTest ; 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);
-
- std::vector<point_t> params;
- params.push_back(a);
-
- // 1d curve in [0,1]
- bezier_curve_t cf1(params.begin(), params.end());
- point_t res1;
- res1 = cf1(0);
- point_t x10 = a ;
- ComparePoints(x10, res1, errMsg + "1(0) ", error);
- res1 = cf1(1);
- ComparePoints(x10, res1, errMsg + "1(1) ", error);
-
- // 2d curve in [0,1]
- params.push_back(b);
- bezier_curve_t cf(params.begin(), params.end());
- res1 = cf(0);
- point_t x20 = a ;
- ComparePoints(x20, res1, errMsg + "2(0) ", error);
-
- point_t x21 = b;
- res1 = cf(1);
- ComparePoints(x21, res1, errMsg + "2(1) ", error);
-
- //3d curve in [0,1]
- params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end());
- res1 = cf3(0);
- ComparePoints(a, res1, errMsg + "3(0) ", error);
-
- res1 = cf3(1);
- ComparePoints(c, res1, errMsg + "3(1) ", error);
-
- //4d curve in [1,2]
- params.push_back(d);
- bezier_curve_t cf4(params.begin(), params.end(), 1., 2.);
-
- //testing bernstein polynomials
- bezier_curve_t cf5(params.begin(), params.end(),1.,2.);
- std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
- for(double d = 1.; d <2.; d+=0.1)
- {
- ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
- ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
- ComparePoints( cf5.compute_derivate(1).evalBernstein(d) , cf5.compute_derivate(1) (d), errMsg2, error);
- ComparePoints( cf5.compute_derivate(1).evalHorner(d) , cf5.compute_derivate(1) (d), errMsg2, error);
- }
-
- bool error_in(true);
-
- try
- {
- cf(-0.4);
- } catch(...)
- {
- error_in = false;
- }
- if(error_in)
- {
- std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
- error = true;
- }
- error_in = true;
-
- try
- {
- cf(1.1);
- } catch(...)
- {
- error_in = false;
- }
- if(error_in)
- {
- std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
- error = true;
- }
- if (!QuasiEqual(cf.max(),1.0))
- {
- error = true;
- std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
- }
- if (!QuasiEqual(cf.min(),0.0))
- {
- error = true;
- std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
- }
+ std::string errMsg("In test BezierCurveTest ; 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);
+ std::vector<point_t> params;
+ params.push_back(a);
+ // 1d curve in [0,1]
+ bezier_curve_t cf1(params.begin(), params.end());
+ point_t res1;
+ res1 = cf1(0);
+ point_t x10 = a ;
+ ComparePoints(x10, res1, errMsg + "1(0) ", error);
+ res1 = cf1(1);
+ ComparePoints(x10, res1, errMsg + "1(1) ", error);
+ // 2d curve in [0,1]
+ params.push_back(b);
+ bezier_curve_t cf(params.begin(), params.end());
+ res1 = cf(0);
+ point_t x20 = a ;
+ ComparePoints(x20, res1, errMsg + "2(0) ", error);
+ point_t x21 = b;
+ res1 = cf(1);
+ ComparePoints(x21, res1, errMsg + "2(1) ", error);
+ //3d curve in [0,1]
+ params.push_back(c);
+ bezier_curve_t cf3(params.begin(), params.end());
+ res1 = cf3(0);
+ ComparePoints(a, res1, errMsg + "3(0) ", error);
+ res1 = cf3(1);
+ ComparePoints(c, res1, errMsg + "3(1) ", error);
+ //4d curve in [1,2]
+ params.push_back(d);
+ bezier_curve_t cf4(params.begin(), params.end(), 1., 2.);
+ //testing bernstein polynomials
+ bezier_curve_t cf5(params.begin(), params.end(),1.,2.);
+ std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
+ for(double d = 1.; d <2.; d+=0.1)
+ {
+ ComparePoints( cf5.evalBernstein(d) , cf5 (d), errMsg2, error);
+ ComparePoints( cf5.evalHorner(d) , cf5 (d), errMsg2, error);
+ ComparePoints( cf5.compute_derivate(1).evalBernstein(d) , cf5.compute_derivate(1) (d), errMsg2, error);
+ ComparePoints( cf5.compute_derivate(1).evalHorner(d) , cf5.compute_derivate(1) (d), errMsg2, error);
+ }
+ bool error_in(true);
+ try
+ {
+ cf(-0.4);
+ }
+ catch(...)
+ {
+ error_in = false;
+ }
+ if(error_in)
+ {
+ std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
+ error = true;
+ }
+ error_in = true;
+ try
+ {
+ cf(1.1);
+ }
+ catch(...)
+ {
+ error_in = false;
+ }
+ if(error_in)
+ {
+ std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
+ error = true;
+ }
+ if (!QuasiEqual(cf.max(),1.0))
+ {
+ error = true;
+ std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
+ }
+ if (!QuasiEqual(cf.min(),0.0))
+ {
+ error = true;
+ std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
+ }
}
-#include <ctime>
+
void BezierCurveTestCompareHornerAndBernstein(bool&) // error
{
- using namespace std;
- std::vector<double> values;
- for (int i =0; i < 100000; ++i)
- values.push_back(rand()/RAND_MAX);
-
- //first compare regular evaluation (low dim pol)
- 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);
-
- // 3d curve
- bezier_curve_t cf(params.begin(), params.end()); // defined in [0,1]
-
- // Check all evaluation of bezier curve
- clock_t s0,e0,s1,e1,s2,e2,s3,e3;
- s0 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf(*cit);
- }
- e0 = clock();
-
- s1 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf.evalBernstein(*cit);
- }
- e1 = clock();
-
- s2 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf.evalHorner(*cit);
- }
- e2 = clock();
-
- s3 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf.evalDeCasteljau(*cit);
- }
- e3 = clock();
-
- std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
-
- std::cout << "now with high order polynomial " << std::endl;
-
- 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 cf2(params.begin(), params.end());
-
- s1 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2.evalBernstein(*cit);
- }
- e1 = clock();
-
- s2 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2.evalHorner(*cit);
- }
- e2 = clock();
-
- s0 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2(*cit);
- }
- e0 = clock();
-
- s3 = clock();
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- cf2.evalDeCasteljau(*cit);
- }
- e3 = clock();
-
- std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
- std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
-
+ using namespace std;
+ std::vector<double> values;
+ for (int i =0; i < 100000; ++i)
+ {
+ values.push_back(rand()/RAND_MAX);
+ }
+ //first compare regular evaluation (low dim pol)
+ 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);
+ // 3d curve
+ bezier_curve_t cf(params.begin(), params.end()); // defined in [0,1]
+ // Check all evaluation of bezier curve
+ clock_t s0,e0,s1,e1,s2,e2,s3,e3;
+ s0 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf(*cit);
+ }
+ e0 = clock();
+ s1 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf.evalBernstein(*cit);
+ }
+ e1 = clock();
+
+ s2 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf.evalHorner(*cit);
+ }
+ e2 = clock();
+ s3 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf.evalDeCasteljau(*cit);
+ }
+ e3 = clock();
+ std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "now with high order polynomial " << std::endl;
+ 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 cf2(params.begin(), params.end());
+ s1 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2.evalBernstein(*cit);
+ }
+ e1 = clock();
+ s2 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2.evalHorner(*cit);
+ }
+ e2 = clock();
+ s0 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2(*cit);
+ }
+ e0 = clock();
+ s3 = clock();
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ cf2.evalDeCasteljau(*cit);
+ }
+ e3 = clock();
+ std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
+ std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
}
void BezierDerivativeCurveTest(bool& error)
{
- std::string errMsg("In test BezierDerivativeCurveTest ;, Error While checking value of point on curve : ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
-
- 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());
-
- ComparePoints(cf3(0), cf3.derivate(0.,0), errMsg, error);
- ComparePoints(cf3(0), cf3.derivate(0.,1), errMsg, error, true);
- ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
+ std::string errMsg("In test BezierDerivativeCurveTest ;, Error While checking value of point on curve : ");
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ 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());
+ ComparePoints(cf3(0), cf3.derivate(0.,0), errMsg, error);
+ ComparePoints(cf3(0), cf3.derivate(0.,1), errMsg, error, true);
+ ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
}
void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
{
- std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest, Error While checking value of point on curve : ");
- 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 Tmin = 0.;
- double Tmax = 2.;
- double diffT = Tmax-Tmin;
- bezier_curve_t cf(params.begin(), params.end());
- bezier_curve_t cfT(params.begin(), params.end(),Tmin,Tmax);
-
- ComparePoints(cf(0.5), cfT(1), errMsg, error);
- ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * (diffT), errMsg, error);
- ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * diffT*diffT, errMsg, error);
+ std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest, Error While checking value of point on curve : ");
+ 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 Tmin = 0.;
+ double Tmax = 2.;
+ double diffT = Tmax-Tmin;
+ bezier_curve_t cf(params.begin(), params.end());
+ bezier_curve_t cfT(params.begin(), params.end(),Tmin,Tmax);
+ ComparePoints(cf(0.5), cfT(1), errMsg, error);
+ ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * (diffT), errMsg, error);
+ ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * diffT*diffT, errMsg, error);
}
void BezierDerivativeCurveConstraintTest(bool& error)
{
- std::string errMsg0("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
- 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::num_t T_min = 1.0;
- bezier_curve_t::num_t T_max = 3.0;
- bezier_curve_t cf(params.begin(), params.end(), constraints, T_min, T_max);
-
- ComparePoints(a, cf(T_min), errMsg0, error);
- ComparePoints(c, cf(T_max), errMsg0, error);
- ComparePoints(constraints.init_vel, cf.derivate(T_min,1), errMsg0, error);
- ComparePoints(constraints.end_vel , cf.derivate(T_max,1), errMsg0, error);
- ComparePoints(constraints.init_acc, cf.derivate(T_min,2), errMsg0, error);
- ComparePoints(constraints.end_vel, cf.derivate(T_max,1), errMsg0, error);
- ComparePoints(constraints.end_acc, cf.derivate(T_max,2), errMsg0, error);
-
- std::string errMsg1("In test BezierDerivativeCurveConstraintTest, Error While checking checking degree of bezier curve :");
- std::string errMsg2("In test BezierDerivativeCurveConstraintTest, Error While checking checking size of bezier curve :");
- if (cf.degree_ != params.size() + 3)
- {
- error = true;
- std::cout << errMsg1 << cf.degree_ << " ; " << params.size()+3 << std::endl;
- }
- if (cf.size_ != params.size() + 4)
- {
- error = true;
- std::cout << errMsg2 << cf.size_ << " ; " << params.size()+4 << std::endl;
- }
+ std::string errMsg0("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
+ 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::num_t T_min = 1.0;
+ bezier_curve_t::num_t T_max = 3.0;
+ bezier_curve_t cf(params.begin(), params.end(), constraints, T_min, T_max);
+ ComparePoints(a, cf(T_min), errMsg0, error);
+ ComparePoints(c, cf(T_max), errMsg0, error);
+ ComparePoints(constraints.init_vel, cf.derivate(T_min,1), errMsg0, error);
+ ComparePoints(constraints.end_vel , cf.derivate(T_max,1), errMsg0, error);
+ ComparePoints(constraints.init_acc, cf.derivate(T_min,2), errMsg0, error);
+ ComparePoints(constraints.end_vel, cf.derivate(T_max,1), errMsg0, error);
+ ComparePoints(constraints.end_acc, cf.derivate(T_max,2), errMsg0, error);
+ std::string errMsg1("In test BezierDerivativeCurveConstraintTest, Error While checking checking degree of bezier curve :");
+ std::string errMsg2("In test BezierDerivativeCurveConstraintTest, Error While checking checking size of bezier curve :");
+ if (cf.degree_ != params.size() + 3)
+ {
+ error = true;
+ std::cout << errMsg1 << cf.degree_ << " ; " << params.size()+3 << std::endl;
+ }
+ if (cf.size_ != params.size() + 4)
+ {
+ error = true;
+ std::cout << errMsg2 << cf.size_ << " ; " << params.size()+4 << std::endl;
+ }
}
void toPolynomialConversionTest(bool& error)
{
- // bezier to polynomial
- std::string errMsg("In test BezierToPolynomialConversionTest, Error While checking value of point on curve : ");
- 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> control_points;
- control_points.push_back(a);
- control_points.push_back(b);
- control_points.push_back(c);
- control_points.push_back(d);
- control_points.push_back(e);
- control_points.push_back(f);
- control_points.push_back(g);
- control_points.push_back(h);
- control_points.push_back(i);
-
- bezier_curve_t::num_t T_min = 1.0;
- bezier_curve_t::num_t T_max = 3.0;
- bezier_curve_t bc(control_points.begin(), control_points.end(),T_min, T_max);
- polynomial_t pol = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc);
- CompareCurves<polynomial_t, bezier_curve_t>(pol, bc, errMsg, error);
+ // bezier to polynomial
+ std::string errMsg("In test BezierToPolynomialConversionTest, Error While checking value of point on curve : ");
+ 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> control_points;
+ control_points.push_back(a);
+ control_points.push_back(b);
+ control_points.push_back(c);
+ control_points.push_back(d);
+ control_points.push_back(e);
+ control_points.push_back(f);
+ control_points.push_back(g);
+ control_points.push_back(h);
+ control_points.push_back(i);
+ bezier_curve_t::num_t T_min = 1.0;
+ bezier_curve_t::num_t T_max = 3.0;
+ bezier_curve_t bc(control_points.begin(), control_points.end(),T_min, T_max);
+ polynomial_t pol = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc);
+ CompareCurves<polynomial_t, bezier_curve_t>(pol, bc, errMsg, error);
}
void cubicConversionTest(bool& error)
{
- std::string errMsg0("In test CubicConversionTest - convert hermite to, Error While checking value of point on curve : ");
- std::string errMsg1("In test CubicConversionTest - convert bezier to, Error While checking value of point on curve : ");
- std::string errMsg2("In test CubicConversionTest - convert polynomial to, Error While checking value of point on curve : ");
-
- // Create cubic hermite spline : Test hermite to bezier/polynomial
- point_t p0(1,2,3);
- point_t m0(2,3,4);
- point_t p1(3,4,5);
- point_t m1(3,6,7);
- pair_point_tangent_t pair0(p0,m0);
- pair_point_tangent_t pair1(p1,m1);
- t_pair_point_tangent_t control_points;
- control_points.push_back(pair0);
- control_points.push_back(pair1);
- std::vector< double > time_control_points;
- polynomial_t::num_t T_min = 1.0;
- polynomial_t::num_t T_max = 3.0;
- time_control_points.push_back(T_min);
- time_control_points.push_back(T_max);
- cubic_hermite_spline_t chs0(control_points.begin(), control_points.end(), time_control_points);
-
- // hermite to bezier
- //std::cout<<"======================= \n";
- //std::cout<<"hermite to bezier \n";
- bezier_curve_t bc0 = bezier_from_curve<bezier_curve_t, cubic_hermite_spline_t>(chs0);
- CompareCurves<cubic_hermite_spline_t, bezier_curve_t>(chs0, bc0, errMsg0, error);
-
- // hermite to pol
- //std::cout<<"======================= \n";
- //std::cout<<"hermite to polynomial \n";
- polynomial_t pol0 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs0);
- CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
-
- // pol to hermite
- //std::cout<<"======================= \n";
- //std::cout<<"polynomial to hermite \n";
- cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol0);
- CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0,chs1,errMsg2,error);
-
- // pol to bezier
- //std::cout<<"======================= \n";
- //std::cout<<"polynomial to bezier \n";
- bezier_curve_t bc1 = bezier_from_curve<bezier_curve_t, polynomial_t>(pol0);
- CompareCurves<bezier_curve_t, polynomial_t>(bc1, pol0, errMsg2, error);
-
- // Bezier to pol
- //std::cout<<"======================= \n";
- //std::cout<<"bezier to polynomial \n";
- polynomial_t pol1 = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc0);
- CompareCurves<bezier_curve_t, polynomial_t>(bc0, pol1, errMsg1, error);
-
- // bezier => hermite
- //std::cout<<"======================= \n";
- //std::cout<<"bezier to hermite \n";
- cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t, bezier_curve_t>(bc0);
- CompareCurves<bezier_curve_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
+ std::string errMsg0("In test CubicConversionTest - convert hermite to, Error While checking value of point on curve : ");
+ std::string errMsg1("In test CubicConversionTest - convert bezier to, Error While checking value of point on curve : ");
+ std::string errMsg2("In test CubicConversionTest - convert polynomial to, Error While checking value of point on curve : ");
+ // Create cubic hermite spline : Test hermite to bezier/polynomial
+ point_t p0(1,2,3);
+ point_t m0(2,3,4);
+ point_t p1(3,4,5);
+ point_t m1(3,6,7);
+ pair_point_tangent_t pair0(p0,m0);
+ pair_point_tangent_t pair1(p1,m1);
+ t_pair_point_tangent_t control_points;
+ control_points.push_back(pair0);
+ control_points.push_back(pair1);
+ std::vector< double > time_control_points;
+ polynomial_t::num_t T_min = 1.0;
+ polynomial_t::num_t T_max = 3.0;
+ time_control_points.push_back(T_min);
+ time_control_points.push_back(T_max);
+ cubic_hermite_spline_t chs0(control_points.begin(), control_points.end(), time_control_points);
+ // hermite to bezier
+ //std::cout<<"======================= \n";
+ //std::cout<<"hermite to bezier \n";
+ bezier_curve_t bc0 = bezier_from_curve<bezier_curve_t, cubic_hermite_spline_t>(chs0);
+ CompareCurves<cubic_hermite_spline_t, bezier_curve_t>(chs0, bc0, errMsg0, error);
+ // hermite to pol
+ //std::cout<<"======================= \n";
+ //std::cout<<"hermite to polynomial \n";
+ polynomial_t pol0 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs0);
+ CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
+ // pol to hermite
+ //std::cout<<"======================= \n";
+ //std::cout<<"polynomial to hermite \n";
+ cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol0);
+ CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0,chs1,errMsg2,error);
+ // pol to bezier
+ //std::cout<<"======================= \n";
+ //std::cout<<"polynomial to bezier \n";
+ bezier_curve_t bc1 = bezier_from_curve<bezier_curve_t, polynomial_t>(pol0);
+ CompareCurves<bezier_curve_t, polynomial_t>(bc1, pol0, errMsg2, error);
+ // Bezier to pol
+ //std::cout<<"======================= \n";
+ //std::cout<<"bezier to polynomial \n";
+ polynomial_t pol1 = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc0);
+ CompareCurves<bezier_curve_t, polynomial_t>(bc0, pol1, errMsg1, error);
+ // bezier => hermite
+ //std::cout<<"======================= \n";
+ //std::cout<<"bezier to hermite \n";
+ cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t, bezier_curve_t>(bc0);
+ CompareCurves<bezier_curve_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
}
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
- // Create an exact cubic spline with 7 waypoints => 6 polynomials defined in [0.0,3.0]
- curves::T_Waypoint waypoints;
- for(double i = 0.0; i <= 3.0; i = i + 0.5)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
-
- // Test number of polynomials in exact cubic
- std::size_t numberSegments = exactCubic.getNumberSplines();
- if (numberSegments != 6)
- {
- error = true;
- std::cout << "In ExactCubicNoErrorTest, Error While checking number of splines" <<
- numberSegments << " ; " << 6 << std::endl;
- }
-
- // Test getSplineAt function
- for (std::size_t i=0; i<numberSegments; i++)
- {
- exactCubic.getSplineAt(i);
- }
-
- // Other tests
- try
- {
- exactCubic(0.0);
- exactCubic(3.0);
- }
- catch(...)
- {
- error = true;
- std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value on bounds\n";
- }
+ // Create an exact cubic spline with 7 waypoints => 6 polynomials defined in [0.0,3.0]
+ curves::T_Waypoint waypoints;
+ for(double i = 0.0; i <= 3.0; i = i + 0.5)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ // Test number of polynomials in exact cubic
+ std::size_t numberSegments = exactCubic.getNumberSplines();
+ if (numberSegments != 6)
+ {
error = true;
- try
- {
- exactCubic(3.2);
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
- }
-
- if (!QuasiEqual(exactCubic.max(),3.0))
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3 but is : "<<exactCubic.max()<<"\n";
- }
- if (!QuasiEqual(exactCubic.min(),0.0))
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0 but is : "<<exactCubic.min()<<"\n";
- }
+ std::cout << "In ExactCubicNoErrorTest, Error While checking number of splines" <<
+ numberSegments << " ; " << 6 << std::endl;
+ }
+ // Test getSplineAt function
+ for (std::size_t i=0; i<numberSegments; i++)
+ {
+ exactCubic.getSplineAt(i);
+ }
+ // Other tests
+ try
+ {
+ exactCubic(0.0);
+ exactCubic(3.0);
+ }
+ catch(...)
+ {
+ error = true;
+ std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value on bounds\n";
+ }
+ error = true;
+ try
+ {
+ exactCubic(3.2);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
+ }
+ if (!QuasiEqual(exactCubic.max(),3.0))
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3 but is : "<<exactCubic.max()<<"\n";
+ }
+ if (!QuasiEqual(exactCubic.min(),0.0))
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0 but is : "<<exactCubic.min()<<"\n";
+ }
}
/*Exact Cubic Function tests*/
void ExactCubicTwoPointsTest(bool& error)
{
- // Create an exact cubic spline with 2 waypoints => 1 polynomial defined in [0.0,1.0]
- curves::T_Waypoint waypoints;
- for(double i = 0.0; i < 2.0; ++i)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
-
- point_t res1 = exactCubic(0);
- std::string errmsg0("in ExactCubicTwoPointsTest, Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(point_t(0,0,0), res1, errmsg0, error);
-
- res1 = exactCubic(1);
- ComparePoints(point_t(1,1,1), res1, errmsg0, error);
-
- // Test number of polynomials in exact cubic
- std::size_t numberSegments = exactCubic.getNumberSplines();
- if (numberSegments != 1)
- {
- error = true;
- std::cout << "In ExactCubicTwoPointsTest, Error While checking number of splines" <<
- numberSegments << " ; " << 1 << std::endl;
- }
-
- // Test getSplineAt
- std::string errmsg1("in ExactCubicTwoPointsTest, Error While checking value on curve");
- ComparePoints(exactCubic(0.5), (exactCubic.getSplineAt(0))(0.5), errmsg1, error);
+ // Create an exact cubic spline with 2 waypoints => 1 polynomial defined in [0.0,1.0]
+ curves::T_Waypoint waypoints;
+ for(double i = 0.0; i < 2.0; ++i)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ point_t res1 = exactCubic(0);
+ std::string errmsg0("in ExactCubicTwoPointsTest, Error While checking that given wayPoints are crossed (expected / obtained)");
+ ComparePoints(point_t(0,0,0), res1, errmsg0, error);
+ res1 = exactCubic(1);
+ ComparePoints(point_t(1,1,1), res1, errmsg0, error);
+ // Test number of polynomials in exact cubic
+ std::size_t numberSegments = exactCubic.getNumberSplines();
+ if (numberSegments != 1)
+ {
+ error = true;
+ std::cout << "In ExactCubicTwoPointsTest, Error While checking number of splines" <<
+ numberSegments << " ; " << 1 << std::endl;
+ }
+ // Test getSplineAt
+ std::string errmsg1("in ExactCubicTwoPointsTest, Error While checking value on curve");
+ ComparePoints(exactCubic(0.5), (exactCubic.getSplineAt(0))(0.5), errmsg1, error);
}
void ExactCubicOneDimTest(bool& error)
{
- curves::T_WaypointOne waypoints;
- point_one zero; zero(0,0) = 9;
- point_one one; one(0,0) = 14;
- point_one two; two(0,0) = 25;
- waypoints.push_back(std::make_pair(0., zero));
- waypoints.push_back(std::make_pair(1., one));
- waypoints.push_back(std::make_pair(2., two));
- exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
-
- point_one res1 = exactCubic(0);
- std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(zero, res1, errmsg, error);
-
- res1 = exactCubic(1);
- ComparePoints(one, res1, errmsg, error);
+ curves::T_WaypointOne waypoints;
+ point_one zero; zero(0,0) = 9;
+ point_one one; one(0,0) = 14;
+ point_one two; two(0,0) = 25;
+ waypoints.push_back(std::make_pair(0., zero));
+ waypoints.push_back(std::make_pair(1., one));
+ waypoints.push_back(std::make_pair(2., two));
+ exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
+ point_one res1 = exactCubic(0);
+ std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints are crossed (expected / obtained)");
+ ComparePoints(zero, res1, errmsg, error);
+ res1 = exactCubic(1);
+ ComparePoints(one, res1, errmsg, error);
}
void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint&, const exact_cubic_t* curve, bool& error )
{
- point_t res1;
- for(double i = 0; i <= 1; i = i + step)
- {
- res1 = (*curve)(i);
- ComparePoints(point_t(i,i,i), res1, errmsg, error);
- }
+ point_t res1;
+ for(double i = 0; i <= 1; i = i + step)
+ {
+ res1 = (*curve)(i);
+ ComparePoints(point_t(i,i,i), res1, errmsg, error);
+ }
}
void CheckDerivative(const std::string& errmsg, const double eval_point, const std::size_t order, const point_t& target, const exact_cubic_t* curve, bool& error )
{
- point_t res1 = curve->derivate(eval_point,order);
- ComparePoints(target, res1, errmsg, error);
+ point_t res1 = curve->derivate(eval_point,order);
+ ComparePoints(target, res1, errmsg, error);
}
void ExactCubicPointsCrossedTest(bool& error)
{
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
- std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
- CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
-
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
+ CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
}
void ExactCubicVelocityConstraintsTest(bool& error)
{
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
- spline_constraints_t constraints;
- constraints.end_vel = point_t(0,0,0);
- constraints.init_vel = point_t(0,0,0);
- constraints.end_acc = point_t(0,0,0);
- constraints.init_acc = point_t(0,0,0);
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end(), constraints);
- // now check that init and end velocity are 0
- CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
- std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
- // now check derivatives
- CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
- CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
- CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
- CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
-
- constraints.end_vel = point_t(1,2,3);
- constraints.init_vel = point_t(-1,-2,-3);
- constraints.end_acc = point_t(4,5,6);
- constraints.init_acc = point_t(-4,-4,-6);
- std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
- exact_cubic_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
- CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
-
- std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
- // now check derivatives
- CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
- CheckDerivative(errmsg4,1,1,constraints.end_vel ,&exactCubic2, error);
- CheckDerivative(errmsg4,0,2,constraints.init_acc,&exactCubic2, error);
- CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
+ spline_constraints_t constraints;
+ constraints.end_vel = point_t(0,0,0);
+ constraints.init_vel = point_t(0,0,0);
+ constraints.end_acc = point_t(0,0,0);
+ constraints.init_acc = point_t(0,0,0);
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end(), constraints);
+ // now check that init and end velocity are 0
+ CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
+ std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
+ // now check derivatives
+ CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
+ CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
+ CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
+ CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
+ constraints.end_vel = point_t(1,2,3);
+ constraints.init_vel = point_t(-1,-2,-3);
+ constraints.end_acc = point_t(4,5,6);
+ constraints.init_acc = point_t(-4,-4,-6);
+ std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
+ exact_cubic_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
+ CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
+ std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
+ // now check derivatives
+ CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
+ CheckDerivative(errmsg4,1,1,constraints.end_vel ,&exactCubic2, error);
+ CheckDerivative(errmsg4,0,2,constraints.init_acc,&exactCubic2, error);
+ CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
}
void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t& B, const double target, const exact_cubic_t* curve, bool& error )
{
- point_t res1 = curve->operator ()(target);
- point_t ar =(res1-A); ar.normalize();
- point_t rb =(B-res1); rb.normalize();
- if(ar.dot(rb) < 0.99999)
- {
- error = true;
- std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " <<
- target << " ; " << res1.transpose() << std::endl;
- }
+ point_t res1 = curve->operator ()(target);
+ point_t ar =(res1-A); ar.normalize();
+ point_t rb =(B-res1); rb.normalize();
+ if(ar.dot(rb) < 0.99999)
+ {
+ error = true;
+ std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " <<
+ target << " ; " << res1.transpose() << std::endl;
+ }
}
void EffectorTrajectoryTest(bool& error)
{
- // create arbitrary trajectory
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::exact_cubic_t* eff_traj = helpers::effector_spline(waypoints.begin(),waypoints.end(),
- Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0,0,2),
- 1,0.02,1,0.5);
- point_t zero(0,0,0);
- point_t off1(0,0,1);
- point_t off2(10,10,10.02);
- point_t end(10,10,10);
- std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
- std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
- //first check start / goal positions
- ComparePoints(zero, (*eff_traj)(0), errmsg, error);
- ComparePoints(off1, (*eff_traj)(1), errmsg, error);
- ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
- ComparePoints(end , (*eff_traj)(10), errmsg, error);
-
- //then check offset at start / goal positions
- // now check derivatives
- CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
- CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
- CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
- CheckDerivative(errmsg2,10,2,zero ,eff_traj, error);
-
- //check that end and init splines are line
- std::string errmsg3("Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / point obtained) \n");
- for(double i = 0.1; i<1; i+=0.1)
- {
- CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
- }
-
- for(double i = 9.981; i<10; i+=0.002)
- {
- CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
- }
- delete eff_traj;
+ // create arbitrary trajectory
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::exact_cubic_t* eff_traj = helpers::effector_spline(waypoints.begin(),waypoints.end(),
+ Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0,0,2),
+ 1,0.02,1,0.5);
+ point_t zero(0,0,0);
+ point_t off1(0,0,1);
+ point_t off2(10,10,10.02);
+ point_t end(10,10,10);
+ std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
+ std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
+ //first check start / goal positions
+ ComparePoints(zero, (*eff_traj)(0), errmsg, error);
+ ComparePoints(off1, (*eff_traj)(1), errmsg, error);
+ ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
+ ComparePoints(end , (*eff_traj)(10), errmsg, error);
+ //then check offset at start / goal positions
+ // now check derivatives
+ CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
+ CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
+ CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
+ CheckDerivative(errmsg2,10,2,zero ,eff_traj, error);
+ //check that end and init splines are line
+ std::string errmsg3("Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / point obtained) \n");
+ for(double i = 0.1; i<1; i+=0.1)
+ {
+ CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
+ }
+ for(double i = 9.981; i<10; i+=0.002)
+ {
+ CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
+ }
+ delete eff_traj;
}
helpers::quat_t GetXRotQuat(const double theta)
{
- Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
- return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
+ Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
+ return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
}
double GetXRotFromQuat(helpers::quat_ref_const_t q)
{
- Eigen::Quaterniond quat (q.data());
- Eigen::AngleAxisd m (quat);
- return m.angle() / M_PI * 180.;
+ Eigen::Quaterniond quat (q.data());
+ Eigen::AngleAxisd m (quat);
+ return m.angle() / M_PI * 180.;
}
void EffectorSplineRotationNoRotationTest(bool& error)
{
- // create arbitrary trajectory
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end());
- helpers::config_t q_init; q_init << 0.,0.,0.,0.,0.,0.,1.;
- helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
- helpers::config_t q_to; q_to << 0.,0,0.02,0.,0.,0.,1.;
- helpers::config_t q_land; q_land << 10,10, 10.02, 0, 0.,0.,1.;
- helpers::config_t q_mod; q_mod << 6.,6.,6.,0.,0.,0.,1.;
- std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
- ComparePoints(q_init , eff_traj(0), errmsg,error);
- ComparePoints(q_to , eff_traj(0.02), errmsg,error);
- ComparePoints(q_land , eff_traj(9.98), errmsg,error);
- ComparePoints(q_mod , eff_traj(6), errmsg,error);
- ComparePoints(q_end , eff_traj(10), errmsg,error);
+ // create arbitrary trajectory
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end());
+ helpers::config_t q_init; q_init << 0.,0.,0.,0.,0.,0.,1.;
+ helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
+ helpers::config_t q_to; q_to << 0.,0,0.02,0.,0.,0.,1.;
+ helpers::config_t q_land; q_land << 10,10, 10.02, 0, 0.,0.,1.;
+ helpers::config_t q_mod; q_mod << 6.,6.,6.,0.,0.,0.,1.;
+ std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
+ ComparePoints(q_init , eff_traj(0), errmsg,error);
+ ComparePoints(q_to , eff_traj(0.02), errmsg,error);
+ ComparePoints(q_land , eff_traj(9.98), errmsg,error);
+ ComparePoints(q_mod , eff_traj(6), errmsg,error);
+ ComparePoints(q_end , eff_traj(10), errmsg,error);
}
void EffectorSplineRotationRotationTest(bool& error)
{
- // create arbitrary trajectory
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::quat_t init_quat = GetXRotQuat(M_PI);
- helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(), init_quat);
- helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
- helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
- helpers::config_t q_to = q_init; q_to(2) +=0.02;
- helpers::config_t q_land = q_end ; q_land(2)+=0.02;
- helpers::quat_t q_mod = GetXRotQuat(M_PI_2);;
- std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
- ComparePoints(q_init, eff_traj(0), errmsg,error);
- ComparePoints(q_to , eff_traj(0.02), errmsg,error);
- ComparePoints(q_land, eff_traj(9.98), errmsg,error);
- ComparePoints(q_mod , eff_traj(5).tail<4>(), errmsg,error);
- ComparePoints(q_end , eff_traj(10), errmsg,error);
+ // create arbitrary trajectory
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::quat_t init_quat = GetXRotQuat(M_PI);
+ helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(), init_quat);
+ helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
+ helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
+ helpers::config_t q_to = q_init; q_to(2) +=0.02;
+ helpers::config_t q_land = q_end ; q_land(2)+=0.02;
+ helpers::quat_t q_mod = GetXRotQuat(M_PI_2);;
+ std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
+ ComparePoints(q_init, eff_traj(0), errmsg,error);
+ ComparePoints(q_to , eff_traj(0.02), errmsg,error);
+ ComparePoints(q_land, eff_traj(9.98), errmsg,error);
+ ComparePoints(q_mod , eff_traj(5).tail<4>(), errmsg,error);
+ ComparePoints(q_end , eff_traj(10), errmsg,error);
}
void EffectorSplineRotationWayPointRotationTest(bool& error)
{
- // create arbitrary trajectory
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 10; i = i + 2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- helpers::quat_t init_quat = GetXRotQuat(0);
- helpers::t_waypoint_quat_t quat_waypoints_;
-
-
- helpers::quat_t q_pi_0 = GetXRotQuat(0);
- helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
- helpers::quat_t q_pi = GetXRotQuat(M_PI);
-
- quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0));
- quat_waypoints_.push_back(std::make_pair(6,q_pi_2));
- quat_waypoints_.push_back(std::make_pair(8,q_pi));
-
-
- helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
- quat_waypoints_.begin(), quat_waypoints_.end());
- helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
- helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.; q_end.tail<4>() = q_pi;
- helpers::config_t q_mod; q_mod.head<3>() = point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2;
- helpers::config_t q_to = q_init; q_to(2) +=0.02;
- helpers::config_t q_land = q_end ; q_land(2)+=0.02;
- std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; while checking waypoints (expected / obtained)");
- ComparePoints(q_init, eff_traj(0), errmsg,error);
- ComparePoints(q_to , eff_traj(0.02), errmsg,error);
- ComparePoints(q_land, eff_traj(9.98), errmsg,error);
- ComparePoints(q_mod , eff_traj(6), errmsg,error);
- ComparePoints(q_end , eff_traj(10), errmsg,error);
+ // create arbitrary trajectory
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 10; i = i + 2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ helpers::quat_t init_quat = GetXRotQuat(0);
+ helpers::t_waypoint_quat_t quat_waypoints_;
+ helpers::quat_t q_pi_0 = GetXRotQuat(0);
+ helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
+ helpers::quat_t q_pi = GetXRotQuat(M_PI);
+ quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0));
+ quat_waypoints_.push_back(std::make_pair(6,q_pi_2));
+ quat_waypoints_.push_back(std::make_pair(8,q_pi));
+ helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
+ quat_waypoints_.begin(), quat_waypoints_.end());
+ helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() = init_quat;
+ helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.; q_end.tail<4>() = q_pi;
+ helpers::config_t q_mod; q_mod.head<3>() = point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2;
+ helpers::config_t q_to = q_init; q_to(2) +=0.02;
+ helpers::config_t q_land = q_end ; q_land(2)+=0.02;
+ std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; while checking waypoints (expected / obtained)");
+ ComparePoints(q_init, eff_traj(0), errmsg,error);
+ ComparePoints(q_to , eff_traj(0.02), errmsg,error);
+ ComparePoints(q_land, eff_traj(9.98), errmsg,error);
+ ComparePoints(q_mod , eff_traj(6), errmsg,error);
+ ComparePoints(q_end , eff_traj(10), errmsg,error);
}
void TestReparametrization(bool& error)
{
- helpers::rotation_spline s;
- const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
- if (!QuasiEqual(sp.min(),0.0))
- {
- std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
- error = true;
- }
- if (!QuasiEqual(sp.max(),1.0))
- {
- std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
- error = true;
- }
- if (!QuasiEqual(sp(1)[0],1.0))
- {
- std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
- error = true;
- }
- if (!QuasiEqual(sp(0)[0],0.0))
- {
- std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
- error = true;
- }
- for(double i =0; i<1; i+=0.002)
+ helpers::rotation_spline s;
+ const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
+ if (!QuasiEqual(sp.min(),0.0))
+ {
+ std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
+ error = true;
+ }
+ if (!QuasiEqual(sp.max(),1.0))
+ {
+ std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
+ error = true;
+ }
+ if (!QuasiEqual(sp(1)[0],1.0))
+ {
+ std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
+ error = true;
+ }
+ if (!QuasiEqual(sp(0)[0],0.0))
+ {
+ std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
+ error = true;
+ }
+ for(double i =0; i<1; i+=0.002)
+ {
+ if(sp(i)[0]>sp(i+0.002)[0])
{
- if(sp(i)[0]>sp(i+0.002)[0])
- {
- std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
- error = true;
- }
+ std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
+ error = true;
}
+ }
}
point_t randomPoint(const double min, const double max )
{
- point_t p;
- for(size_t i = 0 ; i < 3 ; ++i)
- {
- p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
- }
- return p;
+ point_t p;
+ for(size_t i = 0 ; i < 3 ; ++i)
+ {
+ p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
+ }
+ return p;
}
void BezierEvalDeCasteljau(bool& error)
{
- using namespace std;
- std::vector<double> values;
- for (int i =0; i < 100000; ++i)
- {
- values.push_back(rand()/RAND_MAX);
- }
-
- //first compare regular evaluation (low dim pol)
- 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);
-
- // 3d curve
- bezier_curve_t cf(params.begin(), params.end());
-
- std::string errmsg("Error in BezierEvalDeCasteljau; while comparing actual bezier evaluation and de Casteljau : ");
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
- }
-
- 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 cf2(params.begin(), params.end());
-
- for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
- {
- ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
- }
-
+ using namespace std;
+ std::vector<double> values;
+ for (int i =0; i < 100000; ++i)
+ {
+ values.push_back(rand()/RAND_MAX);
+ }
+ //first compare regular evaluation (low dim pol)
+ 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);
+ // 3d curve
+ bezier_curve_t cf(params.begin(), params.end());
+ std::string errmsg("Error in BezierEvalDeCasteljau; while comparing actual bezier evaluation and de Casteljau : ");
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
+ }
+ 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 cf2(params.begin(), params.end());
+ for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
+ {
+ ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
+ }
}
/**
- * @brief BezierSplitCurve test the 'split' method of bezier curve
- * @param error
- */
+* @brief BezierSplitCurve test the 'split' method of bezier curve
+* @param error
+*/
void BezierSplitCurve(bool& error)
{
- // test for degree 5
- size_t n = 5;
- double t_min = 0.2;
- double t_max = 10;
- double aux0, aux1;
- for(size_t i = 0 ; i < 1 ; ++i)
- {
- // build a random curve and split it at random time :
- //std::cout<<"build a random curve"<<std::endl;
- point_t a;
- std::vector<point_t> wps;
- for(size_t j = 0 ; j <= n ; ++j)
- {
- wps.push_back(randomPoint(-10.,10.));
- }
- double t0 = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
- double t1 = (rand()/(double)RAND_MAX )*(t_max-t0) + t0;
- double ts = (rand()/(double)RAND_MAX )*(t1-t0)+t0;
-
- bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
- std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
-
- // test on splitted curves :
- if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
- {
- error = true;
- std::cout<<"BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve"<<std::endl;
- }
- aux0 = c.max()-c.min();
- aux1 = (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min());
- if(!QuasiEqual(aux0, aux1))
- {
- error = true;
- std::cout<<"BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original"<<std::endl;
- }
- if(!QuasiEqual(cs.first.max(), ts))
- {
- error = true;
- std::cout<<"BezierSplitCurve, ERROR timing of the splitted curve doesn't correspond to the original"<<std::endl;
- }
-
- std::string errmsg("BezierSplitCurve, ERROR initial point of the splitted curve doesn't correspond to the original");
- ComparePoints(c(t0), cs.first(t0), errmsg, error);
- errmsg = "BezierSplitCurve, ERROR splitting point of the splitted curve doesn't correspond to the original";
- ComparePoints(cs.first(ts), cs.second(ts), errmsg, error);
- errmsg = "BezierSplitCurve, ERROR final point of the splitted curve doesn't correspond to the original";
- ComparePoints(c(t1), cs.second(cs.second.max()), errmsg, error);
-
- // check along curve :
- double ti = t0;
- errmsg = "BezierSplitCurve, ERROR while checking value on curve and curves splitted";
- while(ti <= ts)
- {
- ComparePoints(cs.first(ti), c(ti), errmsg, error);
- ti += 0.01;
- }
- while(ti <= t1)
- {
- ComparePoints(cs.second(ti), c(ti), errmsg, error);
- ti += 0.01;
- }
- }
+ // test for degree 5
+ size_t n = 5;
+ double t_min = 0.2;
+ double t_max = 10;
+ double aux0, aux1;
+ for(size_t i = 0 ; i < 1 ; ++i)
+ {
+ // build a random curve and split it at random time :
+ //std::cout<<"build a random curve"<<std::endl;
+ point_t a;
+ std::vector<point_t> wps;
+ for(size_t j = 0 ; j <= n ; ++j)
+ {
+ wps.push_back(randomPoint(-10.,10.));
+ }
+ double t0 = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
+ double t1 = (rand()/(double)RAND_MAX )*(t_max-t0) + t0;
+ double ts = (rand()/(double)RAND_MAX )*(t1-t0)+t0;
+
+ bezier_curve_t c(wps.begin(), wps.end(),t0, t1);
+ std::pair<bezier_curve_t,bezier_curve_t> cs = c.split(ts);
+
+ // test on splitted curves :
+ if(! ((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_) ))
+ {
+ error = true;
+ std::cout<<"BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve"<<std::endl;
+ }
+ aux0 = c.max()-c.min();
+ aux1 = (cs.first.max()-cs.first.min() + cs.second.max()-cs.second.min());
+ if(!QuasiEqual(aux0, aux1))
+ {
+ error = true;
+ std::cout<<"BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+ if(!QuasiEqual(cs.first.max(), ts))
+ {
+ error = true;
+ std::cout<<"BezierSplitCurve, ERROR timing of the splitted curve doesn't correspond to the original"<<std::endl;
+ }
+
+ std::string errmsg("BezierSplitCurve, ERROR initial point of the splitted curve doesn't correspond to the original");
+ ComparePoints(c(t0), cs.first(t0), errmsg, error);
+ errmsg = "BezierSplitCurve, ERROR splitting point of the splitted curve doesn't correspond to the original";
+ ComparePoints(cs.first(ts), cs.second(ts), errmsg, error);
+ errmsg = "BezierSplitCurve, ERROR final point of the splitted curve doesn't correspond to the original";
+ ComparePoints(c(t1), cs.second(cs.second.max()), errmsg, error);
+
+ // check along curve :
+ double ti = t0;
+ errmsg = "BezierSplitCurve, ERROR while checking value on curve and curves splitted";
+ while(ti <= ts)
+ {
+ ComparePoints(cs.first(ti), c(ti), errmsg, error);
+ ti += 0.01;
+ }
+ while(ti <= t1)
+ {
+ ComparePoints(cs.second(ti), c(ti), errmsg, error);
+ ti += 0.01;
+ }
+ }
}
/* cubic hermite spline function test */
void CubicHermitePairsPositionDerivativeTest(bool& error)
{
- std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
- std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
- std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
- std::vector< pair_point_tangent_t > control_points;
- point_t res1;
-
- point_t p0(0.,0.,0.);
- point_t p1(1.,2.,3.);
- point_t p2(4.,4.,4.);
-
- tangent_t t0(0.5,0.5,0.5);
- tangent_t t1(0.1,0.2,-0.5);
- tangent_t t2(0.1,0.2,0.3);
-
- std::vector< double > time_control_points;
-
- // Two pairs
- control_points.clear();
- control_points.push_back(pair_point_tangent_t(p0,t0));
- control_points.push_back(pair_point_tangent_t(p1,t1));
- time_control_points.push_back(1.); // Time at P0
- time_control_points.push_back(3.); // Time at P1
- // Create cubic hermite spline
- cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
- //Check
- res1 = cubic_hermite_spline_1Pair(1.); // t=0
- ComparePoints(p0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_1Pair(3.); // t=1
- ComparePoints(p1, res1, errmsg1, error);
- // Test derivative : two pairs
- res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
- ComparePoints(t0, res1, errmsg3, error);
- res1 = cubic_hermite_spline_1Pair.derivate(3.,1);
- ComparePoints(t1, res1, errmsg3, error);
-
- // Three pairs
- control_points.push_back(pair_point_tangent_t(p2,t2));
- time_control_points.clear();
- time_control_points.push_back(0.); // Time at P0
- time_control_points.push_back(2.); // Time at P1
- time_control_points.push_back(5.); // Time at P2
- cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
- //Check
- res1 = cubic_hermite_spline_2Pairs(0.); // t=0
- ComparePoints(p0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_2Pairs(2.); // t=2
- ComparePoints(p1, res1, errmsg2, error);
- res1 = cubic_hermite_spline_2Pairs(5.); // t=5
- ComparePoints(p2, res1, errmsg1, error);
- // Test derivative : three pairs
- res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
- ComparePoints(t0, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
- ComparePoints(t1, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
- ComparePoints(t2, res1, errmsg3, error);
- // Test time control points by default [0,1] => with N control points :
- // Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
- time_control_points.clear();
- time_control_points.push_back(0.); // Time at P0
- time_control_points.push_back(0.5); // Time at P1
- time_control_points.push_back(1.); // Time at P2
- cubic_hermite_spline_2Pairs.setTime(time_control_points);
- res1 = cubic_hermite_spline_2Pairs(0.); // t=0
- ComparePoints(p0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
- ComparePoints(p1, res1, errmsg2, error);
- res1 = cubic_hermite_spline_2Pairs(1.); // t=1
- ComparePoints(p2, res1, errmsg1, error);
- // Test getTime
- try
- {
- cubic_hermite_spline_2Pairs.getTime();
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Cubic hermite spline test, Error when calling getTime\n";
- }
- // Test derivative : three pairs, time default
- res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
- ComparePoints(t0, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
- ComparePoints(t1, res1, errmsg3, error);
- res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
- ComparePoints(t2, res1, errmsg3, error);
+ std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
+ std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
+ std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
+ std::vector< pair_point_tangent_t > control_points;
+ point_t res1;
+
+ point_t p0(0.,0.,0.);
+ point_t p1(1.,2.,3.);
+ point_t p2(4.,4.,4.);
+
+ tangent_t t0(0.5,0.5,0.5);
+ tangent_t t1(0.1,0.2,-0.5);
+ tangent_t t2(0.1,0.2,0.3);
+
+ std::vector< double > time_control_points;
+
+ // Two pairs
+ control_points.clear();
+ control_points.push_back(pair_point_tangent_t(p0,t0));
+ control_points.push_back(pair_point_tangent_t(p1,t1));
+ time_control_points.push_back(1.); // Time at P0
+ time_control_points.push_back(3.); // Time at P1
+ // Create cubic hermite spline
+ cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
+ //Check
+ res1 = cubic_hermite_spline_1Pair(1.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_1Pair(3.); // t=1
+ ComparePoints(p1, res1, errmsg1, error);
+ // Test derivative : two pairs
+ res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_1Pair.derivate(3.,1);
+ ComparePoints(t1, res1, errmsg3, error);
+
+ // Three pairs
+ control_points.push_back(pair_point_tangent_t(p2,t2));
+ time_control_points.clear();
+ time_control_points.push_back(0.); // Time at P0
+ time_control_points.push_back(2.); // Time at P1
+ time_control_points.push_back(5.); // Time at P2
+ cubic_hermite_spline_t cubic_hermite_spline_2Pairs(control_points.begin(), control_points.end(), time_control_points);
+ //Check
+ res1 = cubic_hermite_spline_2Pairs(0.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_2Pairs(2.); // t=2
+ ComparePoints(p1, res1, errmsg2, error);
+ res1 = cubic_hermite_spline_2Pairs(5.); // t=5
+ ComparePoints(p2, res1, errmsg1, error);
+ // Test derivative : three pairs
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(2.,1);
+ ComparePoints(t1, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(5.,1);
+ ComparePoints(t2, res1, errmsg3, error);
+ // Test time control points by default [0,1] => with N control points :
+ // Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
+ time_control_points.clear();
+ time_control_points.push_back(0.); // Time at P0
+ time_control_points.push_back(0.5); // Time at P1
+ time_control_points.push_back(1.); // Time at P2
+ cubic_hermite_spline_2Pairs.setTime(time_control_points);
+ res1 = cubic_hermite_spline_2Pairs(0.); // t=0
+ ComparePoints(p0, res1, errmsg1, error);
+ res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
+ ComparePoints(p1, res1, errmsg2, error);
+ res1 = cubic_hermite_spline_2Pairs(1.); // t=1
+ ComparePoints(p2, res1, errmsg1, error);
+ // Test getTime
+ try
+ {
+ cubic_hermite_spline_2Pairs.getTime();
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Cubic hermite spline test, Error when calling getTime\n";
+ }
+ // Test derivative : three pairs, time default
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.,1);
+ ComparePoints(t0, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(0.5,1);
+ ComparePoints(t1, res1, errmsg3, error);
+ res1 = cubic_hermite_spline_2Pairs.derivate(1.,1);
+ ComparePoints(t2, res1, errmsg3, error);
}
void piecewiseCurveTest(bool& error)
{
- // TEST WITH POLYNOMIALS
- std::string errmsg1("in piecewise polynomial curve test, Error While checking value of point on curve : ");
- point_t a(1,1,1); // in [0,1[
- point_t b(2,1,1); // in [1,2[
- point_t c(3,1,1); // in [2,3]
- point_t res;
- t_point_t vec1, vec2, vec3;
- vec1.push_back(a); // x=1, y=1, z=1
- vec2.push_back(b); // x=2, y=1, z=1
- vec3.push_back(c); // x=3, y=1, z=1
- // Create three polynomials of constant value in the interval of definition
- polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
- polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
- polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
-
- // 1 polynomial in curve
- piecewise_polynomial_curve_t pc(pol1);
- res = pc(0.5);
- ComparePoints(a,res,errmsg1,error);
-
- // 3 polynomials in curve
- pc.add_curve(pol2);
- pc.add_curve(pol3);
-
- // Check values on piecewise curve
- // t in [0,1[ -> res=a
- res = pc(0.);
- ComparePoints(a,res,errmsg1,error);
- res = pc(0.5);
- ComparePoints(a,res,errmsg1,error);
- // t in [1,2[ -> res=b
- res = pc(1.0);
- ComparePoints(b,res,errmsg1,error);
- res = pc(1.5);
- ComparePoints(b,res,errmsg1,error);
- // t in [2,3] -> res=c
- res = pc(2.0);
- ComparePoints(c,res,errmsg1,error);
- res = pc(3.0);
- ComparePoints(c,res,errmsg1,error);
-
- // Create piecewise curve C0 from bezier
- point_t a0(1,2,3);
- point_t b0(2,3,4);
- point_t c0(3,4,5);
- point_t d0(4,5,6);
- std::vector<point_t> params0;
- std::vector<point_t> params1;
- params0.push_back(a0); // bezier between [0,1]
- params0.push_back(b0);
- params0.push_back(c0);
- params0.push_back(d0);
- params1.push_back(d0); // bezier between [1,2]
- params1.push_back(c0);
- params1.push_back(b0);
- params1.push_back(a0);
- bezier_curve_t bc0(params0.begin(), params0.end(), 0., 1.);
- bezier_curve_t bc1(params1.begin(), params1.end(), 1., 2.);
- piecewise_bezier_curve_t pc_C0(bc0);
- pc_C0.add_curve(bc1);
- // Check value in t=0.5 and t=1.5
- res = pc_C0(0.0);
- ComparePoints(a0,res,errmsg1,error);
- res = pc_C0(1.0);
- ComparePoints(d0,res,errmsg1,error);
- res = pc_C0(2.0);
- ComparePoints(a0,res,errmsg1,error);
-
- // Create piecewise curve C1 from Hermite
- point_t p0(0.,0.,0.);
- point_t p1(1.,2.,3.);
- point_t p2(4.,4.,4.);
- tangent_t t0(0.5,0.5,0.5);
- tangent_t t1(0.1,0.2,-0.5);
- tangent_t t2(0.1,0.2,0.3);
- std::vector< pair_point_tangent_t > control_points_0;
- control_points_0.push_back(pair_point_tangent_t(p0,t0));
- control_points_0.push_back(pair_point_tangent_t(p1,t1)); // control_points_0 = 1st piece of curve
- std::vector< pair_point_tangent_t > control_points_1;
- control_points_1.push_back(pair_point_tangent_t(p1,t1));
- control_points_1.push_back(pair_point_tangent_t(p2,t2)); // control_points_1 = 2nd piece of curve
- std::vector< double > time_control_points0, time_control_points1;
- time_control_points0.push_back(0.);
- time_control_points0.push_back(1.); // hermite 0 between [0,1]
- time_control_points1.push_back(1.);
- time_control_points1.push_back(3.); // hermite 1 between [1,3]
- cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points0);
- cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points1);
- piecewise_cubic_hermite_curve_t pc_C1(chs0);
- pc_C1.add_curve(chs1);
-
-
- // Create piecewise curve C2
- point_t a1(0,0,0);
- point_t b1(1,1,1);
- t_point_t veca, vecb;
- // in [0,1[
- veca.push_back(a1);
- veca.push_back(b1); // x=t, y=t, z=t
- // in [1,2]
- vecb.push_back(b1);
- vecb.push_back(b1); // x=(t-1)+1, y=(t-1)+1, z=(t-1)+1
- polynomial_t pola(veca.begin(), veca.end(), 0, 1);
- polynomial_t polb(vecb.begin(), vecb.end(), 1, 2);
- piecewise_polynomial_curve_t pc_C2(pola);
- pc_C2.add_curve(polb);
-
-
- // check C0 continuity
- std::string errmsg2("in piecewise polynomial curve test, Error while checking continuity C0 on ");
- std::string errmsg3("in piecewise polynomial curve test, Error while checking continuity C1 on ");
- std::string errmsg4("in piecewise polynomial curve test, Error while checking continuity C2 on ");
- // not C0
- bool isC0 = pc.is_continuous(0);
- if (isC0)
- {
- std::cout << errmsg2 << " pc " << std::endl;
- error = true;
- }
- // C0
- isC0 = pc_C0.is_continuous(0);
- if (not isC0)
- {
- std::cout << errmsg2 << " pc_C0 " << std::endl;
- error = true;
- }
- // not C1
- bool isC1 = pc_C0.is_continuous(1);
- if (isC1)
- {
- std::cout << errmsg3 << " pc_C0 " << std::endl;
- error = true;
- }
- // C1
- isC1 = pc_C1.is_continuous(1);
- if (not isC1)
- {
- std::cout << errmsg3 << " pc_C1 " << std::endl;
- error = true;
- }
- // not C2
- bool isC2 = pc_C1.is_continuous(2);
- if (isC2)
- {
- std::cout << errmsg4 << " pc_C1 " << std::endl;
- error = true;
- }
- // C2
- isC2 = pc_C2.is_continuous(2);
- if (not isC2)
- {
- std::cout << errmsg4 << " pc_C2 " << std::endl;
- error = true;
- }
-
- // CONVERT PIECEWISE POLYNOMIAL CURVES TO BEZIER AND HERMITE
- std::string errmsg5("in piecewise polynomial curve test, Error while checking piecewise curve conversion");
- piecewise_bezier_curve_t pc_bezier = pc.convert_piecewise_curve_to_bezier<bezier_curve_t>();
- CompareCurves<piecewise_polynomial_curve_t, piecewise_bezier_curve_t>(pc, pc_bezier, errmsg5, error);
- piecewise_cubic_hermite_curve_t pc_hermite = pc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
- CompareCurves<piecewise_polynomial_curve_t, piecewise_cubic_hermite_curve_t>(pc, pc_hermite, errmsg5, error);
- piecewise_polynomial_curve_t pc_polynomial_same = pc.convert_piecewise_curve_to_polynomial<polynomial_t>();
- CompareCurves<piecewise_polynomial_curve_t, piecewise_polynomial_curve_t>(pc, pc_polynomial_same, errmsg5, error);
+ // TEST WITH POLYNOMIALS
+ std::string errmsg1("in piecewise polynomial curve test, Error While checking value of point on curve : ");
+ point_t a(1,1,1); // in [0,1[
+ point_t b(2,1,1); // in [1,2[
+ point_t c(3,1,1); // in [2,3]
+ point_t res;
+ t_point_t vec1, vec2, vec3;
+ vec1.push_back(a); // x=1, y=1, z=1
+ vec2.push_back(b); // x=2, y=1, z=1
+ vec3.push_back(c); // x=3, y=1, z=1
+ // Create three polynomials of constant value in the interval of definition
+ polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
+ polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
+ polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
+ // 1 polynomial in curve
+ piecewise_polynomial_curve_t pc(pol1);
+ res = pc(0.5);
+ ComparePoints(a,res,errmsg1,error);
+ // 3 polynomials in curve
+ pc.add_curve(pol2);
+ pc.add_curve(pol3);
+ // Check values on piecewise curve
+ // t in [0,1[ -> res=a
+ res = pc(0.);
+ ComparePoints(a,res,errmsg1,error);
+ res = pc(0.5);
+ ComparePoints(a,res,errmsg1,error);
+ // t in [1,2[ -> res=b
+ res = pc(1.0);
+ ComparePoints(b,res,errmsg1,error);
+ res = pc(1.5);
+ ComparePoints(b,res,errmsg1,error);
+ // t in [2,3] -> res=c
+ res = pc(2.0);
+ ComparePoints(c,res,errmsg1,error);
+ res = pc(3.0);
+ ComparePoints(c,res,errmsg1,error);
+ // Create piecewise curve C0 from bezier
+ point_t a0(1,2,3);
+ point_t b0(2,3,4);
+ point_t c0(3,4,5);
+ point_t d0(4,5,6);
+ std::vector<point_t> params0;
+ std::vector<point_t> params1;
+ params0.push_back(a0); // bezier between [0,1]
+ params0.push_back(b0);
+ params0.push_back(c0);
+ params0.push_back(d0);
+ params1.push_back(d0); // bezier between [1,2]
+ params1.push_back(c0);
+ params1.push_back(b0);
+ params1.push_back(a0);
+ bezier_curve_t bc0(params0.begin(), params0.end(), 0., 1.);
+ bezier_curve_t bc1(params1.begin(), params1.end(), 1., 2.);
+ piecewise_bezier_curve_t pc_C0(bc0);
+ pc_C0.add_curve(bc1);
+ // Check value in t=0.5 and t=1.5
+ res = pc_C0(0.0);
+ ComparePoints(a0,res,errmsg1,error);
+ res = pc_C0(1.0);
+ ComparePoints(d0,res,errmsg1,error);
+ res = pc_C0(2.0);
+ ComparePoints(a0,res,errmsg1,error);
+ // Create piecewise curve C1 from Hermite
+ point_t p0(0.,0.,0.);
+ point_t p1(1.,2.,3.);
+ point_t p2(4.,4.,4.);
+ tangent_t t0(0.5,0.5,0.5);
+ tangent_t t1(0.1,0.2,-0.5);
+ tangent_t t2(0.1,0.2,0.3);
+ std::vector< pair_point_tangent_t > control_points_0;
+ control_points_0.push_back(pair_point_tangent_t(p0,t0));
+ control_points_0.push_back(pair_point_tangent_t(p1,t1)); // control_points_0 = 1st piece of curve
+ std::vector< pair_point_tangent_t > control_points_1;
+ control_points_1.push_back(pair_point_tangent_t(p1,t1));
+ control_points_1.push_back(pair_point_tangent_t(p2,t2)); // control_points_1 = 2nd piece of curve
+ std::vector< double > time_control_points0, time_control_points1;
+ time_control_points0.push_back(0.);
+ time_control_points0.push_back(1.); // hermite 0 between [0,1]
+ time_control_points1.push_back(1.);
+ time_control_points1.push_back(3.); // hermite 1 between [1,3]
+ cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points0);
+ cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points1);
+ piecewise_cubic_hermite_curve_t pc_C1(chs0);
+ pc_C1.add_curve(chs1);
+ // Create piecewise curve C2
+ point_t a1(0,0,0);
+ point_t b1(1,1,1);
+ t_point_t veca, vecb;
+ // in [0,1[
+ veca.push_back(a1);
+ veca.push_back(b1); // x=t, y=t, z=t
+ // in [1,2]
+ vecb.push_back(b1);
+ vecb.push_back(b1); // x=(t-1)+1, y=(t-1)+1, z=(t-1)+1
+ polynomial_t pola(veca.begin(), veca.end(), 0, 1);
+ polynomial_t polb(vecb.begin(), vecb.end(), 1, 2);
+ piecewise_polynomial_curve_t pc_C2(pola);
+ pc_C2.add_curve(polb);
+ // check C0 continuity
+ std::string errmsg2("in piecewise polynomial curve test, Error while checking continuity C0 on ");
+ std::string errmsg3("in piecewise polynomial curve test, Error while checking continuity C1 on ");
+ std::string errmsg4("in piecewise polynomial curve test, Error while checking continuity C2 on ");
+ // not C0
+ bool isC0 = pc.is_continuous(0);
+ if (isC0)
+ {
+ std::cout << errmsg2 << " pc " << std::endl;
+ error = true;
+ }
+ // C0
+ isC0 = pc_C0.is_continuous(0);
+ if (not isC0)
+ {
+ std::cout << errmsg2 << " pc_C0 " << std::endl;
+ error = true;
+ }
+ // not C1
+ bool isC1 = pc_C0.is_continuous(1);
+ if (isC1)
+ {
+ std::cout << errmsg3 << " pc_C0 " << std::endl;
+ error = true;
+ }
+ // C1
+ isC1 = pc_C1.is_continuous(1);
+ if (not isC1)
+ {
+ std::cout << errmsg3 << " pc_C1 " << std::endl;
+ error = true;
+ }
+ // not C2
+ bool isC2 = pc_C1.is_continuous(2);
+ if (isC2)
+ {
+ std::cout << errmsg4 << " pc_C1 " << std::endl;
+ error = true;
+ }
+ // C2
+ isC2 = pc_C2.is_continuous(2);
+ if (not isC2)
+ {
+ std::cout << errmsg4 << " pc_C2 " << std::endl;
+ error = true;
+ }
+ // CONVERT PIECEWISE POLYNOMIAL CURVES TO BEZIER AND HERMITE
+ std::string errmsg5("in piecewise polynomial curve test, Error while checking piecewise curve conversion");
+ piecewise_bezier_curve_t pc_bezier = pc.convert_piecewise_curve_to_bezier<bezier_curve_t>();
+ CompareCurves<piecewise_polynomial_curve_t, piecewise_bezier_curve_t>(pc, pc_bezier, errmsg5, error);
+ piecewise_cubic_hermite_curve_t pc_hermite = pc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
+ CompareCurves<piecewise_polynomial_curve_t, piecewise_cubic_hermite_curve_t>(pc, pc_hermite, errmsg5, error);
+ piecewise_polynomial_curve_t pc_polynomial_same = pc.convert_piecewise_curve_to_polynomial<polynomial_t>();
+ CompareCurves<piecewise_polynomial_curve_t, piecewise_polynomial_curve_t>(pc, pc_polynomial_same, errmsg5, error);
}
void curveAbcDimDynamicTest(bool& error)
{
- typedef curve_abc<double,double,true> curve_abc_test_t;
- typedef polynomial <double, double, 3, true> polynomial_test_t;
- typedef exact_cubic <double, double, 3, true> exact_cubic_test_t;
- typedef exact_cubic_test_t::spline_constraints spline_constraints_test_t;
- typedef bezier_curve <double, double, 3, true> bezier_curve_test_t;
- typedef cubic_hermite_spline <double, double, 3, true> cubic_hermite_spline_test_t;
- curve_abc_test_t * pt_curve_abc;
- // POLYNOMIAL
- point_t a(1,1,1);
- point_t b(2,2,2);
- t_point_t vec;
- vec.push_back(a);
- vec.push_back(b);
- polynomial_test_t pol(vec.begin(), vec.end(), 0, 1);
- try
- {
- pol(0);
- pol(1);
- }
- catch(...)
- {
- error = false;
- }
- // BEZIER
- bezier_curve_test_t bc = bezier_from_curve<bezier_curve_test_t, polynomial_test_t>(pol);
- try
- {
- bc(0);
- bc(1);
- }
- catch(...)
- {
- error = false;
- }
- // CUBIC HERMITE
- cubic_hermite_spline_test_t chs = hermite_from_curve<cubic_hermite_spline_test_t, polynomial_test_t>(pol);
- try
- {
- chs(0);
- chs(1);
- }
- catch(...)
- {
- error = false;
- }
- // EXACT CUBIC : NOT SUPPORTED, problem to fix later
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
- spline_constraints_test_t constraints;
- constraints.end_vel = point_t(0,0,0);
- constraints.init_vel = point_t(0,0,0);
- constraints.end_acc = point_t(0,0,0);
- constraints.init_acc = point_t(0,0,0);
- exact_cubic_test_t ec(waypoints.begin(), waypoints.end(), constraints);
- try
- {
- ec(0);
- ec(1);
- }
- catch(...)
- {
- error = false;
- }
-
- // Test with pointer to curve_abc type
- try
- {
- pt_curve_abc = &pol;
- (*pt_curve_abc)(0);
- (*pt_curve_abc)(1);
- pt_curve_abc = &bc;
- (*pt_curve_abc)(0);
- (*pt_curve_abc)(1);
- pt_curve_abc = &chs;
- (*pt_curve_abc)(0);
- (*pt_curve_abc)(1);
- pt_curve_abc = &ec;
- (*pt_curve_abc)(0);
- (*pt_curve_abc)(1);
- }
- catch(...)
- {
- error = false;
- }
-
+ typedef curve_abc<double,double,true> curve_abc_test_t;
+ typedef polynomial <double, double, 3, true> polynomial_test_t;
+ typedef exact_cubic <double, double, 3, true> exact_cubic_test_t;
+ typedef exact_cubic_test_t::spline_constraints spline_constraints_test_t;
+ typedef bezier_curve <double, double, 3, true> bezier_curve_test_t;
+ typedef cubic_hermite_spline <double, double, 3, true> cubic_hermite_spline_test_t;
+ curve_abc_test_t * pt_curve_abc;
+ // POLYNOMIAL
+ point_t a(1,1,1);
+ point_t b(2,2,2);
+ t_point_t vec;
+ vec.push_back(a);
+ vec.push_back(b);
+ polynomial_test_t pol(vec.begin(), vec.end(), 0, 1);
+ try
+ {
+ pol(0);
+ pol(1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ // BEZIER
+ bezier_curve_test_t bc = bezier_from_curve<bezier_curve_test_t, polynomial_test_t>(pol);
+ try
+ {
+ bc(0);
+ bc(1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ // CUBIC HERMITE
+ cubic_hermite_spline_test_t chs = hermite_from_curve<cubic_hermite_spline_test_t, polynomial_test_t>(pol);
+ try
+ {
+ chs(0);
+ chs(1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ // EXACT CUBIC : NOT SUPPORTED, problem to fix later
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
+ spline_constraints_test_t constraints;
+ constraints.end_vel = point_t(0,0,0);
+ constraints.init_vel = point_t(0,0,0);
+ constraints.end_acc = point_t(0,0,0);
+ constraints.init_acc = point_t(0,0,0);
+ exact_cubic_test_t ec(waypoints.begin(), waypoints.end(), constraints);
+ try
+ {
+ ec(0);
+ ec(1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ // Test with pointer to curve_abc type
+ try
+ {
+ pt_curve_abc = &pol;
+ (*pt_curve_abc)(0);
+ (*pt_curve_abc)(1);
+ pt_curve_abc = &bc;
+ (*pt_curve_abc)(0);
+ (*pt_curve_abc)(1);
+ pt_curve_abc = &chs;
+ (*pt_curve_abc)(0);
+ (*pt_curve_abc)(1);
+ pt_curve_abc = &ec;
+ (*pt_curve_abc)(0);
+ (*pt_curve_abc)(1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
}
void piecewiseCurveConversionFromDiscretePointsTest(bool& error)
{
- std::string errMsg("piecewiseCurveConversionFromDiscretePointsTest, Error, value on curve is wrong : ");
- point_t p0(0.,0.,0.);
- point_t p1(1.,2.,3.);
- point_t p2(4.,4.,4.);
- point_t p3(10.,10.,10.);
- point_t p_test_0_5 = (p0+p1)/2.0;
- t_point_t points;
- points.push_back(p0);
- points.push_back(p1);
- points.push_back(p2);
- points.push_back(p3);
- double T_min = 1.0;
- double T_max = 3.0;
- double timestep = (T_max-T_min)/double(points.size()-1);
- piecewise_polynomial_curve_t ppc = piecewise_polynomial_curve_t::
- convert_discrete_points_to_polynomial<polynomial_t>(points,T_min,T_max);
- if (!ppc.is_continuous(0))
- {
- std::cout<<"piecewiseCurveConversionFromDiscretePointsTest, Error, piecewise curve is not C0"<<std::endl;
- error = true;
- }
- ComparePoints(p0, ppc(T_min), errMsg, error);
- ComparePoints(p_test_0_5, ppc(T_min+timestep/2.0), errMsg, error);
- ComparePoints(p1, ppc(T_min+timestep), errMsg, error);
- ComparePoints(p2, ppc(T_min+2*timestep), errMsg, error);
- ComparePoints(p3, ppc(T_max), errMsg, error);
+ std::string errMsg("piecewiseCurveConversionFromDiscretePointsTest, Error, value on curve is wrong : ");
+ point_t p0(0.,0.,0.);
+ point_t p1(1.,2.,3.);
+ point_t p2(4.,4.,4.);
+ point_t p3(10.,10.,10.);
+ point_t p_test_0_5 = (p0+p1)/2.0;
+ t_point_t points;
+ points.push_back(p0);
+ points.push_back(p1);
+ points.push_back(p2);
+ points.push_back(p3);
+ double T_min = 1.0;
+ double T_max = 3.0;
+ double timestep = (T_max-T_min)/double(points.size()-1);
+ piecewise_polynomial_curve_t ppc = piecewise_polynomial_curve_t::
+ convert_discrete_points_to_polynomial<polynomial_t>(points,T_min,T_max);
+ if (!ppc.is_continuous(0))
+ {
+ std::cout<<"piecewiseCurveConversionFromDiscretePointsTest, Error, piecewise curve is not C0"<<std::endl;
+ error = true;
+ }
+ ComparePoints(p0, ppc(T_min), errMsg, error);
+ ComparePoints(p_test_0_5, ppc(T_min+timestep/2.0), errMsg, error);
+ ComparePoints(p1, ppc(T_min+timestep), errMsg, error);
+ ComparePoints(p2, ppc(T_min+2*timestep), errMsg, error);
+ ComparePoints(p3, ppc(T_max), errMsg, error);
}
void serializationCurvesTest(bool& error)
{
- std::string errMsg1("in serializationCurveTest, Error While serializing Polynomial : ");
- std::string errMsg2("in serializationCurveTest, Error While serializing Bezier : ");
- std::string errMsg3("in serializationCurveTest, Error While serializing Cubic Hermite : ");
- std::string errMsg4("in serializationCurveTest, Error While serializing Piecewise curves : ");
- std::string errMsg5("in serializationCurveTest, Error While serializing Exact cubic : ");
- point_t a(1,1,1); // in [0,1[
- point_t b(2,1,1); // in [1,2[
- point_t c(3,1,1); // in [2,3]
- point_t res;
- t_point_t vec1, vec2, vec3;
- vec1.push_back(a); // x=1, y=1, z=1
- vec2.push_back(b); // x=2, y=1, z=1
- vec3.push_back(c); // x=3, y=1, z=1
- polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
- polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
- polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
- piecewise_polynomial_curve_t ppc(pol1);
- ppc.add_curve(pol2);
- ppc.add_curve(pol3);
- std::string fileName("fileTest.test");
-
- // Test serialization on Polynomial
- pol1.saveAsText(fileName);
- polynomial_t pol_test;
- pol_test.loadFromText(fileName);
- CompareCurves<polynomial_t, polynomial_t>(pol1, pol_test, errMsg1, error);
- // Test serialization on Bezier
- bezier_curve_t bc = bezier_from_curve<bezier_curve_t, polynomial_t>(pol1);
- bc.saveAsText(fileName);
- bezier_curve_t bc_test;
- bc_test.loadFromText(fileName);
- CompareCurves<polynomial_t, bezier_curve_t>(pol1, bc_test, errMsg2, error);
- // Test serialization on Cubic Hermite
- cubic_hermite_spline_t chs = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol1);
- chs.saveAsText(fileName);
- cubic_hermite_spline_t chs_test;
- chs_test.loadFromText(fileName);
- CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol1, chs_test, errMsg3, error);
-
- // Test serialization on Piecewise Polynomial curve
- ppc.saveAsText(fileName);
- piecewise_polynomial_curve_t ppc_test;
- ppc_test.loadFromText(fileName);
- CompareCurves<piecewise_polynomial_curve_t,piecewise_polynomial_curve_t>(ppc, ppc_test, errMsg4, error);
- // Test serialization on Piecewise Bezier curve
- piecewise_bezier_curve_t pbc = ppc.convert_piecewise_curve_to_bezier<bezier_curve_t>();
- pbc.saveAsText(fileName);
- piecewise_bezier_curve_t pbc_test;
- pbc_test.loadFromText(fileName);
- CompareCurves<piecewise_polynomial_curve_t,piecewise_bezier_curve_t>(ppc, pbc_test, errMsg4, error);
- // Test serialization on Piecewise Cubic Hermite curve
- piecewise_cubic_hermite_curve_t pchc = ppc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
- pchc.saveAsText(fileName);
- piecewise_cubic_hermite_curve_t pchc_test;
- pchc_test.loadFromText(fileName);
- CompareCurves<piecewise_polynomial_curve_t,piecewise_cubic_hermite_curve_t>(ppc, pchc_test, errMsg4, error);
-
- // Test serialization on exact cubic
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- spline_constraints_t constraints;
- constraints.end_vel = point_t(0,0,0);
- constraints.init_vel = point_t(0,0,0);
- constraints.end_acc = point_t(0,0,0);
- constraints.init_acc = point_t(0,0,0);
- exact_cubic_t ec(waypoints.begin(), waypoints.end(), constraints);
- ec.saveAsText(fileName);
- exact_cubic_t ec_test;
- ec_test.loadFromText(fileName);
- CompareCurves<exact_cubic_t,exact_cubic_t>(ec, ec_test, errMsg5, error);
+ std::string errMsg1("in serializationCurveTest, Error While serializing Polynomial : ");
+ std::string errMsg2("in serializationCurveTest, Error While serializing Bezier : ");
+ std::string errMsg3("in serializationCurveTest, Error While serializing Cubic Hermite : ");
+ std::string errMsg4("in serializationCurveTest, Error While serializing Piecewise curves : ");
+ std::string errMsg5("in serializationCurveTest, Error While serializing Exact cubic : ");
+ point_t a(1,1,1); // in [0,1[
+ point_t b(2,1,1); // in [1,2[
+ point_t c(3,1,1); // in [2,3]
+ point_t res;
+ t_point_t vec1, vec2, vec3;
+ vec1.push_back(a); // x=1, y=1, z=1
+ vec2.push_back(b); // x=2, y=1, z=1
+ vec3.push_back(c); // x=3, y=1, z=1
+ polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
+ polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
+ polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
+ piecewise_polynomial_curve_t ppc(pol1);
+ ppc.add_curve(pol2);
+ ppc.add_curve(pol3);
+ std::string fileName("fileTest.test");
+ // Simple curves
+ // Test serialization on Polynomial
+ pol1.saveAsText(fileName);
+ polynomial_t pol_test;
+ pol_test.loadFromText(fileName);
+ CompareCurves<polynomial_t, polynomial_t>(pol1, pol_test, errMsg1, error);
+ // Test serialization on Bezier
+ bezier_curve_t bc = bezier_from_curve<bezier_curve_t, polynomial_t>(pol1);
+ bc.saveAsText(fileName);
+ bezier_curve_t bc_test;
+ bc_test.loadFromText(fileName);
+ CompareCurves<polynomial_t, bezier_curve_t>(pol1, bc_test, errMsg2, error);
+ // Test serialization on Cubic Hermite
+ cubic_hermite_spline_t chs = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol1);
+ chs.saveAsText(fileName);
+ cubic_hermite_spline_t chs_test;
+ chs_test.loadFromText(fileName);
+ CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol1, chs_test, errMsg3, error);
+ // Piecewise curves
+ // Test serialization on Piecewise Polynomial curve
+ ppc.saveAsText(fileName);
+ piecewise_polynomial_curve_t ppc_test;
+ ppc_test.loadFromText(fileName);
+ CompareCurves<piecewise_polynomial_curve_t,piecewise_polynomial_curve_t>(ppc, ppc_test, errMsg4, error);
+ // Test serialization on Piecewise Bezier curve
+ piecewise_bezier_curve_t pbc = ppc.convert_piecewise_curve_to_bezier<bezier_curve_t>();
+ pbc.saveAsText(fileName);
+ piecewise_bezier_curve_t pbc_test;
+ pbc_test.loadFromText(fileName);
+ CompareCurves<piecewise_polynomial_curve_t,piecewise_bezier_curve_t>(ppc, pbc_test, errMsg4, error);
+ // Test serialization on Piecewise Cubic Hermite curve
+ piecewise_cubic_hermite_curve_t pchc = ppc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
+ pchc.saveAsText(fileName);
+ piecewise_cubic_hermite_curve_t pchc_test;
+ pchc_test.loadFromText(fileName);
+ CompareCurves<piecewise_polynomial_curve_t,piecewise_cubic_hermite_curve_t>(ppc, pchc_test, errMsg4, error);
+ // Test serialization on exact cubic
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ spline_constraints_t constraints;
+ constraints.end_vel = point_t(0,0,0);
+ constraints.init_vel = point_t(0,0,0);
+ constraints.end_acc = point_t(0,0,0);
+ constraints.init_acc = point_t(0,0,0);
+ exact_cubic_t ec(waypoints.begin(), waypoints.end(), constraints);
+ ec.saveAsText(fileName);
+ exact_cubic_t ec_test;
+ ec_test.loadFromText(fileName);
+ CompareCurves<exact_cubic_t,exact_cubic_t>(ec, ec_test, errMsg5, error);
}
int main(int /*argc*/, char** /*argv[]*/)
{
- std::cout << "performing tests... \n";
- bool error = false;
- CubicFunctionTest(error);
- ExactCubicNoErrorTest(error);
- ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
- ExactCubicTwoPointsTest(error);
- ExactCubicOneDimTest(error);
- ExactCubicVelocityConstraintsTest(error);
- EffectorTrajectoryTest(error);
- EffectorSplineRotationNoRotationTest(error);
- EffectorSplineRotationRotationTest(error);
- TestReparametrization(error);
- EffectorSplineRotationWayPointRotationTest(error);
- BezierCurveTest(error);
- BezierDerivativeCurveTest(error);
- BezierDerivativeCurveConstraintTest(error);
- BezierCurveTestCompareHornerAndBernstein(error);
- BezierDerivativeCurveTimeReparametrizationTest(error);
- BezierEvalDeCasteljau(error);
- BezierSplitCurve(error);
- CubicHermitePairsPositionDerivativeTest(error);
- piecewiseCurveTest(error);
- piecewiseCurveConversionFromDiscretePointsTest(error);
- toPolynomialConversionTest(error);
- cubicConversionTest(error);
- curveAbcDimDynamicTest(error);
- serializationCurvesTest(error);
-
- if(error)
- {
- std::cout << "There were some errors\n";
- return -1;
- } else
- {
- std::cout << "no errors found \n";
- return 0;
- }
+ std::cout << "performing tests... \n";
+ bool error = false;
+ CubicFunctionTest(error);
+ ExactCubicNoErrorTest(error);
+ ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
+ ExactCubicTwoPointsTest(error);
+ ExactCubicOneDimTest(error);
+ ExactCubicVelocityConstraintsTest(error);
+ EffectorTrajectoryTest(error);
+ EffectorSplineRotationNoRotationTest(error);
+ EffectorSplineRotationRotationTest(error);
+ TestReparametrization(error);
+ EffectorSplineRotationWayPointRotationTest(error);
+ BezierCurveTest(error);
+ BezierDerivativeCurveTest(error);
+ BezierDerivativeCurveConstraintTest(error);
+ BezierCurveTestCompareHornerAndBernstein(error);
+ BezierDerivativeCurveTimeReparametrizationTest(error);
+ BezierEvalDeCasteljau(error);
+ BezierSplitCurve(error);
+ CubicHermitePairsPositionDerivativeTest(error);
+ piecewiseCurveTest(error);
+ piecewiseCurveConversionFromDiscretePointsTest(error);
+ toPolynomialConversionTest(error);
+ cubicConversionTest(error);
+ curveAbcDimDynamicTest(error);
+ serializationCurvesTest(error);
+ if(error)
+ {
+ std::cout << "There were some errors\n";
+ return -1;
+ } else
+ {
+ std::cout << "no errors found \n";
+ return 0;
+ }
}