From a68a1a7dd5ab74a88704e21f3db8e714cec8b080 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Fri, 1 Nov 2019 12:13:04 +0100
Subject: [PATCH] Format
---
include/curves/bezier_curve.h | 27 +-
include/curves/curve_abc.h | 18 +-
include/curves/curve_constraint.h | 46 +-
include/curves/linear_variable.h | 241 ++-
include/curves/optimization/definitions.h | 150 +-
include/curves/optimization/details.h | 600 ++++---
include/curves/optimization/integral_cost.h | 72 +-
.../curves/optimization/quadratic_problem.h | 62 +-
include/curves/piecewise_curve.h | 23 +-
include/curves/quadratic_variable.h | 304 ++--
include/curves/se3_curve.h | 383 ++---
include/curves/so3_linear.h | 391 ++---
python/curves_python.cpp | 1519 +++++++++--------
python/namespace.cpp | 27 +-
python/namespace.h | 30 +-
python/optimization_python.cpp | 353 ++--
python/optimization_python.h | 17 +-
python/python_variables.cpp | 128 +-
python/python_variables.h | 173 +-
tests/Main.cpp | 1086 ++++++------
tests/load_problem.h | 126 +-
21 files changed, 2737 insertions(+), 3039 deletions(-)
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index e9f66a4..c0f9cfb 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -39,7 +39,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
- typedef piecewise_curve <Time, Numeric, Safe, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
+ typedef piecewise_curve<Time, Numeric, Safe, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
/* Constructors - destructors */
public:
@@ -296,7 +296,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return new_pts;
}
-
/// \brief Split the bezier curve in 2 at time t.
/// \param t : list of points.
/// \param u : unNormalized time.
@@ -324,27 +323,23 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return std::make_pair(c_first, c_second);
}
-
/// \brief Split the bezier curve in several curves, all accessible
/// within a piecewise_bezier_curve_t.
/// \param times : list of times of size n.
/// \return a piecewise_bezier_curve_t comprising n+1 curves
///
- piecewise_bezier_curve_t split(const vector_x_t& times) const
- {
- typename piecewise_bezier_curve_t::t_curve_t curves;
- bezier_curve_t current = *this;
- for(int i = 0; i < times.rows(); ++i)
- {
- std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
- curves.push_back(pairsplit.first);
- current = pairsplit.second;
- }
- curves.push_back(current);
- return piecewise_bezier_curve_t(curves);
+ piecewise_bezier_curve_t split(const vector_x_t& times) const {
+ typename piecewise_bezier_curve_t::t_curve_t curves;
+ bezier_curve_t current = *this;
+ for (int i = 0; i < times.rows(); ++i) {
+ std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
+ curves.push_back(pairsplit.first);
+ current = pairsplit.second;
+ }
+ curves.push_back(current);
+ return piecewise_bezier_curve_t(curves);
}
-
/// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the actual bezier curve
/// defined between \f$[T_{min},T_{max}]\f$ with \f$T_{min} \leq t_1 \leq t_2 \leq T_{max}\f$.
/// \param t1 : start time of bezier curve extracted.
diff --git a/include/curves/curve_abc.h b/include/curves/curve_abc.h
index cc3bef7..d5d0583 100644
--- a/include/curves/curve_abc.h
+++ b/include/curves/curve_abc.h
@@ -17,20 +17,16 @@
#include <functional>
-
-namespace curves
-{
+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> , typename Point_derivate = Point>
-struct curve_abc : std::unary_function<Time, Point>,
- public serialization::Serializable
-{
- typedef Point point_t;
- typedef Point_derivate point_derivate_t;
- typedef Time time_t;
+template <typename Time = double, typename Numeric = Time, bool Safe = false,
+ typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, typename Point_derivate = Point>
+struct curve_abc : std::unary_function<Time, Point>, public serialization::Serializable {
+ typedef Point point_t;
+ typedef Point_derivate point_derivate_t;
+ typedef Time time_t;
/* Constructors - destructors */
public:
diff --git a/include/curves/curve_constraint.h b/include/curves/curve_constraint.h
index 5be6666..8a68df6 100644
--- a/include/curves/curve_constraint.h
+++ b/include/curves/curve_constraint.h
@@ -21,33 +21,31 @@ template <typename Point>
struct curve_constraints {
typedef Point point_t;
curve_constraints(const size_t dim = 3)
- :init_vel (point_t::Zero(dim))
- ,init_acc (point_t::Zero(dim))
- ,init_jerk(point_t::Zero(dim))
- ,end_vel (point_t::Zero(dim))
- ,end_acc (point_t::Zero(dim))
- ,end_jerk (point_t::Zero(dim))
- ,dim_ (dim){}
+ : init_vel(point_t::Zero(dim)),
+ init_acc(point_t::Zero(dim)),
+ init_jerk(point_t::Zero(dim)),
+ end_vel(point_t::Zero(dim)),
+ end_acc(point_t::Zero(dim)),
+ end_jerk(point_t::Zero(dim)),
+ dim_(dim) {}
curve_constraints(const curve_constraints& other)
- :init_vel (other.init_vel )
- ,init_acc (other.init_acc )
- ,init_jerk(other.init_jerk)
- ,end_vel (other.end_vel )
- ,end_acc (other.end_acc )
- ,end_jerk (other.end_jerk )
- ,dim_ (other.dim_ )
- {}
+ : init_vel(other.init_vel),
+ init_acc(other.init_acc),
+ init_jerk(other.init_jerk),
+ end_vel(other.end_vel),
+ end_acc(other.end_acc),
+ end_jerk(other.end_jerk),
+ dim_(other.dim_) {}
- ~curve_constraints(){}
- point_t init_vel;
- point_t init_acc;
- point_t init_jerk;
- point_t end_vel;
- point_t end_acc;
- point_t end_jerk;
- size_t dim_;
+ ~curve_constraints() {}
+ point_t init_vel;
+ point_t init_acc;
+ point_t init_jerk;
+ point_t end_vel;
+ point_t end_acc;
+ point_t end_jerk;
+ size_t dim_;
};
} // namespace curves
#endif //_CLASS_CUBICZEROVELACC
-
diff --git a/include/curves/linear_variable.h b/include/curves/linear_variable.h
index 7dc4868..67348fa 100644
--- a/include/curves/linear_variable.h
+++ b/include/curves/linear_variable.h
@@ -1,11 +1,10 @@
/**
-* \file linear_variable.h
-* \brief storage for variable points of the form p_i = B_i x + c_i
-* \author Steve T.
-* \version 0.1
-* \date 07/02/2019
-*/
-
+ * \file linear_variable.h
+ * \brief storage for variable points of the form p_i = B_i x + c_i
+ * \author Steve T.
+ * \version 0.1
+ * \date 07/02/2019
+ */
#ifndef _CLASS_LINEAR_VARIABLE
#define _CLASS_LINEAR_VARIABLE
@@ -22,152 +21,126 @@
#include <Eigen/Core>
#include <stdexcept>
-namespace curves
-{
-template <typename Numeric=double, bool Safe=true>
-struct linear_variable : public serialization::Serializable
-{
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
- typedef linear_variable<Numeric> linear_variable_t;
-
- linear_variable(): B_(matrix_x_t::Identity(0,0)), c_(vector_x_t::Zero(0)), zero(true){} //variable
- linear_variable(const vector_x_t& c):B_(matrix_x_t::Zero(c.size(),c.size())),c_(c), zero(false) {} // constant
- linear_variable(const matrix_x_t& B, const vector_x_t& c):B_(B),c_(c), zero(false) {} //mixed
-
- // linear evaluation
- vector_x_t operator()(const Eigen::Ref<const vector_x_t>& val) const
- {
- if(isZero())
- return c();
- if(Safe && B().cols() != val.rows() )
- throw std::length_error("Cannot evaluate linear variable, variable value does not have the correct dimension");
- return B() * val + c();
- }
-
- linear_variable_t& operator+=(const linear_variable_t& w1)
- {
- if (w1.isZero())
- return *this;
- if(isZero())
- {
- this->B_ = w1.B_;
- zero = w1.isZero();
- }
- else
- {
- this->B_ += w1.B_;
- }
- this->c_ += w1.c_;
- return *this;
- }
- linear_variable_t& operator-=(const linear_variable_t& w1)
- {
- if (w1.isZero())
- return *this;
- if(isZero())
- {
- this->B_ = -w1.B_;
- zero = w1.isZero();
- }
- else
- {
- this->B_ -= w1.B_;
- }
- this->c_ -= w1.c_;
- return *this;
- }
- linear_variable_t& operator/=(const double d)
- {
- B_ /= d;
- c_ /= d;
- return *this;
- }
- linear_variable_t& operator*=(const double d)
- {
- B_ *= d;
- c_ *= d;
- return *this;
- }
-
- static linear_variable_t Zero(size_t dim=0){
- return linear_variable_t(matrix_x_t::Identity(dim,dim), vector_x_t::Zero(dim));
+namespace curves {
+template <typename Numeric = double, bool Safe = true>
+struct linear_variable : public serialization::Serializable {
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
+ typedef linear_variable<Numeric> linear_variable_t;
+
+ linear_variable() : B_(matrix_x_t::Identity(0, 0)), c_(vector_x_t::Zero(0)), zero(true) {} // variable
+ linear_variable(const vector_x_t& c) : B_(matrix_x_t::Zero(c.size(), c.size())), c_(c), zero(false) {} // constant
+ linear_variable(const matrix_x_t& B, const vector_x_t& c) : B_(B), c_(c), zero(false) {} // mixed
+
+ // linear evaluation
+ vector_x_t operator()(const Eigen::Ref<const vector_x_t>& val) const {
+ if (isZero()) return c();
+ if (Safe && B().cols() != val.rows())
+ throw std::length_error("Cannot evaluate linear variable, variable value does not have the correct dimension");
+ return B() * val + c();
+ }
+
+ linear_variable_t& operator+=(const linear_variable_t& w1) {
+ if (w1.isZero()) return *this;
+ if (isZero()) {
+ this->B_ = w1.B_;
+ zero = w1.isZero();
+ } else {
+ this->B_ += w1.B_;
}
-
- std::size_t size() const {return zero ? 0 : std::max(B_.cols(), c_.size()) ;}
-
- Numeric norm() const
- {
- return isZero() ? 0 : (B_.norm() + c_.norm());
+ this->c_ += w1.c_;
+ return *this;
+ }
+ linear_variable_t& operator-=(const linear_variable_t& w1) {
+ if (w1.isZero()) return *this;
+ if (isZero()) {
+ this->B_ = -w1.B_;
+ zero = w1.isZero();
+ } else {
+ this->B_ -= w1.B_;
}
-
- const matrix_x_t& B() const {return B_;}
- const vector_x_t& c () const {return c_;}
- bool isZero () const {return zero;}
-
-
- // 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("B_", B_);
- ar& boost::serialization::make_nvp("c_", c_);
- ar& boost::serialization::make_nvp("zero", zero);
+ this->c_ -= w1.c_;
+ return *this;
+ }
+ linear_variable_t& operator/=(const double d) {
+ B_ /= d;
+ c_ /= d;
+ return *this;
+ }
+ linear_variable_t& operator*=(const double d) {
+ B_ *= d;
+ c_ *= d;
+ return *this;
+ }
+
+ static linear_variable_t Zero(size_t dim = 0) {
+ return linear_variable_t(matrix_x_t::Identity(dim, dim), vector_x_t::Zero(dim));
+ }
+
+ std::size_t size() const { return zero ? 0 : std::max(B_.cols(), c_.size()); }
+
+ Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
+
+ const matrix_x_t& B() const { return B_; }
+ const vector_x_t& c() const { return c_; }
+ bool isZero() const { return zero; }
+
+ // 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 ?
}
-
-private:
- matrix_x_t B_;
- vector_x_t c_;
- bool zero;
+ ar& boost::serialization::make_nvp("B_", B_);
+ ar& boost::serialization::make_nvp("c_", c_);
+ ar& boost::serialization::make_nvp("zero", zero);
+ }
+
+ private:
+ matrix_x_t B_;
+ vector_x_t c_;
+ bool zero;
};
template <typename N, bool S>
-inline linear_variable<N,S> operator+(const linear_variable<N,S>& w1, const linear_variable<N,S>& w2)
-{
- linear_variable<N,S> res(w1.B(), w1.c());
- return res+=w2;
+inline linear_variable<N, S> operator+(const linear_variable<N, S>& w1, const linear_variable<N, S>& w2) {
+ linear_variable<N, S> res(w1.B(), w1.c());
+ return res += w2;
}
template <typename N, bool S>
-linear_variable<N,S> operator-(const linear_variable<N,S>& w1, const linear_variable<N,S>& w2)
-{
- linear_variable<N,S> res(w1.B(), w1.c());
- return res-=w2;
+linear_variable<N, S> operator-(const linear_variable<N, S>& w1, const linear_variable<N, S>& w2) {
+ linear_variable<N, S> res(w1.B(), w1.c());
+ return res -= w2;
}
template <typename N, bool S>
-linear_variable<N,S> operator*(const double k, const linear_variable<N,S>& w){
- linear_variable<N,S> res(w.B(), w.c());
- return res*=k;
+linear_variable<N, S> operator*(const double k, const linear_variable<N, S>& w) {
+ linear_variable<N, S> res(w.B(), w.c());
+ return res *= k;
}
template <typename N, bool S>
-linear_variable<N,S> operator*(const linear_variable<N,S>& w,const double k)
-{
- linear_variable<N,S> res(w.B(), w.c());
- return res*=k;
+linear_variable<N, S> operator*(const linear_variable<N, S>& w, const double k) {
+ linear_variable<N, S> res(w.B(), w.c());
+ return res *= k;
}
template <typename N, bool S>
-linear_variable<N,S> operator/(const linear_variable<N,S>& w,const double k)
-{
- linear_variable<N,S> res(w.B(), w.c());
- return res/=k;
+linear_variable<N, S> operator/(const linear_variable<N, S>& w, const double k) {
+ linear_variable<N, S> res(w.B(), w.c());
+ return res /= k;
}
-template <typename BezierFixed, typename BezierLinear, typename X >
-BezierFixed evaluateLinear(const BezierLinear& bIn, const X x)
-{
- typename BezierFixed::t_point_t fixed_wps;
- for (typename BezierLinear::cit_point_t cit = bIn.waypoints().begin(); cit != bIn.waypoints().end(); ++cit)
- fixed_wps.push_back(cit->operator()(x));
- return BezierFixed(fixed_wps.begin(),fixed_wps.end(), bIn.T_min_,bIn.T_max_);
+template <typename BezierFixed, typename BezierLinear, typename X>
+BezierFixed evaluateLinear(const BezierLinear& bIn, const X x) {
+ typename BezierFixed::t_point_t fixed_wps;
+ for (typename BezierLinear::cit_point_t cit = bIn.waypoints().begin(); cit != bIn.waypoints().end(); ++cit)
+ fixed_wps.push_back(cit->operator()(x));
+ return BezierFixed(fixed_wps.begin(), fixed_wps.end(), bIn.T_min_, bIn.T_max_);
}
-} // namespace curves
-#endif //_CLASS_LINEAR_VARIABLE
-
+} // namespace curves
+#endif //_CLASS_LINEAR_VARIABLE
diff --git a/include/curves/optimization/definitions.h b/include/curves/optimization/definitions.h
index 52143e1..ae5e2f4 100644
--- a/include/curves/optimization/definitions.h
+++ b/include/curves/optimization/definitions.h
@@ -1,97 +1,85 @@
/**
-* \file definitions.h
-* \brief utils for defining optimization problems
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*/
-
+ * \file definitions.h
+ * \brief utils for defining optimization problems
+ * \author Steve T.
+ * \version 0.1
+ * \date 06/17/2013
+ */
#ifndef _CLASS_DEFINITIONS_H
#define _CLASS_DEFINITIONS_H
-
#include <curves/bezier_curve.h>
#include <curves/linear_variable.h>
#include <curves/quadratic_variable.h>
#include <curves/curve_constraint.h>
-
-namespace curves
-{
-namespace optimization
-{
-
-enum constraint_flag{
- INIT_POS = 0x001,
- INIT_VEL = 0x002,
- INIT_ACC = 0x004,
- INIT_JERK = 0x008,
- END_POS = 0x010,
- END_VEL = 0x020,
- END_ACC = 0x040,
- END_JERK = 0x080,
- ALL = 0x0ff,
- NONE = 0x100
+namespace curves {
+namespace optimization {
+
+enum constraint_flag {
+ INIT_POS = 0x001,
+ INIT_VEL = 0x002,
+ INIT_ACC = 0x004,
+ INIT_JERK = 0x008,
+ END_POS = 0x010,
+ END_VEL = 0x020,
+ END_ACC = 0x040,
+ END_JERK = 0x080,
+ ALL = 0x0ff,
+ NONE = 0x100
};
-
-
-template<typename Point, typename Numeric>
-struct quadratic_problem
-{
- Eigen::Matrix<Numeric,Eigen::Dynamic, Eigen::Dynamic> ineqMatrix;
- Eigen::Matrix<Numeric,Eigen::Dynamic, 1> ineqVector;
- quadratic_variable<Numeric> cost;
+template <typename Point, typename Numeric>
+struct quadratic_problem {
+ Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> ineqMatrix;
+ Eigen::Matrix<Numeric, Eigen::Dynamic, 1> ineqVector;
+ quadratic_variable<Numeric> cost;
};
-
-template<typename Point, typename Numeric>
-struct problem_definition : public curve_constraints<Point>
-{
- typedef Point point_t;
- typedef Numeric num_t;
- typedef curve_constraints<point_t> curve_constraints_t;
- typedef Eigen::Matrix< num_t , Eigen::Dynamic , 1> vector_x_t;
- typedef Eigen::Matrix< num_t , Eigen::Dynamic , Eigen::Dynamic> matrix_x_t;
- typedef std::vector<matrix_x_t, Eigen::aligned_allocator<matrix_x_t> > T_matrix_x_t;
- typedef std::vector<vector_x_t, Eigen::aligned_allocator<vector_x_t> > T_vector_x_t;
- typedef typename T_matrix_x_t::const_iterator CIT_matrix_x_t;
- typedef typename T_vector_x_t::const_iterator CIT_vector_x_t;
-
- problem_definition(const int dim)
- : curve_constraints_t(dim)
- , flag(NONE)
- , init_pos(point_t::Zero(dim))
- , end_pos(point_t::Zero(dim))
- , degree(5)
- , totalTime(1.)
- , splitTimes_(vector_x_t::Zero(0))
- , dim_(dim){}
-
- problem_definition(const curve_constraints_t& parent)
- : curve_constraints_t(parent)
- , flag(NONE)
- , init_pos(point_t::Zero(parent.dim_))
- , end_pos(point_t::Zero(parent.dim_))
- , degree(5)
- , totalTime(1.)
- , splitTimes_(vector_x_t::Zero(0))
- , dim_(parent.dim_){}
-
-
- constraint_flag flag;
- point_t init_pos;
- point_t end_pos;
- std::size_t degree;
- num_t totalTime;
- vector_x_t splitTimes_;
- T_matrix_x_t inequalityMatrices_; // must be of size (splitTimes_ + 1)
- T_vector_x_t inequalityVectors_; // must be of size (splitTimes_ + 1)
- const int dim_;
+template <typename Point, typename Numeric>
+struct problem_definition : public curve_constraints<Point> {
+ typedef Point point_t;
+ typedef Numeric num_t;
+ typedef curve_constraints<point_t> curve_constraints_t;
+ typedef Eigen::Matrix<num_t, Eigen::Dynamic, 1> vector_x_t;
+ typedef Eigen::Matrix<num_t, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
+ typedef std::vector<matrix_x_t, Eigen::aligned_allocator<matrix_x_t> > T_matrix_x_t;
+ typedef std::vector<vector_x_t, Eigen::aligned_allocator<vector_x_t> > T_vector_x_t;
+ typedef typename T_matrix_x_t::const_iterator CIT_matrix_x_t;
+ typedef typename T_vector_x_t::const_iterator CIT_vector_x_t;
+
+ problem_definition(const int dim)
+ : curve_constraints_t(dim),
+ flag(NONE),
+ init_pos(point_t::Zero(dim)),
+ end_pos(point_t::Zero(dim)),
+ degree(5),
+ totalTime(1.),
+ splitTimes_(vector_x_t::Zero(0)),
+ dim_(dim) {}
+
+ problem_definition(const curve_constraints_t& parent)
+ : curve_constraints_t(parent),
+ flag(NONE),
+ init_pos(point_t::Zero(parent.dim_)),
+ end_pos(point_t::Zero(parent.dim_)),
+ degree(5),
+ totalTime(1.),
+ splitTimes_(vector_x_t::Zero(0)),
+ dim_(parent.dim_) {}
+
+ constraint_flag flag;
+ point_t init_pos;
+ point_t end_pos;
+ std::size_t degree;
+ num_t totalTime;
+ vector_x_t splitTimes_;
+ T_matrix_x_t inequalityMatrices_; // must be of size (splitTimes_ + 1)
+ T_vector_x_t inequalityVectors_; // must be of size (splitTimes_ + 1)
+ const int dim_;
};
-} // namespace optimization
-} // namespace curves
-#endif //_CLASS_DEFINITIONS_H
-
+} // namespace optimization
+} // namespace curves
+#endif //_CLASS_DEFINITIONS_H
diff --git a/include/curves/optimization/details.h b/include/curves/optimization/details.h
index f3691b6..71ea6bb 100644
--- a/include/curves/optimization/details.h
+++ b/include/curves/optimization/details.h
@@ -1,11 +1,10 @@
/**
-* \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
-*/
-
+ * \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 _CLASS_LINEAR_PROBLEM_DETAILS
#define _CLASS_LINEAR_PROBLEM_DETAILS
@@ -16,348 +15,319 @@
#include <curves/optimization/definitions.h>
#include <curves/bernstein.h>
-namespace curves
-{
-namespace optimization
-{
-template<typename Point, typename Numeric, bool Safe = true>
-struct problem_data
-{
- problem_data(const int dim) : bezier(0), dim_(dim){}
- ~problem_data() {if (bezier) delete bezier;}
-
- typedef linear_variable<Numeric> var_t;
- typedef std::vector<var_t> T_var_t;
- typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > bezier_t;
-
- std::vector<var_t> variables_; // includes constant variables
- std::size_t numVariables; // total number of variable (/ DIM for total size)
- std::size_t numControlPoints; // total number of control Points (variables + waypoints) / DIM )
- std::size_t startVariableIndex; //before that index, variables are constant
- std::size_t numStateConstraints;
- bezier_t* bezier;
- const int dim_;
-
-
-
- problem_data(const problem_data& other)
- : variables_(other.variables_)
- , numVariables(other.numVariables)
- , numControlPoints(other.numControlPoints)
- , startVariableIndex(other.startVariableIndex)
- , numStateConstraints(other.numStateConstraints)
- , dim_(other.dim_)
- {
- const bezier_t& b = *other.bezier;
- bezier = new bezier_t(b.waypoints().begin(), b.waypoints().end(),b.T_min_, b.T_max_, b.mult_T_);
- }
+namespace curves {
+namespace optimization {
+template <typename Point, typename Numeric, bool Safe = true>
+struct problem_data {
+ problem_data(const int dim) : bezier(0), dim_(dim) {}
+ ~problem_data() {
+ if (bezier) delete bezier;
+ }
+
+ typedef linear_variable<Numeric> var_t;
+ typedef std::vector<var_t> T_var_t;
+ typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > bezier_t;
+
+ std::vector<var_t> variables_; // includes constant variables
+ std::size_t numVariables; // total number of variable (/ DIM for total size)
+ std::size_t numControlPoints; // total number of control Points (variables + waypoints) / DIM )
+ std::size_t startVariableIndex; // before that index, variables are constant
+ std::size_t numStateConstraints;
+ bezier_t* bezier;
+ const int dim_;
+
+ problem_data(const problem_data& other)
+ : variables_(other.variables_),
+ numVariables(other.numVariables),
+ numControlPoints(other.numControlPoints),
+ startVariableIndex(other.startVariableIndex),
+ numStateConstraints(other.numStateConstraints),
+ dim_(other.dim_) {
+ const bezier_t& b = *other.bezier;
+ bezier = new bezier_t(b.waypoints().begin(), b.waypoints().end(), b.T_min_, b.T_max_, b.mult_T_);
+ }
};
-inline std::size_t num_active_constraints(const constraint_flag& flag)
-{
- long lValue = (long)(flag);
- std::size_t iCount = 0;
- while (lValue != 0)
- {
- lValue = lValue & (lValue - 1);
- iCount++;
- }
- return (flag & NONE) ? iCount-1 : iCount;
+inline std::size_t num_active_constraints(const constraint_flag& flag) {
+ long lValue = (long)(flag);
+ std::size_t iCount = 0;
+ while (lValue != 0) {
+ lValue = lValue & (lValue - 1);
+ iCount++;
+ }
+ return (flag & NONE) ? iCount - 1 : iCount;
}
template <typename Numeric, typename LinearVar>
-LinearVar fill_with_zeros(const LinearVar& var,const std::size_t i,
- const std::size_t startVariableIndex,
- const std::size_t numVariables, const int Dim)
-{
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
- typename LinearVar::matrix_x_t B;
- B = matrix_t::Zero(Dim,numVariables*Dim);
- if( startVariableIndex <= i && i<= startVariableIndex +numVariables-1 && var.size() >0 )
- B.block(0,Dim*(i-startVariableIndex),Dim,Dim) = var.B();
- return LinearVar (B,var.c());
+LinearVar fill_with_zeros(const LinearVar& var, const std::size_t i, const std::size_t startVariableIndex,
+ const std::size_t numVariables, const int Dim) {
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
+ typename LinearVar::matrix_x_t B;
+ B = matrix_t::Zero(Dim, numVariables * Dim);
+ if (startVariableIndex <= i && i <= startVariableIndex + numVariables - 1 && var.size() > 0)
+ B.block(0, Dim * (i - startVariableIndex), Dim, Dim) = var.B();
+ return LinearVar(B, var.c());
}
-
template <typename Point, typename Numeric, typename Bezier, typename LinearVar>
Bezier* compute_linear_control_points(const problem_data<Point, Numeric>& pData,
- const std::vector<LinearVar>& linearVars, const Numeric totalTime)
-{
- std::vector<LinearVar> res;
- // now need to fill all this with zeros...
- std::size_t totalvar = linearVars.size();
- for (std::size_t i = 0; i < totalvar; ++i)
- res.push_back( fill_with_zeros<Numeric, LinearVar>(linearVars[i],i,
- pData.startVariableIndex,
- pData.numVariables,
- pData.dim_));
- return new Bezier(res.begin(),res.end(), 0., totalTime);
+ const std::vector<LinearVar>& linearVars, const Numeric totalTime) {
+ std::vector<LinearVar> res;
+ // now need to fill all this with zeros...
+ std::size_t totalvar = linearVars.size();
+ for (std::size_t i = 0; i < totalvar; ++i)
+ res.push_back(fill_with_zeros<Numeric, LinearVar>(linearVars[i], i, pData.startVariableIndex, pData.numVariables,
+ pData.dim_));
+ return new Bezier(res.begin(), res.end(), 0., totalTime);
}
-
-template<typename Point,typename Numeric, bool Safe>
-problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition<Point, Numeric>& pDef)
-{
- typedef Numeric num_t;
- typedef Point point_t;
- typedef linear_variable<Numeric> var_t;
- typedef problem_data<Point, Numeric> problem_data_t;
-
- const std::size_t& degree = pDef.degree;
- const constraint_flag& flag = pDef.flag;
-
- const std::size_t numControlPoints = pDef.degree +1;
- const std::size_t numActiveConstraints = num_active_constraints(flag);
- if (numActiveConstraints >= numControlPoints)
- throw std::runtime_error("In setup_control_points; too many constraints for the considered degree");
-
-
- problem_data_t problemData(pDef.dim_);
- typename problem_data_t::T_var_t& variables_ = problemData.variables_;
-
- std::size_t numConstants = 0;
- std::size_t i =0;
- if(flag & INIT_POS)
- {
- variables_.push_back(var_t(pDef.init_pos));
+template <typename Point, typename Numeric, bool Safe>
+problem_data<Point, Numeric, Safe> setup_control_points(const problem_definition<Point, Numeric>& pDef) {
+ typedef Numeric num_t;
+ typedef Point point_t;
+ typedef linear_variable<Numeric> var_t;
+ typedef problem_data<Point, Numeric> problem_data_t;
+
+ const std::size_t& degree = pDef.degree;
+ const constraint_flag& flag = pDef.flag;
+
+ const std::size_t numControlPoints = pDef.degree + 1;
+ const std::size_t numActiveConstraints = num_active_constraints(flag);
+ if (numActiveConstraints >= numControlPoints)
+ throw std::runtime_error("In setup_control_points; too many constraints for the considered degree");
+
+ problem_data_t problemData(pDef.dim_);
+ typename problem_data_t::T_var_t& variables_ = problemData.variables_;
+
+ std::size_t numConstants = 0;
+ std::size_t i = 0;
+ if (flag & INIT_POS) {
+ variables_.push_back(var_t(pDef.init_pos));
+ ++numConstants;
+ ++i;
+ if (flag & INIT_VEL) {
+ point_t vel = pDef.init_pos + (pDef.init_vel / (num_t)degree) / pDef.totalTime;
+ variables_.push_back(var_t(vel));
+ ++numConstants;
+ ++i;
+ if (flag & INIT_ACC) {
+ point_t acc = (pDef.init_acc / (num_t)(degree * (degree - 1))) / (pDef.totalTime * pDef.totalTime) + 2 * vel -
+ pDef.init_pos;
+ ;
+ variables_.push_back(var_t(acc));
++numConstants;
++i;
- if(flag & INIT_VEL)
- {
- point_t vel = pDef.init_pos + (pDef.init_vel / (num_t)degree) / pDef.totalTime;
- variables_.push_back(var_t(vel));
- ++numConstants;
- ++i;
- if(flag & INIT_ACC)
- {
- point_t acc = (pDef.init_acc / (num_t)(degree * (degree-1)))
- / (pDef.totalTime * pDef.totalTime)
- + 2* vel- pDef.init_pos;;
- variables_.push_back(var_t(acc));
- ++numConstants;
- ++i;
- if(flag & INIT_JERK){
- point_t jerk = pDef.init_jerk*pDef.totalTime*pDef.totalTime*pDef.totalTime/(num_t)(degree*(degree-1)*(degree-2))
- + 3*acc -3*vel +pDef.init_pos;
- variables_.push_back(var_t(jerk));
- ++numConstants;
- ++i;
- }
- }
+ if (flag & INIT_JERK) {
+ point_t jerk = pDef.init_jerk * pDef.totalTime * pDef.totalTime * pDef.totalTime /
+ (num_t)(degree * (degree - 1) * (degree - 2)) +
+ 3 * acc - 3 * vel + pDef.init_pos;
+ variables_.push_back(var_t(jerk));
+ ++numConstants;
+ ++i;
}
+ }
}
- const std::size_t first_variable_idx = i;
- // variables
- for(; i + 4< numControlPoints; ++i)
- variables_.push_back(var_t::Zero(pDef.dim_));
- //end constraints
- if(flag & END_POS)
- {
- if(flag & END_VEL)
- {
- point_t vel = pDef.end_pos - (pDef.end_vel / (num_t)degree) / pDef.totalTime;
- if(flag & END_ACC)
- {
- point_t acc = (pDef.end_acc / (num_t)(degree * (degree-1)))
- / (pDef.totalTime) * (pDef.totalTime)
- + 2* vel - pDef.end_pos;
- if(flag & END_JERK){
- point_t jerk = -pDef.end_jerk*pDef.totalTime*pDef.totalTime*pDef.totalTime/(num_t)(degree*(degree-1)*(degree-2))
- + 3*acc -3*vel + pDef.end_pos;
- variables_.push_back(var_t(jerk));
- ++numConstants;
- ++i;
- }else while(i<numControlPoints -3){
- variables_.push_back(var_t::Zero(pDef.dim_));
- ++i;
- }
- variables_.push_back(var_t(acc));
- ++numConstants; ++i;
- }
- else while(i<numControlPoints-2)
- {
- variables_.push_back(var_t::Zero(pDef.dim_));
- ++i;
- }
- variables_.push_back(var_t(vel));
- ++numConstants; ++i;
- }
- else
- {
- while(i<numControlPoints-1)
- {
- variables_.push_back(var_t::Zero(pDef.dim_));
- ++i;
- }
+ }
+ const std::size_t first_variable_idx = i;
+ // variables
+ for (; i + 4 < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
+ // end constraints
+ if (flag & END_POS) {
+ if (flag & END_VEL) {
+ point_t vel = pDef.end_pos - (pDef.end_vel / (num_t)degree) / pDef.totalTime;
+ if (flag & END_ACC) {
+ point_t acc = (pDef.end_acc / (num_t)(degree * (degree - 1))) / (pDef.totalTime) * (pDef.totalTime) + 2 * vel -
+ pDef.end_pos;
+ if (flag & END_JERK) {
+ point_t jerk = -pDef.end_jerk * pDef.totalTime * pDef.totalTime * pDef.totalTime /
+ (num_t)(degree * (degree - 1) * (degree - 2)) +
+ 3 * acc - 3 * vel + pDef.end_pos;
+ variables_.push_back(var_t(jerk));
+ ++numConstants;
+ ++i;
+ } else
+ while (i < numControlPoints - 3) {
+ variables_.push_back(var_t::Zero(pDef.dim_));
+ ++i;
+ }
+ variables_.push_back(var_t(acc));
+ ++numConstants;
+ ++i;
+ } else
+ while (i < numControlPoints - 2) {
+ variables_.push_back(var_t::Zero(pDef.dim_));
+ ++i;
}
- variables_.push_back(var_t(pDef.end_pos));
- ++numConstants; ++i;
- }
- // add remaining variables (only if no end_pos constraints)
- for(; i<numControlPoints; ++i)
+ variables_.push_back(var_t(vel));
+ ++numConstants;
+ ++i;
+ } else {
+ while (i < numControlPoints - 1) {
variables_.push_back(var_t::Zero(pDef.dim_));
-
- assert(numControlPoints > numConstants);
- assert(numControlPoints == variables_.size());
-
-
- problemData.numControlPoints = numControlPoints;
- problemData.numVariables = numControlPoints-numConstants;
- problemData.startVariableIndex =first_variable_idx;
- problemData.numStateConstraints = numActiveConstraints - problemData.numVariables;
- problemData.bezier = compute_linear_control_points<Point, Numeric,
- bezier_curve<Numeric, Numeric, true,var_t>,
- var_t>(problemData, variables_, pDef.totalTime);
- return problemData;
+ ++i;
+ }
+ }
+ variables_.push_back(var_t(pDef.end_pos));
+ ++numConstants;
+ ++i;
+ }
+ // add remaining variables (only if no end_pos constraints)
+ for (; i < numControlPoints; ++i) variables_.push_back(var_t::Zero(pDef.dim_));
+
+ assert(numControlPoints > numConstants);
+ assert(numControlPoints == variables_.size());
+
+ problemData.numControlPoints = numControlPoints;
+ problemData.numVariables = numControlPoints - numConstants;
+ problemData.startVariableIndex = first_variable_idx;
+ problemData.numStateConstraints = numActiveConstraints - problemData.numVariables;
+ problemData.bezier =
+ compute_linear_control_points<Point, Numeric, bezier_curve<Numeric, Numeric, true, var_t>, var_t>(
+ problemData, variables_, pDef.totalTime);
+ return problemData;
}
-
// TODO assumes constant are inside constraints...
-template<typename Point, typename Numeric>
-long compute_num_ineq_control_points
-(const problem_definition<Point, Numeric>& pDef, const problem_data<Point, Numeric> & pData)
-{
- typedef problem_definition<Point, Numeric> problem_definition_t;
- long rows(0);
- // rows depends on each constraint size, and the number of waypoints
- for (typename problem_definition_t::CIT_vector_x_t cit = pDef.inequalityVectors_.begin();
- cit != pDef.inequalityVectors_.end(); ++cit)
- rows += cit->rows() * pData.numControlPoints;
- return rows;
+template <typename Point, typename Numeric>
+long compute_num_ineq_control_points(const problem_definition<Point, Numeric>& pDef,
+ const problem_data<Point, Numeric>& pData) {
+ typedef problem_definition<Point, Numeric> problem_definition_t;
+ long rows(0);
+ // rows depends on each constraint size, and the number of waypoints
+ for (typename problem_definition_t::CIT_vector_x_t cit = pDef.inequalityVectors_.begin();
+ cit != pDef.inequalityVectors_.end(); ++cit)
+ rows += cit->rows() * pData.numControlPoints;
+ return rows;
}
-template<typename Point, typename Numeric>
-std::vector<bezier_curve<Numeric, Numeric, true,
- linear_variable<Numeric> > >
-split(const problem_definition<Point, Numeric>& pDef, problem_data<Point, Numeric> & pData)
-{
- typedef linear_variable<Numeric> linear_variable_t;
- typedef bezier_curve< Numeric, Numeric, true,linear_variable_t> bezier_t;
- typedef std::vector<bezier_t> T_bezier_t;
-
- const Eigen::VectorXd& times = pDef.splitTimes_;
- T_bezier_t res;
- bezier_t& current = *pData.bezier;
- Numeric current_time = 0.;
- Numeric tmp;
- for(int i = 0; i < times.rows(); ++i)
- {
- tmp =times[i];
- std::pair<bezier_t, bezier_t> pairsplit = current.split(tmp-current_time);
- res.push_back(pairsplit.first);
- current = pairsplit.second;
- current_time += tmp-current_time;
- }
- res.push_back(current);
- return res;
+template <typename Point, typename Numeric>
+std::vector<bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > > split(
+ const problem_definition<Point, Numeric>& pDef, problem_data<Point, Numeric>& pData) {
+ typedef linear_variable<Numeric> linear_variable_t;
+ typedef bezier_curve<Numeric, Numeric, true, linear_variable_t> bezier_t;
+ typedef std::vector<bezier_t> T_bezier_t;
+
+ const Eigen::VectorXd& times = pDef.splitTimes_;
+ T_bezier_t res;
+ bezier_t& current = *pData.bezier;
+ Numeric current_time = 0.;
+ Numeric tmp;
+ for (int i = 0; i < times.rows(); ++i) {
+ tmp = times[i];
+ std::pair<bezier_t, bezier_t> pairsplit = current.split(tmp - current_time);
+ res.push_back(pairsplit.first);
+ current = pairsplit.second;
+ current_time += tmp - current_time;
+ }
+ res.push_back(current);
+ return res;
}
-template<typename Point, typename Numeric>
-void initInequalityMatrix
-(const problem_definition<Point, Numeric>& pDef, problem_data<Point, Numeric> & pData,
- quadratic_problem<Point, Numeric>& prob)
-{
- const int& Dim = pData.dim_;
- typedef problem_definition<Point, Numeric> problem_definition_t;
- typedef typename problem_definition_t::matrix_x_t matrix_x_t;
- typedef typename problem_definition_t::vector_x_t vector_x_t;
- typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > bezier_t;
- typedef std::vector<bezier_t> T_bezier_t;
- typedef typename T_bezier_t::const_iterator CIT_bezier_t;
- typedef typename bezier_t::t_point_t t_point;
- typedef typename bezier_t::t_point_t::const_iterator cit_point;
-
- long cols = pData.numVariables * Dim;
- long rows = compute_num_ineq_control_points<Point, Numeric>(pDef, pData);
- prob.ineqMatrix = matrix_x_t::Zero(rows,cols);
- prob.ineqVector = vector_x_t::Zero(rows);
-
- if(pDef.inequalityMatrices_.size() == 0)
- return;
-
- // compute sub-bezier curves
- T_bezier_t beziers = split<Point, Numeric>(pDef,pData);
-
- assert(pDef.inequalityMatrices_.size() == pDef.inequalityVectors_.size());
- assert(pDef.inequalityMatrices_.size() == beziers.size());
-
- long currentRowIdx = 0;
- typename problem_definition_t::CIT_matrix_x_t cmit = pDef.inequalityMatrices_.begin();
- typename problem_definition_t::CIT_vector_x_t cvit = pDef.inequalityVectors_.begin();
- // for each bezier split ..
- for (CIT_bezier_t bit = beziers.begin();
- bit != beziers.end(); ++bit, ++cvit, ++cmit)
- {
- //compute vector of linear expressions of each control point
- const t_point& wps = bit->waypoints();
- // each control has a linear expression depending on all variables
- for(cit_point cit = wps.begin(); cit != wps.end(); ++cit)
- {
- prob.ineqMatrix.block(currentRowIdx, 0,cmit->rows(),cols)
- = (*cmit)*(cit->B()) ; // constraint inequality for current bezier * expression of control point
- prob.ineqVector.segment(currentRowIdx,cmit->rows()) = *cvit - (*cmit)*(cit->c()) ;
- currentRowIdx += cmit->rows();
- }
+template <typename Point, typename Numeric>
+void initInequalityMatrix(const problem_definition<Point, Numeric>& pDef, problem_data<Point, Numeric>& pData,
+ quadratic_problem<Point, Numeric>& prob) {
+ const int& Dim = pData.dim_;
+ typedef problem_definition<Point, Numeric> problem_definition_t;
+ typedef typename problem_definition_t::matrix_x_t matrix_x_t;
+ typedef typename problem_definition_t::vector_x_t vector_x_t;
+ typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > bezier_t;
+ typedef std::vector<bezier_t> T_bezier_t;
+ typedef typename T_bezier_t::const_iterator CIT_bezier_t;
+ typedef typename bezier_t::t_point_t t_point;
+ typedef typename bezier_t::t_point_t::const_iterator cit_point;
+
+ long cols = pData.numVariables * Dim;
+ long rows = compute_num_ineq_control_points<Point, Numeric>(pDef, pData);
+ prob.ineqMatrix = matrix_x_t::Zero(rows, cols);
+ prob.ineqVector = vector_x_t::Zero(rows);
+
+ if (pDef.inequalityMatrices_.size() == 0) return;
+
+ // compute sub-bezier curves
+ T_bezier_t beziers = split<Point, Numeric>(pDef, pData);
+
+ assert(pDef.inequalityMatrices_.size() == pDef.inequalityVectors_.size());
+ assert(pDef.inequalityMatrices_.size() == beziers.size());
+
+ long currentRowIdx = 0;
+ typename problem_definition_t::CIT_matrix_x_t cmit = pDef.inequalityMatrices_.begin();
+ typename problem_definition_t::CIT_vector_x_t cvit = pDef.inequalityVectors_.begin();
+ // for each bezier split ..
+ for (CIT_bezier_t bit = beziers.begin(); bit != beziers.end(); ++bit, ++cvit, ++cmit) {
+ // compute vector of linear expressions of each control point
+ const t_point& wps = bit->waypoints();
+ // each control has a linear expression depending on all variables
+ for (cit_point cit = wps.begin(); cit != wps.end(); ++cit) {
+ prob.ineqMatrix.block(currentRowIdx, 0, cmit->rows(), cols) =
+ (*cmit) * (cit->B()); // constraint inequality for current bezier * expression of control point
+ prob.ineqVector.segment(currentRowIdx, cmit->rows()) = *cvit - (*cmit) * (cit->c());
+ currentRowIdx += cmit->rows();
}
- assert (rows == currentRowIdx); // we filled all the constraints
+ }
+ assert(rows == currentRowIdx); // we filled all the constraints
}
-template<typename Point, typename Numeric, typename In >
-quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In PointsBegin2, In PointsEnd2, const int /*Dim*/)
-{
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
- unsigned int nPoints1 = (unsigned int)(std::distance(PointsBegin1,PointsEnd1)),
- nPoints2 = (unsigned int)(std::distance(PointsBegin2,PointsEnd2));
- assert(nPoints1 > 0); assert(nPoints2 > 0);
- unsigned int deg1 = nPoints1-1, deg2 = nPoints2 -1;
- unsigned int newDeg = (deg1 + deg2);
- // the integral of the primitive will simply be the last control points of the primitive,
- // divided by the degree of the primitive, newDeg. We will store this in matrices for bilinear terms,
- // and a vector for the linear terms, as well as another one for the constants.
- quadratic_variable<Numeric> res(vector_x_t::Zero(PointsBegin1->B().cols()));
- // depending on the index, the fraction coefficient of the bernstein polynom
- // is either the fraction given by (i+j)/ (deg1+deg2), or 1 - (i+j)/ (deg1+deg2).
- // The trick is that the condition is given by whether the current index in
- // the combinatorial is odd or even.
- // time parametrization is not relevant for the cost
-
- Numeric ratio;
- for(unsigned int i = 0; i < newDeg+1; ++i)
- {
- unsigned int j = i > deg2 ? i-deg2 : 0;
- for(; j< std::min(deg1,i)+1;++j)
- {
- ratio = (Numeric)(bin(deg1,j)*bin(deg2,i-j)) / (Numeric)(bin(newDeg,i));
- In itj = PointsBegin1 + j ;
- In iti = PointsBegin2 +(i-j) ;
- res+= ((*itj) * (*iti)) * ratio;
- }
+template <typename Point, typename Numeric, typename In>
+quadratic_variable<Numeric> bezier_product(In PointsBegin1, In PointsEnd1, In PointsBegin2, In PointsEnd2,
+ const int /*Dim*/) {
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> vector_x_t;
+ unsigned int nPoints1 = (unsigned int)(std::distance(PointsBegin1, PointsEnd1)),
+ nPoints2 = (unsigned int)(std::distance(PointsBegin2, PointsEnd2));
+ assert(nPoints1 > 0);
+ assert(nPoints2 > 0);
+ unsigned int deg1 = nPoints1 - 1, deg2 = nPoints2 - 1;
+ unsigned int newDeg = (deg1 + deg2);
+ // the integral of the primitive will simply be the last control points of the primitive,
+ // divided by the degree of the primitive, newDeg. We will store this in matrices for bilinear terms,
+ // and a vector for the linear terms, as well as another one for the constants.
+ quadratic_variable<Numeric> res(vector_x_t::Zero(PointsBegin1->B().cols()));
+ // depending on the index, the fraction coefficient of the bernstein polynom
+ // is either the fraction given by (i+j)/ (deg1+deg2), or 1 - (i+j)/ (deg1+deg2).
+ // The trick is that the condition is given by whether the current index in
+ // the combinatorial is odd or even.
+ // time parametrization is not relevant for the cost
+
+ Numeric ratio;
+ for (unsigned int i = 0; i < newDeg + 1; ++i) {
+ unsigned int j = i > deg2 ? i - deg2 : 0;
+ for (; j < std::min(deg1, i) + 1; ++j) {
+ ratio = (Numeric)(bin(deg1, j) * bin(deg2, i - j)) / (Numeric)(bin(newDeg, i));
+ In itj = PointsBegin1 + j;
+ In iti = PointsBegin2 + (i - j);
+ res += ((*itj) * (*iti)) * ratio;
}
- return res/(newDeg+1);
+ }
+ return res / (newDeg + 1);
}
-inline constraint_flag operator~(constraint_flag a)
-{return static_cast<constraint_flag>(~static_cast<const int>(a));}
-
-inline constraint_flag operator|(constraint_flag a, constraint_flag b)
-{return static_cast<constraint_flag>(static_cast<const int>(a) | static_cast<const int>(b));}
+inline constraint_flag operator~(constraint_flag a) {
+ return static_cast<constraint_flag>(~static_cast<const int>(a));
+}
-inline constraint_flag operator&(constraint_flag a, constraint_flag b)
-{return static_cast<constraint_flag>(static_cast<const int>(a) & static_cast<const int>(b));}
+inline constraint_flag operator|(constraint_flag a, constraint_flag b) {
+ return static_cast<constraint_flag>(static_cast<const int>(a) | static_cast<const int>(b));
+}
-inline constraint_flag operator^(constraint_flag a, constraint_flag b)
-{return static_cast<constraint_flag>(static_cast<const int>(a) ^ static_cast<const int>(b));}
+inline constraint_flag operator&(constraint_flag a, constraint_flag b) {
+ return static_cast<constraint_flag>(static_cast<const int>(a) & static_cast<const int>(b));
+}
-inline constraint_flag& operator|=(constraint_flag& a, constraint_flag b)
-{return (constraint_flag&)((int&)(a) |= static_cast<const int>(b));}
+inline constraint_flag operator^(constraint_flag a, constraint_flag b) {
+ return static_cast<constraint_flag>(static_cast<const int>(a) ^ static_cast<const int>(b));
+}
-inline constraint_flag& operator&=(constraint_flag& a, constraint_flag b)
-{return (constraint_flag&)((int&)(a) &= static_cast<const int>(b));}
+inline constraint_flag& operator|=(constraint_flag& a, constraint_flag b) {
+ return (constraint_flag&)((int&)(a) |= static_cast<const int>(b));
+}
-inline constraint_flag& operator^=(constraint_flag& a, constraint_flag b)
-{return (constraint_flag&)((int&)(a) ^= static_cast<const int>(b));}
+inline constraint_flag& operator&=(constraint_flag& a, constraint_flag b) {
+ return (constraint_flag&)((int&)(a) &= static_cast<const int>(b));
+}
-} // namespace optimization
-} // namespace curves
-#endif //_CLASS_LINEAR_PROBLEM_DETAILS
+inline constraint_flag& operator^=(constraint_flag& a, constraint_flag b) {
+ return (constraint_flag&)((int&)(a) ^= static_cast<const int>(b));
+}
+} // namespace optimization
+} // namespace curves
+#endif //_CLASS_LINEAR_PROBLEM_DETAILS
diff --git a/include/curves/optimization/integral_cost.h b/include/curves/optimization/integral_cost.h
index 4ce6650..28f9207 100644
--- a/include/curves/optimization/integral_cost.h
+++ b/include/curves/optimization/integral_cost.h
@@ -1,11 +1,10 @@
/**
-* \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
-*/
-
+ * \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 _CLASS_QUADRATIC_COST
#define _CLASS_QUADRATIC_COST
@@ -15,43 +14,36 @@
#include <Eigen/Core>
-namespace curves
-{
-namespace optimization
-{
-
+namespace curves {
+namespace optimization {
-enum integral_cost_flag{
- DISTANCE = 0x000,
- VELOCITY = 0x001,
- ACCELERATION = 0x002,
- JERK = 0x003,
- FOURTH = 0x004,
- FIFTH = 0x005
+enum integral_cost_flag {
+ DISTANCE = 0x000,
+ VELOCITY = 0x001,
+ ACCELERATION = 0x002,
+ JERK = 0x003,
+ FOURTH = 0x004,
+ FIFTH = 0x005
};
-template<typename Point, typename Numeric>
-quadratic_variable<Numeric> compute_integral_cost_internal
- (const problem_data<Point, Numeric>& pData, const std::size_t num_derivate)
-{
- typedef bezier_curve<Numeric, Numeric, true,linear_variable<Numeric> > bezier_t;
- typedef typename bezier_t::t_point_t t_point_t;
- typedef typename t_point_t::const_iterator cit_point_t;
- bezier_t acc = pData.bezier->compute_derivate(num_derivate);
- const t_point_t& wps = acc.waypoints();
- return bezier_product<Point, Numeric, cit_point_t>
- (wps.begin(),wps.end(),wps.begin(),wps.end(), pData.dim_);
+template <typename Point, typename Numeric>
+quadratic_variable<Numeric> compute_integral_cost_internal(const problem_data<Point, Numeric>& pData,
+ const std::size_t num_derivate) {
+ typedef bezier_curve<Numeric, Numeric, true, linear_variable<Numeric> > bezier_t;
+ typedef typename bezier_t::t_point_t t_point_t;
+ typedef typename t_point_t::const_iterator cit_point_t;
+ bezier_t acc = pData.bezier->compute_derivate(num_derivate);
+ const t_point_t& wps = acc.waypoints();
+ return bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_);
}
-template<typename Point, typename Numeric>
-quadratic_variable<Numeric> compute_integral_cost
- (const problem_data<Point, Numeric>& pData, const integral_cost_flag flag)
-{
- std::size_t size = (std::size_t)(flag) ;
- return compute_integral_cost_internal<Point,Numeric>(pData, size);
+template <typename Point, typename Numeric>
+quadratic_variable<Numeric> compute_integral_cost(const problem_data<Point, Numeric>& pData,
+ const integral_cost_flag flag) {
+ std::size_t size = (std::size_t)(flag);
+ return compute_integral_cost_internal<Point, Numeric>(pData, size);
}
-} // namespace optimization
-} // namespace curves
-#endif //_CLASS_QUADRATIC_COST
-
+} // namespace optimization
+} // namespace curves
+#endif //_CLASS_QUADRATIC_COST
diff --git a/include/curves/optimization/quadratic_problem.h b/include/curves/optimization/quadratic_problem.h
index 3e520e7..fb1a582 100644
--- a/include/curves/optimization/quadratic_problem.h
+++ b/include/curves/optimization/quadratic_problem.h
@@ -1,11 +1,10 @@
/**
-* \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
-*/
-
+ * \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 _CLASS_LINEAR_PROBLEM
#define _CLASS_LINEAR_PROBLEM
@@ -16,33 +15,28 @@
#include <Eigen/Core>
-namespace curves
-{
-namespace optimization
-{
-
-template<typename Point, typename Numeric, bool Safe>
-quadratic_problem<Point, Numeric> generate_problem
- (const problem_definition<Point, Numeric>& pDef, const quadratic_variable<Numeric>& cost)
-{
- quadratic_problem<Point, Numeric> prob;
- problem_data<Point, Numeric> pData = setup_control_points<Point, Numeric, Safe>(pDef);
- initInequalityMatrix<Point, Numeric>(pDef,pData,prob);
- prob.cost = cost;
- return prob;
+namespace curves {
+namespace optimization {
+
+template <typename Point, typename Numeric, bool Safe>
+quadratic_problem<Point, Numeric> generate_problem(const problem_definition<Point, Numeric>& pDef,
+ const quadratic_variable<Numeric>& cost) {
+ quadratic_problem<Point, Numeric> prob;
+ problem_data<Point, Numeric> pData = setup_control_points<Point, Numeric, Safe>(pDef);
+ initInequalityMatrix<Point, Numeric>(pDef, pData, prob);
+ prob.cost = cost;
+ return prob;
}
-template<typename Point, typename Numeric, bool Safe>
-quadratic_problem<Point, Numeric> generate_problem
- (const problem_definition<Point, Numeric>& pDef, const integral_cost_flag costFlag)
-{
- quadratic_problem<Point, Numeric> prob;
- problem_data<Point, Numeric> pData = setup_control_points<Point, Numeric, Safe>(pDef);
- initInequalityMatrix<Point, Numeric>(pDef,pData,prob);
- prob.cost = compute_integral_cost<Point, Numeric>(pData, costFlag);
- return prob;
+template <typename Point, typename Numeric, bool Safe>
+quadratic_problem<Point, Numeric> generate_problem(const problem_definition<Point, Numeric>& pDef,
+ const integral_cost_flag costFlag) {
+ quadratic_problem<Point, Numeric> prob;
+ problem_data<Point, Numeric> pData = setup_control_points<Point, Numeric, Safe>(pDef);
+ initInequalityMatrix<Point, Numeric>(pDef, pData, prob);
+ prob.cost = compute_integral_cost<Point, Numeric>(pData, costFlag);
+ return prob;
}
-} // namespace optimization
-} // namespace curves
-#endif //_CLASS_LINEAR_PROBLEM
-
+} // namespace optimization
+} // namespace curves
+#endif //_CLASS_LINEAR_PROBLEM
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index 7734a9c..6601846 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -157,23 +157,16 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
return isContinuous;
}
- std::size_t num_curves() const
- {
- return curves_.size();
- }
+ std::size_t num_curves() const { return curves_.size(); }
- const curve_t& curve_at_time(const time_t t) const
- {
- return curves_[find_interval(t)];
- }
+ const curve_t& curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
- const curve_t& curve_at_index(const std::size_t idx) const
- {
- if (Safe && idx >= num_curves())
- {
- throw std::length_error("curve_at_index: requested index greater than number of curves in piecewise_curve instance");
- }
- return curves_[idx];
+ const curve_t& curve_at_index(const std::size_t idx) const {
+ if (Safe && idx >= num_curves()) {
+ throw std::length_error(
+ "curve_at_index: requested index greater than number of curves in piecewise_curve instance");
+ }
+ return curves_[idx];
}
template <typename Bezier>
diff --git a/include/curves/quadratic_variable.h b/include/curves/quadratic_variable.h
index aa1322e..2bc9897 100644
--- a/include/curves/quadratic_variable.h
+++ b/include/curves/quadratic_variable.h
@@ -1,11 +1,10 @@
/**
-* \file quadratic_variable.h
-* \brief storage for variable points of the form p_i = x' A_i x + B_i x + c_i
-* \author Steve T.
-* \version 0.1
-* \date 07/02/2019
-*/
-
+ * \file quadratic_variable.h
+ * \brief storage for variable points of the form p_i = x' A_i x + B_i x + c_i
+ * \author Steve T.
+ * \version 0.1
+ * \date 07/02/2019
+ */
#ifndef _CLASS_QUADRATIC_VARIABLE
#define _CLASS_QUADRATIC_VARIABLE
@@ -20,190 +19,165 @@
#include <Eigen/Core>
#include <stdexcept>
-namespace curves
-{
-
-template <typename Numeric=double>
-struct quadratic_variable
-{
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
- typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> point_t;
- typedef quadratic_variable<Numeric> quadratic_variable_t;
-
- quadratic_variable()
- {
- c_ = 0.;
- b_ = point_t::Zero(1);
- A_ = matrix_x_t::Zero(1,1);
- zero = true;
- }
-
- quadratic_variable(const matrix_x_t& A, const point_t& b, const Numeric c = 0):
- c_(c),
- b_(b),
- A_(A),
- zero(false){assert(A.cols() == b.rows() && A.cols() == A.rows());}
-
- quadratic_variable(const point_t& b, const Numeric c = 0):
- c_(c),
- b_(b),
- A_(matrix_x_t::Zero((int)(b.rows()),(int)(b.rows()))),
- zero(false){}
-
- static quadratic_variable_t Zero(size_t dim=0){
- return quadratic_variable_t();
+namespace curves {
+
+template <typename Numeric = double>
+struct quadratic_variable {
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> point_t;
+ typedef quadratic_variable<Numeric> quadratic_variable_t;
+
+ quadratic_variable() {
+ c_ = 0.;
+ b_ = point_t::Zero(1);
+ A_ = matrix_x_t::Zero(1, 1);
+ zero = true;
+ }
+
+ quadratic_variable(const matrix_x_t& A, const point_t& b, const Numeric c = 0) : c_(c), b_(b), A_(A), zero(false) {
+ assert(A.cols() == b.rows() && A.cols() == A.rows());
+ }
+
+ quadratic_variable(const point_t& b, const Numeric c = 0)
+ : c_(c), b_(b), A_(matrix_x_t::Zero((int)(b.rows()), (int)(b.rows()))), zero(false) {}
+
+ static quadratic_variable_t Zero(size_t dim = 0) { return quadratic_variable_t(); }
+
+ // linear evaluation
+ Numeric operator()(const Eigen::Ref<const point_t>& val) const {
+ assert(!isZero());
+ return val.transpose() * A() * val + b().transpose() * val + c();
+ }
+
+ quadratic_variable& operator+=(const quadratic_variable& w1) {
+ if (w1.isZero()) return *this;
+ if (isZero()) {
+ this->A_ = w1.A_;
+ this->b_ = w1.b_;
+ this->c_ = w1.c_;
+ zero = false;
+ } else {
+ this->A_ += w1.A_;
+ this->b_ += w1.b_;
+ this->c_ += w1.c_;
}
-
- // linear evaluation
- Numeric operator()(const Eigen::Ref<const point_t>& val) const
- {
- assert(!isZero());
- return val.transpose() * A() * val + b().transpose() * val + c();
+ return *this;
+ }
+ quadratic_variable& operator-=(const quadratic_variable& w1) {
+ if (w1.isZero()) return *this;
+ if (isZero()) {
+ this->A_ = -w1.A_;
+ this->b_ = -w1.b_;
+ this->c_ = -w1.c_;
+ zero = false;
+ } else {
+ this->A_ -= w1.A_;
+ this->b_ -= w1.b_;
+ this->c_ -= w1.c_;
}
-
-
- quadratic_variable& operator+=(const quadratic_variable& w1)
- {
- if (w1.isZero())
- return *this;
- if(isZero())
- {
- this->A_ = w1.A_;
- this->b_ = w1.b_;
- this->c_ = w1.c_;
- zero = false;
- }
- else
- {
- this->A_ += w1.A_;
- this->b_ += w1.b_;
- this->c_ += w1.c_;
- }
- return *this;
+ return *this;
+ }
+
+ quadratic_variable& operator/=(const double d) {
+ // handling zero case
+ if (!isZero()) {
+ this->A_ /= d;
+ this->b_ /= d;
+ this->c_ /= d;
}
- quadratic_variable& operator-=(const quadratic_variable& w1)
- {
- if (w1.isZero())
- return *this;
- if(isZero())
- {
- this->A_ = -w1.A_;
- this->b_ = -w1.b_;
- this->c_ = -w1.c_;
- zero = false;
- }
- else
- {
- this->A_ -= w1.A_;
- this->b_ -= w1.b_;
- this->c_ -= w1.c_;
- }
- return *this;
+ return *this;
+ }
+ quadratic_variable& operator*=(const double d) {
+ // handling zero case
+ if (!isZero()) {
+ this->A_ *= d;
+ this->b_ *= d;
+ this->c_ *= d;
}
-
- quadratic_variable& operator/=(const double d)
- {
- // handling zero case
- if(!isZero())
- {
- this->A_ /= d;
- this->b_ /= d;
- this->c_ /= d;
- }
- return *this;
- }
- quadratic_variable& operator*=(const double d)
- {
- // handling zero case
- if(!isZero())
- {
- this->A_ *= d;
- this->b_ *= d;
- this->c_ *= d;
- }
- return *this;
- }
-
- const matrix_x_t& A() const {assert(!isZero()); return A_;}
- const point_t& b () const {assert(!isZero());return b_;}
- const Numeric c () const {assert(!isZero());return c_;}
- bool isZero() const {return zero;}
- std::size_t size() const {return zero ? 0 : std::max(A_.cols(), (std::max(b_.cols(), c_.size()))) ;}
-
-private:
- Numeric c_;
- point_t b_;
- matrix_x_t A_;
- bool zero;
+ return *this;
+ }
+
+ const matrix_x_t& A() const {
+ assert(!isZero());
+ return A_;
+ }
+ const point_t& b() const {
+ assert(!isZero());
+ return b_;
+ }
+ const Numeric c() const {
+ assert(!isZero());
+ return c_;
+ }
+ bool isZero() const { return zero; }
+ std::size_t size() const { return zero ? 0 : std::max(A_.cols(), (std::max(b_.cols(), c_.size()))); }
+
+ private:
+ Numeric c_;
+ point_t b_;
+ matrix_x_t A_;
+ bool zero;
};
-
/// \brief Transforms a vector into a diagonal matrix
template <typename N>
-Eigen::Matrix<N,Eigen::Dynamic,Eigen::Dynamic>
- to_diagonal(const Eigen::Ref<const Eigen::Matrix<N,Eigen::Dynamic,1> > vec)
-{
- typedef typename Eigen::Matrix<N,Eigen::Dynamic,Eigen::Dynamic> matrix_t;
- return vec.asDiagonal();
- matrix_t res(matrix_t::Zero(vec.rows(),vec.rows()));
- for(int i =0; i<vec.rows(); ++i)
- res(i,i) = vec(i);
- return res;
+Eigen::Matrix<N, Eigen::Dynamic, Eigen::Dynamic> to_diagonal(
+ const Eigen::Ref<const Eigen::Matrix<N, Eigen::Dynamic, 1> > vec) {
+ typedef typename Eigen::Matrix<N, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
+ return vec.asDiagonal();
+ matrix_t res(matrix_t::Zero(vec.rows(), vec.rows()));
+ for (int i = 0; i < vec.rows(); ++i) res(i, i) = vec(i);
+ return res;
}
// only works with diagonal linear variables
template <typename N>
-inline quadratic_variable<N> operator*(const linear_variable<N>& w1, const linear_variable<N>& w2)
-{
- typedef quadratic_variable<N> quad_var_t;
- typedef linear_variable<N> lin_var_t;
- typedef typename quad_var_t::matrix_x_t matrix_x_t;
- typedef typename quad_var_t::point_t point_t;
- typedef typename lin_var_t::vector_x_t point_dim_t;
- point_dim_t ones = point_dim_t::Ones(w1.c().size());
- point_t b1 = w1.B().transpose()*ones, b2 = w2.B().transpose()*ones;
- matrix_x_t B1 = to_diagonal<N>(b1);
- matrix_x_t B2 = to_diagonal<N>(b2); //b1.array().square()
- // omitting all transposes since A matrices are diagonals
- matrix_x_t A = B1.transpose() * B2;
- point_t b = w1.c().transpose() * w2.B() + w2.c().transpose() * w1.B();
- N c = w1.c().transpose() * w2.c();
- return quad_var_t(A,b,c);
+inline quadratic_variable<N> operator*(const linear_variable<N>& w1, const linear_variable<N>& w2) {
+ typedef quadratic_variable<N> quad_var_t;
+ typedef linear_variable<N> lin_var_t;
+ typedef typename quad_var_t::matrix_x_t matrix_x_t;
+ typedef typename quad_var_t::point_t point_t;
+ typedef typename lin_var_t::vector_x_t point_dim_t;
+ point_dim_t ones = point_dim_t::Ones(w1.c().size());
+ point_t b1 = w1.B().transpose() * ones, b2 = w2.B().transpose() * ones;
+ matrix_x_t B1 = to_diagonal<N>(b1);
+ matrix_x_t B2 = to_diagonal<N>(b2); // b1.array().square()
+ // omitting all transposes since A matrices are diagonals
+ matrix_x_t A = B1.transpose() * B2;
+ point_t b = w1.c().transpose() * w2.B() + w2.c().transpose() * w1.B();
+ N c = w1.c().transpose() * w2.c();
+ return quad_var_t(A, b, c);
}
-
template <typename N>
-inline quadratic_variable<N> operator+(const quadratic_variable<N>& w1, const quadratic_variable<N>& w2)
-{
- quadratic_variable<N> res(w1.A(),w1.b(), w1.c());
- return res+=w2;
+inline quadratic_variable<N> operator+(const quadratic_variable<N>& w1, const quadratic_variable<N>& w2) {
+ quadratic_variable<N> res(w1.A(), w1.b(), w1.c());
+ return res += w2;
}
template <typename N>
-quadratic_variable<N> operator-(const quadratic_variable<N>& w1, const quadratic_variable<N>& w2)
-{
- quadratic_variable<N> res(w1.A(),w1.b(), w1.c());
- return res-=w2;
+quadratic_variable<N> operator-(const quadratic_variable<N>& w1, const quadratic_variable<N>& w2) {
+ quadratic_variable<N> res(w1.A(), w1.b(), w1.c());
+ return res -= w2;
}
template <typename N>
-quadratic_variable<N> operator*(const double k, const quadratic_variable<N>& w){
- quadratic_variable<N> res(w.A(),w.b(), w.c());
- return res*=k;
+quadratic_variable<N> operator*(const double k, const quadratic_variable<N>& w) {
+ quadratic_variable<N> res(w.A(), w.b(), w.c());
+ return res *= k;
}
template <typename N>
-quadratic_variable<N> operator*(const quadratic_variable<N>& w,const double k){
- quadratic_variable<N> res(w.A(),w.b(), w.c());
- return res*=k;
+quadratic_variable<N> operator*(const quadratic_variable<N>& w, const double k) {
+ quadratic_variable<N> res(w.A(), w.b(), w.c());
+ return res *= k;
}
template <typename N>
-quadratic_variable<N> operator/(const quadratic_variable<N>& w,const double k){
- quadratic_variable<N> res(w.A(),w.b(), w.c());
- return res/=k;
+quadratic_variable<N> operator/(const quadratic_variable<N>& w, const double k) {
+ quadratic_variable<N> res(w.A(), w.b(), w.c());
+ return res /= k;
}
-} // namespace curves
-#endif //_CLASS_QUADRATIC_VARIABLE
-
+} // namespace curves
+#endif //_CLASS_QUADRATIC_VARIABLE
diff --git a/include/curves/se3_curve.h b/include/curves/se3_curve.h
index b4af84b..98ebf36 100644
--- a/include/curves/se3_curve.h
+++ b/include/curves/se3_curve.h
@@ -1,7 +1,6 @@
#ifndef _STRUCT_SE3_CURVE_H
#define _STRUCT_SE3_CURVE_H
-
#include "MathDefs.h"
#include "curve_abc.h"
#include "so3_linear.h"
@@ -9,216 +8,208 @@
#include <boost/math/constants/constants.hpp>
#include <Eigen/Dense>
-namespace curves
-{
-
- /// \class SE3Curve.
- /// \brief Composition of a curve of any type of dimension 3 and a curve representing an rotation
- /// (in current implementation, only SO3Linear can be used for the rotation part)
- /// The output is a vector of size 7 (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w)
- /// The output of the derivative of any order is a vector of size 6 (linear_x,linear_y,linear_z,angular_x,angular_y,angular_z)
+namespace curves {
+
+/// \class SE3Curve.
+/// \brief Composition of a curve of any type of dimension 3 and a curve representing an rotation
+/// (in current implementation, only SO3Linear can be used for the rotation part)
+/// The output is a vector of size 7 (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w)
+/// The output of the derivative of any order is a vector of size 6
+/// (linear_x,linear_y,linear_z,angular_x,angular_y,angular_z)
+///
+///
+template <typename Time = double, typename Numeric = Time, bool Safe = false>
+struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric, 3, Eigen::Affine>,
+ Eigen::Matrix<Numeric, 6, 1> > {
+ typedef Numeric Scalar;
+ typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
+ typedef transform_t point_t;
+ typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
+ typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
+ typedef Eigen::Matrix<Scalar, -1, 1> pointX_t;
+ typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
+ typedef Eigen::Quaternion<Scalar> Quaternion;
+ typedef Time time_t;
+ typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; // parent class
+ typedef curve_abc<Time, Numeric, Safe, pointX_t> curve_X_t; // generic class of curve
+ typedef curve_abc<Time, Numeric, Safe, matrix3_t, point3_t>
+ curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
+ typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
+ typedef polynomial<Time, Numeric, Safe, pointX_t> polynomial_t;
+ typedef SE3Curve<Time, Numeric, Safe> SE3Curve_t;
+
+ public:
+ /* Constructors - destructors */
+ /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
- ///
- template<typename Time= double, typename Numeric=Time, bool Safe=false>
- struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric,3,Eigen::Affine>,Eigen::Matrix<Numeric,6, 1> >
- {
- typedef Numeric Scalar;
- typedef Eigen::Transform<Numeric,3,Eigen::Affine> transform_t;
- typedef transform_t point_t;
- typedef Eigen::Matrix<Scalar,6, 1> point_derivate_t;
- typedef Eigen::Matrix<Scalar,3, 1> point3_t;
- typedef Eigen::Matrix<Scalar, -1, 1> pointX_t;
- typedef Eigen::Matrix<Scalar,3, 3> matrix3_t;
- typedef Eigen::Quaternion<Scalar> Quaternion;
- typedef Time time_t;
- typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_abc_t; // parent class
- typedef curve_abc<Time, Numeric, Safe, pointX_t> curve_X_t; // generic class of curve
- typedef curve_abc<Time, Numeric, Safe, matrix3_t,point3_t> curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
- typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
- typedef polynomial <Time, Numeric, Safe, pointX_t> polynomial_t;
- typedef SE3Curve<Time, Numeric, Safe> SE3Curve_t;
-
-
- public:
- /* Constructors - destructors */
- /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
- ///
- SE3Curve()
- : curve_abc_t(), dim_(3),translation_curve_(NULL),rotation_curve_ (NULL), T_min_(0), T_max_(0)
- { }
-
- /// \brief Destructor
- ~SE3Curve()
- {
- // should we delete translation_curve and rotation_curve here ?
- // better switch to shared ptr
- }
-
-
- /* Constructor without curve object for the translation : */
- /// \brief Constructor from init/end transform use polynomial of degree 1 for position and SO3Linear for rotation
- SE3Curve(const transform_t& init_transform, const transform_t& end_transform,const time_t& t_min, const time_t& t_max)
+ SE3Curve() : curve_abc_t(), dim_(3), translation_curve_(NULL), rotation_curve_(NULL), T_min_(0), T_max_(0) {}
+
+ /// \brief Destructor
+ ~SE3Curve() {
+ // should we delete translation_curve and rotation_curve here ?
+ // better switch to shared ptr
+ }
+
+ /* Constructor without curve object for the translation : */
+ /// \brief Constructor from init/end transform use polynomial of degree 1 for position and SO3Linear for rotation
+ SE3Curve(const transform_t& init_transform, const transform_t& end_transform, const time_t& t_min,
+ const time_t& t_max)
: curve_abc_t(),
dim_(6),
- translation_curve_(new polynomial_t(pointX_t(init_transform.translation()),pointX_t(end_transform.translation()),t_min,t_max)),
- rotation_curve_(new SO3Linear_t(init_transform.rotation(),end_transform.rotation(),t_min,t_max)),
- T_min_(t_min), T_max_(t_max)
- {
- safe_check();
- }
-
- /// \brief Constructor from init/end pose, with quaternion. use polynomial of degree 1 for position and SO3Linear for rotation
- SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const Quaternion& init_rot, const Quaternion& end_rot,const time_t& t_min, const time_t& t_max)
+ translation_curve_(new polynomial_t(pointX_t(init_transform.translation()),
+ pointX_t(end_transform.translation()), t_min, t_max)),
+ rotation_curve_(new SO3Linear_t(init_transform.rotation(), end_transform.rotation(), t_min, t_max)),
+ T_min_(t_min),
+ T_max_(t_max) {
+ safe_check();
+ }
+
+ /// \brief Constructor from init/end pose, with quaternion. use polynomial of degree 1 for position and SO3Linear for
+ /// rotation
+ SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const Quaternion& init_rot, const Quaternion& end_rot,
+ const time_t& t_min, const time_t& t_max)
: curve_abc_t(),
- dim_(6),translation_curve_(new polynomial_t(init_pos,end_pos,t_min,t_max)),
- rotation_curve_(new SO3Linear_t(init_rot,end_rot,t_min,t_max)),
- T_min_(t_min), T_max_(t_max)
- {
- safe_check();
- }
-
- /// \brief Constructor from init/end pose, with rotation matrix. use polynomial of degree 1 for position and SO3Linear for rotation
- SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const matrix3_t& init_rot, const matrix3_t& end_rot,const time_t& t_min, const time_t& t_max)
+ dim_(6),
+ translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
+ rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
+ T_min_(t_min),
+ T_max_(t_max) {
+ safe_check();
+ }
+
+ /// \brief Constructor from init/end pose, with rotation matrix. use polynomial of degree 1 for position and
+ /// SO3Linear for rotation
+ SE3Curve(const pointX_t& init_pos, const pointX_t& end_pos, const matrix3_t& init_rot, const matrix3_t& end_rot,
+ const time_t& t_min, const time_t& t_max)
: curve_abc_t(),
- dim_(6),translation_curve_(new polynomial_t(init_pos,end_pos,t_min,t_max)),
- rotation_curve_(new SO3Linear_t(init_rot,end_rot,t_min,t_max)),
- T_min_(t_min), T_max_(t_max)
- {
- safe_check();
- }
-
- /* Constructor with curve object for the translation : */
- /// \brief Constructor from curve for the translation and init/end rotation, with quaternion.
- /// Use SO3Linear for rotation with the same time bounds as the
- SE3Curve(curve_X_t* translation_curve,const Quaternion& init_rot, const Quaternion& end_rot)
+ dim_(6),
+ translation_curve_(new polynomial_t(init_pos, end_pos, t_min, t_max)),
+ rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
+ T_min_(t_min),
+ T_max_(t_max) {
+ safe_check();
+ }
+
+ /* Constructor with curve object for the translation : */
+ /// \brief Constructor from curve for the translation and init/end rotation, with quaternion.
+ /// Use SO3Linear for rotation with the same time bounds as the
+ SE3Curve(curve_X_t* translation_curve, const Quaternion& init_rot, const Quaternion& end_rot)
: curve_abc_t(),
- dim_(6),translation_curve_(translation_curve),
- rotation_curve_(new SO3Linear_t(init_rot,end_rot,translation_curve->min(),translation_curve->max())),
- T_min_(translation_curve->min()), T_max_(translation_curve->max())
- {
- safe_check();
- }
- /// \brief Constructor from curve for the translation and init/end rotation, with rotation matrix.
- /// Use SO3Linear for rotation with the same time bounds as the
- SE3Curve(curve_X_t* translation_curve,const matrix3_t& init_rot, const matrix3_t& end_rot)
+ dim_(6),
+ translation_curve_(translation_curve),
+ rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())),
+ T_min_(translation_curve->min()),
+ T_max_(translation_curve->max()) {
+ safe_check();
+ }
+ /// \brief Constructor from curve for the translation and init/end rotation, with rotation matrix.
+ /// Use SO3Linear for rotation with the same time bounds as the
+ SE3Curve(curve_X_t* translation_curve, const matrix3_t& init_rot, const matrix3_t& end_rot)
: curve_abc_t(),
- dim_(6),translation_curve_(translation_curve),
- rotation_curve_(new SO3Linear_t(init_rot,end_rot,translation_curve->min(),translation_curve->max())),
- T_min_(translation_curve->min()), T_max_(translation_curve->max())
- {
- safe_check();
- }
-
- /* Constructor from translation and rotation curves object : */
- /// \brief Constructor from from translation and rotation curves object
- SE3Curve(curve_X_t* translation_curve,curve_rotation_t* rotation_curve)
+ dim_(6),
+ translation_curve_(translation_curve),
+ rotation_curve_(new SO3Linear_t(init_rot, end_rot, translation_curve->min(), translation_curve->max())),
+ T_min_(translation_curve->min()),
+ T_max_(translation_curve->max()) {
+ safe_check();
+ }
+
+ /* Constructor from translation and rotation curves object : */
+ /// \brief Constructor from from translation and rotation curves object
+ SE3Curve(curve_X_t* translation_curve, curve_rotation_t* rotation_curve)
: curve_abc_t(),
- dim_(6),translation_curve_(translation_curve),
+ dim_(6),
+ translation_curve_(translation_curve),
rotation_curve_(rotation_curve),
- T_min_(translation_curve->min()), T_max_(translation_curve->max())
- {
- if(translation_curve->dim() != 3 ){
- throw std::invalid_argument("The translation curve should be of dimension 3.");
- }
- if(rotation_curve->min() != T_min_){
- throw std::invalid_argument("Min bounds of translation and rotation curve are not the same.");
- }
- if(rotation_curve->max() != T_max_){
- throw std::invalid_argument("Max bounds of translation and rotation curve are not the same.");
- }
- safe_check();
+ T_min_(translation_curve->min()),
+ T_max_(translation_curve->max()) {
+ if (translation_curve->dim() != 3) {
+ throw std::invalid_argument("The translation curve should be of dimension 3.");
}
-
-
-
- /// \brief Evaluation of the SE3Curve at time t
- /// \param t : time when to evaluate the spline.
- /// \return \f$x(t)\f$ point corresponding on spline at time t. (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w)
- virtual point_t operator()(const time_t t) const
- {
- if(translation_curve_->dim() != 3){
- throw std::invalid_argument("Translation curve should always be of dimension 3");
- }
- point_t res = point_t::Identity();
- res.translate(point3_t((*translation_curve_)(t)));
- res.rotate((*rotation_curve_)(t));
- return res;
+ if (rotation_curve->min() != T_min_) {
+ throw std::invalid_argument("Min bounds of translation and rotation curve are not the same.");
}
-
- /// \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_derivate_t derivate(const time_t t, const std::size_t order) const
- {
- if(translation_curve_->dim() != 3){
- throw std::invalid_argument("Translation curve should always be of dimension 3");
- }
- point_derivate_t res = point_derivate_t::Zero();
- res.segment(0,3) = point3_t(translation_curve_->derivate(t,order));
- res.segment(3,3) = rotation_curve_->derivate(t,order);
- return res;
+ if (rotation_curve->max() != T_max_) {
+ throw std::invalid_argument("Max bounds of translation and rotation curve are not the same.");
}
-
-
-
- /*Helpers*/
- /// \brief Get dimension of curve.
- /// \return dimension of curve.
- std::size_t virtual dim() const{return dim_;};
- /// \brief Get the minimum time for which the curve is defined
- /// \return \f$t_{min}\f$ lower bound of time range.
- 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.
- time_t max() const {return T_max_;}
- /*Helpers*/
-
- /*Attributes*/
- std::size_t dim_; //dim doesn't mean anything in this class ...
- curve_X_t* translation_curve_ = NULL;
- curve_rotation_t* rotation_curve_= NULL;
- time_t T_min_, T_max_;
- /*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("dim", dim_);
- ar & boost::serialization::make_nvp("translation_curve", translation_curve_);
- ar & boost::serialization::make_nvp("rotation_curve", rotation_curve_);
- ar & boost::serialization::make_nvp("T_min", T_min_);
- ar & boost::serialization::make_nvp("T_max", T_max_);
+ safe_check();
+ }
+
+ /// \brief Evaluation of the SE3Curve at time t
+ /// \param t : time when to evaluate the spline.
+ /// \return \f$x(t)\f$ point corresponding on spline at time t. (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w)
+ virtual point_t operator()(const time_t t) const {
+ if (translation_curve_->dim() != 3) {
+ throw std::invalid_argument("Translation curve should always be of dimension 3");
}
-
- private:
- void safe_check()
- {
- if(Safe)
- {
- if(T_min_ > T_max_)
- {
- throw std::invalid_argument("Tmin should be inferior to Tmax");
- }
- if(translation_curve_->dim() != 3){
- throw std::invalid_argument("Translation curve should always be of dimension 3");
- }
+ point_t res = point_t::Identity();
+ res.translate(point3_t((*translation_curve_)(t)));
+ res.rotate((*rotation_curve_)(t));
+ return res;
+ }
+
+ /// \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_derivate_t derivate(const time_t t, const std::size_t order) const {
+ if (translation_curve_->dim() != 3) {
+ throw std::invalid_argument("Translation curve should always be of dimension 3");
+ }
+ point_derivate_t res = point_derivate_t::Zero();
+ res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order));
+ res.segment(3, 3) = rotation_curve_->derivate(t, order);
+ return res;
+ }
+
+ /*Helpers*/
+ /// \brief Get dimension of curve.
+ /// \return dimension of curve.
+ std::size_t virtual dim() const { return dim_; };
+ /// \brief Get the minimum time for which the curve is defined
+ /// \return \f$t_{min}\f$ lower bound of time range.
+ 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.
+ time_t max() const { return T_max_; }
+ /*Helpers*/
+
+ /*Attributes*/
+ std::size_t dim_; // dim doesn't mean anything in this class ...
+ curve_X_t* translation_curve_ = NULL;
+ curve_rotation_t* rotation_curve_ = NULL;
+ time_t T_min_, T_max_;
+ /*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("dim", dim_);
+ ar& boost::serialization::make_nvp("translation_curve", translation_curve_);
+ ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_);
+ ar& boost::serialization::make_nvp("T_min", T_min_);
+ ar& boost::serialization::make_nvp("T_max", T_max_);
+ }
+
+ private:
+ void safe_check() {
+ if (Safe) {
+ if (T_min_ > T_max_) {
+ throw std::invalid_argument("Tmin should be inferior to Tmax");
+ }
+ if (translation_curve_->dim() != 3) {
+ throw std::invalid_argument("Translation curve should always be of dimension 3");
}
}
+ }
+}; // SE3Curve
+} // namespace curves
- };//SE3Curve
-
-} // namespace curve
-
-
-
-#endif // SE3_CURVE_H
-
+#endif // SE3_CURVE_H
diff --git a/include/curves/so3_linear.h b/include/curves/so3_linear.h
index d41b9db..f88072b 100644
--- a/include/curves/so3_linear.h
+++ b/include/curves/so3_linear.h
@@ -1,227 +1,208 @@
#ifndef _STRUCT_SO3_LINEAR_H
#define _STRUCT_SO3_LINEAR_H
-
#include "MathDefs.h"
#include "curve_abc.h"
#include <Eigen/Geometry>
#include <boost/math/constants/constants.hpp>
-
-namespace curves
-{
-
-
- /// \class SO3Linear.
- /// \brief Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion
- ///
+namespace curves {
+
+/// \class SO3Linear.
+/// \brief Represents a linear interpolation in SO3, using the slerp method provided by Eigen::Quaternion
+///
+///
+template <typename Time = double, typename Numeric = Time, bool Safe = false>
+struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, 3, 3>, Eigen::Matrix<Numeric, 3, 1> > {
+ typedef Numeric Scalar;
+ typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
+ typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
+ typedef Eigen::Quaternion<Scalar> quaternion_t;
+ typedef Time time_t;
+ typedef curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> curve_abc_t;
+ typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
+
+ public:
+ /* Constructors - destructors */
+ /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
- template<typename Time= double, typename Numeric=Time, bool Safe=false>
- struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,3, 3>,Eigen::Matrix<Numeric,3, 1> >
- {
- typedef Numeric Scalar;
- typedef Eigen::Matrix<Scalar,3, 1> point3_t;
- typedef Eigen::Matrix<Scalar,3, 3> matrix3_t;
- typedef Eigen::Quaternion<Scalar> quaternion_t;
- typedef Time time_t;
- typedef curve_abc<Time, Numeric, Safe, matrix3_t,point3_t> curve_abc_t;
- typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
-
- public:
- /* Constructors - destructors */
- /// \brief Empty constructor. Curve obtained this way can not perform other class functions.
- ///
- SO3Linear()
- : curve_abc_t(), dim_(3),init_rot_(),end_rot_(),angular_vel_(), T_min_(0), T_max_(0)
- {}
-
- /// \brief constructor with initial and final rotation and time bounds
- SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot, const time_t t_min, const time_t t_max)
- : curve_abc_t(), dim_(3),init_rot_(init_rot),end_rot_(end_rot),angular_vel_(), T_min_(t_min), T_max_(t_max)
- {
- angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix())/(T_max_ - T_min_);
- safe_check();
- }
-
- /// \brief constructor with initial and final rotation expressed as rotation matrix and time bounds
- SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot, const time_t t_min, const time_t t_max)
- : curve_abc_t(), dim_(3),init_rot_(quaternion_t(init_rot)),end_rot_(quaternion_t(end_rot)),angular_vel_(), T_min_(t_min), T_max_(t_max)
- {
- angular_vel_ = log3(init_rot.transpose() * end_rot)/(T_max_ - T_min_);
- safe_check();
- }
-
- /// \brief constructor with initial and final rotation, time bounds are set to [0;1]
- SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot)
- : curve_abc_t(), dim_(3),init_rot_(init_rot),end_rot_(end_rot),angular_vel_(), T_min_(0.), T_max_(1.)
- {
- angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix());
- }
-
-
-
- /// \brief constructor with initial and final rotation expressed as rotation matrix, time bounds are set to [0;1]
- SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot)
- : curve_abc_t(), dim_(3),init_rot_(quaternion_t(init_rot)),end_rot_(quaternion_t(end_rot)),angular_vel_(), T_min_(0.), T_max_(1.)
- {
- angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix());
- }
-
-
- /// \brief Destructor
- ~SO3Linear()
- {
+ SO3Linear() : curve_abc_t(), dim_(3), init_rot_(), end_rot_(), angular_vel_(), T_min_(0), T_max_(0) {}
+
+ /// \brief constructor with initial and final rotation and time bounds
+ SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot, const time_t t_min, const time_t t_max)
+ : curve_abc_t(), dim_(3), init_rot_(init_rot), end_rot_(end_rot), angular_vel_(), T_min_(t_min), T_max_(t_max) {
+ angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix()) / (T_max_ - T_min_);
+ safe_check();
+ }
+
+ /// \brief constructor with initial and final rotation expressed as rotation matrix and time bounds
+ SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot, const time_t t_min, const time_t t_max)
+ : curve_abc_t(),
+ dim_(3),
+ init_rot_(quaternion_t(init_rot)),
+ end_rot_(quaternion_t(end_rot)),
+ angular_vel_(),
+ T_min_(t_min),
+ T_max_(t_max) {
+ angular_vel_ = log3(init_rot.transpose() * end_rot) / (T_max_ - T_min_);
+ safe_check();
+ }
+
+ /// \brief constructor with initial and final rotation, time bounds are set to [0;1]
+ SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot)
+ : curve_abc_t(), dim_(3), init_rot_(init_rot), end_rot_(end_rot), angular_vel_(), T_min_(0.), T_max_(1.) {
+ angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix());
+ }
+
+ /// \brief constructor with initial and final rotation expressed as rotation matrix, time bounds are set to [0;1]
+ SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot)
+ : curve_abc_t(),
+ dim_(3),
+ init_rot_(quaternion_t(init_rot)),
+ end_rot_(quaternion_t(end_rot)),
+ angular_vel_(),
+ T_min_(0.),
+ T_max_(1.) {
+ angular_vel_ = log3(init_rot_.toRotationMatrix().transpose() * end_rot_.toRotationMatrix());
+ }
+
+ /// \brief Destructor
+ ~SO3Linear() {
// NOTHING
+ }
+
+ SO3Linear(const SO3Linear& other)
+ : dim_(other.dim_),
+ init_rot_(other.init_rot_),
+ end_rot_(other.end_rot_),
+ T_min_(other.T_min_),
+ T_max_(other.T_max_) {}
+
+ quaternion_t computeAsQuaternion(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
}
-
-
- SO3Linear(const SO3Linear& other)
- : dim_(other.dim_), init_rot_(other.init_rot_), end_rot_(other.end_rot_),
- T_min_(other.T_min_), T_max_(other.T_max_)
- {}
-
-
- quaternion_t computeAsQuaternion(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
- }
- if(t > T_max_)
- return end_rot_;
- if(t < T_min_)
- return init_rot_;
- Scalar u = (t-T_min_) / (T_max_ - T_min_);
- return init_rot_.slerp(u,end_rot_);
- }
-
- /// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
- /// \param t : time when to evaluate the spline.
- /// \return \f$x(t)\f$ point corresponding on spline at time t.
- virtual matrix3_t operator()(const time_t t) const
- {
- return computeAsQuaternion(t).toRotationMatrix ();
+ if (t > T_max_) return end_rot_;
+ if (t < T_min_) return init_rot_;
+ Scalar u = (t - T_min_) / (T_max_ - T_min_);
+ return init_rot_.slerp(u, end_rot_);
+ }
+
+ /// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
+ /// \param t : time when to evaluate the spline.
+ /// \return \f$x(t)\f$ point corresponding on spline at time t.
+ virtual matrix3_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
+
+ /// \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 point3_t derivate(const time_t t, const std::size_t order) const {
+ if (order > 1 || t > T_max_ || t < T_min_) {
+ return point3_t::Zero(3);
+ } else if (order == 1) {
+ return angular_vel_;
+ } else {
+ throw std::invalid_argument("Order must be > 0 ");
}
-
- /// \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 point3_t derivate(const time_t t, const std::size_t order) const
- {
- if(order > 1 || t > T_max_ || t < T_min_){
- return point3_t::Zero(3);
- }else if(order == 1){
- return angular_vel_;
- }else{
- throw std::invalid_argument("Order must be > 0 ");
- }
+ }
+
+ /*Helpers*/
+ /// \brief Get dimension of curve.
+ /// \return dimension of curve.
+ std::size_t virtual dim() const { return dim_; };
+ /// \brief Get the minimum time for which the curve is defined
+ /// \return \f$t_{min}\f$ lower bound of time range.
+ 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.
+ time_t max() const { return T_max_; }
+ /*Helpers*/
+
+ /*Attributes*/
+ std::size_t dim_; // const
+ quaternion_t init_rot_, end_rot_;
+ point3_t angular_vel_;
+ time_t T_min_, T_max_; // const
+ /*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 ?
}
-
-
- /*Helpers*/
- /// \brief Get dimension of curve.
- /// \return dimension of curve.
- std::size_t virtual dim() const{return dim_;};
- /// \brief Get the minimum time for which the curve is defined
- /// \return \f$t_{min}\f$ lower bound of time range.
- 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.
- time_t max() const {return T_max_;}
- /*Helpers*/
-
- /*Attributes*/
- std::size_t dim_; //const
- quaternion_t init_rot_,end_rot_;
- point3_t angular_vel_;
- time_t T_min_, T_max_; //const
- /*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("dim", dim_);
- ar & boost::serialization::make_nvp("init_rot", init_rot_);
- ar & boost::serialization::make_nvp("end_rot", end_rot_);
- ar & boost::serialization::make_nvp("angular_vel", angular_vel_);
- ar & boost::serialization::make_nvp("T_min", T_min_);
- ar & boost::serialization::make_nvp("T_max", T_max_);
+ ar& boost::serialization::make_nvp("dim", dim_);
+ ar& boost::serialization::make_nvp("init_rot", init_rot_);
+ ar& boost::serialization::make_nvp("end_rot", end_rot_);
+ ar& boost::serialization::make_nvp("angular_vel", angular_vel_);
+ ar& boost::serialization::make_nvp("T_min", T_min_);
+ ar& boost::serialization::make_nvp("T_max", T_max_);
+ }
+
+ /// \brief Log: SO3 -> so3.
+ ///
+ /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
+ ///
+ /// Code from pinocchio.
+ ///
+ /// \param[in] R the rotation matrix. ///
+ /// \return The angular velocity vector associated to the rotation matrix.
+ ///
+ point3_t log3(const matrix3_t& R) {
+ Scalar theta;
+ static const Scalar PI_value = boost::math::constants::pi<Scalar>();
+
+ point3_t res;
+ const Scalar tr = R.trace();
+ if (tr > Scalar(3))
+ theta = 0; // acos((3-1)/2)
+ else if (tr < Scalar(-1))
+ theta = PI_value; // acos((-1-1)/2)
+ else
+ theta = acos((tr - Scalar(1)) / Scalar(2));
+ assert(theta == theta && "theta contains some NaN"); // theta != NaN
+
+ // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
+ if (theta < PI_value - 1e-2) {
+ const Scalar t = ((theta > std::pow(std::numeric_limits<Scalar>::epsilon(),
+ Scalar(1) / Scalar(4))) // precision of taylor serie of degree 3
+ ? theta / sin(theta)
+ : Scalar(1)) /
+ Scalar(2);
+ res(0) = t * (R(2, 1) - R(1, 2));
+ res(1) = t * (R(0, 2) - R(2, 0));
+ res(2) = t * (R(1, 0) - R(0, 1));
+ } else {
+ // 1e-2: A low value is not required since the computation is
+ // using explicit formula. However, the precision of this method
+ // is the square root of the precision with the antisymmetric
+ // method (Nominal case).
+ const Scalar cphi = cos(theta - PI_value);
+ const Scalar beta = theta * theta / (Scalar(1) + cphi);
+ point3_t tmp((R.diagonal().array() + cphi) * beta);
+ res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
+ res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
+ res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
}
+ return res;
+ }
- /// \brief Log: SO3 -> so3.
- ///
- /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
- ///
- /// Code from pinocchio.
- ///
- /// \param[in] R the rotation matrix. ///
- /// \return The angular velocity vector associated to the rotation matrix.
- ///
- point3_t log3(const matrix3_t & R)
- {
- Scalar theta;
- static const Scalar PI_value = boost::math::constants::pi<Scalar>();
-
- point3_t res;
- const Scalar tr = R.trace();
- if (tr > Scalar(3)) theta = 0; // acos((3-1)/2)
- else if (tr < Scalar(-1)) theta = PI_value; // acos((-1-1)/2)
- else theta = acos((tr - Scalar(1))/Scalar(2));
- assert(theta == theta && "theta contains some NaN"); // theta != NaN
-
- // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
- if (theta < PI_value - 1e-2)
- {
- const Scalar t = ((theta > std::pow(std::numeric_limits<Scalar>::epsilon(),Scalar(1)/Scalar(4))) // precision of taylor serie of degree 3
- ? theta / sin(theta)
- : Scalar(1)) / Scalar(2);
- res(0) = t * (R (2, 1) - R (1, 2));
- res(1) = t * (R (0, 2) - R (2, 0));
- res(2) = t * (R (1, 0) - R (0, 1));
- }
- else
- {
- // 1e-2: A low value is not required since the computation is
- // using explicit formula. However, the precision of this method
- // is the square root of the precision with the antisymmetric
- // method (Nominal case).
- const Scalar cphi = cos(theta - PI_value);
- const Scalar beta = theta*theta / ( Scalar(1) + cphi );
- point3_t tmp((R.diagonal().array() + cphi) * beta);
- res(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
- res(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
- res(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
- }
-
- return res;
- }
-
- private:
- void safe_check()
- {
- if(Safe)
- {
- if(T_min_ > T_max_)
- {
- throw std::invalid_argument("Tmin should be inferior to Tmax");
- }
+ private:
+ void safe_check() {
+ if (Safe) {
+ if (T_min_ > T_max_) {
+ throw std::invalid_argument("Tmin should be inferior to Tmax");
}
}
+ }
+}; // struct SO3Linear
+} // namespace curves
-
- }; // struct SO3Linear
-
-}// namespace curves
-
-
-#endif // _STRUCT_SO3_LINEAR_H
-
+#endif // _STRUCT_SO3_LINEAR_H
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index a951502..249c839 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -7,451 +7,392 @@
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
#include <pinocchio/spatial/se3.hpp>
#include <pinocchio/spatial/motion.hpp>
-#endif //CURVES_WITH_PINOCCHIO_SUPPORT
-
-namespace curves
-{
- using namespace boost::python;
-
- /* base wrap of curve_abc and others parent abstract class: must implement all pure virtual methods */
- struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t>
- {
- point_t operator()(const real) { return this->get_override("operator()")();}
- point_t derivate(const real, const std::size_t) { return this->get_override("derivate")();}
- std::size_t dim() { return this->get_override("dim")();}
- real min() { return this->get_override("min")();}
- real max() { return this->get_override("max")();}
-
- };
- struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t>
- {
- point_t operator()(const real) { return this->get_override("operator()")();}
- point_t derivate(const real, const std::size_t) { return this->get_override("derivate")();}
- std::size_t dim() { return this->get_override("dim")();}
- real min() { return this->get_override("min")();}
- real max() { return this->get_override("max")();}
-
- };
- struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_t>
- {
- point_t operator()(const real) { return this->get_override("operator()")();}
- point_t derivate(const real, const std::size_t) { return this->get_override("derivate")();}
- std::size_t dim() { return this->get_override("dim")();}
- real min() { return this->get_override("min")();}
- real max() { return this->get_override("max")();}
-
- };
- /* end base wrap of curve_abc */
-
- /* 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.)
- {
- T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
- return new Bezier(asVector.begin(), asVector.end(), constraints, T_min, T_max);
- }
- /* End Template constructor bezier */
- /* Helper converter constraintsX -> constraints 3 */
- curve_constraints3_t convertToConstraints3(curve_constraints_t constraintsX){
- curve_constraints3_t constraints3(3);
- constraints3.init_vel =point3_t(constraintsX.init_vel);
- constraints3.init_acc = point3_t(constraintsX.init_acc);
- constraints3.end_vel = point3_t(constraintsX.end_vel);
- constraints3.end_acc = point3_t(constraintsX.end_acc);
- return constraints3;
- }
- /* END helper converter constraintsX -> constraints 3 */
-
- /*3D constructors bezier */
- bezier3_t* wrapBezier3Constructor(const pointX_list_t& array)
- {
- return wrapBezierConstructorTemplate<bezier3_t, pointX_list_t, t_point3_t>(array) ;
- }
- bezier3_t* wrapBezier3ConstructorBounds(const pointX_list_t& array, const real T_min, const real T_max)
- {
- return wrapBezierConstructorTemplate<bezier3_t, pointX_list_t, t_point3_t>(array, T_min, T_max) ;
- }
- bezier3_t* wrapBezier3ConstructorConstraints(const pointX_list_t& array, const curve_constraints_t& constraints)
- {
- return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(array, convertToConstraints3(constraints)) ;
- }
- bezier3_t* wrapBezier3ConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
- const real T_min, const real T_max)
- {
- return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(array, convertToConstraints3(constraints),T_min, T_max) ;
- }
- /*END 3D constructors bezier */
-
- /*constructors bezier */
- bezier_t* wrapBezierConstructor(const pointX_list_t& array)
- {
- return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array) ;
- }
- bezier_t* wrapBezierConstructorBounds(const pointX_list_t& array, const real T_min, const real T_max)
- {
- return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array, T_min, T_max) ;
- }
- bezier_t* wrapBezierConstructorConstraints(const pointX_list_t& array, const curve_constraints_t& constraints)
- {
- return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints) ;
- }
- bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
- const real T_min, const real T_max)
- {
- return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(array, constraints,
- T_min, T_max) ;
- }
- /*END constructors bezier */
-
- /* Wrap Cubic hermite spline */
- t_pair_pointX_tangent_t getPairsPointTangent(const pointX_list_t& points, const pointX_list_t& tangents)
- {
- t_pair_pointX_tangent_t res;
- if (points.size() != tangents.size())
- {
- 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_pointX_tangent_t(points.col(i), tangents.col(i)));
- }
- return res;
- }
-
- cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const pointX_list_t& points, const pointX_list_t& tangents,
- const time_waypoints_t& time_pts)
- {
- t_pair_pointX_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]);
- }
- return new cubic_hermite_spline_t(ppt.begin(), ppt.end(), time_control_pts);
- }
- /* End wrap Cubic hermite spline */
-
- /* Wrap polynomial */
- polynomial_t* wrapPolynomialConstructor1(const coeff_t& array, const real min, const real max)
- {
- return new polynomial_t(array, min, max);
- }
- polynomial_t* wrapPolynomialConstructor2(const coeff_t& array)
- {
- return new polynomial_t(array, 0., 1.);
- }
- polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree1(const pointX_t& init,const pointX_t& end,const real min, const real max)
- {
- return new polynomial_t(init,end,min,max);
- }
- polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree3(const pointX_t& init,const pointX_t& d_init,const pointX_t& end,const pointX_t& d_end,const real min, const real max)
- {
- return new polynomial_t(init,d_init,end,d_end,min,max);
- }
- polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree5(const pointX_t& init,const pointX_t& d_init,const pointX_t& dd_init,const pointX_t& end,const point_t& d_end,const point_t& dd_end,const real min, const real max)
- {
- return new polynomial_t(init,d_init,dd_init,end,d_end,dd_end,min,max);
- }
- /* End wrap polynomial */
-
- /* Wrap piecewise curve */
- piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol)
- {
- return new piecewise_polynomial_curve_t(pol);
- }
- piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveEmptyConstructor()
- {
- return new piecewise_polynomial_curve_t();
- }
- piecewise_bezier_curve_t* wrapPiecewiseBezierCurveConstructor(const bezier_t& bc)
- {
- return new piecewise_bezier_curve_t(bc);
- }
- piecewise_bezier_linear_curve_t* wrapPiecewiseLinearBezierCurveConstructor(const bezier_linear_variable_t& bc)
- {
- return new piecewise_bezier_linear_curve_t(bc);
- }
- piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch)
- {
- return new piecewise_cubic_hermite_curve_t(ch);
- }
- static piecewise_polynomial_curve_t discretPointToPolynomialC0(const pointX_list_t& points, const time_waypoints_t& time_points){
- t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
- t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
- return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,time_points_list);
- }
- static piecewise_polynomial_curve_t discretPointToPolynomialC1(const pointX_list_t& points,const pointX_list_t& points_derivative, const time_waypoints_t& time_points){
- t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
- t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
- t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
- return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,time_points_list);
- }
- static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list_t& points,const pointX_list_t& points_derivative,const pointX_list_t& points_second_derivative, const time_waypoints_t& time_points){
- t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
- t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
- t_pointX_t points_second_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_second_derivative);
-
- t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
- return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,points_second_derivative_list,time_points_list);
- }
- void addFinalPointC0(piecewise_polynomial_curve_t self,const pointX_t& end,const real time){
- if(self.is_continuous(1))
- std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only guarantee C0 continuity."<<std::endl;
- polynomial_t pol(self(self.max()),end,self.max(),time);
- self.add_curve(pol);
- }
- void addFinalPointC1(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const real time){
- if(self.is_continuous(2))
- std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C2 continuity and only guarantee C1 continuity."<<std::endl;
- if(!self.is_continuous(1))
- std::cout<<"Warning: the current piecewise curve is not C1 continuous."<<std::endl;
- polynomial_t pol(self(self.max()),self.derivate(self.max(),1),end,d_end,self.max(),time);
- self.add_curve(pol);
- }
- void addFinalPointC2(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const pointX_t& dd_end,const real time){
- if(self.is_continuous(3))
- std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C3 continuity and only guarantee C2 continuity."<<std::endl;
- if(!self.is_continuous(2))
- std::cout<<"Warning: the current piecewise curve is not C2 continuous."<<std::endl;
- polynomial_t pol(self(self.max()),self.derivate(self.max(),1),self.derivate(self.max(),2),end,d_end,dd_end,self.max(),time);
- self.add_curve(pol);
- }
-
-
- /* end wrap piecewise polynomial curve */
-
- /* Wrap exact cubic spline */
- 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)));
- }
- return res;
- }
-
- 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)
- {
- t_waypoint_t wps = getWayPoints(array, time_wp);
- return new exact_cubic_t(wps.begin(), wps.end(), constraints);
- }
-
- /// For constraints XD
- pointX_t get_init_vel(const curve_constraints_t& c)
- {
- return c.init_vel;
- }
-
- pointX_t get_init_acc(const curve_constraints_t& c)
- {
- return c.init_acc;
- }
-
- pointX_t get_init_jerk(const curve_constraints_t& c)
- {
- return c.init_jerk;
- }
-
- pointX_t get_end_vel(const curve_constraints_t& c)
- {
- return c.end_vel;
- }
+#endif // CURVES_WITH_PINOCCHIO_SUPPORT
+
+namespace curves {
+using namespace boost::python;
+
+/* base wrap of curve_abc and others parent abstract class: must implement all pure virtual methods */
+struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t> {
+ point_t operator()(const real) { return this->get_override("operator()")(); }
+ point_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
+ std::size_t dim() { return this->get_override("dim")(); }
+ real min() { return this->get_override("min")(); }
+ real max() { return this->get_override("max")(); }
+};
+struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t> {
+ point_t operator()(const real) { return this->get_override("operator()")(); }
+ point_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
+ std::size_t dim() { return this->get_override("dim")(); }
+ real min() { return this->get_override("min")(); }
+ real max() { return this->get_override("max")(); }
+};
+struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_t> {
+ point_t operator()(const real) { return this->get_override("operator()")(); }
+ point_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
+ std::size_t dim() { return this->get_override("dim")(); }
+ real min() { return this->get_override("min")(); }
+ real max() { return this->get_override("max")(); }
+};
+/* end base wrap of curve_abc */
+
+/* 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.) {
+ T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
+ return new Bezier(asVector.begin(), asVector.end(), constraints, T_min, T_max);
+}
+/* End Template constructor bezier */
+/* Helper converter constraintsX -> constraints 3 */
+curve_constraints3_t convertToConstraints3(curve_constraints_t constraintsX) {
+ curve_constraints3_t constraints3(3);
+ constraints3.init_vel = point3_t(constraintsX.init_vel);
+ constraints3.init_acc = point3_t(constraintsX.init_acc);
+ constraints3.end_vel = point3_t(constraintsX.end_vel);
+ constraints3.end_acc = point3_t(constraintsX.end_acc);
+ return constraints3;
+}
+/* END helper converter constraintsX -> constraints 3 */
+
+/*3D constructors bezier */
+bezier3_t* wrapBezier3Constructor(const pointX_list_t& array) {
+ return wrapBezierConstructorTemplate<bezier3_t, pointX_list_t, t_point3_t>(array);
+}
+bezier3_t* wrapBezier3ConstructorBounds(const pointX_list_t& array, const real T_min, const real T_max) {
+ return wrapBezierConstructorTemplate<bezier3_t, pointX_list_t, t_point3_t>(array, T_min, T_max);
+}
+bezier3_t* wrapBezier3ConstructorConstraints(const pointX_list_t& array, const curve_constraints_t& constraints) {
+ return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(
+ array, convertToConstraints3(constraints));
+}
+bezier3_t* wrapBezier3ConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
+ const real T_min, const real T_max) {
+ return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(
+ array, convertToConstraints3(constraints), T_min, T_max);
+}
+/*END 3D constructors bezier */
+
+/*constructors bezier */
+bezier_t* wrapBezierConstructor(const pointX_list_t& array) {
+ return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array);
+}
+bezier_t* wrapBezierConstructorBounds(const pointX_list_t& array, const real T_min, const real T_max) {
+ return wrapBezierConstructorTemplate<bezier_t, pointX_list_t, t_pointX_t>(array, T_min, T_max);
+}
+bezier_t* wrapBezierConstructorConstraints(const pointX_list_t& array, const curve_constraints_t& constraints) {
+ return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(
+ array, constraints);
+}
+bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, const curve_constraints_t& constraints,
+ const real T_min, const real T_max) {
+ return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(
+ array, constraints, T_min, T_max);
+}
+/*END constructors bezier */
+
+/* Wrap Cubic hermite spline */
+t_pair_pointX_tangent_t getPairsPointTangent(const pointX_list_t& points, const pointX_list_t& tangents) {
+ t_pair_pointX_tangent_t res;
+ if (points.size() != tangents.size()) {
+ 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_pointX_tangent_t(points.col(i), tangents.col(i)));
+ }
+ return res;
+}
+
+cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const pointX_list_t& points, const pointX_list_t& tangents,
+ const time_waypoints_t& time_pts) {
+ t_pair_pointX_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]);
+ }
+ return new cubic_hermite_spline_t(ppt.begin(), ppt.end(), time_control_pts);
+}
+/* End wrap Cubic hermite spline */
+
+/* Wrap polynomial */
+polynomial_t* wrapPolynomialConstructor1(const coeff_t& array, const real min, const real max) {
+ return new polynomial_t(array, min, max);
+}
+polynomial_t* wrapPolynomialConstructor2(const coeff_t& array) { return new polynomial_t(array, 0., 1.); }
+polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree1(const pointX_t& init, const pointX_t& end,
+ const real min, const real max) {
+ return new polynomial_t(init, end, min, max);
+}
+polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree3(const pointX_t& init, const pointX_t& d_init,
+ const pointX_t& end, const pointX_t& d_end,
+ const real min, const real max) {
+ return new polynomial_t(init, d_init, end, d_end, min, max);
+}
+polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree5(const pointX_t& init, const pointX_t& d_init,
+ const pointX_t& dd_init, const pointX_t& end,
+ const point_t& d_end, const point_t& dd_end,
+ const real min, const real max) {
+ return new polynomial_t(init, d_init, dd_init, end, d_end, dd_end, min, max);
+}
+/* End wrap polynomial */
+
+/* Wrap piecewise curve */
+piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol) {
+ return new piecewise_polynomial_curve_t(pol);
+}
+piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveEmptyConstructor() {
+ return new piecewise_polynomial_curve_t();
+}
+piecewise_bezier_curve_t* wrapPiecewiseBezierCurveConstructor(const bezier_t& bc) {
+ return new piecewise_bezier_curve_t(bc);
+}
+piecewise_bezier_linear_curve_t* wrapPiecewiseLinearBezierCurveConstructor(const bezier_linear_variable_t& bc) {
+ return new piecewise_bezier_linear_curve_t(bc);
+}
+piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch) {
+ return new piecewise_cubic_hermite_curve_t(ch);
+}
+static piecewise_polynomial_curve_t discretPointToPolynomialC0(const pointX_list_t& points,
+ const time_waypoints_t& time_points) {
+ t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
+ t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
+ return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,
+ time_points_list);
+}
+static piecewise_polynomial_curve_t discretPointToPolynomialC1(const pointX_list_t& points,
+ const pointX_list_t& points_derivative,
+ const time_waypoints_t& time_points) {
+ t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
+ t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative);
+ t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
+ return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(
+ points_list, points_derivative_list, time_points_list);
+}
+static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list_t& points,
+ const pointX_list_t& points_derivative,
+ const pointX_list_t& points_second_derivative,
+ const time_waypoints_t& time_points) {
+ t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
+ t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative);
+ t_pointX_t points_second_derivative_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative);
+
+ t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
+ return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(
+ points_list, points_derivative_list, points_second_derivative_list, time_points_list);
+}
+void addFinalPointC0(piecewise_polynomial_curve_t self, const pointX_t& end, const real time) {
+ if (self.is_continuous(1))
+ std::cout << "Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only "
+ "guarantee C0 continuity."
+ << std::endl;
+ polynomial_t pol(self(self.max()), end, self.max(), time);
+ self.add_curve(pol);
+}
+void addFinalPointC1(piecewise_polynomial_curve_t self, const pointX_t& end, const pointX_t& d_end, const real time) {
+ if (self.is_continuous(2))
+ std::cout << "Warning: by adding this final point to the piecewise curve, you loose C2 continuity and only "
+ "guarantee C1 continuity."
+ << std::endl;
+ if (!self.is_continuous(1)) std::cout << "Warning: the current piecewise curve is not C1 continuous." << std::endl;
+ polynomial_t pol(self(self.max()), self.derivate(self.max(), 1), end, d_end, self.max(), time);
+ self.add_curve(pol);
+}
+void addFinalPointC2(piecewise_polynomial_curve_t self, const pointX_t& end, const pointX_t& d_end,
+ const pointX_t& dd_end, const real time) {
+ if (self.is_continuous(3))
+ std::cout << "Warning: by adding this final point to the piecewise curve, you loose C3 continuity and only "
+ "guarantee C2 continuity."
+ << std::endl;
+ if (!self.is_continuous(2)) std::cout << "Warning: the current piecewise curve is not C2 continuous." << std::endl;
+ polynomial_t pol(self(self.max()), self.derivate(self.max(), 1), self.derivate(self.max(), 2), end, d_end, dd_end,
+ self.max(), time);
+ self.add_curve(pol);
+}
+
+/* end wrap piecewise polynomial curve */
+
+/* Wrap exact cubic spline */
+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)));
+ }
+ return res;
+}
+
+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) {
+ t_waypoint_t wps = getWayPoints(array, time_wp);
+ return new exact_cubic_t(wps.begin(), wps.end(), constraints);
+}
+
+/// For constraints XD
+pointX_t get_init_vel(const curve_constraints_t& c) { return c.init_vel; }
+
+pointX_t get_init_acc(const curve_constraints_t& c) { return c.init_acc; }
+
+pointX_t get_init_jerk(const curve_constraints_t& c) { return c.init_jerk; }
+
+pointX_t get_end_vel(const curve_constraints_t& c) { return c.end_vel; }
+
+pointX_t get_end_acc(const curve_constraints_t& c) { return c.end_acc; }
+
+pointX_t get_end_jerk(const curve_constraints_t& c) { return c.end_jerk; }
+
+void set_init_vel(curve_constraints_t& c, const pointX_t& val) { c.init_vel = val; }
+
+void set_init_acc(curve_constraints_t& c, const pointX_t& val) { c.init_acc = val; }
+
+void set_init_jerk(curve_constraints_t& c, const pointX_t& val) { c.init_jerk = val; }
+
+void set_end_vel(curve_constraints_t& c, const pointX_t& val) { c.end_vel = val; }
+
+void set_end_acc(curve_constraints_t& c, const pointX_t& val) { c.end_acc = val; }
+
+void set_end_jerk(curve_constraints_t& c, const pointX_t& val) { c.end_jerk = val; }
+
+bezier_t* bezier_linear_variable_t_evaluate(const bezier_linear_variable_t* b, const pointX_t& x) {
+ return new bezier_t(evaluateLinear<bezier_t, bezier_linear_variable_t>(*b, x));
+}
+
+bezier_t::piecewise_bezier_curve_t (bezier_t::*splitspe)(const bezier_t::vector_x_t&) const = &bezier_t::split;
+bezier_linear_variable_t::piecewise_bezier_curve_t (bezier_linear_variable_t::*split_py)(
+ const bezier_linear_variable_t::vector_x_t&) const = &bezier_linear_variable_t::split;
+
+/* End wrap exact cubic spline */
+
+/* Wrap SO3Linear */
+SO3Linear_t* wrapSO3LinearConstructorFromQuaternion(const quaternion_t& init_rot, const quaternion_t& end_rot,
+ const real min, const real max) {
+ return new SO3Linear_t(init_rot, end_rot, min, max);
+}
+
+SO3Linear_t* wrapSO3LinearConstructorFromMatrix(const matrix3_t& init_rot, const matrix3_t& end_rot, const real min,
+ const real max) {
+ return new SO3Linear_t(init_rot, end_rot, min, max);
+}
+
+/* End wrap SO3Linear */
+
+/* Wrap SE3Curves */
+SE3Curve_t* wrapSE3CurveFromTransform(const matrix4_t& init_pose, const matrix4_t& end_pose, const real min,
+ const real max) {
+ return new SE3Curve_t(transform_t(init_pose), transform_t(end_pose), min, max);
+}
+
+SE3Curve_t* wrapSE3CurveFromBezier3Translation(bezier3_t& translation_curve, const matrix3_t& init_rot,
+ const matrix3_t& end_rot) {
+ bezier_t* translation = new bezier_t(translation_curve.waypoints().begin(), translation_curve.waypoints().end(),
+ translation_curve.min(), translation_curve.max());
+ return new SE3Curve_t(translation, init_rot, end_rot);
+}
- pointX_t get_end_acc(const curve_constraints_t& c)
- {
- return c.end_acc;
- }
+SE3Curve_t* wrapSE3CurveFromTranslation(curve_abc_t& translation_curve, const matrix3_t& init_rot,
+ const matrix3_t& end_rot) {
+ return new SE3Curve_t(&translation_curve, init_rot, end_rot);
+}
- pointX_t get_end_jerk(const curve_constraints_t& c)
- {
- return c.end_jerk;
- }
-
- void set_init_vel(curve_constraints_t& c, const pointX_t& val)
- {
- c.init_vel = val;
- }
-
- void set_init_acc(curve_constraints_t& c, const pointX_t& val)
- {
- c.init_acc = val;
- }
-
- void set_init_jerk(curve_constraints_t& c, const pointX_t& val)
- {
- c.init_jerk = val;
- }
-
- void set_end_vel(curve_constraints_t& c, const pointX_t& val)
- {
- c.end_vel = val;
- }
-
- void set_end_acc(curve_constraints_t& c, const pointX_t& val)
- {
- c.end_acc = val;
- }
-
- void set_end_jerk(curve_constraints_t& c, const pointX_t& val)
- {
- c.end_jerk = val;
- }
-
- bezier_t* bezier_linear_variable_t_evaluate(const bezier_linear_variable_t* b, const pointX_t& x)
- {
- return new bezier_t(evaluateLinear<bezier_t, bezier_linear_variable_t>(*b, x));
- }
+SE3Curve_t* wrapSE3CurveFromTwoCurves(curve_abc_t& translation_curve, curve_rotation_t& rotation_curve) {
+ return new SE3Curve_t(&translation_curve, &rotation_curve);
+}
- bezier_t::piecewise_bezier_curve_t (bezier_t::*splitspe)(const bezier_t::vector_x_t&) const = &bezier_t::split;
- bezier_linear_variable_t::piecewise_bezier_curve_t (bezier_linear_variable_t::*split_py)(const bezier_linear_variable_t::vector_x_t&) const = &bezier_linear_variable_t::split;
-
- /* End wrap exact cubic spline */
-
- /* Wrap SO3Linear */
- SO3Linear_t* wrapSO3LinearConstructorFromQuaternion(const quaternion_t& init_rot, const quaternion_t& end_rot, const real min, const real max)
- {
- return new SO3Linear_t(init_rot,end_rot, min, max);
- }
-
- SO3Linear_t* wrapSO3LinearConstructorFromMatrix(const matrix3_t& init_rot, const matrix3_t& end_rot, const real min, const real max)
- {
- return new SO3Linear_t(init_rot,end_rot, min, max);
- }
-
- /* End wrap SO3Linear */
-
- /* Wrap SE3Curves */
- SE3Curve_t* wrapSE3CurveFromTransform(const matrix4_t& init_pose, const matrix4_t& end_pose, const real min, const real max)
- {
- return new SE3Curve_t(transform_t(init_pose),transform_t(end_pose), min, max);
- }
-
-
- SE3Curve_t* wrapSE3CurveFromBezier3Translation(bezier3_t& translation_curve,const matrix3_t& init_rot, const matrix3_t& end_rot )
- {
- bezier_t* translation = new bezier_t(translation_curve.waypoints().begin(),translation_curve.waypoints().end(),translation_curve.min(),translation_curve.max());
- return new SE3Curve_t(translation,init_rot,end_rot);
- }
-
- SE3Curve_t* wrapSE3CurveFromTranslation(curve_abc_t& translation_curve,const matrix3_t& init_rot, const matrix3_t& end_rot )
- {
- return new SE3Curve_t(&translation_curve,init_rot,end_rot);
- }
-
-
- SE3Curve_t* wrapSE3CurveFromTwoCurves(curve_abc_t& translation_curve, curve_rotation_t& rotation_curve)
- {
- return new SE3Curve_t(&translation_curve,&rotation_curve);
- }
-
- #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
- typedef pinocchio::SE3Tpl<real,0> SE3_t;
- typedef pinocchio::MotionTpl<real,0> Motion_t;
-
- SE3Curve_t* wrapSE3CurveFromSE3Pinocchio(const SE3_t& init_pose, const SE3_t& end_pose, const real min, const real max)
- {
-
- return new SE3Curve_t(transform_t(init_pose.toHomogeneousMatrix()),transform_t(end_pose.toHomogeneousMatrix()), min, max);
- }
-
- SE3_t se3ReturnPinocchio(const SE3Curve_t& curve, const real t)
- {
- return SE3_t(curve(t).matrix());
- }
-
- Motion_t se3ReturnDerivatePinocchio(const SE3Curve_t& curve, const real t, const std::size_t order)
- {
- return Motion_t(curve.derivate(t,order));
- }
- #endif //CURVES_WITH_PINOCCHIO_SUPPORT
-
- matrix4_t se3Return(const SE3Curve_t& curve, const real t)
- {
- return curve(t).matrix();
- }
-
- pointX_t se3ReturnDerivate(const SE3Curve_t& curve, const real t, const std::size_t order)
- {
- return curve.derivate(t,order);
- }
-
- matrix3_t se3returnRotation(const SE3Curve_t& curve, const real t)
- {
- return curve(t).rotation();
- }
-
- pointX_t se3returnTranslation(const SE3Curve_t& curve, const real t)
- {
- return pointX_t(curve(t).translation());
- }
-
- /* End wrap SE3Curves */
-
- // TO DO : Replace all load and save function for serialization in class by using
- // SerializableVisitor in archive_python_binding.
- BOOST_PYTHON_MODULE(curves)
- {
- /** BEGIN eigenpy init**/
- eigenpy::enableEigenPy();
- eigenpy::enableEigenPySpecific<pointX_t,pointX_t>();
- eigenpy::enableEigenPySpecific<pointX_list_t,pointX_list_t>();
- eigenpy::enableEigenPySpecific<coeff_t,coeff_t>();
- eigenpy::enableEigenPySpecific<point_list_t,point_list_t>();
- eigenpy::enableEigenPySpecific<matrix3_t,matrix3_t>();
- eigenpy::enableEigenPySpecific<matrix4_t,matrix4_t>();
- //eigenpy::enableEigenPySpecific<quaternion_t,quaternion_t>();
- eigenpy::exposeQuaternion();
- /*eigenpy::exposeAngleAxis();
- eigenpy::exposeQuaternion();*/
- /** END eigenpy init**/
- class_<CurveWrapper,boost::noncopyable>("curve",no_init)
- .def("__call__", pure_virtual(&curve_abc_t::operator()),"Evaluate the curve at the given time.",args("self","t"))
- .def("derivate", pure_virtual(&curve_abc_t::derivate),"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
- .def("min", pure_virtual(&curve_abc_t::min), "Get the LOWER bound on interval definition of the curve.")
- .def("max", pure_virtual(&curve_abc_t::max),"Get the HIGHER bound on interval definition of the curve.")
- .def("dim", pure_virtual(&curve_abc_t::dim),"Get the dimension of the curve.")
- .def("saveAsText", pure_virtual(&curve_abc_t::saveAsText<curve_abc_t>),bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",pure_virtual(&curve_abc_t::loadFromText<curve_abc_t>),bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",pure_virtual(&curve_abc_t::saveAsXML<curve_abc_t>),bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",pure_virtual(&curve_abc_t::loadFromXML<curve_abc_t>),bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",pure_virtual(&curve_abc_t::saveAsBinary<curve_abc_t>),bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",pure_virtual(&curve_abc_t::loadFromBinary<curve_abc_t>),bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- class_<Curve3Wrapper,boost::noncopyable, bases<curve_abc_t> >("curve3",no_init)
- .def("__call__", pure_virtual(&curve_3_t::operator()),"Evaluate the curve at the given time.",args("self","t"))
- .def("derivate", pure_virtual(&curve_3_t::derivate),"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
- .def("min", pure_virtual(&curve_3_t::min), "Get the LOWER bound on interval definition of the curve.")
- .def("max", pure_virtual(&curve_3_t::max),"Get the HIGHER bound on interval definition of the curve.")
- .def("dim", pure_virtual(&curve_3_t::dim),"Get the dimension of the curve.")
- ;
-
- class_<CurveRotationWrapper,boost::noncopyable, bases<curve_abc_t> >("curve_rotation",no_init)
- .def("__call__", pure_virtual(&curve_rotation_t::operator()),"Evaluate the curve at the given time.",args("self","t"))
- .def("derivate", pure_virtual(&curve_rotation_t::derivate),"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
- .def("min", pure_virtual(&curve_rotation_t::min), "Get the LOWER bound on interval definition of the curve.")
- .def("max", pure_virtual(&curve_rotation_t::max),"Get the HIGHER bound on interval definition of the curve.")
- .def("dim", pure_virtual(&curve_rotation_t::dim),"Get the dimension of the curve.")
- ;
-
- /** BEGIN bezier3 curve**/
- class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
+#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+typedef pinocchio::SE3Tpl<real, 0> SE3_t;
+typedef pinocchio::MotionTpl<real, 0> Motion_t;
+
+SE3Curve_t* wrapSE3CurveFromSE3Pinocchio(const SE3_t& init_pose, const SE3_t& end_pose, const real min,
+ const real max) {
+ return new SE3Curve_t(transform_t(init_pose.toHomogeneousMatrix()), transform_t(end_pose.toHomogeneousMatrix()), min,
+ max);
+}
+
+SE3_t se3ReturnPinocchio(const SE3Curve_t& curve, const real t) { return SE3_t(curve(t).matrix()); }
+
+Motion_t se3ReturnDerivatePinocchio(const SE3Curve_t& curve, const real t, const std::size_t order) {
+ return Motion_t(curve.derivate(t, order));
+}
+#endif // CURVES_WITH_PINOCCHIO_SUPPORT
+
+matrix4_t se3Return(const SE3Curve_t& curve, const real t) { return curve(t).matrix(); }
+
+pointX_t se3ReturnDerivate(const SE3Curve_t& curve, const real t, const std::size_t order) {
+ return curve.derivate(t, order);
+}
+
+matrix3_t se3returnRotation(const SE3Curve_t& curve, const real t) { return curve(t).rotation(); }
+
+pointX_t se3returnTranslation(const SE3Curve_t& curve, const real t) { return pointX_t(curve(t).translation()); }
+
+/* End wrap SE3Curves */
+
+// TO DO : Replace all load and save function for serialization in class by using
+// SerializableVisitor in archive_python_binding.
+BOOST_PYTHON_MODULE(curves) {
+ /** BEGIN eigenpy init**/
+ eigenpy::enableEigenPy();
+ eigenpy::enableEigenPySpecific<pointX_t, pointX_t>();
+ eigenpy::enableEigenPySpecific<pointX_list_t, pointX_list_t>();
+ eigenpy::enableEigenPySpecific<coeff_t, coeff_t>();
+ eigenpy::enableEigenPySpecific<point_list_t, point_list_t>();
+ eigenpy::enableEigenPySpecific<matrix3_t, matrix3_t>();
+ eigenpy::enableEigenPySpecific<matrix4_t, matrix4_t>();
+ // eigenpy::enableEigenPySpecific<quaternion_t,quaternion_t>();
+ eigenpy::exposeQuaternion();
+ /*eigenpy::exposeAngleAxis();
+ eigenpy::exposeQuaternion();*/
+ /** END eigenpy init**/
+ class_<CurveWrapper, boost::noncopyable>("curve", no_init)
+ .def("__call__", pure_virtual(&curve_abc_t::operator()), "Evaluate the curve at the given time.",
+ args("self", "t"))
+ .def("derivate", pure_virtual(&curve_abc_t::derivate), "Evaluate the derivative of order N of curve at time t.",
+ args("self", "t", "N"))
+ .def("min", pure_virtual(&curve_abc_t::min), "Get the LOWER bound on interval definition of the curve.")
+ .def("max", pure_virtual(&curve_abc_t::max), "Get the HIGHER bound on interval definition of the curve.")
+ .def("dim", pure_virtual(&curve_abc_t::dim), "Get the dimension of the curve.")
+ .def("saveAsText", pure_virtual(&curve_abc_t::saveAsText<curve_abc_t>), bp::args("filename"),
+ "Saves *this inside a text file.")
+ .def("loadFromText", pure_virtual(&curve_abc_t::loadFromText<curve_abc_t>), bp::args("filename"),
+ "Loads *this from a text file.")
+ .def("saveAsXML", pure_virtual(&curve_abc_t::saveAsXML<curve_abc_t>), bp::args("filename", "tag_name"),
+ "Saves *this inside a XML file.")
+ .def("loadFromXML", pure_virtual(&curve_abc_t::loadFromXML<curve_abc_t>), bp::args("filename", "tag_name"),
+ "Loads *this from a XML file.")
+ .def("saveAsBinary", pure_virtual(&curve_abc_t::saveAsBinary<curve_abc_t>), bp::args("filename"),
+ "Saves *this inside a binary file.")
+ .def("loadFromBinary", pure_virtual(&curve_abc_t::loadFromBinary<curve_abc_t>), bp::args("filename"),
+ "Loads *this from a binary file.");
+
+ class_<Curve3Wrapper, boost::noncopyable, bases<curve_abc_t> >("curve3", no_init)
+ .def("__call__", pure_virtual(&curve_3_t::operator()), "Evaluate the curve at the given time.",
+ args("self", "t"))
+ .def("derivate", pure_virtual(&curve_3_t::derivate), "Evaluate the derivative of order N of curve at time t.",
+ args("self", "t", "N"))
+ .def("min", pure_virtual(&curve_3_t::min), "Get the LOWER bound on interval definition of the curve.")
+ .def("max", pure_virtual(&curve_3_t::max), "Get the HIGHER bound on interval definition of the curve.")
+ .def("dim", pure_virtual(&curve_3_t::dim), "Get the dimension of the curve.");
+
+ class_<CurveRotationWrapper, boost::noncopyable, bases<curve_abc_t> >("curve_rotation", no_init)
+ .def("__call__", pure_virtual(&curve_rotation_t::operator()), "Evaluate the curve at the given time.",
+ args("self", "t"))
+ .def("derivate", pure_virtual(&curve_rotation_t::derivate),
+ "Evaluate the derivative of order N of curve at time t.", args("self", "t", "N"))
+ .def("min", pure_virtual(&curve_rotation_t::min), "Get the LOWER bound on interval definition of the curve.")
+ .def("max", pure_virtual(&curve_rotation_t::max), "Get the HIGHER bound on interval definition of the curve.")
+ .def("dim", pure_virtual(&curve_rotation_t::dim), "Get the dimension of the curve.");
+
+ /** BEGIN bezier3 curve**/
+ class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
.def("__init__", make_constructor(&wrapBezier3Constructor))
.def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
.def("__init__", make_constructor(&wrapBezier3ConstructorConstraints))
@@ -461,17 +402,21 @@ namespace curves
.def("waypointAtIndex", &bezier3_t::waypointAtIndex)
.def_readonly("degree", &bezier3_t::degree_)
.def_readonly("nbWaypoints", &bezier3_t::size_)
- .def("saveAsText", &bezier3_t::saveAsText<bezier3_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&bezier3_t::loadFromText<bezier3_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&bezier3_t::saveAsXML<bezier3_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&bezier3_t::loadFromXML<bezier3_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&bezier3_t::saveAsBinary<bezier3_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&bezier3_t::loadFromBinary<bezier3_t>,bp::args("filename"),"Loads *this from a binary file.")
+ .def("saveAsText", &bezier3_t::saveAsText<bezier3_t>, bp::args("filename"), "Saves *this inside a text file.")
+ .def("loadFromText", &bezier3_t::loadFromText<bezier3_t>, bp::args("filename"), "Loads *this from a text file.")
+ .def("saveAsXML", &bezier3_t::saveAsXML<bezier3_t>, bp::args("filename", "tag_name"),
+ "Saves *this inside a XML file.")
+ .def("loadFromXML", &bezier3_t::loadFromXML<bezier3_t>, bp::args("filename", "tag_name"),
+ "Loads *this from a XML file.")
+ .def("saveAsBinary", &bezier3_t::saveAsBinary<bezier3_t>, bp::args("filename"),
+ "Saves *this inside a binary file.")
+ .def("loadFromBinary", &bezier3_t::loadFromBinary<bezier3_t>, bp::args("filename"),
+ "Loads *this from a binary file.")
//.def(SerializableVisitor<bezier_t>())
- ;
- /** END bezier3 curve**/
- /** BEGIN bezier curve**/
- class_<bezier_t, bases<curve_abc_t> >("bezier", init<>())
+ ;
+ /** END bezier3 curve**/
+ /** BEGIN bezier curve**/
+ class_<bezier_t, bases<curve_abc_t> >("bezier", init<>())
.def("__init__", make_constructor(&wrapBezierConstructor))
.def("__init__", make_constructor(&wrapBezierConstructorBounds))
.def("__init__", make_constructor(&wrapBezierConstructorConstraints))
@@ -482,343 +427,417 @@ namespace curves
.def_readonly("degree", &bezier_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_)
.def("split", splitspe)
- .def("saveAsText", &bezier_t::saveAsText<bezier_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&bezier_t::loadFromText<bezier_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&bezier_t::saveAsXML<bezier_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&bezier_t::loadFromXML<bezier_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&bezier_t::saveAsBinary<bezier_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&bezier_t::loadFromBinary<bezier_t>,bp::args("filename"),"Loads *this from a binary file.")
+ .def("saveAsText", &bezier_t::saveAsText<bezier_t>, bp::args("filename"), "Saves *this inside a text file.")
+ .def("loadFromText", &bezier_t::loadFromText<bezier_t>, bp::args("filename"), "Loads *this from a text file.")
+ .def("saveAsXML", &bezier_t::saveAsXML<bezier_t>, bp::args("filename", "tag_name"),
+ "Saves *this inside a XML file.")
+ .def("loadFromXML", &bezier_t::loadFromXML<bezier_t>, bp::args("filename", "tag_name"),
+ "Loads *this from a XML file.")
+ .def("saveAsBinary", &bezier_t::saveAsBinary<bezier_t>, bp::args("filename"),
+ "Saves *this inside a binary file.")
+ .def("loadFromBinary", &bezier_t::loadFromBinary<bezier_t>, bp::args("filename"),
+ "Loads *this from a binary file.")
//.def(SerializableVisitor<bezier_t>())
- ;
- /** END bezier curve**/
- /** BEGIN variable points bezier curve**/
- class_<matrix_pair>
- ("matrix_pair", no_init)
- .def_readonly("A", &matrix_pair::A)
- .def_readonly("b", &matrix_pair::b)
- ;
-
- class_<LinearBezierVector>
- ("bezierVarVector", no_init)
+ ;
+ /** END bezier curve**/
+ /** BEGIN variable points bezier curve**/
+ class_<matrix_pair>("matrix_pair", no_init).def_readonly("A", &matrix_pair::A).def_readonly("b", &matrix_pair::b);
+
+ class_<LinearBezierVector>("bezierVarVector", no_init)
.def_readonly("size", &LinearBezierVector::size)
- .def("at", &LinearBezierVector::at, return_value_policy<manage_new_object>())
- ;
-
- class_<linear_variable_t>
- ("linear_variable", init<>())
- .def(init<linear_variable_t::vector_x_t>())
- .def(init<linear_variable_t::matrix_x_t>())
- .def(init<linear_variable_t::matrix_x_t, linear_variable_t::vector_x_t>())
- .def(init<linear_variable_t::matrix_x_t, linear_variable_t::vector_x_t>())
- .def("__call__", &linear_variable_t::operator())
- .def(self += linear_variable_t())
- .def(self -= linear_variable_t())
- .def(self *= double())
- .def(self /= double())
- .def("B", &linear_variable_t::B, return_value_policy<copy_const_reference>())
- .def("c", &linear_variable_t::c, return_value_policy<copy_const_reference>())
- .def("size", &linear_variable_t::size)
- .def("isZero", &linear_variable_t::isZero)
- .def("norm", &linear_variable_t::norm)
- ;
-
- class_<bezier_linear_variable_t>
- ("bezier_linear_variable", 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_variable_t::operator())
- .def("evaluate", &bezier_linear_variable_t_evaluate, bp::return_value_policy<bp::manage_new_object>())
- .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_py)
- .def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
- .def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex)
- .def_readonly("degree", &bezier_linear_variable_t::degree_)
- .def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
- ;
-
-
- class_<quadratic_variable_t >
- ("cost", no_init)
- .add_property("A", &cost_t_quad)
- .add_property("b", &cost_t_linear)
- .add_property("c", &cost_t_constant)
- ;
-
- /** END variable points bezier curve**/
- /** BEGIN polynomial curve function**/
- class_<polynomial_t , bases<curve_abc_t> >("polynomial", init<>())
- .def("__init__", make_constructor(&wrapPolynomialConstructor1,default_call_policies(),args("coeffs","min","max")),
+ .def("at", &LinearBezierVector::at, return_value_policy<manage_new_object>());
+
+ class_<linear_variable_t>("linear_variable", init<>())
+ .def(init<linear_variable_t::vector_x_t>())
+ .def(init<linear_variable_t::matrix_x_t>())
+ .def(init<linear_variable_t::matrix_x_t, linear_variable_t::vector_x_t>())
+ .def(init<linear_variable_t::matrix_x_t, linear_variable_t::vector_x_t>())
+ .def("__call__", &linear_variable_t::operator())
+ .def(self += linear_variable_t())
+ .def(self -= linear_variable_t())
+ .def(self *= double())
+ .def(self /= double())
+ .def("B", &linear_variable_t::B, return_value_policy<copy_const_reference>())
+ .def("c", &linear_variable_t::c, return_value_policy<copy_const_reference>())
+ .def("size", &linear_variable_t::size)
+ .def("isZero", &linear_variable_t::isZero)
+ .def("norm", &linear_variable_t::norm);
+
+ class_<bezier_linear_variable_t>("bezier_linear_variable", 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_variable_t::operator())
+ .def("evaluate", &bezier_linear_variable_t_evaluate, bp::return_value_policy<bp::manage_new_object>())
+ .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_py)
+ .def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
+ .def("waypointAtIndex", &bezier_linear_variable_t::waypointAtIndex)
+ .def_readonly("degree", &bezier_linear_variable_t::degree_)
+ .def_readonly("nbWaypoints", &bezier_linear_variable_t::size_);
+
+ class_<quadratic_variable_t>("cost", no_init)
+ .add_property("A", &cost_t_quad)
+ .add_property("b", &cost_t_linear)
+ .add_property("c", &cost_t_constant);
+
+ /** END variable points bezier curve**/
+ /** BEGIN polynomial curve function**/
+ class_<polynomial_t, bases<curve_abc_t> >("polynomial", init<>())
+ .def("__init__",
+ make_constructor(&wrapPolynomialConstructor1, default_call_policies(), args("coeffs", "min", "max")),
"Create polynomial spline from an Eigen matrix of coefficient defined for t \in [min,max]."
- " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
+ " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest "
+ "order."
" Spline order is given by the number of the columns -1.")
- .def("__init__", make_constructor(&wrapPolynomialConstructor2,default_call_policies(),arg("coeffs")),
+ .def("__init__", make_constructor(&wrapPolynomialConstructor2, default_call_policies(), arg("coeffs")),
"Create polynomial spline from an Eigen matrix of coefficient defined for t \in [0,1]."
- " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
+ " The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest "
+ "order."
" Spline order is given by the number of the columns -1.")
- .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree1,
- default_call_policies(),args("init","end","min","max")),
+ .def("__init__",
+ make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree1, default_call_policies(),
+ args("init", "end", "min", "max")),
"Create a polynomial of degree 1 defined for t \in [min,max], "
"such that c(min) == init and c(max) == end.")
- .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree3,
- default_call_policies(),args("init","d_init","end","d_end","min","max")),
- "Create a polynomial of degree 3 defined for t \in [min,max], "
- "such that c(min) == init and c(max) == end"
- " dc(min) == d_init and dc(max) == d_end")
- .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree5,
- default_call_policies(),
- args("init","d_init","dd_init","end","d_end","dd_end","min","max")),
+ .def("__init__",
+ make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree3, default_call_policies(),
+ args("init", "d_init", "end", "d_end", "min", "max")),
+ "Create a polynomial of degree 3 defined for t \in [min,max], "
+ "such that c(min) == init and c(max) == end"
+ " dc(min) == d_init and dc(max) == d_end")
+ .def("__init__",
+ make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree5, default_call_policies(),
+ args("init", "d_init", "dd_init", "end", "d_end", "dd_end", "min", "max")),
"Create a polynomial of degree 5 defined for t \in [min,max], "
"such that c(min) == init and c(max) == end"
" dc(min) == d_init and dc(max) == d_end"
" ddc(min) == dd_init and ddc(max) == dd_end")
.def("coeffAtDegree", &polynomial_t::coeffAtDegree)
.def("coeff", &polynomial_t::coeff)
- .def("compute_derivate", &polynomial_t::compute_derivate,"Compute derivative of order N of curve at time t.")
- .def("saveAsText", &polynomial_t::saveAsText<polynomial_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&polynomial_t::loadFromText<polynomial_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&polynomial_t::saveAsXML<polynomial_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&polynomial_t::loadFromXML<polynomial_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&polynomial_t::saveAsBinary<polynomial_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&polynomial_t::loadFromBinary<polynomial_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- /** END polynomial function**/
- /** BEGIN piecewise curve function **/
- class_<piecewise_polynomial_curve_t , bases<curve_abc_t> >
- ("piecewise_polynomial_curve", init<>())
- .def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor,default_call_policies(),arg("curve")),
+ .def("compute_derivate", &polynomial_t::compute_derivate, "Compute derivative of order N of curve at time t.")
+ .def("saveAsText", &polynomial_t::saveAsText<polynomial_t>, bp::args("filename"),
+ "Saves *this inside a text file.")
+ .def("loadFromText", &polynomial_t::loadFromText<polynomial_t>, bp::args("filename"),
+ "Loads *this from a text file.")
+ .def("saveAsXML", &polynomial_t::saveAsXML<polynomial_t>, bp::args("filename", "tag_name"),
+ "Saves *this inside a XML file.")
+ .def("loadFromXML", &polynomial_t::loadFromXML<polynomial_t>, bp::args("filename", "tag_name"),
+ "Loads *this from a XML file.")
+ .def("saveAsBinary", &polynomial_t::saveAsBinary<polynomial_t>, bp::args("filename"),
+ "Saves *this inside a binary file.")
+ .def("loadFromBinary", &polynomial_t::loadFromBinary<polynomial_t>, bp::args("filename"),
+ "Loads *this from a binary file.");
+
+ /** END polynomial function**/
+ /** BEGIN piecewise curve function **/
+ class_<piecewise_polynomial_curve_t, bases<curve_abc_t> >("piecewise_polynomial_curve", init<>())
+ .def("__init__",
+ make_constructor(&wrapPiecewisePolynomialCurveConstructor, default_call_policies(), arg("curve")),
"Create a peicewise-polynomial curve containing the given polynomial curve.")
- .def("FromPointsList",&discretPointToPolynomialC0,
- "Create a piecewise-polynomial connecting exactly all the given points at the given time. The created piecewise is C0 continuous.",args("points","time_points"))
- .def("FromPointsList",&discretPointToPolynomialC1,
- "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative values. The created piecewise is C1 continuous.",args("points","points_derivative","time_points"))
- .def("FromPointsList",&discretPointToPolynomialC2,
- "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative and second derivative values. The created piecewise is C2 continuous.",args("points","points_derivative","points_second_derivative","time_points"))
+ .def("FromPointsList", &discretPointToPolynomialC0,
+ "Create a piecewise-polynomial connecting exactly all the given points at the given time. The created "
+ "piecewise is C0 continuous.",
+ args("points", "time_points"))
+ .def("FromPointsList", &discretPointToPolynomialC1,
+ "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the "
+ "given points derivative values. The created piecewise is C1 continuous.",
+ args("points", "points_derivative", "time_points"))
+ .def("FromPointsList", &discretPointToPolynomialC2,
+ "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the "
+ "given points derivative and second derivative values. The created piecewise is C2 continuous.",
+ args("points", "points_derivative", "points_second_derivative", "time_points"))
.staticmethod("FromPointsList")
- .def("append",&addFinalPointC0,
- "Append a new polynomial curve of degree 1 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end",args("self","end","time"))
- .def("append",&addFinalPointC1,
- "Append a new polynomial curve of degree 3 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C1 continuity and guarantee that self.derivate(time,1) == d_end",args("self","end","d_end","time"))
- .def("append",&addFinalPointC2,
- "Append a new polynomial curve of degree 5 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C2 continuity and guarantee that self.derivate(time,1) == d_end and self.derivate(time,2) == dd_end",args("self","end","d_end","d_end","time"))
- .def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
+ .def("append", &addFinalPointC0,
+ "Append a new polynomial curve of degree 1 at the end of the piecewise curve, defined between self.max() "
+ "and time and connecting exactly self(self.max()) and end",
+ args("self", "end", "time"))
+ .def("append", &addFinalPointC1,
+ "Append a new polynomial curve of degree 3 at the end of the piecewise curve, defined between self.max() "
+ "and time and connecting exactly self(self.max()) and end. It guarantee C1 continuity and guarantee that "
+ "self.derivate(time,1) == d_end",
+ args("self", "end", "d_end", "time"))
+ .def("append", &addFinalPointC2,
+ "Append a new polynomial curve of degree 5 at the end of the piecewise curve, defined between self.max() "
+ "and time and connecting exactly self(self.max()) and end. It guarantee C2 continuity and guarantee that "
+ "self.derivate(time,1) == d_end and self.derivate(time,2) == dd_end",
+ args("self", "end", "d_end", "d_end", "time"))
+ .def("compute_derivate", &piecewise_polynomial_curve_t::compute_derivate,
+ "Return a piecewise_polynomial curve which is the derivate of this.", args("self", "order"))
.def("append", &piecewise_polynomial_curve_t::add_curve,
"Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
"where T_{min} is equal toT_{max} of the actual piecewise curve.")
- .def("is_continuous", &piecewise_polynomial_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
- .def("convert_piecewise_curve_to_bezier", &piecewise_polynomial_curve_t::convert_piecewise_curve_to_bezier<bezier_t>,
+ .def("is_continuous", &piecewise_polynomial_curve_t::is_continuous,
+ "Check if the curve is continuous at the given order.")
+ .def("convert_piecewise_curve_to_bezier",
+ &piecewise_polynomial_curve_t::convert_piecewise_curve_to_bezier<bezier_t>,
"Convert a piecewise polynomial curve to to a piecewise bezier curve")
- .def("convert_piecewise_curve_to_cubic_hermite", &piecewise_polynomial_curve_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
+ .def("convert_piecewise_curve_to_cubic_hermite",
+ &piecewise_polynomial_curve_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
"Convert a piecewise polynomial curve to to a piecewise cubic hermite spline")
- .def("curve_at_index",&piecewise_polynomial_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
- .def("curve_at_time" ,&piecewise_polynomial_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
- .def("num_curves" ,&piecewise_polynomial_curve_t::num_curves)
- .def("saveAsText", &piecewise_polynomial_curve_t::saveAsText<piecewise_polynomial_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&piecewise_polynomial_curve_t::loadFromText<piecewise_polynomial_curve_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&piecewise_polynomial_curve_t::saveAsXML<piecewise_polynomial_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&piecewise_polynomial_curve_t::loadFromXML<piecewise_polynomial_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&piecewise_polynomial_curve_t::saveAsBinary<piecewise_polynomial_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&piecewise_polynomial_curve_t::loadFromBinary<piecewise_polynomial_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- class_<piecewise_bezier_curve_t , bases<curve_abc_t> >
- ("piecewise_bezier_curve", init<>())
+ .def("curve_at_index", &piecewise_polynomial_curve_t::curve_at_index,
+ return_value_policy<copy_const_reference>())
+ .def("curve_at_time", &piecewise_polynomial_curve_t::curve_at_time, return_value_policy<copy_const_reference>())
+ .def("num_curves", &piecewise_polynomial_curve_t::num_curves)
+ .def("saveAsText", &piecewise_polynomial_curve_t::saveAsText<piecewise_polynomial_curve_t>, bp::args("filename"),
+ "Saves *this inside a text file.")
+ .def("loadFromText", &piecewise_polynomial_curve_t::loadFromText<piecewise_polynomial_curve_t>,
+ bp::args("filename"), "Loads *this from a text file.")
+ .def("saveAsXML", &piecewise_polynomial_curve_t::saveAsXML<piecewise_polynomial_curve_t>,
+ bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+ .def("loadFromXML", &piecewise_polynomial_curve_t::loadFromXML<piecewise_polynomial_curve_t>,
+ bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+ .def("saveAsBinary", &piecewise_polynomial_curve_t::saveAsBinary<piecewise_polynomial_curve_t>,
+ bp::args("filename"), "Saves *this inside a binary file.")
+ .def("loadFromBinary", &piecewise_polynomial_curve_t::loadFromBinary<piecewise_polynomial_curve_t>,
+ bp::args("filename"), "Loads *this from a binary file.");
+
+ class_<piecewise_bezier_curve_t, bases<curve_abc_t> >("piecewise_bezier_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewiseBezierCurveConstructor))
- .def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
+ .def("compute_derivate", &piecewise_polynomial_curve_t::compute_derivate,
+ "Return a piecewise_polynomial curve which is the derivate of this.", args("self", "order"))
.def("add_curve", &piecewise_bezier_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_curve_t::is_continuous)
- .def("convert_piecewise_curve_to_polynomial", &piecewise_bezier_curve_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
+ .def("convert_piecewise_curve_to_polynomial",
+ &piecewise_bezier_curve_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
"Convert a piecewise bezier curve to to a piecewise polynomial curve")
- .def("convert_piecewise_curve_to_cubic_hermite", &piecewise_bezier_curve_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
+ .def("convert_piecewise_curve_to_cubic_hermite",
+ &piecewise_bezier_curve_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
"Convert a piecewise bezier curve to to a piecewise cubic hermite spline")
- .def("curve_at_index",&piecewise_bezier_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
- .def("curve_at_time" ,&piecewise_bezier_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
- .def("num_curves" ,&piecewise_bezier_curve_t::num_curves)
- .def("saveAsText", &piecewise_bezier_curve_t::saveAsText<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&piecewise_bezier_curve_t::loadFromText<piecewise_bezier_curve_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&piecewise_bezier_curve_t::saveAsXML<piecewise_bezier_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&piecewise_bezier_curve_t::loadFromXML<piecewise_bezier_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&piecewise_bezier_curve_t::saveAsBinary<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&piecewise_bezier_curve_t::loadFromBinary<piecewise_bezier_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- class_<piecewise_cubic_hermite_curve_t, bases<curve_abc_t> >
- ("piecewise_cubic_hermite_curve", init<>())
+ .def("curve_at_index", &piecewise_bezier_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
+ .def("curve_at_time", &piecewise_bezier_curve_t::curve_at_time, return_value_policy<copy_const_reference>())
+ .def("num_curves", &piecewise_bezier_curve_t::num_curves)
+ .def("saveAsText", &piecewise_bezier_curve_t::saveAsText<piecewise_bezier_curve_t>, bp::args("filename"),
+ "Saves *this inside a text file.")
+ .def("loadFromText", &piecewise_bezier_curve_t::loadFromText<piecewise_bezier_curve_t>, bp::args("filename"),
+ "Loads *this from a text file.")
+ .def("saveAsXML", &piecewise_bezier_curve_t::saveAsXML<piecewise_bezier_curve_t>,
+ bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+ .def("loadFromXML", &piecewise_bezier_curve_t::loadFromXML<piecewise_bezier_curve_t>,
+ bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+ .def("saveAsBinary", &piecewise_bezier_curve_t::saveAsBinary<piecewise_bezier_curve_t>, bp::args("filename"),
+ "Saves *this inside a binary file.")
+ .def("loadFromBinary", &piecewise_bezier_curve_t::loadFromBinary<piecewise_bezier_curve_t>, bp::args("filename"),
+ "Loads *this from a binary file.");
+
+ class_<piecewise_cubic_hermite_curve_t, bases<curve_abc_t> >("piecewise_cubic_hermite_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
.def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
.def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
- .def("convert_piecewise_curve_to_polynomial", &piecewise_cubic_hermite_curve_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
+ .def("convert_piecewise_curve_to_polynomial",
+ &piecewise_cubic_hermite_curve_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
"Convert a piecewise cubic hermite spline to to a piecewise polynomial curve")
- .def("convert_piecewise_curve_to_bezier", &piecewise_cubic_hermite_curve_t::convert_piecewise_curve_to_bezier<bezier_t>,
+ .def("convert_piecewise_curve_to_bezier",
+ &piecewise_cubic_hermite_curve_t::convert_piecewise_curve_to_bezier<bezier_t>,
"Convert a piecewise cubic hermite spline to to a piecewise bezier curve")
- .def("curve_at_index",&piecewise_cubic_hermite_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
- .def("curve_at_time" ,&piecewise_cubic_hermite_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
- .def("num_curves" ,&piecewise_cubic_hermite_curve_t::num_curves)
- .def("saveAsText", &piecewise_cubic_hermite_curve_t::saveAsText<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&piecewise_cubic_hermite_curve_t::loadFromText<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&piecewise_cubic_hermite_curve_t::saveAsXML<piecewise_cubic_hermite_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&piecewise_cubic_hermite_curve_t::loadFromXML<piecewise_cubic_hermite_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&piecewise_cubic_hermite_curve_t::saveAsBinary<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&piecewise_cubic_hermite_curve_t::loadFromBinary<piecewise_cubic_hermite_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- class_<piecewise_bezier_linear_curve_t, bases<curve_abc_t> >
- ("piecewise_bezier_linear_curve_t", init<>())
+ .def("curve_at_index", &piecewise_cubic_hermite_curve_t::curve_at_index,
+ return_value_policy<copy_const_reference>())
+ .def("curve_at_time", &piecewise_cubic_hermite_curve_t::curve_at_time,
+ return_value_policy<copy_const_reference>())
+ .def("num_curves", &piecewise_cubic_hermite_curve_t::num_curves)
+ .def("saveAsText", &piecewise_cubic_hermite_curve_t::saveAsText<piecewise_cubic_hermite_curve_t>,
+ bp::args("filename"), "Saves *this inside a text file.")
+ .def("loadFromText", &piecewise_cubic_hermite_curve_t::loadFromText<piecewise_cubic_hermite_curve_t>,
+ bp::args("filename"), "Loads *this from a text file.")
+ .def("saveAsXML", &piecewise_cubic_hermite_curve_t::saveAsXML<piecewise_cubic_hermite_curve_t>,
+ bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+ .def("loadFromXML", &piecewise_cubic_hermite_curve_t::loadFromXML<piecewise_cubic_hermite_curve_t>,
+ bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+ .def("saveAsBinary", &piecewise_cubic_hermite_curve_t::saveAsBinary<piecewise_cubic_hermite_curve_t>,
+ bp::args("filename"), "Saves *this inside a binary file.")
+ .def("loadFromBinary", &piecewise_cubic_hermite_curve_t::loadFromBinary<piecewise_cubic_hermite_curve_t>,
+ bp::args("filename"), "Loads *this from a binary file.");
+
+ class_<piecewise_bezier_linear_curve_t, bases<curve_abc_t> >("piecewise_bezier_linear_curve_t", init<>())
.def("__init__", make_constructor(&wrapPiecewiseLinearBezierCurveConstructor))
.def("add_curve", &piecewise_bezier_linear_curve_t::add_curve)
- .def("is_continuous", &piecewise_bezier_linear_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
- .def("curve_at_index",&piecewise_bezier_linear_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
- .def("curve_at_time" ,&piecewise_bezier_linear_curve_t::curve_at_time , return_value_policy<copy_const_reference>())
- .def("num_curves" ,&piecewise_bezier_linear_curve_t::num_curves)
- .def("saveAsText", &piecewise_bezier_linear_curve_t::saveAsText<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&piecewise_bezier_linear_curve_t::loadFromText<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&piecewise_bezier_linear_curve_t::saveAsXML<piecewise_bezier_linear_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&piecewise_bezier_linear_curve_t::loadFromXML<piecewise_bezier_linear_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&piecewise_bezier_linear_curve_t::saveAsBinary<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&piecewise_bezier_linear_curve_t::loadFromBinary<piecewise_bezier_linear_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- /** END piecewise curve function **/
- /** BEGIN exact_cubic curve**/
- class_<exact_cubic_t, bases<curve_abc_t> >
- ("exact_cubic", init<>())
+ .def("is_continuous", &piecewise_bezier_linear_curve_t::is_continuous,
+ "Check if the curve is continuous at the given order.")
+ .def("curve_at_index", &piecewise_bezier_linear_curve_t::curve_at_index,
+ return_value_policy<copy_const_reference>())
+ .def("curve_at_time", &piecewise_bezier_linear_curve_t::curve_at_time,
+ return_value_policy<copy_const_reference>())
+ .def("num_curves", &piecewise_bezier_linear_curve_t::num_curves)
+ .def("saveAsText", &piecewise_bezier_linear_curve_t::saveAsText<piecewise_bezier_linear_curve_t>,
+ bp::args("filename"), "Saves *this inside a text file.")
+ .def("loadFromText", &piecewise_bezier_linear_curve_t::loadFromText<piecewise_bezier_linear_curve_t>,
+ bp::args("filename"), "Loads *this from a text file.")
+ .def("saveAsXML", &piecewise_bezier_linear_curve_t::saveAsXML<piecewise_bezier_linear_curve_t>,
+ bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+ .def("loadFromXML", &piecewise_bezier_linear_curve_t::loadFromXML<piecewise_bezier_linear_curve_t>,
+ bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+ .def("saveAsBinary", &piecewise_bezier_linear_curve_t::saveAsBinary<piecewise_bezier_linear_curve_t>,
+ bp::args("filename"), "Saves *this inside a binary file.")
+ .def("loadFromBinary", &piecewise_bezier_linear_curve_t::loadFromBinary<piecewise_bezier_linear_curve_t>,
+ bp::args("filename"), "Loads *this from a binary file.");
+
+ /** END piecewise curve function **/
+ /** BEGIN exact_cubic curve**/
+ class_<exact_cubic_t, bases<curve_abc_t> >("exact_cubic", init<>())
.def("__init__", make_constructor(&wrapExactCubicConstructor))
.def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
.def("getNumberSplines", &exact_cubic_t::getNumberSplines)
.def("getSplineAt", &exact_cubic_t::getSplineAt)
- .def("saveAsText", &exact_cubic_t::saveAsText<exact_cubic_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&exact_cubic_t::loadFromText<exact_cubic_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&exact_cubic_t::saveAsXML<exact_cubic_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&exact_cubic_t::loadFromXML<exact_cubic_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&exact_cubic_t::saveAsBinary<exact_cubic_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&exact_cubic_t::loadFromBinary<exact_cubic_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- /** END exact_cubic curve**/
- /** BEGIN cubic_hermite_spline **/
- class_<cubic_hermite_spline_t, bases<curve_abc_t> >
- ("cubic_hermite_spline", init<>())
+ .def("saveAsText", &exact_cubic_t::saveAsText<exact_cubic_t>, bp::args("filename"),
+ "Saves *this inside a text file.")
+ .def("loadFromText", &exact_cubic_t::loadFromText<exact_cubic_t>, bp::args("filename"),
+ "Loads *this from a text file.")
+ .def("saveAsXML", &exact_cubic_t::saveAsXML<exact_cubic_t>, bp::args("filename", "tag_name"),
+ "Saves *this inside a XML file.")
+ .def("loadFromXML", &exact_cubic_t::loadFromXML<exact_cubic_t>, bp::args("filename", "tag_name"),
+ "Loads *this from a XML file.")
+ .def("saveAsBinary", &exact_cubic_t::saveAsBinary<exact_cubic_t>, bp::args("filename"),
+ "Saves *this inside a binary file.")
+ .def("loadFromBinary", &exact_cubic_t::loadFromBinary<exact_cubic_t>, bp::args("filename"),
+ "Loads *this from a binary file.");
+
+ /** END exact_cubic curve**/
+ /** BEGIN cubic_hermite_spline **/
+ class_<cubic_hermite_spline_t, bases<curve_abc_t> >("cubic_hermite_spline", init<>())
.def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
- .def("saveAsText", &cubic_hermite_spline_t::saveAsText<cubic_hermite_spline_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&cubic_hermite_spline_t::loadFromText<cubic_hermite_spline_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&cubic_hermite_spline_t::saveAsXML<cubic_hermite_spline_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&cubic_hermite_spline_t::loadFromXML<cubic_hermite_spline_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&cubic_hermite_spline_t::saveAsBinary<cubic_hermite_spline_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&cubic_hermite_spline_t::loadFromBinary<cubic_hermite_spline_t>,bp::args("filename"),"Loads *this from a binary file.")
+ .def("saveAsText", &cubic_hermite_spline_t::saveAsText<cubic_hermite_spline_t>, bp::args("filename"),
+ "Saves *this inside a text file.")
+ .def("loadFromText", &cubic_hermite_spline_t::loadFromText<cubic_hermite_spline_t>, bp::args("filename"),
+ "Loads *this from a text file.")
+ .def("saveAsXML", &cubic_hermite_spline_t::saveAsXML<cubic_hermite_spline_t>, bp::args("filename", "tag_name"),
+ "Saves *this inside a XML file.")
+ .def("loadFromXML", &cubic_hermite_spline_t::loadFromXML<cubic_hermite_spline_t>,
+ bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+ .def("saveAsBinary", &cubic_hermite_spline_t::saveAsBinary<cubic_hermite_spline_t>, bp::args("filename"),
+ "Saves *this inside a binary file.")
+ .def("loadFromBinary", &cubic_hermite_spline_t::loadFromBinary<cubic_hermite_spline_t>, bp::args("filename"),
+ "Loads *this from a binary file.");
+
+ /** END cubic_hermite_spline **/
+ /** BEGIN curve constraints**/
+ class_<curve_constraints_t>("curve_constraints", init<int>())
+ .add_property("init_vel", &get_init_vel, &set_init_vel)
+ .add_property("init_acc", &get_init_acc, &set_init_acc)
+ .add_property("init_jerk", &get_init_jerk, &set_init_jerk)
+ .add_property("end_vel", &get_end_vel, &set_end_vel)
+ .add_property("end_acc", &get_end_acc, &set_end_acc)
+ .add_property("end_jerk", &get_end_jerk, &set_end_jerk);
+ /** END curve constraints**/
+ /** BEGIN bernstein polynomial**/
+ class_<bernstein_t>("bernstein", init<const unsigned int, const unsigned int>())
+ .def("__call__", &bernstein_t::operator());
+ /** END bernstein polynomial**/
+
+ /** BEGIN SO3 Linear**/
+ class_<SO3Linear_t, bases<curve_rotation_t> >("SO3Linear", init<>())
+ .def("__init__",
+ make_constructor(&wrapSO3LinearConstructorFromMatrix, default_call_policies(),
+ args("init_rotation", "end_rotation", "min", "max")),
+ "Create a SO3 Linear curve between two rotations, defined for t \in [min,max]."
+ " The input rotations are expressed as 3x3 matrix.")
+ .def("__init__",
+ make_constructor(&wrapSO3LinearConstructorFromQuaternion, default_call_policies(),
+ args("init_rotation", "end_rotation", "min", "max")),
+ "Create a SO3 Linear curve between two rotations, defined for t \in [min,max]."
+ " The input rotations are expressed as Quaternions.")
+ .def("computeAsQuaternion", &SO3Linear_t::computeAsQuaternion,
+ "Output the quaternion of the rotation at the given time. This rotation is obtained by a Spherical Linear "
+ "Interpolation between the initial and final rotation.")
+ // .def("saveAsText", &SO3Linear_t::saveAsText<SO3Linear_t>,bp::args("filename"),"Saves *this inside a text
+ // file.") .def("loadFromText",&SO3Linear_t::loadFromText<SO3Linear_t>,bp::args("filename"),"Loads *this
+ // from a text file.")
+ // .def("saveAsXML",&SO3Linear_t::saveAsXML<SO3Linear_t>,bp::args("filename","tag_name"),"Saves *this inside
+ // a XML file.")
+ // .def("loadFromXML",&SO3Linear_t::loadFromXML<SO3Linear_t>,bp::args("filename","tag_name"),"Loads *this
+ // from a XML file.")
+ // .def("saveAsBinary",&SO3Linear_t::saveAsBinary<SO3Linear_t>,bp::args("filename"),"Saves *this inside a
+ // binary file.")
+ // .def("loadFromBinary",&SO3Linear_t::loadFromBinary<SO3Linear_t>,bp::args("filename"),"Loads *this from a
+ // binary file.")
;
- /** END cubic_hermite_spline **/
- /** BEGIN curve constraints**/
- class_<curve_constraints_t>
- ("curve_constraints", init<int>())
- .add_property("init_vel", &get_init_vel, &set_init_vel)
- .add_property("init_acc", &get_init_acc, &set_init_acc)
- .add_property("init_jerk", &get_init_jerk, &set_init_jerk)
- .add_property("end_vel", &get_end_vel, &set_end_vel)
- .add_property("end_acc", &get_end_acc, &set_end_acc)
- .add_property("end_jerk", &get_end_jerk, &set_end_jerk)
- ;
- /** END curve constraints**/
- /** BEGIN bernstein polynomial**/
- class_<bernstein_t>
- ("bernstein", init<const unsigned int, const unsigned int>())
- .def("__call__", &bernstein_t::operator())
- ;
- /** END bernstein polynomial**/
-
- /** BEGIN SO3 Linear**/
- class_<SO3Linear_t, bases<curve_rotation_t> >("SO3Linear", init<>())
- .def("__init__", make_constructor(&wrapSO3LinearConstructorFromMatrix,default_call_policies(),args("init_rotation","end_rotation","min","max")),"Create a SO3 Linear curve between two rotations, defined for t \in [min,max]."
- " The input rotations are expressed as 3x3 matrix.")
- .def("__init__", make_constructor(&wrapSO3LinearConstructorFromQuaternion,default_call_policies(),args("init_rotation","end_rotation","min","max")),"Create a SO3 Linear curve between two rotations, defined for t \in [min,max]."
- " The input rotations are expressed as Quaternions.")
- .def("computeAsQuaternion",&SO3Linear_t::computeAsQuaternion,"Output the quaternion of the rotation at the given time. This rotation is obtained by a Spherical Linear Interpolation between the initial and final rotation.")
-// .def("saveAsText", &SO3Linear_t::saveAsText<SO3Linear_t>,bp::args("filename"),"Saves *this inside a text file.")
-// .def("loadFromText",&SO3Linear_t::loadFromText<SO3Linear_t>,bp::args("filename"),"Loads *this from a text file.")
-// .def("saveAsXML",&SO3Linear_t::saveAsXML<SO3Linear_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
-// .def("loadFromXML",&SO3Linear_t::loadFromXML<SO3Linear_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
-// .def("saveAsBinary",&SO3Linear_t::saveAsBinary<SO3Linear_t>,bp::args("filename"),"Saves *this inside a binary file.")
-// .def("loadFromBinary",&SO3Linear_t::loadFromBinary<SO3Linear_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- /** END SO3 Linear**/
- /** BEGIN SE3 Curve**/
- class_<SE3Curve_t, bases<curve_abc_t> >("SE3Curve", init<>())
+ /** END SO3 Linear**/
+ /** BEGIN SE3 Curve**/
+ class_<SE3Curve_t, bases<curve_abc_t> >("SE3Curve", init<>())
+ .def("__init__",
+ make_constructor(&wrapSE3CurveFromTransform, default_call_policies(),
+ args("init_transform", "end_transform", "min", "max")),
+ "Create a SE3 curve between two transform, defined for t \in [min,max]."
+ " Using linear interpolation for translation and slerp for rotation between init and end."
+ " The input transform are expressed as 4x4 matrix.")
+ .def("__init__",
+ make_constructor(&wrapSE3CurveFromTwoCurves, default_call_policies(),
+ args("translation_curve", "rotation_curve")),
+ "Create a SE3 curve from a translation curve and a rotation one."
+ "The translation curve should be of dimension 3 and the rotation one should output 3x3 matrix"
+ "Both curves should have the same time bounds.")
.def("__init__",
- make_constructor(&wrapSE3CurveFromTransform,default_call_policies(),
- args("init_transform","end_transform","min","max")),
- "Create a SE3 curve between two transform, defined for t \in [min,max]."
- " Using linear interpolation for translation and slerp for rotation between init and end."
- " The input transform are expressed as 4x4 matrix.")
+ make_constructor(&wrapSE3CurveFromTranslation, default_call_policies(),
+ args("translation_curve", "init_rotation", "end_rotation")),
+ "Create a SE3 curve from a translation curve and two rotation"
+ "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the "
+ "translation curve."
+ "The orientation along the SE3Curve will be a slerp between the two given rotations."
+ "The orientations should be represented as 3x3 rotation matrix")
.def("__init__",
- make_constructor(&wrapSE3CurveFromTwoCurves,
- default_call_policies(),
- args("translation_curve","rotation_curve")),
- "Create a SE3 curve from a translation curve and a rotation one."
- "The translation curve should be of dimension 3 and the rotation one should output 3x3 matrix"
- "Both curves should have the same time bounds.")
- .def("__init__",
- make_constructor(&wrapSE3CurveFromTranslation,
- default_call_policies(),
- args("translation_curve","init_rotation","end_rotation")),
- "Create a SE3 curve from a translation curve and two rotation"
- "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the translation curve."
- "The orientation along the SE3Curve will be a slerp between the two given rotations."
- "The orientations should be represented as 3x3 rotation matrix")
- .def("__init__",
- make_constructor(&wrapSE3CurveFromBezier3Translation,
- default_call_policies(),
- args("translation_curve","init_rotation","end_rotation")),
- "Create a SE3 curve from a translation curve and two rotation"
- "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the translation curve."
- "The orientation along the SE3Curve will be a slerp between the two given rotations."
- "The orientations should be represented as 3x3 rotation matrix")
- .def("rotation", &se3returnRotation,"Output the rotation (as a 3x3 matrix) at the given time.",args("self","time"))
- .def("translation", &se3returnTranslation,"Output the rotation (as a vector 3) at the given time.",args("self","time"))
- .def("__call__", &se3Return,"Evaluate the curve at the given time. Return as an homogeneous matrix",args("self","t"))
- .def("derivate", &se3ReturnDerivate,"Evaluate the derivative of order N of curve at time t. Return as a vector 6",args("self","t","N"))
- .def("min", &SE3Curve_t::min, "Get the LOWER bound on interval definition of the curve.")
- .def("max", &SE3Curve_t::max,"Get the HIGHER bound on interval definition of the curve.")
- .def("dim", &SE3Curve_t::dim,"Get the dimension of the curve.")
- #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
- .def("__init__",
- make_constructor(&wrapSE3CurveFromSE3Pinocchio,default_call_policies(),
- args("init_SE3","end_SE3","min","max")),
- "Create a SE3 curve between two SE3 objects from Pinocchio, defined for t \in [min,max]."
- " Using linear interpolation for translation and slerp for rotation between init and end.")
- .def("evaluateAsSE3", &se3Return,"Evaluate the curve at the given time. Return as a pinocchio.SE3 object",args("self","t"))
- .def("derivateAsMotion", &se3ReturnDerivate,"Evaluate the derivative of order N of curve at time t. Return as a pinocchio.Motion",args("self","t","N"))
- #endif //CURVES_WITH_PINOCCHIO_SUPPORT
-// .def("saveAsText", &SE3Curve_t::saveAsText<SE3Curve_t>,bp::args("filename"),"Saves *this inside a text file.")
-// .def("loadFromText",&SE3Curve_t::loadFromText<SE3Curve_t>,bp::args("filename"),"Loads *this from a text file.")
-// .def("saveAsXML",&SE3Curve_t::saveAsXML<SE3Curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
-// .def("loadFromXML",&SE3Curve_t::loadFromXML<SE3Curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
-// .def("saveAsBinary",&SE3Curve_t::saveAsBinary<SE3Curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
-// .def("loadFromBinary",&SE3Curve_t::loadFromBinary<SE3Curve_t>,bp::args("filename"),"Loads *this from a binary file.")
- ;
-
- /** END SE3 Curve**/
- /** BEGIN curves conversion**/
- def("polynomial_from_bezier", polynomial_from_curve<polynomial_t,bezier_t>);
- def("polynomial_from_hermite", polynomial_from_curve<polynomial_t,cubic_hermite_spline_t>);
- def("bezier_from_hermite", bezier_from_curve<bezier_t,cubic_hermite_spline_t>);
- def("bezier_from_polynomial", bezier_from_curve<bezier_t,polynomial_t>);
- def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier_t>);
- def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial_t>);
- /** END curves conversion**/
-
- optimization::python::exposeOptimization();
-
-
- #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
- scope().attr("CURVES_WITH_PINOCCHIO_SUPPORT") = true;
- #else
- scope().attr("CURVES_WITH_PINOCCHIO_SUPPORT") = false;
- #endif
-
- } // End BOOST_PYTHON_MODULE
-} // namespace curves
+ make_constructor(&wrapSE3CurveFromBezier3Translation, default_call_policies(),
+ args("translation_curve", "init_rotation", "end_rotation")),
+ "Create a SE3 curve from a translation curve and two rotation"
+ "The translation curve should be of dimension 3, the time definition of the SE3curve will the same as the "
+ "translation curve."
+ "The orientation along the SE3Curve will be a slerp between the two given rotations."
+ "The orientations should be represented as 3x3 rotation matrix")
+ .def("rotation", &se3returnRotation, "Output the rotation (as a 3x3 matrix) at the given time.",
+ args("self", "time"))
+ .def("translation", &se3returnTranslation, "Output the rotation (as a vector 3) at the given time.",
+ args("self", "time"))
+ .def("__call__", &se3Return, "Evaluate the curve at the given time. Return as an homogeneous matrix",
+ args("self", "t"))
+ .def("derivate", &se3ReturnDerivate,
+ "Evaluate the derivative of order N of curve at time t. Return as a vector 6", args("self", "t", "N"))
+ .def("min", &SE3Curve_t::min, "Get the LOWER bound on interval definition of the curve.")
+ .def("max", &SE3Curve_t::max, "Get the HIGHER bound on interval definition of the curve.")
+ .def("dim", &SE3Curve_t::dim, "Get the dimension of the curve.")
+#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+ .def("__init__",
+ make_constructor(&wrapSE3CurveFromSE3Pinocchio, default_call_policies(),
+ args("init_SE3", "end_SE3", "min", "max")),
+ "Create a SE3 curve between two SE3 objects from Pinocchio, defined for t \in [min,max]."
+ " Using linear interpolation for translation and slerp for rotation between init and end.")
+ .def("evaluateAsSE3", &se3Return, "Evaluate the curve at the given time. Return as a pinocchio.SE3 object",
+ args("self", "t"))
+ .def("derivateAsMotion", &se3ReturnDerivate,
+ "Evaluate the derivative of order N of curve at time t. Return as a pinocchio.Motion",
+ args("self", "t", "N"))
+#endif // CURVES_WITH_PINOCCHIO_SUPPORT
+ // .def("saveAsText", &SE3Curve_t::saveAsText<SE3Curve_t>,bp::args("filename"),"Saves *this inside a text
+ // file.") .def("loadFromText",&SE3Curve_t::loadFromText<SE3Curve_t>,bp::args("filename"),"Loads *this
+ // from a text file.")
+ // .def("saveAsXML",&SE3Curve_t::saveAsXML<SE3Curve_t>,bp::args("filename","tag_name"),"Saves *this inside
+ // a XML file.")
+ // .def("loadFromXML",&SE3Curve_t::loadFromXML<SE3Curve_t>,bp::args("filename","tag_name"),"Loads *this
+ // from a XML file.")
+ // .def("saveAsBinary",&SE3Curve_t::saveAsBinary<SE3Curve_t>,bp::args("filename"),"Saves *this inside a
+ // binary file.")
+ // .def("loadFromBinary",&SE3Curve_t::loadFromBinary<SE3Curve_t>,bp::args("filename"),"Loads *this from a
+ // binary file.")
+ ;
+
+ /** END SE3 Curve**/
+ /** BEGIN curves conversion**/
+ def("polynomial_from_bezier", polynomial_from_curve<polynomial_t, bezier_t>);
+ def("polynomial_from_hermite", polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>);
+ def("bezier_from_hermite", bezier_from_curve<bezier_t, cubic_hermite_spline_t>);
+ def("bezier_from_polynomial", bezier_from_curve<bezier_t, polynomial_t>);
+ def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier_t>);
+ def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial_t>);
+ /** END curves conversion**/
+
+ optimization::python::exposeOptimization();
+
+#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+ scope().attr("CURVES_WITH_PINOCCHIO_SUPPORT") = true;
+#else
+ scope().attr("CURVES_WITH_PINOCCHIO_SUPPORT") = false;
+#endif
+
+} // End BOOST_PYTHON_MODULE
+} // namespace curves
diff --git a/python/namespace.cpp b/python/namespace.cpp
index 5b5d8fc..dc78295 100644
--- a/python/namespace.cpp
+++ b/python/namespace.cpp
@@ -4,21 +4,18 @@
#include "namespace.h"
-namespace curves
-{
- namespace python
- {
+namespace curves {
+namespace python {
- bp::object getOrCreatePythonNamespace(const std::string & submodule_name)
- {
- bp::scope current_scope;
- std::string current_scope_name(bp::extract<const char*>(current_scope.attr("__name__")));
- std::string complete_submodule_name = current_scope_name + "." + submodule_name;
+bp::object getOrCreatePythonNamespace(const std::string& submodule_name) {
+ bp::scope current_scope;
+ std::string current_scope_name(bp::extract<const char*>(current_scope.attr("__name__")));
+ std::string complete_submodule_name = current_scope_name + "." + submodule_name;
- bp::object submodule(bp::borrowed(PyImport_AddModule(complete_submodule_name.c_str())));
- current_scope.attr(submodule_name.c_str()) = submodule;
+ bp::object submodule(bp::borrowed(PyImport_AddModule(complete_submodule_name.c_str())));
+ current_scope.attr(submodule_name.c_str()) = submodule;
- return submodule;
- }
- } // namespace python
-} // namespace curves
+ return submodule;
+}
+} // namespace python
+} // namespace curves
diff --git a/python/namespace.h b/python/namespace.h
index 6b3878e..2821c34 100644
--- a/python/namespace.h
+++ b/python/namespace.h
@@ -7,21 +7,19 @@
#include <boost/python.hpp>
-namespace curves
-{
- namespace python
- {
- namespace bp = boost::python;
+namespace curves {
+namespace python {
+namespace bp = boost::python;
- ///
- /// \brief Helper to create or simply return an existing namespace in Python
- ///
- /// \param[in] submodule_name name of the submodule
- ///
- /// \returns The submodule related to the namespace name.
- ///
- bp::object getOrCreatePythonNamespace(const std::string & submodule_name);
- } // namespace python
-} // namespace curves
+///
+/// \brief Helper to create or simply return an existing namespace in Python
+///
+/// \param[in] submodule_name name of the submodule
+///
+/// \returns The submodule related to the namespace name.
+///
+bp::object getOrCreatePythonNamespace(const std::string& submodule_name);
+} // namespace python
+} // namespace curves
-#endif // ifndef __python_namespace_h__
+#endif // ifndef __python_namespace_h__
diff --git a/python/optimization_python.cpp b/python/optimization_python.cpp
index d2a075e..f760d89 100644
--- a/python/optimization_python.cpp
+++ b/python/optimization_python.cpp
@@ -8,215 +8,144 @@
#include <boost/python/enum.hpp>
#include <boost/python/bases.hpp>
-namespace curves
-{
-namespace optimization
-{
- namespace python
- {
- static const bool safe = true;
- typedef problem_definition<pointX_t, real> problem_definition_t;
- typedef problem_data<pointX_t, real>problem_data_t;
- typedef quadratic_problem<pointX_t, real> quadratic_problem_t;
-
- problem_data_t setup_control_points_t(problem_definition_t &pDef)
- {
- problem_data_t pData = setup_control_points<pointX_t,real, safe>(pDef);
- return pData;//return new problem_data_t(pData);
- }
-
- quadratic_variable_t problem_t_cost(const quadratic_problem_t& p)
- {
- return p.cost;
- }
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> problem_t_ineqMatrix(const quadratic_problem_t& p)
- {
- return p.ineqMatrix;
- }
- Eigen::Matrix<real, Eigen::Dynamic, 1> problem_t_ineqVector(const quadratic_problem_t& p)
- {
- return p.ineqVector;
- }
-
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> cost_t_quad(const quadratic_variable_t& p)
- {
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A = p.A();
- return A;
- }
- Eigen::Matrix<real, Eigen::Dynamic, 1> cost_t_linear(const quadratic_variable_t & p)
- {
- Eigen::Matrix<real, Eigen::Dynamic, 1> b = p.b();
- return b;
- }
- real cost_t_constant(const quadratic_variable_t & p)
- {
- return p.c();
- }
-
- quadratic_problem_t generate_problem_t(const problem_definition_t &pDef, const quadratic_variable_t & c)
- {
- return generate_problem<pointX_t,real, true>(pDef, c);
- }
-
- quadratic_problem_t generate_integral_problem_t(const problem_definition_t &pDef, const integral_cost_flag c)
- {
- return generate_problem<problem_definition_t::point_t,real, true>(pDef, c);
- }
-
- void set_pd_flag(problem_definition_t* pDef, const int flag)
- {
- pDef->flag = (constraint_flag)(flag);
- }
- void set_start(problem_definition_t* pDef, const pointX_t &val )
- {
- pDef->init_pos = val;
- }
- void set_end(problem_definition_t* pDef, const pointX_t &val )
- {
- pDef->end_pos = val;
- }
- void set_degree(problem_definition_t* pDef, const std::size_t val )
- {
- pDef->degree = val;
- }
- void set_total_time(problem_definition_t* pDef, const double val )
- {
- pDef->totalTime = val;
- }
- void set_split_time(problem_definition_t* pDef, const Eigen::VectorXd& val )
- {
- pDef->splitTimes_ = val;
- }
- Eigen::VectorXd get_split_times(const problem_definition_t* pDef)
- {
- return pDef->splitTimes_;
- }
-
- constraint_flag get_pd_flag(const problem_definition_t* pDef)
- {
- return pDef->flag;
- }
- Eigen::VectorXd get_start(const problem_definition_t* pDef)
- {
- return pDef->init_pos;
- }
- Eigen::VectorXd get_end(const problem_definition_t* pDef)
- {
- return pDef->end_pos;
- }
- std::size_t get_degree(const problem_definition_t* pDef)
- {
- return pDef->degree;
- }
- double get_total_time(const problem_definition_t* pDef)
- {
- return pDef->totalTime;
- }
-
- matrix_pair *get_ineq_at(const problem_definition_t* pDef, const std::size_t idx)
- {
- if (idx > pDef->inequalityMatrices_.size() - 1)
- throw std::runtime_error("required id is beyond number of inequality matrices");
- matrix_pair* res = new matrix_pair(pDef->inequalityMatrices_[idx], pDef->inequalityVectors_[idx]);
- return res;
- }
- bool del_ineq_at(problem_definition_t* pDef, const std::size_t idx)
- {
- if (idx > pDef->inequalityMatrices_.size() - 1)
- return false;
- pDef->inequalityMatrices_.erase(pDef->inequalityMatrices_.begin() + idx -1);
- pDef->inequalityVectors_.erase(pDef->inequalityVectors_.begin() + idx -1);
- return true;
- }
- bool add_ineq_at(problem_definition_t* pDef, const Eigen::MatrixXd ineq, const Eigen::VectorXd vec)
- {
- if (ineq.rows() != vec.rows())
- throw std::runtime_error("ineq vector and matrix do not have the same number of rows");
- if (!(pDef->inequalityMatrices_.empty()) && ineq.cols() != pDef->inequalityMatrices_.back().cols())
- throw std::runtime_error("inequality matrix does not have the same variable dimension as existing matrices");
- pDef->inequalityMatrices_.push_back(ineq);
- pDef->inequalityVectors_.push_back(vec);
- return true;
- }
-
- bezier_linear_variable_t* pDataBezier(const problem_data_t* pData)
- {
- const bezier_linear_variable_t& b = *pData->bezier;
- return new bezier_linear_variable_t(b.waypoints().begin(), b.waypoints().end(),b.min(), b.max(), b.mult_T_);
- }
-
-
- problem_definition_t* wrapProblemDefinitionConstructor(const curve_constraints_t* c)
- {
- return new problem_definition_t(*c);
- }
-
- void exposeOptimization()
- {
- // using the optimization scope
- bp::scope current_scope = curves::python::getOrCreatePythonNamespace("optimization");
- /** BEGIN enums**/
- bp::enum_<constraint_flag>("constraint_flag")
- .value("INIT_POS", INIT_POS)
- .value("INIT_VEL", INIT_VEL)
- .value("INIT_ACC", INIT_ACC)
- .value("INIT_JERK", INIT_JERK)
- .value("END_POS", END_POS)
- .value("END_VEL", END_VEL)
- .value("END_ACC", END_ACC)
- .value("END_JERK", END_JERK)
- .value("ALL", ALL)
- .value("NONE", NONE)
- .export_values();
-
- bp::enum_<integral_cost_flag>("integral_cost_flag")
- .value("DISTANCE", DISTANCE)
- .value("VELOCITY", VELOCITY)
- .value("ACCELERATION", ACCELERATION)
- .value("JERK", JERK)
- .value("FOURTH", FOURTH)
- .value("FIFTH", FIFTH)
- .export_values();
- /** END enum**/
-
-
- bp::class_<quadratic_problem_t>
- ("quadratic_problem", bp::init<>())
- .add_property("cost", &problem_t_cost)
- .add_property("A", &problem_t_ineqMatrix)
- .add_property("b", &problem_t_ineqVector)
- ;
-
- bp::def("setup_control_points", &setup_control_points_t);
- bp::def("generate_problem", &generate_problem_t);
- bp::def("generate_integral_problem", &generate_integral_problem_t);
-
- bp::class_<problem_data_t>
- ("problem_data", bp::no_init)
- .def("bezier", &pDataBezier, bp::return_value_policy<bp::manage_new_object>())
- .def_readonly("numControlPoints", &problem_data_t::numControlPoints)
- .def_readonly("numVariables", &problem_data_t::numVariables)
- .def_readonly("startVariableIndex", &problem_data_t::startVariableIndex)
- .def_readonly("numStateConstraints", &problem_data_t::numStateConstraints)
- ;
-
-
- bp::class_<problem_definition_t, bp::bases<curve_constraints_t> >
- ("problem_definition", bp::init<int>())
- .def("__init__", bp::make_constructor(&wrapProblemDefinitionConstructor))
- .add_property("flag", &get_pd_flag, &set_pd_flag)
- .add_property("init_pos", &get_start, &set_start)
- .add_property("end_pos", &get_end, &set_end)
- .add_property("degree", &get_degree, &set_degree)
- .add_property("totalTime", &get_total_time, &set_total_time)
- .add_property("splits", &get_split_times, &set_split_time)
- .def("inequality", &get_ineq_at, bp::return_value_policy<bp::manage_new_object>())
- .def("removeInequality", &del_ineq_at)
- .def("addInequality", &add_ineq_at)
- ;
-
- }
-
- } // namespace python
-} // namespace optimization
-} // namespace curves
+namespace curves {
+namespace optimization {
+namespace python {
+static const bool safe = true;
+typedef problem_definition<pointX_t, real> problem_definition_t;
+typedef problem_data<pointX_t, real> problem_data_t;
+typedef quadratic_problem<pointX_t, real> quadratic_problem_t;
+
+problem_data_t setup_control_points_t(problem_definition_t& pDef) {
+ problem_data_t pData = setup_control_points<pointX_t, real, safe>(pDef);
+ return pData; // return new problem_data_t(pData);
+}
+
+quadratic_variable_t problem_t_cost(const quadratic_problem_t& p) { return p.cost; }
+Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> problem_t_ineqMatrix(const quadratic_problem_t& p) {
+ return p.ineqMatrix;
+}
+Eigen::Matrix<real, Eigen::Dynamic, 1> problem_t_ineqVector(const quadratic_problem_t& p) { return p.ineqVector; }
+
+Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> cost_t_quad(const quadratic_variable_t& p) {
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A = p.A();
+ return A;
+}
+Eigen::Matrix<real, Eigen::Dynamic, 1> cost_t_linear(const quadratic_variable_t& p) {
+ Eigen::Matrix<real, Eigen::Dynamic, 1> b = p.b();
+ return b;
+}
+real cost_t_constant(const quadratic_variable_t& p) { return p.c(); }
+
+quadratic_problem_t generate_problem_t(const problem_definition_t& pDef, const quadratic_variable_t& c) {
+ return generate_problem<pointX_t, real, true>(pDef, c);
+}
+
+quadratic_problem_t generate_integral_problem_t(const problem_definition_t& pDef, const integral_cost_flag c) {
+ return generate_problem<problem_definition_t::point_t, real, true>(pDef, c);
+}
+
+void set_pd_flag(problem_definition_t* pDef, const int flag) { pDef->flag = (constraint_flag)(flag); }
+void set_start(problem_definition_t* pDef, const pointX_t& val) { pDef->init_pos = val; }
+void set_end(problem_definition_t* pDef, const pointX_t& val) { pDef->end_pos = val; }
+void set_degree(problem_definition_t* pDef, const std::size_t val) { pDef->degree = val; }
+void set_total_time(problem_definition_t* pDef, const double val) { pDef->totalTime = val; }
+void set_split_time(problem_definition_t* pDef, const Eigen::VectorXd& val) { pDef->splitTimes_ = val; }
+Eigen::VectorXd get_split_times(const problem_definition_t* pDef) { return pDef->splitTimes_; }
+
+constraint_flag get_pd_flag(const problem_definition_t* pDef) { return pDef->flag; }
+Eigen::VectorXd get_start(const problem_definition_t* pDef) { return pDef->init_pos; }
+Eigen::VectorXd get_end(const problem_definition_t* pDef) { return pDef->end_pos; }
+std::size_t get_degree(const problem_definition_t* pDef) { return pDef->degree; }
+double get_total_time(const problem_definition_t* pDef) { return pDef->totalTime; }
+
+matrix_pair* get_ineq_at(const problem_definition_t* pDef, const std::size_t idx) {
+ if (idx > pDef->inequalityMatrices_.size() - 1)
+ throw std::runtime_error("required id is beyond number of inequality matrices");
+ matrix_pair* res = new matrix_pair(pDef->inequalityMatrices_[idx], pDef->inequalityVectors_[idx]);
+ return res;
+}
+bool del_ineq_at(problem_definition_t* pDef, const std::size_t idx) {
+ if (idx > pDef->inequalityMatrices_.size() - 1) return false;
+ pDef->inequalityMatrices_.erase(pDef->inequalityMatrices_.begin() + idx - 1);
+ pDef->inequalityVectors_.erase(pDef->inequalityVectors_.begin() + idx - 1);
+ return true;
+}
+bool add_ineq_at(problem_definition_t* pDef, const Eigen::MatrixXd ineq, const Eigen::VectorXd vec) {
+ if (ineq.rows() != vec.rows())
+ throw std::runtime_error("ineq vector and matrix do not have the same number of rows");
+ if (!(pDef->inequalityMatrices_.empty()) && ineq.cols() != pDef->inequalityMatrices_.back().cols())
+ throw std::runtime_error("inequality matrix does not have the same variable dimension as existing matrices");
+ pDef->inequalityMatrices_.push_back(ineq);
+ pDef->inequalityVectors_.push_back(vec);
+ return true;
+}
+
+bezier_linear_variable_t* pDataBezier(const problem_data_t* pData) {
+ const bezier_linear_variable_t& b = *pData->bezier;
+ return new bezier_linear_variable_t(b.waypoints().begin(), b.waypoints().end(), b.min(), b.max(), b.mult_T_);
+}
+
+problem_definition_t* wrapProblemDefinitionConstructor(const curve_constraints_t* c) {
+ return new problem_definition_t(*c);
+}
+
+void exposeOptimization() {
+ // using the optimization scope
+ bp::scope current_scope = curves::python::getOrCreatePythonNamespace("optimization");
+ /** BEGIN enums**/
+ bp::enum_<constraint_flag>("constraint_flag")
+ .value("INIT_POS", INIT_POS)
+ .value("INIT_VEL", INIT_VEL)
+ .value("INIT_ACC", INIT_ACC)
+ .value("INIT_JERK", INIT_JERK)
+ .value("END_POS", END_POS)
+ .value("END_VEL", END_VEL)
+ .value("END_ACC", END_ACC)
+ .value("END_JERK", END_JERK)
+ .value("ALL", ALL)
+ .value("NONE", NONE)
+ .export_values();
+
+ bp::enum_<integral_cost_flag>("integral_cost_flag")
+ .value("DISTANCE", DISTANCE)
+ .value("VELOCITY", VELOCITY)
+ .value("ACCELERATION", ACCELERATION)
+ .value("JERK", JERK)
+ .value("FOURTH", FOURTH)
+ .value("FIFTH", FIFTH)
+ .export_values();
+ /** END enum**/
+
+ bp::class_<quadratic_problem_t>("quadratic_problem", bp::init<>())
+ .add_property("cost", &problem_t_cost)
+ .add_property("A", &problem_t_ineqMatrix)
+ .add_property("b", &problem_t_ineqVector);
+
+ bp::def("setup_control_points", &setup_control_points_t);
+ bp::def("generate_problem", &generate_problem_t);
+ bp::def("generate_integral_problem", &generate_integral_problem_t);
+
+ bp::class_<problem_data_t>("problem_data", bp::no_init)
+ .def("bezier", &pDataBezier, bp::return_value_policy<bp::manage_new_object>())
+ .def_readonly("numControlPoints", &problem_data_t::numControlPoints)
+ .def_readonly("numVariables", &problem_data_t::numVariables)
+ .def_readonly("startVariableIndex", &problem_data_t::startVariableIndex)
+ .def_readonly("numStateConstraints", &problem_data_t::numStateConstraints);
+
+ bp::class_<problem_definition_t, bp::bases<curve_constraints_t> >("problem_definition", bp::init<int>())
+ .def("__init__", bp::make_constructor(&wrapProblemDefinitionConstructor))
+ .add_property("flag", &get_pd_flag, &set_pd_flag)
+ .add_property("init_pos", &get_start, &set_start)
+ .add_property("end_pos", &get_end, &set_end)
+ .add_property("degree", &get_degree, &set_degree)
+ .add_property("totalTime", &get_total_time, &set_total_time)
+ .add_property("splits", &get_split_times, &set_split_time)
+ .def("inequality", &get_ineq_at, bp::return_value_policy<bp::manage_new_object>())
+ .def("removeInequality", &del_ineq_at)
+ .def("addInequality", &add_ineq_at);
+}
+
+} // namespace python
+} // namespace optimization
+} // namespace curves
diff --git a/python/optimization_python.h b/python/optimization_python.h
index 8cbeca0..49ce8a1 100644
--- a/python/optimization_python.h
+++ b/python/optimization_python.h
@@ -9,16 +9,13 @@
#ifndef _OPTIMIZATION_PYTHON
#define _OPTIMIZATION_PYTHON
-namespace curves
-{
-namespace optimization
-{
-namespace python
-{
+namespace curves {
+namespace optimization {
+namespace python {
void exposeOptimization();
-} // namespace python
-} // namespace optimization
-} // namespace curves
+} // namespace python
+} // namespace optimization
+} // namespace curves
-#endif //_OPTIMIZATION_PYTHON
+#endif //_OPTIMIZATION_PYTHON
diff --git a/python/python_variables.cpp b/python/python_variables.cpp
index 159ca36..13e66c9 100644
--- a/python/python_variables.cpp
+++ b/python/python_variables.cpp
@@ -3,85 +3,67 @@
#include <Eigen/Core>
-namespace curves
-{
- std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list_t& matrices, const point_list_t& vectors)
- {
- assert(vectors.cols() * 3 == matrices.cols() ) ;
- std::vector<linear_variable_t> res;
- for(int i =0;i<vectors.cols();++i)
- {
- res.push_back(linear_variable_t(matrices.block<3,3>(0,i*3), vectors.col(i)));
- }
- return res;
+namespace curves {
+std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list_t& matrices, const point_list_t& vectors) {
+ assert(vectors.cols() * 3 == matrices.cols());
+ std::vector<linear_variable_t> res;
+ for (int i = 0; i < vectors.cols(); ++i) {
+ res.push_back(linear_variable_t(matrices.block<3, 3>(0, i * 3), vectors.col(i)));
}
+ return res;
+}
- linear_variable_t fillWithZeros(const linear_variable_t& var, const std::size_t totalvar, const std::size_t i)
- {
- linear_variable_t::matrix_x_t B(linear_variable_t::matrix_x_t::Zero(dim,totalvar*dim));
- B.block(0,dim*i,dim,dim) = var.B();
- return linear_variable_t (B,var.c());
- }
+linear_variable_t fillWithZeros(const linear_variable_t& var, const std::size_t totalvar, const std::size_t i) {
+ linear_variable_t::matrix_x_t B(linear_variable_t::matrix_x_t::Zero(dim, totalvar * dim));
+ B.block(0, dim * i, dim, dim) = var.B();
+ return linear_variable_t(B, var.c());
+}
- std::vector<linear_variable_t> computeLinearControlPoints(const point_list_t& matrices, const point_list_t& vectors)
- {
- std::vector<linear_variable_t> res;
- std::vector<linear_variable_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));
- return res;
- }
+std::vector<linear_variable_t> computeLinearControlPoints(const point_list_t& matrices, const point_list_t& vectors) {
+ std::vector<linear_variable_t> res;
+ std::vector<linear_variable_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));
+ return res;
+}
- /*linear variable control points*/
- bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors)
- {
- std::vector<linear_variable_t> asVector = computeLinearControlPoints(matrices, vectors);
- return new bezier_linear_variable_t(asVector.begin(), asVector.end()) ;
- }
-
- 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<linear_variable_t> asVector = computeLinearControlPoints(matrices, vectors);
- return new bezier_linear_variable_t(asVector.begin(), asVector.end(), T_min, T_max) ;
- }
+/*linear variable control points*/
+bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors) {
+ std::vector<linear_variable_t> asVector = computeLinearControlPoints(matrices, vectors);
+ return new bezier_linear_variable_t(asVector.begin(), asVector.end());
+}
+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<linear_variable_t> asVector = computeLinearControlPoints(matrices, vectors);
+ return new bezier_linear_variable_t(asVector.begin(), asVector.end(), T_min, T_max);
+}
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> cost_t_quad(const quadratic_variable_t& p)
- {
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A = p.A();
- return A;
- }
- Eigen::Matrix<real, Eigen::Dynamic, 1> cost_t_linear(const quadratic_variable_t & p)
- {
- Eigen::Matrix<real, Eigen::Dynamic, 1> b = p.b();
- return b;
- }
- real cost_t_constant(const quadratic_variable_t & p)
- {
- return p.c();
- }
-
+Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> cost_t_quad(const quadratic_variable_t& p) {
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A = p.A();
+ return A;
+}
+Eigen::Matrix<real, Eigen::Dynamic, 1> cost_t_linear(const quadratic_variable_t& p) {
+ Eigen::Matrix<real, Eigen::Dynamic, 1> b = p.b();
+ return b;
+}
+real cost_t_constant(const quadratic_variable_t& p) { return p.c(); }
- linear_variable_t*
- 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();
- // retrieve num variables.
- std::size_t dim = wps[0].B().cols();
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices (dim,wps.size() * 3);
- Eigen::Matrix<real, Eigen::Dynamic, 1> vectors =
- Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(3*wps.size());
- int i = 0;
- for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++i)
- {
- matrices.block(0,i*3,dim,3) = cit->B().transpose();
- vectors.segment<3>(i*3,i*3+2) = cit->c();
- }
- return new linear_variable_t(matrices.transpose(), vectors.transpose());
+linear_variable_t* 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();
+ // retrieve num variables.
+ std::size_t dim = wps[0].B().cols();
+ Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices(dim, wps.size() * 3);
+ Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(3 * wps.size());
+ int i = 0;
+ for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++i) {
+ matrices.block(0, i * 3, dim, 3) = cit->B().transpose();
+ vectors.segment<3>(i * 3, i * 3 + 2) = cit->c();
}
+ return new linear_variable_t(matrices.transpose(), vectors.transpose());
+}
-} // namespace curves
+} // namespace curves
diff --git a/python/python_variables.h b/python/python_variables.h
index 8beb920..59ae2fa 100644
--- a/python/python_variables.h
+++ b/python/python_variables.h
@@ -20,93 +20,89 @@
#ifndef _VARIABLES_PYTHON_BINDINGS
#define _VARIABLES_PYTHON_BINDINGS
-
-namespace curves
-{
- static const int dim = 3;
- typedef linear_variable<real, true> linear_variable_t;
- typedef quadratic_variable<real> quadratic_variable_t;
- typedef bezier_curve <real, real, true, linear_variable_t> bezier_linear_variable_t;
-
- /*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);
-
- typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
-
- struct matrix_pair
- {
- matrix_pair() {}
- matrix_pair(const Eigen::Ref<const matrix_x_t > A, const Eigen::Ref<const matrix_x_t > b)
- : A_(A), b_(b){}
- matrix_x_t A_;
- matrix_x_t b_;
- matrix_x_t A() {return A_;}
- matrix_x_t b() {return b_;}
- };
-
- Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> cost_t_quad(const quadratic_variable_t& p);
- Eigen::Matrix<real, Eigen::Dynamic, 1> cost_t_linear(const quadratic_variable_t & p);
- real cost_t_constant(const quadratic_variable_t & p);
-
- linear_variable_t *wayPointsToLists(const bezier_linear_variable_t& self);
-
- 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]);
- }
- };
-
-
- /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
- typedef double real;
- typedef Eigen::VectorXd time_waypoints_t;
-
- typedef Eigen::VectorXd pointX_t;
- typedef Eigen::Matrix<double,3, 1> point3_t;
- typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1> ret_pointX_t;
- typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
- typedef Eigen::MatrixXd pointX_list_t;
- typedef std::vector<pointX_t,Eigen::aligned_allocator<pointX_t> > t_pointX_t;
- typedef std::vector<pointX_t,Eigen::aligned_allocator<point3_t> > t_point3_t;
- typedef std::vector<pair_pointX_tangent_t,Eigen::aligned_allocator<pair_pointX_tangent_t> > t_pair_pointX_tangent_t;
- typedef curves::curve_constraints<pointX_t> curve_constraints_t;
- typedef curves::curve_constraints<point3_t> curve_constraints3_t;
- typedef std::pair<real, pointX_t> waypoint_t;
- typedef std::vector<waypoint_t> t_waypoint_t;
- typedef Eigen::Matrix<real,3, 3> matrix3_t;
- typedef Eigen::Matrix<real,4, 4> matrix4_t;
- typedef Eigen::Transform<double,3,Eigen::Affine> transform_t;
- typedef Eigen::Quaternion<real> quaternion_t;
-
-
- // Curves
- typedef curve_abc<real, real, true, pointX_t> curve_abc_t; // generic class of curve
- typedef curve_abc<real, real, true, point3_t> curve_3_t; // generic class of curve of size 3
- typedef curve_abc<real, real, true, matrix3_t,point3_t> curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
- typedef curves::cubic_hermite_spline <real, real, true, pointX_t> cubic_hermite_spline_t;
- typedef curves::bezier_curve <real, real, true, pointX_t> bezier_t;
- typedef curves::bezier_curve <real, real, true, Eigen::Vector3d> bezier3_t;
- typedef curves::polynomial <real, real, true, pointX_t, t_pointX_t> polynomial_t;
- typedef polynomial_t::coeff_t coeff_t;
- typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
- typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, bezier_t> piecewise_bezier_curve_t;
- typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
- typedef curves::piecewise_curve <real, real, true, linear_variable_t, std::vector<linear_variable_t, Eigen::aligned_allocator<linear_variable_t> >, bezier_linear_variable_t> piecewise_bezier_linear_curve_t;
- typedef curves::exact_cubic <real, real, true, pointX_t, t_pointX_t> exact_cubic_t;
- typedef SO3Linear <double, double, true> SO3Linear_t;
- typedef SE3Curve <double, double, true> SE3Curve_t;
- typedef curves::Bern<double> bernstein_t;
-
- /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
-} //namespace curve.
-
+namespace curves {
+static const int dim = 3;
+typedef linear_variable<real, true> linear_variable_t;
+typedef quadratic_variable<real> quadratic_variable_t;
+typedef bezier_curve<real, real, true, linear_variable_t> bezier_linear_variable_t;
+
+/*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);
+
+typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
+
+struct matrix_pair {
+ matrix_pair() {}
+ matrix_pair(const Eigen::Ref<const matrix_x_t> A, const Eigen::Ref<const matrix_x_t> b) : A_(A), b_(b) {}
+ matrix_x_t A_;
+ matrix_x_t b_;
+ matrix_x_t A() { return A_; }
+ matrix_x_t b() { return b_; }
+};
+
+Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> cost_t_quad(const quadratic_variable_t& p);
+Eigen::Matrix<real, Eigen::Dynamic, 1> cost_t_linear(const quadratic_variable_t& p);
+real cost_t_constant(const quadratic_variable_t& p);
+
+linear_variable_t* wayPointsToLists(const bezier_linear_variable_t& self);
+
+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]);
+ }
+};
+
+/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
+typedef double real;
+typedef Eigen::VectorXd time_waypoints_t;
+
+typedef Eigen::VectorXd pointX_t;
+typedef Eigen::Matrix<double, 3, 1> point3_t;
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1> ret_pointX_t;
+typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
+typedef Eigen::MatrixXd pointX_list_t;
+typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
+typedef std::vector<pointX_t, Eigen::aligned_allocator<point3_t> > t_point3_t;
+typedef std::vector<pair_pointX_tangent_t, Eigen::aligned_allocator<pair_pointX_tangent_t> > t_pair_pointX_tangent_t;
+typedef curves::curve_constraints<pointX_t> curve_constraints_t;
+typedef curves::curve_constraints<point3_t> curve_constraints3_t;
+typedef std::pair<real, pointX_t> waypoint_t;
+typedef std::vector<waypoint_t> t_waypoint_t;
+typedef Eigen::Matrix<real, 3, 3> matrix3_t;
+typedef Eigen::Matrix<real, 4, 4> matrix4_t;
+typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
+typedef Eigen::Quaternion<real> quaternion_t;
+
+// Curves
+typedef curve_abc<real, real, true, pointX_t> curve_abc_t; // generic class of curve
+typedef curve_abc<real, real, true, point3_t> curve_3_t; // generic class of curve of size 3
+typedef curve_abc<real, real, true, matrix3_t, point3_t>
+ curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
+typedef curves::cubic_hermite_spline<real, real, true, pointX_t> cubic_hermite_spline_t;
+typedef curves::bezier_curve<real, real, true, pointX_t> bezier_t;
+typedef curves::bezier_curve<real, real, true, Eigen::Vector3d> bezier3_t;
+typedef curves::polynomial<real, real, true, pointX_t, t_pointX_t> polynomial_t;
+typedef polynomial_t::coeff_t coeff_t;
+typedef curves::piecewise_curve<real, real, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
+typedef curves::piecewise_curve<real, real, true, pointX_t, t_pointX_t, bezier_t> piecewise_bezier_curve_t;
+typedef curves::piecewise_curve<real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t>
+ piecewise_cubic_hermite_curve_t;
+typedef curves::piecewise_curve<real, real, true, linear_variable_t,
+ std::vector<linear_variable_t, Eigen::aligned_allocator<linear_variable_t> >,
+ bezier_linear_variable_t>
+ piecewise_bezier_linear_curve_t;
+typedef curves::exact_cubic<real, real, true, pointX_t, t_pointX_t> exact_cubic_t;
+typedef SO3Linear<double, double, true> SO3Linear_t;
+typedef SE3Curve<double, double, true> SE3Curve_t;
+typedef curves::Bern<double> bernstein_t;
+
+/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
+} // namespace curves
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::cubic_hermite_spline_t)
@@ -127,5 +123,4 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::linear_variable_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier_linear_variable_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::matrix_pair)
-
-#endif //_VARIABLES_PYTHON_BINDINGS
+#endif //_VARIABLES_PYTHON_BINDINGS
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 655d530..d76678c 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -18,47 +18,43 @@
using namespace std;
-namespace curves
-{
+namespace curves {
typedef Eigen::Vector3d point_t;
typedef Eigen::VectorXd pointX_t;
typedef Eigen::Quaternion<double> quaternion_t;
-typedef std::vector<pointX_t,Eigen::aligned_allocator<pointX_t> > t_pointX_t;
-typedef curve_abc <double, double, true, pointX_t> curve_abc_t;
-typedef polynomial <double, double, true, pointX_t, t_pointX_t> polynomial_t;
-typedef exact_cubic <double, double, true, pointX_t> exact_cubic_t;
-typedef exact_cubic <double, double, true, Eigen::Matrix<double,1,1> > exact_cubic_one;
-typedef bezier_curve <double, double, true, pointX_t> bezier_curve_t;
-typedef cubic_hermite_spline <double, double, true, pointX_t> cubic_hermite_spline_t;
-typedef piecewise_curve <double, double, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
-typedef piecewise_curve <double, double, true, pointX_t, t_pointX_t, bezier_curve_t> piecewise_bezier_curve_t;
-typedef piecewise_curve <double, double, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
+typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
+typedef curve_abc<double, double, true, pointX_t> curve_abc_t;
+typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
+typedef exact_cubic<double, double, true, pointX_t> exact_cubic_t;
+typedef exact_cubic<double, double, true, Eigen::Matrix<double, 1, 1> > exact_cubic_one;
+typedef bezier_curve<double, double, true, pointX_t> bezier_curve_t;
+typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
+typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
+typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, bezier_curve_t> piecewise_bezier_curve_t;
+typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, cubic_hermite_spline_t>
+ piecewise_cubic_hermite_curve_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef std::pair<double, pointX_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
-typedef Eigen::Matrix<double,1,1> point_one;
+typedef Eigen::Matrix<double, 1, 1> point_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
typedef std::pair<pointX_t, pointX_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 SO3Linear <double, double, true> SO3Linear_t;
-typedef SE3Curve <double, double, true> SE3Curve_t;
-typedef Eigen::Transform<double,3,Eigen::Affine> transform_t;
+typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
+typedef SO3Linear<double, double, true> SO3Linear_t;
+typedef SE3Curve<double, double, true> SE3Curve_t;
+typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
const double margin = 1e-3;
-bool QuasiEqual(const double a, const double b)
-{
- return std::fabs(a-b)<margin;
-}
-bool QuasiEqual(const point_t a, const point_t b)
-{
+bool QuasiEqual(const double a, const double b) { return std::fabs(a - b) < margin; }
+bool QuasiEqual(const point_t a, const point_t b) {
bool equal = true;
- for(size_t i = 0 ; i < 3 ; ++i){
- equal = equal && QuasiEqual(a[i],b[i]);
+ for (size_t i = 0; i < 3; ++i) {
+ equal = equal && QuasiEqual(a[i], b[i]);
}
return equal;
}
-} // End namespace curves
+} // End namespace curves
using namespace curves;
@@ -1313,21 +1309,20 @@ void curveAbcDimDynamicTest(bool& error) {
}
}
-void PiecewisePolynomialCurveFromDiscretePoints(bool& error)
-{
+void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
std::string errMsg("PiecewisePolynomialCurveFromDiscretePoints, 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 d0(1.,1.,1.);
- point_t d1(2.,2.,2.);
- point_t d2(3.,3.,3.);
- point_t d3(5.,5.,5.);
- point_t dd0(1.5,1.5,1.5);
- point_t dd1(2.5,2.5,2.5);
- point_t dd2(3.5,3.5,3.5);
- point_t dd3(5.5,5.5,5.5);
+ 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 d0(1., 1., 1.);
+ point_t d1(2., 2., 2.);
+ point_t d2(3., 3., 3.);
+ point_t d3(5., 5., 5.);
+ point_t dd0(1.5, 1.5, 1.5);
+ point_t dd1(2.5, 2.5, 2.5);
+ point_t dd2(3.5, 3.5, 3.5);
+ point_t dd3(5.5, 5.5, 5.5);
double t0 = 1.0;
double t1 = 1.5;
double t2 = 3.0;
@@ -1354,53 +1349,44 @@ void PiecewisePolynomialCurveFromDiscretePoints(bool& error)
time_points.push_back(t3);
// Piecewise polynomial curve C0 => Linear interpolation between points
- piecewise_polynomial_curve_t ppc_C0 = piecewise_polynomial_curve_t::
- convert_discrete_points_to_polynomial<polynomial_t>(points,time_points);
- if (!ppc_C0.is_continuous(0))
- {
- std::cout<<"PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C0"<<std::endl;
+ piecewise_polynomial_curve_t ppc_C0 =
+ piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points, time_points);
+ if (!ppc_C0.is_continuous(0)) {
+ std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C0" << std::endl;
error = true;
}
- for (std::size_t i=0; i<points.size(); i++)
- {
+ for (std::size_t i = 0; i < points.size(); i++) {
ComparePoints(points[i], ppc_C0(time_points[i]), errMsg, error);
}
- point_t pos_between_po_and_p1( (p1[0]+p0[0])/2.0, (p1[1]+p0[1])/2.0,(p1[2]+p0[2])/2.0 );
- double time_between_po_and_p1 = (t0+t1)/2.0;
+ point_t pos_between_po_and_p1((p1[0] + p0[0]) / 2.0, (p1[1] + p0[1]) / 2.0, (p1[2] + p0[2]) / 2.0);
+ double time_between_po_and_p1 = (t0 + t1) / 2.0;
ComparePoints(pos_between_po_and_p1, ppc_C0(time_between_po_and_p1), errMsg, error);
// Piecewise polynomial curve C1
- piecewise_polynomial_curve_t ppc_C1 = piecewise_polynomial_curve_t::
- convert_discrete_points_to_polynomial<polynomial_t>(points,
- points_derivative,
- time_points);
- if (!ppc_C1.is_continuous(1))
- {
- std::cout<<"PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1"<<std::endl;
+ piecewise_polynomial_curve_t ppc_C1 =
+ piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points, points_derivative,
+ time_points);
+ if (!ppc_C1.is_continuous(1)) {
+ std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1" << std::endl;
error = true;
}
- for (std::size_t i=0; i<points.size(); i++)
- {
+ for (std::size_t i = 0; i < points.size(); i++) {
ComparePoints(points[i], ppc_C1(time_points[i]), errMsg, error);
- ComparePoints(points_derivative[i], ppc_C1.derivate(time_points[i],1), errMsg, error);
+ ComparePoints(points_derivative[i], ppc_C1.derivate(time_points[i], 1), errMsg, error);
}
// Piecewise polynomial curve C2
- piecewise_polynomial_curve_t ppc_C2 = piecewise_polynomial_curve_t::
- convert_discrete_points_to_polynomial<polynomial_t>(points,
- points_derivative,
- points_second_derivative,
- time_points);
- if (!ppc_C2.is_continuous(2))
- {
- std::cout<<"PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1"<<std::endl;
- error = true;
- }
- for (std::size_t i=0; i<points.size(); i++)
- {
+ piecewise_polynomial_curve_t ppc_C2 =
+ piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(
+ points, points_derivative, points_second_derivative, time_points);
+ if (!ppc_C2.is_continuous(2)) {
+ std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1" << std::endl;
+ error = true;
+ }
+ for (std::size_t i = 0; i < points.size(); i++) {
ComparePoints(points[i], ppc_C2(time_points[i]), errMsg, error);
- ComparePoints(points_derivative[i], ppc_C2.derivate(time_points[i],1), errMsg, error);
- ComparePoints(points_second_derivative[i], ppc_C2.derivate(time_points[i],2), errMsg, error);
+ ComparePoints(points_derivative[i], ppc_C2.derivate(time_points[i], 1), errMsg, error);
+ ComparePoints(points_second_derivative[i], ppc_C2.derivate(time_points[i], 2), errMsg, error);
}
}
@@ -1633,140 +1619,147 @@ void polynomialFromBoundaryConditions(bool& error) {
}
}
+void so3LinearTest(bool& error) {
+ quaternion_t q0(1, 0, 0, 0);
+ quaternion_t q1(0.7071, 0.7071, 0, 0);
+ SO3Linear_t so3Traj(q0, q1, 0., 1.5);
-void so3LinearTest(bool& error){
- quaternion_t q0(1,0,0,0);
- quaternion_t q1(0.7071,0.7071,0,0);
- SO3Linear_t so3Traj(q0,q1,0.,1.5);
-
- if(so3Traj.min() != 0 ){
- error=true;
- std::cout<<"Min bound not respected"<<std::endl;
+ if (so3Traj.min() != 0) {
+ error = true;
+ std::cout << "Min bound not respected" << std::endl;
}
- if(so3Traj.max() != 1.5 ){
- error=true;
- std::cout<<"Max bound not respected"<<std::endl;
+ if (so3Traj.max() != 1.5) {
+ error = true;
+ std::cout << "Max bound not respected" << std::endl;
}
- if(!so3Traj.computeAsQuaternion(0).isApprox(q0)){
- error=true;
- std::cout<<"evaluate at t=0 is not the init quaternion"<<std::endl;
+ if (!so3Traj.computeAsQuaternion(0).isApprox(q0)) {
+ error = true;
+ std::cout << "evaluate at t=0 is not the init quaternion" << std::endl;
}
- if(so3Traj(0) != q0.toRotationMatrix()){
- error=true;
- std::cout<<"evaluate at t=0 is not the init rotation"<<std::endl;
+ if (so3Traj(0) != q0.toRotationMatrix()) {
+ error = true;
+ std::cout << "evaluate at t=0 is not the init rotation" << std::endl;
}
- if(!so3Traj.computeAsQuaternion(1.5).isApprox(q1)){
- error=true;
- std::cout<<"evaluate at t=max is not the final quaternion"<<std::endl;
+ if (!so3Traj.computeAsQuaternion(1.5).isApprox(q1)) {
+ error = true;
+ std::cout << "evaluate at t=max is not the final quaternion" << std::endl;
}
- if(so3Traj(1.5) != q1.toRotationMatrix()){
- error=true;
- std::cout<<"evaluate at t=max is not the final rotation"<<std::endl;
+ if (so3Traj(1.5) != q1.toRotationMatrix()) {
+ error = true;
+ std::cout << "evaluate at t=max is not the final rotation" << std::endl;
}
- //check derivatives :
- if(so3Traj.derivate(0,1) != so3Traj.derivate(1.,1)){
- error=true;
- std::cout<<"first order derivative should be constant."<<std::endl;
+ // check derivatives :
+ if (so3Traj.derivate(0, 1) != so3Traj.derivate(1., 1)) {
+ error = true;
+ std::cout << "first order derivative should be constant." << std::endl;
}
- if(so3Traj.derivate(0,2) != point_t::Zero(3)){
- error=true;
- std::cout<<"second order derivative should be null"<<std::endl;
+ if (so3Traj.derivate(0, 2) != point_t::Zero(3)) {
+ error = true;
+ std::cout << "second order derivative should be null" << std::endl;
}
// check if errors are correctly raised :
- try{
+ try {
so3Traj(-0.1);
error = true;
- std::cout<<"SO3Linear: calling () with t < tmin should raise an invalid_argument error"<<std::endl;
- }catch(std::invalid_argument e){ }
- try{
+ std::cout << "SO3Linear: calling () with t < tmin should raise an invalid_argument error" << std::endl;
+ } catch (std::invalid_argument e) {
+ }
+ try {
so3Traj(1.7);
error = true;
- std::cout<<"SO3Linear: calling () with t > tmin should raise an invalid_argument error"<<std::endl;
- }catch(std::invalid_argument e){ }
- try{
- so3Traj.derivate(0,0);
+ std::cout << "SO3Linear: calling () with t > tmin should raise an invalid_argument error" << std::endl;
+ } catch (std::invalid_argument e) {
+ }
+ try {
+ so3Traj.derivate(0, 0);
error = true;
- std::cout<<"SO3Linear: calling derivate with order = 0 should raise an invalid_argument error"<<std::endl;
- }catch(std::invalid_argument e){ }
+ std::cout << "SO3Linear: calling derivate with order = 0 should raise an invalid_argument error" << std::endl;
+ } catch (std::invalid_argument e) {
+ }
}
-void se3CurveTest(bool& error){
- quaternion_t q0(1,0,0,0);
- quaternion_t q1(0.,1.,0,0);
- pointX_t p0 = point_t(1.,1.5,-2.);
- pointX_t p1 = point_t(3.,0,1.);
+void se3CurveTest(bool& error) {
+ quaternion_t q0(1, 0, 0, 0);
+ quaternion_t q1(0., 1., 0, 0);
+ pointX_t p0 = point_t(1., 1.5, -2.);
+ pointX_t p1 = point_t(3., 0, 1.);
- double min = 0.5,max = 2.;
+ double min = 0.5, max = 2.;
- // constructor from init/end position/rotation : automatically create a linear interpolation for position and slerp for rotation
- SE3Curve_t cLinear(p0,p1,q0,q1,min,max);
+ // constructor from init/end position/rotation : automatically create a linear interpolation for position and slerp
+ // for rotation
+ SE3Curve_t cLinear(p0, p1, q0, q1, min, max);
transform_t transformInit = cLinear(min);
transform_t transformEnd = cLinear(max);
- transform_t transformMid = cLinear((max+min)/2.);
- if(! transformInit.translation().isApprox(p0)){
- error=true;
- std::cout<<"Init position of the curve is not correct."<<std::endl;
+ transform_t transformMid = cLinear((max + min) / 2.);
+ if (!transformInit.translation().isApprox(p0)) {
+ error = true;
+ std::cout << "Init position of the curve is not correct." << std::endl;
}
- if(! transformInit.rotation().isApprox(q0.toRotationMatrix())){
- error=true;
- std::cout<<"Init rotation of the curve is not correct."<<std::endl;
+ if (!transformInit.rotation().isApprox(q0.toRotationMatrix())) {
+ error = true;
+ std::cout << "Init rotation of the curve is not correct." << std::endl;
}
- if(! transformEnd.translation().isApprox(p1)){
- error=true;
- std::cout<<"End position of the curve is not correct."<<std::endl;
+ if (!transformEnd.translation().isApprox(p1)) {
+ error = true;
+ std::cout << "End position of the curve is not correct." << std::endl;
}
- if(! transformEnd.rotation().isApprox(q1.toRotationMatrix())){
- error=true;
- std::cout<<"End rotation of the curve is not correct."<<std::endl;
+ if (!transformEnd.rotation().isApprox(q1.toRotationMatrix())) {
+ error = true;
+ std::cout << "End rotation of the curve is not correct." << std::endl;
}
- quaternion_t qMid(sqrt(2.)/2.,sqrt(2.)/2.,0,0);
- point_t pMid = (p0+p1)/2.;
- if(! transformMid.translation().isApprox(pMid)){
- error=true;
- std::cout<<"Mid position of the curve is not correct."<<std::endl;
+ quaternion_t qMid(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0);
+ point_t pMid = (p0 + p1) / 2.;
+ if (!transformMid.translation().isApprox(pMid)) {
+ error = true;
+ std::cout << "Mid position of the curve is not correct." << std::endl;
}
- if(! transformMid.rotation().isApprox(qMid.toRotationMatrix())){
- error=true;
- std::cout<<"Mid rotation of the curve is not correct."<<std::endl;
+ if (!transformMid.rotation().isApprox(qMid.toRotationMatrix())) {
+ error = true;
+ std::cout << "Mid rotation of the curve is not correct." << std::endl;
}
- //constructor with specific translation curve
+ // constructor with specific translation curve
SE3Curve_t cBezier;
- { // inner scope to check what happen when translation_bezier is out of scope
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
- point_t d(3,6,7);
+ { // inner scope to check what happen when translation_bezier is out of scope
+ 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);params.push_back(b);params.push_back(c);params.push_back(d);
- bezier_curve_t* translation_bezier = new bezier_curve_t(params.begin(),params.end(),min,max);
- cBezier = SE3Curve_t(translation_bezier,q0.toRotationMatrix(),q1.toRotationMatrix());
+ params.push_back(a);
+ params.push_back(b);
+ params.push_back(c);
+ params.push_back(d);
+ bezier_curve_t* translation_bezier = new bezier_curve_t(params.begin(), params.end(), min, max);
+ cBezier = SE3Curve_t(translation_bezier, q0.toRotationMatrix(), q1.toRotationMatrix());
p0 = (*translation_bezier)(min);
p1 = (*translation_bezier)(max);
- pMid = (*translation_bezier)((max+min)/2.);
- if(cBezier.min() != min){
+ pMid = (*translation_bezier)((max + min) / 2.);
+ if (cBezier.min() != min) {
error = true;
- std::cout<<"SE3 constructor from translation bezier do not respect the min time interval"<<std::endl;
+ std::cout << "SE3 constructor from translation bezier do not respect the min time interval" << std::endl;
}
- if(cBezier.max() != max){
+ if (cBezier.max() != max) {
error = true;
- std::cout<<"SE3 constructor from translation bezier do not respect the max time interval"<<std::endl;
+ std::cout << "SE3 constructor from translation bezier do not respect the max time interval" << std::endl;
}
double t = min;
- while(t<max){
- if(!cBezier(t).translation().isApprox((*translation_bezier)(t))){
+ while (t < max) {
+ if (!cBezier(t).translation().isApprox((*translation_bezier)(t))) {
error = true;
- std::cout<<"SE3 translation is not equivalent to bezier for t = "<<t<<std::endl;
+ std::cout << "SE3 translation is not equivalent to bezier for t = " << t << std::endl;
}
t += 0.1;
}
// check the derivatives for translation:
- for(size_t i = 1 ; i < 3 ; i++){
+ for (size_t i = 1; i < 3; i++) {
t = min;
- while(t<max){
- if(!cBezier.derivate(t,i).head<3>().isApprox(translation_bezier->derivate(t,i))){
+ while (t < max) {
+ if (!cBezier.derivate(t, i).head<3>().isApprox(translation_bezier->derivate(t, i))) {
error = true;
- std::cout<<"SE3 curve derivative is not equivalent to bezier for t = "<<t<<" and order = "<<i<<std::endl;
+ std::cout << "SE3 curve derivative is not equivalent to bezier for t = " << t << " and order = " << i
+ << std::endl;
}
t += 0.1;
}
@@ -1775,62 +1768,63 @@ void se3CurveTest(bool& error){
// check the rotation
transformInit = cBezier(min);
transformEnd = cBezier(max);
- transformMid = cBezier((max+min)/2.);
- if(! transformInit.translation().isApprox(p0)){
- error=true;
- std::cout<<"Init position of the curve is not correct."<<std::endl;
+ transformMid = cBezier((max + min) / 2.);
+ if (!transformInit.translation().isApprox(p0)) {
+ error = true;
+ std::cout << "Init position of the curve is not correct." << std::endl;
}
- if(! transformInit.rotation().isApprox(q0.toRotationMatrix())){
- error=true;
- std::cout<<"Init rotation of the curve is not correct."<<std::endl;
+ if (!transformInit.rotation().isApprox(q0.toRotationMatrix())) {
+ error = true;
+ std::cout << "Init rotation of the curve is not correct." << std::endl;
}
- if(! transformEnd.translation().isApprox(p1)){
- error=true;
- std::cout<<"End position of the curve is not correct."<<std::endl;
+ if (!transformEnd.translation().isApprox(p1)) {
+ error = true;
+ std::cout << "End position of the curve is not correct." << std::endl;
}
- if(! transformEnd.rotation().isApprox(q1.toRotationMatrix())){
- error=true;
- std::cout<<"End rotation of the curve is not correct."<<std::endl;
+ if (!transformEnd.rotation().isApprox(q1.toRotationMatrix())) {
+ error = true;
+ std::cout << "End rotation of the curve is not correct." << std::endl;
}
- if(! transformMid.translation().isApprox(pMid)){
- error=true;
- std::cout<<"Mid position of the curve is not correct."<<std::endl;
+ if (!transformMid.translation().isApprox(pMid)) {
+ error = true;
+ std::cout << "Mid position of the curve is not correct." << std::endl;
}
- if(! transformMid.rotation().isApprox(qMid.toRotationMatrix())){
- error=true;
- std::cout<<"Mid rotation of the curve is not correct."<<std::endl;
+ if (!transformMid.rotation().isApprox(qMid.toRotationMatrix())) {
+ error = true;
+ std::cout << "Mid rotation of the curve is not correct." << std::endl;
}
- //check derivatives for rotation:
- if(cBezier.derivate(min,1).tail<3>() != cBezier.derivate(max,1).tail<3>()){
- error=true;
- std::cout<<"SE3 curve : first order derivative for rotation should be constant."<<std::endl;
+ // check derivatives for rotation:
+ if (cBezier.derivate(min, 1).tail<3>() != cBezier.derivate(max, 1).tail<3>()) {
+ error = true;
+ std::cout << "SE3 curve : first order derivative for rotation should be constant." << std::endl;
}
- if(cBezier.derivate(min,2).tail<3>() != point_t::Zero(3)){
- error=true;
- std::cout<<"SE3 curve : second order derivative for rotation should be null"<<std::endl;
+ if (cBezier.derivate(min, 2).tail<3>() != point_t::Zero(3)) {
+ error = true;
+ std::cout << "SE3 curve : second order derivative for rotation should be null" << std::endl;
}
// check if errors are correctly raised
- try{
+ try {
cBezier(0.1);
error = true;
- std::cout<<"SE3 curve: calling () with t < tmin should raise an invalid_argument error"<<std::endl;
- }catch(std::invalid_argument e){ }
- try{
+ std::cout << "SE3 curve: calling () with t < tmin should raise an invalid_argument error" << std::endl;
+ } catch (std::invalid_argument e) {
+ }
+ try {
cBezier(2.3);
error = true;
- std::cout<<"SE3 curve: calling () with t > tmin should raise an invalid_argument error"<<std::endl;
- }catch(std::invalid_argument e){ }
- try{
- cBezier.derivate(0.6,0);
+ std::cout << "SE3 curve: calling () with t > tmin should raise an invalid_argument error" << std::endl;
+ } catch (std::invalid_argument e) {
+ }
+ try {
+ cBezier.derivate(0.6, 0);
error = true;
- std::cout<<"SE3 curve: calling derivate with order = 0 should raise an invalid_argument error"<<std::endl;
- }catch(std::invalid_argument e){ }
+ std::cout << "SE3 curve: calling derivate with order = 0 should raise an invalid_argument error" << std::endl;
+ } catch (std::invalid_argument e) {
+ }
}
-
-
/**
* @brief BezierLinearProblemTests test the generation of linear / quadratic problems with
* variable control points bezier curves
@@ -1839,40 +1833,30 @@ void se3CurveTest(bool& error){
using namespace curves::optimization;
-
-var_pair_t setup_control_points(const std::size_t degree,
- const constraint_flag flag,
- const point_t& initPos = point_t(),
- const point_t& endPos = point_t(),
- const constraint_linear& constraints = constraint_linear(3),
- const double totalTime = 1.)
-{
- problem_definition_t pDef(constraints);
- pDef.init_pos = initPos; pDef.end_pos = endPos;
- pDef.flag = flag;
- pDef.totalTime = totalTime;
- pDef.degree = degree;
- problem_data_t pData = setup_control_points<point_t, double,true>(pDef);
- return std::make_pair(pData.variables_,
- std::make_pair(pData.startVariableIndex, pData.numVariables));
+var_pair_t setup_control_points(const std::size_t degree, const constraint_flag flag,
+ const point_t& initPos = point_t(), const point_t& endPos = point_t(),
+ const constraint_linear& constraints = constraint_linear(3),
+ const double totalTime = 1.) {
+ problem_definition_t pDef(constraints);
+ pDef.init_pos = initPos;
+ pDef.end_pos = endPos;
+ pDef.flag = flag;
+ pDef.totalTime = totalTime;
+ pDef.degree = degree;
+ problem_data_t pData = setup_control_points<point_t, double, true>(pDef);
+ return std::make_pair(pData.variables_, std::make_pair(pData.startVariableIndex, pData.numVariables));
}
-enum vartype
-{
- variable,
- constant
-};
+enum vartype { variable, constant };
-bool isVar(const linear_variable_t& var)
-{
- return ! var.isZero() && var.B() == linear_variable_t::matrix_x_t::Identity(3,3) &&
- var.c() == linear_variable_t::vector_x_t::Zero(3);
+bool isVar(const linear_variable_t& var) {
+ return !var.isZero() && var.B() == linear_variable_t::matrix_x_t::Identity(3, 3) &&
+ var.c() == linear_variable_t::vector_x_t::Zero(3);
}
-bool isConstant(const linear_variable_t& var)
-{
- return var.isZero() || (var.B() == linear_variable_t::matrix_x_t::Zero(3,3) &&
- var.c() != linear_variable_t::vector_x_t::Zero(3));
+bool isConstant(const linear_variable_t& var) {
+ return var.isZero() ||
+ (var.B() == linear_variable_t::matrix_x_t::Zero(3, 3) && var.c() != linear_variable_t::vector_x_t::Zero(3));
}
/*bool isMixed(const linear_variable_t& var)
@@ -1881,372 +1865,340 @@ bool isConstant(const linear_variable_t& var)
var.b_ != linear_variable_t::point_t::Zero();
}*/
-bool checkValue(const linear_variable_t& var, const vartype vart)
-{
- if (vart == constant)
- return isConstant(var);
- else
- return isVar(var);
+bool checkValue(const linear_variable_t& var, const vartype vart) {
+ if (vart == constant)
+ return isConstant(var);
+ else
+ return isVar(var);
}
-void checksequence(const T_linear_variable_t& vars, vartype* expected,
- const std::string testname, bool& error)
-{
- int i =0;
- for(CIT_linear_variable_t cit = vars.begin();
- cit != vars.end(); ++cit, ++i)
- {
- if(! checkValue(*cit, expected[i]))
- {
- std::cout << "in test: " << testname << ": wrong type for variable at position " << i << std::endl;
- error = true;
- }
+void checksequence(const T_linear_variable_t& vars, vartype* expected, const std::string testname, bool& error) {
+ int i = 0;
+ for (CIT_linear_variable_t cit = vars.begin(); cit != vars.end(); ++cit, ++i) {
+ if (!checkValue(*cit, expected[i])) {
+ std::cout << "in test: " << testname << ": wrong type for variable at position " << i << std::endl;
+ error = true;
}
+ }
}
-void checkNumVar(const T_linear_variable_t& vars, const std::size_t expected,
- const std::string testname, bool& error)
-{
- if(vars.size() != expected)
- {
- error = true;
- std::cout << "incorrect number of variables in "
- << testname << "(" << expected << "," << vars.size() << ")" << std::endl;
- }
+void checkNumVar(const T_linear_variable_t& vars, const std::size_t expected, const std::string testname,
+ bool& error) {
+ if (vars.size() != expected) {
+ error = true;
+ std::cout << "incorrect number of variables in " << testname << "(" << expected << "," << vars.size() << ")"
+ << std::endl;
+ }
}
void checkPair(const pair_size_t pair, const std::size_t start_index, const std::size_t num_vars,
- const std::string testname, bool& error)
-{
- if(pair.first != start_index)
- {
- error = true;
- std::cout << "incorrect starting index for variablesin "
+ const std::string testname, bool& error) {
+ if (pair.first != start_index) {
+ error = true;
+ std::cout << "incorrect starting index for variablesin "
- << testname << "(" << start_index << "," << pair.first << ")" << std::endl;
- }
- if(pair.second != num_vars)
- {
- error = true;
- std::cout << "incorrect number of identified variablesin "
- << testname << "(" << num_vars << "," << pair.second << ")" << std::endl;
- }
+ << testname << "(" << start_index << "," << pair.first << ")" << std::endl;
+ }
+ if (pair.second != num_vars) {
+ error = true;
+ std::cout << "incorrect number of identified variablesin " << testname << "(" << num_vars << "," << pair.second
+ << ")" << std::endl;
+ }
}
-void BezierLinearProblemsetup_control_pointsNoConstraint(bool& error){
- constraint_flag flag = optimization::NONE;
- var_pair_t res_no_constraints = setup_control_points(5, flag);
- T_linear_variable_t& vars = res_no_constraints.first;
- vartype exptecdvars [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "setup_control_pointsNoConstraint", error);
- checksequence(vars,exptecdvars,"setup_control_pointsNoConstraint", error);
+void BezierLinearProblemsetup_control_pointsNoConstraint(bool& error) {
+ constraint_flag flag = optimization::NONE;
+ var_pair_t res_no_constraints = setup_control_points(5, flag);
+ T_linear_variable_t& vars = res_no_constraints.first;
+ vartype exptecdvars[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "setup_control_pointsNoConstraint", error);
+ checksequence(vars, exptecdvars, "setup_control_pointsNoConstraint", error);
}
-constraint_linear makeConstraint()
-{
- point_t init_pos = point_t(1.,1.,1.);
- constraint_linear cl(3);
- init_pos*=2;
- cl.init_vel = init_pos;
- init_pos*=2;
- cl.init_acc = init_pos;
- init_pos*=2;
- cl.end_acc = init_pos;
- init_pos*=2;
- cl.end_vel = init_pos;
- return cl;
+constraint_linear makeConstraint() {
+ point_t init_pos = point_t(1., 1., 1.);
+ constraint_linear cl(3);
+ init_pos *= 2;
+ cl.init_vel = init_pos;
+ init_pos *= 2;
+ cl.init_acc = init_pos;
+ init_pos *= 2;
+ cl.end_acc = init_pos;
+ init_pos *= 2;
+ cl.end_vel = init_pos;
+ return cl;
}
-void BezierLinearProblemsetup_control_pointsVarCombinatorialInit(bool& error){
- constraint_flag flag = optimization::INIT_POS;
- point_t init_pos = point_t(1.,1.,1.);
- var_pair_t res = setup_control_points(5, flag,init_pos);
- T_linear_variable_t& vars = res.first;
- vartype exptecdvars [] = {constant,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialInit", error);
- checksequence(vars,exptecdvars,"VarCombinatorialInit", error);
- checkPair(res.second, 1, 5, "VarCombinatorialInit", error);
-
- constraint_linear constraints = makeConstraint();
+void BezierLinearProblemsetup_control_pointsVarCombinatorialInit(bool& error) {
+ constraint_flag flag = optimization::INIT_POS;
+ point_t init_pos = point_t(1., 1., 1.);
+ var_pair_t res = setup_control_points(5, flag, init_pos);
+ T_linear_variable_t& vars = res.first;
+ vartype exptecdvars[] = {constant, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialInit", error);
+ checksequence(vars, exptecdvars, "VarCombinatorialInit", error);
+ checkPair(res.second, 1, 5, "VarCombinatorialInit", error);
+
+ constraint_linear constraints = makeConstraint();
+ flag = INIT_POS | INIT_VEL;
+ res = setup_control_points(5, flag, init_pos, point_t(), constraints);
+ vars = res.first;
+ vartype exptecdvar1[] = {constant, constant, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialInit", error);
+ checksequence(vars, exptecdvar1, "VarCombinatorialInit", error);
+ checkPair(res.second, 2, 4, "VarCombinatorialInit", error);
+
+ flag = INIT_POS | INIT_VEL | INIT_ACC;
+ res = setup_control_points(5, flag, init_pos, point_t(), constraints);
+ vars = res.first;
+ vartype exptecdvar2[] = {constant, constant, constant, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialInit", error);
+ checksequence(vars, exptecdvar2, "VarCombinatorialInit", error);
+ checkPair(res.second, 3, 3, "VarCombinatorialInit", error);
+
+ flag = INIT_VEL;
+ res = setup_control_points(5, flag, init_pos, point_t(), constraints);
+ vars = res.first;
+ vartype exptecdvar3[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialInit", error);
+ checksequence(vars, exptecdvar3, "VarCombinatorialInit", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
+
+ flag = INIT_ACC;
+ res = setup_control_points(5, flag, init_pos, point_t(), constraints);
+ vars = res.first;
+ vartype exptecdvar4[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialInit", error);
+ checksequence(vars, exptecdvar4, "VarCombinatorialInit", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
+
+ flag = INIT_ACC | INIT_VEL;
+ res = setup_control_points(5, flag, init_pos, point_t(), constraints);
+ vars = res.first;
+ vartype exptecdvar5[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialInit", error);
+ checksequence(vars, exptecdvar5, "VarCombinatorialInit", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
+
+ bool err = true;
+ try {
flag = INIT_POS | INIT_VEL;
- res = setup_control_points(5, flag,init_pos,point_t(),constraints);
- vars = res.first;
- vartype exptecdvar1 [] = {constant,constant,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialInit", error);
- checksequence(vars,exptecdvar1,"VarCombinatorialInit", error);
- checkPair(res.second, 2, 4, "VarCombinatorialInit", error);
-
- flag = INIT_POS | INIT_VEL | INIT_ACC;
- res = setup_control_points(5, flag,init_pos,point_t(),constraints);
- vars = res.first;
- vartype exptecdvar2 [] = {constant,constant,constant,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialInit", error);
- checksequence(vars,exptecdvar2,"VarCombinatorialInit", error);
- checkPair(res.second, 3, 3, "VarCombinatorialInit", error);
-
- flag = INIT_VEL;
- res = setup_control_points(5, flag,init_pos,point_t(),constraints);
- vars = res.first;
- vartype exptecdvar3 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialInit", error);
- checksequence(vars,exptecdvar3,"VarCombinatorialInit", error);
- checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
-
- flag = INIT_ACC;
- res = setup_control_points(5, flag,init_pos,point_t(),constraints);
- vars = res.first;
- vartype exptecdvar4 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialInit", error);
- checksequence(vars,exptecdvar4,"VarCombinatorialInit", error);
- checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
-
- flag = INIT_ACC | INIT_VEL;
- res = setup_control_points(5, flag,init_pos,point_t(),constraints);
- vars = res.first;
- vartype exptecdvar5 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialInit", error);
- checksequence(vars,exptecdvar5,"VarCombinatorialInit", error);
- checkPair(res.second, 0, 6, "VarCombinatorialInit", error);
-
-
- bool err = true;
- try
- {
- flag = INIT_POS | INIT_VEL;
- res = setup_control_points(1, flag,init_pos,point_t(),constraints);
- }
- catch(...)
- {
- err = false;
- }
- if(err)
- {
- error = true;
- std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints " << std::endl;
- }
+ res = setup_control_points(1, flag, init_pos, point_t(), constraints);
+ } catch (...) {
+ err = false;
+ }
+ if (err) {
+ error = true;
+ std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints "
+ << std::endl;
+ }
}
-void BezierLinearProblemsetup_control_pointsVarCombinatorialEnd(bool& error){
- constraint_flag flag = optimization::END_POS;
- point_t init_pos = point_t(1.,1.,1.);
- var_pair_t res = setup_control_points(5, flag,init_pos);
- T_linear_variable_t& vars = res.first;
- vartype exptecdvars [] = {variable,variable,variable,variable,variable,constant};
- checkNumVar(vars, 6, "VarCombinatorialEnd", error);
- checksequence(vars,exptecdvars,"VarCombinatorialEnd", error);
- checkPair(res.second, 0, 5, "VarCombinatorialEnd", error);
-
- constraint_linear constraints = makeConstraint();
- flag = END_POS | END_VEL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar1 [] = {variable,variable,variable,variable,constant,constant};
- checkNumVar(vars, 6, "VarCombinatorialEnd", error);
- checksequence(vars,exptecdvar1,"VarCombinatorialEnd", error);
- checkPair(res.second, 0, 4, "VarCombinatorialEnd", error);
-
- flag = END_POS | END_VEL | END_ACC;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar2 [] = {variable,variable,variable,constant,constant,constant};
- checkNumVar(vars, 6, "VarCombinatorialEnd", error);
- checksequence(vars,exptecdvar2,"VarCombinatorialEnd", error);
- checkPair(res.second, 0, 3, "VarCombinatorialEnd", error);
-
- flag = END_VEL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar3 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialEnd", error);
- checksequence(vars,exptecdvar3,"VarCombinatorialEnd", error);
- checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
-
- flag = END_ACC;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar4 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialEnd", error);
- checksequence(vars,exptecdvar4,"VarCombinatorialEnd", error);
- checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
-
+void BezierLinearProblemsetup_control_pointsVarCombinatorialEnd(bool& error) {
+ constraint_flag flag = optimization::END_POS;
+ point_t init_pos = point_t(1., 1., 1.);
+ var_pair_t res = setup_control_points(5, flag, init_pos);
+ T_linear_variable_t& vars = res.first;
+ vartype exptecdvars[] = {variable, variable, variable, variable, variable, constant};
+ checkNumVar(vars, 6, "VarCombinatorialEnd", error);
+ checksequence(vars, exptecdvars, "VarCombinatorialEnd", error);
+ checkPair(res.second, 0, 5, "VarCombinatorialEnd", error);
+
+ constraint_linear constraints = makeConstraint();
+ flag = END_POS | END_VEL;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar1[] = {variable, variable, variable, variable, constant, constant};
+ checkNumVar(vars, 6, "VarCombinatorialEnd", error);
+ checksequence(vars, exptecdvar1, "VarCombinatorialEnd", error);
+ checkPair(res.second, 0, 4, "VarCombinatorialEnd", error);
+
+ flag = END_POS | END_VEL | END_ACC;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar2[] = {variable, variable, variable, constant, constant, constant};
+ checkNumVar(vars, 6, "VarCombinatorialEnd", error);
+ checksequence(vars, exptecdvar2, "VarCombinatorialEnd", error);
+ checkPair(res.second, 0, 3, "VarCombinatorialEnd", error);
+
+ flag = END_VEL;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar3[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialEnd", error);
+ checksequence(vars, exptecdvar3, "VarCombinatorialEnd", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
+
+ flag = END_ACC;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar4[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialEnd", error);
+ checksequence(vars, exptecdvar4, "VarCombinatorialEnd", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
+
+ flag = END_ACC | END_VEL;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar5[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialEnd", error);
+ checksequence(vars, exptecdvar5, "VarCombinatorialEnd", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
+
+ bool err = true;
+ try {
flag = END_ACC | END_VEL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar5 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialEnd", error);
- checksequence(vars,exptecdvar5,"VarCombinatorialEnd", error);
- checkPair(res.second, 0, 6, "VarCombinatorialEnd", error);
-
-
- bool err = true;
- try
- {
- flag = END_ACC | END_VEL;
- res = setup_control_points(1, flag,init_pos,point_t(),constraints);
- }
- catch(...)
- {
- err = false;
- }
- if(err)
- {
- error = true;
- std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints " << std::endl;
- }
+ res = setup_control_points(1, flag, init_pos, point_t(), constraints);
+ } catch (...) {
+ err = false;
+ }
+ if (err) {
+ error = true;
+ std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints "
+ << std::endl;
+ }
}
-
-void BezierLinearProblemsetup_control_pointsVarCombinatorialMix(bool& error){
- constraint_flag flag = END_POS | INIT_POS;
- point_t init_pos = point_t(1.,1.,1.);
- var_pair_t res = setup_control_points(5, flag,init_pos);
- T_linear_variable_t& vars = res.first;
- vartype exptecdvars [] = {constant,variable,variable,variable,variable,constant};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvars,"VarCombinatorialMix", error);
- checkPair(res.second, 1, 4, "VarCombinatorialMix", error);
-
- constraint_linear constraints = makeConstraint();
- flag = END_POS | END_VEL | INIT_VEL | INIT_POS;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar1 [] = {constant,constant,variable,variable,constant,constant};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar1,"VarCombinatorialMix", error);
- checkPair(res.second, 2, 2, "VarCombinatorialMix", error);
-
- flag = END_POS | END_VEL | END_ACC | INIT_VEL | INIT_POS;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar2 [] = {constant,constant,variable,constant,constant,constant};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar2,"VarCombinatorialMix", error);
- checkPair(res.second, 2, 1, "VarCombinatorialMix", error);
-
+void BezierLinearProblemsetup_control_pointsVarCombinatorialMix(bool& error) {
+ constraint_flag flag = END_POS | INIT_POS;
+ point_t init_pos = point_t(1., 1., 1.);
+ var_pair_t res = setup_control_points(5, flag, init_pos);
+ T_linear_variable_t& vars = res.first;
+ vartype exptecdvars[] = {constant, variable, variable, variable, variable, constant};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvars, "VarCombinatorialMix", error);
+ checkPair(res.second, 1, 4, "VarCombinatorialMix", error);
+
+ constraint_linear constraints = makeConstraint();
+ flag = END_POS | END_VEL | INIT_VEL | INIT_POS;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar1[] = {constant, constant, variable, variable, constant, constant};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar1, "VarCombinatorialMix", error);
+ checkPair(res.second, 2, 2, "VarCombinatorialMix", error);
+
+ flag = END_POS | END_VEL | END_ACC | INIT_VEL | INIT_POS;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar2[] = {constant, constant, variable, constant, constant, constant};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar2, "VarCombinatorialMix", error);
+ checkPair(res.second, 2, 1, "VarCombinatorialMix", error);
+
+ flag = ALL;
+ res = setup_control_points(8, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar3[] = {constant, constant, constant, constant, variable, constant, constant, constant, constant};
+ checkNumVar(vars, 9, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar3, "VarCombinatorialMix", error);
+ checkPair(res.second, 4, 1, "VarCombinatorialMix", error);
+
+ flag = END_VEL | END_ACC | INIT_VEL;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar4[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar4, "VarCombinatorialMix", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
+
+ flag = END_VEL | INIT_VEL;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar5[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar5, "VarCombinatorialMix", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
+
+ bool err = true;
+ try {
flag = ALL;
- res = setup_control_points(8, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar3 [] = {constant,constant,constant,constant,variable,constant,constant,constant,constant};
- checkNumVar(vars, 9, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar3,"VarCombinatorialMix", error);
- checkPair(res.second, 4, 1, "VarCombinatorialMix", error);
-
- flag = END_VEL | END_ACC | INIT_VEL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar4 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar4,"VarCombinatorialMix", error);
- checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
-
- flag = END_VEL | INIT_VEL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar5 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar5,"VarCombinatorialMix", error);
- checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
-
-
- bool err = true;
- try
- {
- flag = ALL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- }
- catch(...)
- {
- err = false;
- }
- if(err)
- {
- error = true;
- std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints " << std::endl;
- }
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ } catch (...) {
+ err = false;
+ }
+ if (err) {
+ error = true;
+ std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints "
+ << std::endl;
+ }
}
-void BezierLinearProblemInitInequalities(bool& error){
- constraint_flag flag = INIT_POS | END_POS;
- point_t init_pos = point_t(1.,1.,1.);
- var_pair_t res = setup_control_points(5, flag,init_pos);
- T_linear_variable_t& vars = res.first;
- vartype exptecdvars [] = {constant,variable,variable,variable,variable,constant};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvars,"VarCombinatorialMix", error);
- checkPair(res.second, 1, 4, "VarCombinatorialMix", error);
-
- constraint_linear constraints = makeConstraint();
- flag = END_POS | END_VEL | INIT_VEL | INIT_POS;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar1 [] = {constant,constant,variable,variable,constant,constant};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar1,"VarCombinatorialMix", error);
- checkPair(res.second, 2, 2, "VarCombinatorialMix", error);
-
- flag = END_POS | END_VEL | END_ACC | INIT_VEL | INIT_POS;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar2 [] = {constant,constant,variable,constant,constant,constant};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar2,"VarCombinatorialMix", error);
- checkPair(res.second, 2, 1, "VarCombinatorialMix", error);
-
+void BezierLinearProblemInitInequalities(bool& error) {
+ constraint_flag flag = INIT_POS | END_POS;
+ point_t init_pos = point_t(1., 1., 1.);
+ var_pair_t res = setup_control_points(5, flag, init_pos);
+ T_linear_variable_t& vars = res.first;
+ vartype exptecdvars[] = {constant, variable, variable, variable, variable, constant};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvars, "VarCombinatorialMix", error);
+ checkPair(res.second, 1, 4, "VarCombinatorialMix", error);
+
+ constraint_linear constraints = makeConstraint();
+ flag = END_POS | END_VEL | INIT_VEL | INIT_POS;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar1[] = {constant, constant, variable, variable, constant, constant};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar1, "VarCombinatorialMix", error);
+ checkPair(res.second, 2, 2, "VarCombinatorialMix", error);
+
+ flag = END_POS | END_VEL | END_ACC | INIT_VEL | INIT_POS;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar2[] = {constant, constant, variable, constant, constant, constant};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar2, "VarCombinatorialMix", error);
+ checkPair(res.second, 2, 1, "VarCombinatorialMix", error);
+
+ flag = ALL;
+ res = setup_control_points(6, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar3[] = {constant, constant, constant, variable, constant, constant, constant};
+ checkNumVar(vars, 7, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar3, "VarCombinatorialMix", error);
+ checkPair(res.second, 3, 1, "VarCombinatorialMix", error);
+
+ flag = END_VEL | END_ACC | INIT_VEL;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar4[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar4, "VarCombinatorialMix", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
+
+ flag = END_VEL | INIT_VEL;
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ vars = res.first;
+ vartype exptecdvar5[] = {variable, variable, variable, variable, variable, variable};
+ checkNumVar(vars, 6, "VarCombinatorialMix", error);
+ checksequence(vars, exptecdvar5, "VarCombinatorialMix", error);
+ checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
+
+ bool err = true;
+ try {
flag = ALL;
- res = setup_control_points(6, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar3 [] = {constant,constant,constant,variable,constant,constant,constant};
- checkNumVar(vars, 7, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar3,"VarCombinatorialMix", error);
- checkPair(res.second, 3, 1, "VarCombinatorialMix", error);
-
- flag = END_VEL | END_ACC | INIT_VEL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar4 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar4,"VarCombinatorialMix", error);
- checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
-
- flag = END_VEL | INIT_VEL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- vars = res.first;
- vartype exptecdvar5 [] = {variable,variable,variable,variable,variable,variable};
- checkNumVar(vars, 6, "VarCombinatorialMix", error);
- checksequence(vars,exptecdvar5,"VarCombinatorialMix", error);
- checkPair(res.second, 0, 6, "VarCombinatorialMix", error);
-
-
- bool err = true;
- try
- {
- flag = ALL;
- res = setup_control_points(5, flag,init_pos,init_pos,constraints);
- }
- catch(...)
- {
- err = false;
- }
- if(err)
- {
- error = true;
- std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints " << std::endl;
- }
+ res = setup_control_points(5, flag, init_pos, init_pos, constraints);
+ } catch (...) {
+ err = false;
+ }
+ if (err) {
+ error = true;
+ std::cout << "exception should be raised when degree of bezier curve is not high enough to handle constraints "
+ << std::endl;
+ }
}
static const std::string path = "../../tests/data/";
-void BezierLinearProblemsetupLoadProblem(bool& /*error*/)
-{
- problem_definition_t pDef = loadproblem(path+"test.pb");
- //problem_data_t pData = setup_control_points<point_t, 3, double>(pDef);
- generate_problem<point_t,double,true>(pDef, VELOCITY);
- //initInequalityMatrix<point_t,3,double>(pDef,pData,prob);
+void BezierLinearProblemsetupLoadProblem(bool& /*error*/) {
+ problem_definition_t pDef = loadproblem(path + "test.pb");
+ // problem_data_t pData = setup_control_points<point_t, 3, double>(pDef);
+ generate_problem<point_t, double, true>(pDef, VELOCITY);
+ // initInequalityMatrix<point_t,3,double>(pDef,pData,prob);
}
-
-int main(int /*argc*/, char** /*argv[]*/)
-{
+int main(int /*argc*/, char** /*argv[]*/) {
std::cout << "performing tests... \n";
bool error = false;
PolynomialCubicFunctionTest(error);
@@ -2283,9 +2235,7 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierLinearProblemsetup_control_pointsVarCombinatorialMix(error);
BezierLinearProblemsetupLoadProblem(error);
-
- if(error)
- {
+ if (error) {
std::cout << "There were some errors\n";
return -1;
} else {
diff --git a/tests/load_problem.h b/tests/load_problem.h
index f48182c..8774e45 100644
--- a/tests/load_problem.h
+++ b/tests/load_problem.h
@@ -2,7 +2,6 @@
#ifndef _CLASS_LOAD_TEST_PROBLEMS
#define _CLASS_LOAD_TEST_PROBLEMS
-
#include "curves/exact_cubic.h"
#include "curves/bezier_curve.h"
#include "curves/helpers/effector_spline.h"
@@ -19,99 +18,86 @@
namespace curves {
typedef Eigen::Vector3d point_t;
-typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
+typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
typedef Eigen::VectorXd pointX_t;
typedef std::pair<double, pointX_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
-
-typedef Eigen::Matrix<double,1,1> point_one;
+typedef Eigen::Matrix<double, 1, 1> point_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
-namespace optimization
-{
+namespace optimization {
typedef curve_constraints<point_t> constraint_linear;
-typedef linear_variable<double,true> linear_variable_t;
+typedef linear_variable<double, true> linear_variable_t;
typedef std::vector<linear_variable_t> T_linear_variable_t;
typedef T_linear_variable_t::const_iterator CIT_linear_variable_t;
-typedef std::pair<std::size_t, std::size_t > pair_size_t;
-typedef std::pair<T_linear_variable_t, pair_size_t > var_pair_t;
+typedef std::pair<std::size_t, std::size_t> pair_size_t;
+typedef std::pair<T_linear_variable_t, pair_size_t> var_pair_t;
typedef problem_data<point_t, double> problem_data_t;
typedef problem_definition<point_t, double> problem_definition_t;
typedef quadratic_problem<point_t, double> problem_t;
+#define MAXBUFSIZE ((int)1e6)
-#define MAXBUFSIZE ((int) 1e6)
-
-Eigen::MatrixXd readMatrix(std::ifstream& infile)
-{
- int cols = 0, rows = 0;
- double buff[MAXBUFSIZE];
+Eigen::MatrixXd readMatrix(std::ifstream& infile) {
+ int cols = 0, rows = 0;
+ double buff[MAXBUFSIZE];
- // Read numbers from file into buffer.
- //ifstream infile;
- //infile.open(filename);
- std::string line = "noise";
- while (!infile.eof() && !line.empty())
- {
- std::getline(infile, line);
+ // Read numbers from file into buffer.
+ // ifstream infile;
+ // infile.open(filename);
+ std::string line = "noise";
+ while (!infile.eof() && !line.empty()) {
+ std::getline(infile, line);
- int temp_cols = 0;
- std::stringstream stream(line);
- while(! stream.eof())
- stream >> buff[cols*rows+temp_cols++];
+ int temp_cols = 0;
+ std::stringstream stream(line);
+ while (!stream.eof()) stream >> buff[cols * rows + temp_cols++];
- if (temp_cols == 0)
- continue;
+ if (temp_cols == 0) continue;
- if (cols == 0)
- cols = temp_cols;
+ if (cols == 0) cols = temp_cols;
- rows++;
- }
- //infile.close();
- rows--;
+ rows++;
+ }
+ // infile.close();
+ rows--;
- // Populate matrix with numbers.
- Eigen::MatrixXd result(rows,cols);
- for (int i = 0; i < rows; i++)
- for (int j = 0; j < cols; j++)
- result(i,j) = buff[ cols*i+j ];
+ // Populate matrix with numbers.
+ Eigen::MatrixXd result(rows, cols);
+ for (int i = 0; i < rows; i++)
+ for (int j = 0; j < cols; j++) result(i, j) = buff[cols * i + j];
- return result;
+ return result;
}
-problem_definition_t loadproblem(const std::string& filename)
-{
- problem_definition_t pDef(3);
- std::ifstream in (filename.c_str());
- if (!in.is_open())
- throw std::runtime_error("cant open filename");
- //first line is degree totaltime flag
- Eigen::Vector3d degTimeFlag = readMatrix(in);
- pDef.degree = (std::size_t)(degTimeFlag[0]);
- pDef.totalTime =degTimeFlag[1];
- pDef.flag = (constraint_flag)(degTimeFlag[2]);
- //Then startpos then empty line
- pDef.init_pos = readMatrix(in);
- //Then endpos then empty line
- pDef.end_pos = readMatrix(in);
- //Then splittimes then empty line
- pDef.splitTimes_ = readMatrix(in);
- // The inequality matrices, empty line, inequality vector as many times
- for (int i = 0; i< pDef.splitTimes_.rows()+1; ++i)
- {
- pDef.inequalityMatrices_.push_back(readMatrix(in));
- pDef.inequalityVectors_.push_back(readMatrix(in));
- }
- in.close();
- return pDef;
- // TODO curve constraints
-}
-
-}
+problem_definition_t loadproblem(const std::string& filename) {
+ problem_definition_t pDef(3);
+ std::ifstream in(filename.c_str());
+ if (!in.is_open()) throw std::runtime_error("cant open filename");
+ // first line is degree totaltime flag
+ Eigen::Vector3d degTimeFlag = readMatrix(in);
+ pDef.degree = (std::size_t)(degTimeFlag[0]);
+ pDef.totalTime = degTimeFlag[1];
+ pDef.flag = (constraint_flag)(degTimeFlag[2]);
+ // Then startpos then empty line
+ pDef.init_pos = readMatrix(in);
+ // Then endpos then empty line
+ pDef.end_pos = readMatrix(in);
+ // Then splittimes then empty line
+ pDef.splitTimes_ = readMatrix(in);
+ // The inequality matrices, empty line, inequality vector as many times
+ for (int i = 0; i < pDef.splitTimes_.rows() + 1; ++i) {
+ pDef.inequalityMatrices_.push_back(readMatrix(in));
+ pDef.inequalityVectors_.push_back(readMatrix(in));
+ }
+ in.close();
+ return pDef;
+ // TODO curve constraints
}
+} // namespace optimization
+} // namespace curves
-#endif //_CLASS_LOAD_TEST_PROBLEMS
+#endif //_CLASS_LOAD_TEST_PROBLEMS
--
GitLab