diff --git a/include/spline/cubic_function.h b/include/spline/cubic_function.h
index 61f9c45d48731fa0921922cd21c23d7ad5dcc8e4..8705375b63765d3e64f421306650adba74b75f02 100644
--- a/include/spline/cubic_function.h
+++ b/include/spline/cubic_function.h
@@ -40,7 +40,7 @@ struct cubic_function : public curve_abc<Time, Numeric, Dim, Safe, Point>
cubic_function(point_t const& a, point_t const& b, point_t const& c, point_t const &d, time_t min, time_t max)
:a_(a), b_(b), c_(c), d_(d), t_min_(min), t_max_(max)
{
- if(t_min_ >= t_max_ & Safe)
+ if(t_min_ >= t_max_ && Safe)
{
std::out_of_range("TODO");
}
diff --git a/include/spline/optimization/OptimizeSpline.h b/include/spline/optimization/OptimizeSpline.h
new file mode 100644
index 0000000000000000000000000000000000000000..0f3aef41dcc3278aa136e0ac8f5cb8a83dd464a1
--- /dev/null
+++ b/include/spline/optimization/OptimizeSpline.h
@@ -0,0 +1,520 @@
+/**
+* \file OptimizeSpline.h
+* \brief Optimization loop for cubic spline generations
+* \author Steve T.
+* \version 0.1
+* \date 06/17/2013
+*
+* This file uses the mosek library to optimize the waypoints location
+* to generate exactCubic spline
+*/
+
+#ifndef _CLASS_SPLINEOPTIMIZER
+#define _CLASS_SPLINEOPTIMIZER
+
+#include "spline/MathDefs.h"
+#include "spline/exact_cubic.h"
+
+#include "mosek/mosek.h"
+#include <Eigen/SparseCore>
+
+#include <utility>
+
+namespace spline
+{
+/// \class SplineOptimizer
+/// \brief Mosek connection to produce optimized splines
+template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
+, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
+struct SplineOptimizer
+{
+typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+typedef Point point_t;
+typedef Time time_t;
+typedef Numeric num_t;
+typedef exact_cubic<time_t, Numeric, Dim, Safe, Point> exact_cubic_t;
+typedef SplineOptimizer<time_t, Numeric, Dim, Safe, Point> splineOptimizer_t;
+
+/* Constructors - destructors */
+public:
+ ///\brief Initializes optimizer environment
+ SplineOptimizer()
+ {
+ MSKrescodee r_ = MSK_makeenv(&env_,NULL);
+ assert(r_ == MSK_RES_OK);
+ }
+
+ ///\brief Destructor
+ ~SplineOptimizer()
+ {
+ MSK_deleteenv(&env_);
+ }
+
+private:
+ SplineOptimizer(const SplineOptimizer&);
+ SplineOptimizer& operator=(const SplineOptimizer&);
+/* Constructors - destructors */
+
+/*Operations*/
+public:
+ /// \brief Starts an optimization loop to create curve
+ /// \param waypoints : a list comprising at least 2 waypoints in ascending time order
+ /// \return An Optimised curve
+ template<typename In>
+ exact_cubic_t* GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const;
+/*Operations*/
+
+private:
+ template<typename In>
+ void ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
+ MatrixX& h1, MatrixX& h2, MatrixX& h3, MatrixX& h4) const;
+
+/*Attributes*/
+private:
+ MSKenv_t env_;
+/*Attributes*/
+
+private:
+ typedef std::pair<time_t, Point> waypoint_t;
+ typedef std::vector<waypoint_t> T_waypoints_t;
+};
+
+template<typename Time, typename Numeric, int Dim, bool Safe, typename Point>
+template<typename In>
+inline void SplineOptimizer<Time, Numeric, Dim, Safe, Point>::ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
+ MatrixX& h1, MatrixX& h2, MatrixX& h3, MatrixX& h4) const
+{
+ std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+ assert((!Safe) || (size > 1));
+
+ In it(wayPointsBegin), next(wayPointsBegin);
+ ++next;
+ Numeric t_previous((*it).first);
+
+ for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
+ {
+ num_t const dTi((*next).first - (*it).first);
+ num_t const dTi_sqr(dTi * dTi);
+ // filling matrices values
+ h3(i,i) = -3 / dTi_sqr;
+ h3(i,i+1) = 3 / dTi_sqr;
+ h4(i,i) = -2 / dTi;
+ h4(i,i+1) = -1 / dTi;
+ if( i+2 < size)
+ {
+ In it2(next); ++ it2;
+ num_t const dTi_1(1/((*it2).first - (*next).first));
+ num_t const dTi_1sqr(dTi_1 * dTi_1);
+ // this can be optimized but let's focus on clarity as long as not needed
+ h1(i+1, i) = 2 / dTi;
+ h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
+ h1(i+1, i+2) = 2 / dTi_1;
+ h2(i+1, i) = -6 / dTi_sqr;
+ h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
+ h2(i+1, i+2) = 6 / dTi_1sqr;
+ }
+ }
+}
+
+template<typename Time, typename Numeric, int Dim, bool Safe, typename Point>
+template<typename In>
+inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
+ SplineOptimizer<Time, Numeric, Dim, Safe, Point>::GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
+{
+ exact_cubic_t* res = 0;
+ int const size((int)std::distance(wayPointsBegin, wayPointsEnd));
+ if(Safe && size < 1)
+ {
+ throw; // TODO
+ }
+ // refer to the paper to understand all this.
+ MatrixX h1 = MatrixX::Zero(size, size);
+ MatrixX h2 = MatrixX::Zero(size, size);
+ MatrixX h3 = MatrixX::Zero(size, size);
+ MatrixX h4 = MatrixX::Zero(size, size);
+
+ // remove this for the time being
+ /*MatrixX g1 = MatrixX::Zero(size, size);
+ MatrixX g2 = MatrixX::Zero(size, size);
+ MatrixX g3 = MatrixX::Zero(size, size);
+ MatrixX g4 = MatrixX::Zero(size, size);*/
+
+ ComputeHMatrices(wayPointsBegin, wayPointsEnd, h1, h2, h3, h4);
+
+ // number of Waypoints : T + 1 => T mid points. Dim variables per points, + acceleration + derivations
+ // (T * t+ 1 ) * Dim * 3 = nb var
+
+// NOG const MSKint32t numvar = ( size + size - 1) * 3 * 3;
+ const MSKint32t numvar = (size) * Dim * 3;
+ /*
+ We store the variables in that order to simplifly matrix computation ( see later )
+// NOG [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn.. x0' --- zn..' ] T
+ [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn..] T
+ */
+
+ /*the first constraint is H1x. = H2x => H2x - H1x. = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */
+
+ int ptOff = (int) Dim * size; // . offest
+ int ptptOff = (int) Dim * 2 * size; // .. offest
+ int prOff = (int) 3 * ptOff; // ' offest
+// NOG int prptOff = (int) prOff + ptOff; // '. offest
+// NOG int prptptOff = (int) prptOff + ptOff; // '.. offest
+
+ MatrixX h2x_h1x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [H2(0) 0 0 -H1(0) 0-------------------0]
+ [ 0 0 H2(0) 0 0 -H1(0)---------------0]
+ [ 0 0 0 H2(0) 0 0 H1(0)--------0]
+ ...
+ [ 0 0 0 0 H2(n) 0 0 0 -H1(n)-0] // row n
+
+ Overall it's fairly easy to fill
+ */
+ for(int i = 0; i < size*Dim; i = i + 3)
+ {
+ for(int j = 0; j<Dim; ++j)
+ {
+ int id = i + j;
+ h2x_h1x.block(id, j*size , 1, size) = h2.row(i%3);
+ h2x_h1x.block(id, j*size + ptOff, 1, size) -= h1.row(i%3);
+ }
+ }
+
+
+ /*the second constraint is x' = G1x + G2x. => G1x + G2x. - x' = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */MatrixX g1x_g2x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [G1(0) 0 0 G2(0) 0-----------------------0 -1 0]
+ [ 0 0 G1(0) 0 0 G2(0)-------------------0 -1 0]
+ [ 0 0 0 G1(0) 0 0 G2(0)-----------0 -1 0]
+ ...
+ [ 0 0 0 0 G1(n) 0 0 0 G2(n)-0 -1 0] // row n
+
+ Overall it's fairly easy to fill
+ */
+// NOG
+ /*for(int j = 0; j<3; ++j)
+ {
+ for(int j =0; j<3; ++j)
+ {
+ int id = i + j;
+ g1x_g2x.block(id, j*size , 1, size) = g1.row(i);
+ g1x_g2x.block(id, j*size + ptOff, 1, size) = g2.row(i);
+ g1x_g2x.block(id, j*size + prOff, 1, size) -= MatrixX::Ones(1, size);
+ }
+ }*/
+
+ /*the third constraint is x.' = G3x + G4x. => G3x + G4x. - x.' = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */MatrixX g3x_g4x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [G3(0) 0 0 G4(0) 0-------------------0 -1 0]
+ [ 0 0 G3(0) 0 0 G4(0)---------------0 -1 0]
+ [ 0 0 0 G3(0) 0 0 G4(0)--------0 -1 0]
+ ...
+ [ 0 0 0 0 G3(n) 0 0 0 G4(n)-0 -1 0] // row n
+
+ Overall it's fairly easy to fill
+ */
+// NOG
+ /*for(int j = 0; j<3; ++j)
+ {
+ for(int j =0; j<3; ++j)
+ {
+ int id = i + j;
+ g3x_g4x.block(id, j*size , 1, size) = g3.row(i);
+ g3x_g4x.block(id, j*size + ptOff, 1, size) = g4.row(i);
+ g3x_g4x.block(id, j*size + prptOff, 1, size) -= MatrixX::Ones(1, size);
+ }
+ }
+*/
+ /*the fourth constraint is x.. = 1/2(H3x + H4x.) => 1/2(H3x + H4x.) - x.. = 0
+ => H3x + H4x. - 2x.. = 0
+ this will give us size * 3 inequalities constraints
+ So this line of A will be writen
+ H2 -H1 0 0 0 0
+ */
+ MatrixX h3x_h4x = MatrixX::Zero(size * Dim, numvar);
+ /**A looks something like that : (n = size)
+ [H3(0) 0 0 H4(0) 0-------------------0 -2 0]
+ [ 0 0 H3(0) 0 0 H4(0)---------------0 -2 0]
+ [ 0 0 0 H3(0) 0 0 H4(0)-------0 -2 0]
+ ...
+ [ 0 0 0 0 H3(n) 0 0 0 H4(n)-0 -2 0] // row n
+
+ Overall it's fairly easy to fill
+ */
+ for(int i = 0; i < size*Dim; i = i + Dim)
+ {
+ for(int j = 0; j<Dim; ++j)
+ {
+ int id = i + j;
+ h3x_h4x.block(id, j*size , 1, size) = h3.row(i%3);
+ h3x_h4x.block(id, j*size + ptOff , 1, size) = h4.row(i%3);
+ h3x_h4x.block(id, j*size + ptptOff, 1, size) = MatrixX::Ones(1, size) * -2;
+ }
+ }
+
+ /*the following constraints are easy to understand*/
+
+ /*x0,: = x^0*/
+ MatrixX x0_x0 = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ x0_x0(0, 0) = 1;
+ x0_x0(1, size) = 1;
+ x0_x0(2, size * 2) = 1;
+ }
+
+ /*x0.,: = 0*/
+ MatrixX x0p_0 = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ x0p_0(0, ptOff) = 1;
+ x0p_0(1, ptOff + size) = 1;
+ x0p_0(2, ptOff + size * 2) = 1;
+ }
+
+ /*xt,: = x^t*/
+ MatrixX xt_xt = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ xt_xt(0, size - 1) = 1;
+ xt_xt(1, 2 * size - 1) = 1;
+ xt_xt(2, 3* size - 1) = 1;
+ }
+
+ /*xT.,: = 0*/
+ MatrixX xtp_0 = MatrixX::Zero(Dim, numvar);
+ for(int j = 0; j<Dim; ++j)
+ {
+ xtp_0(0, ptOff + size - 1) = 1;
+ xtp_0(1, ptOff + size + size - 1) = 1;
+ xtp_0(2, ptOff + size * 2 + size - 1)= 1;
+ }
+
+ //skipping constraints on x and y accelerations for the time being
+ // to compute A i'll create an eigen matrix, then i'll convert it to a sparse one and fill those tables
+
+ //total number of constraints
+// NOG h2x_h1x (size * Dim) + h3x_h4x (size * Dim ) + g1x_g2x (size * Dim ) + g3x_g4x (size*Dim)
+// NOG + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 4 * Dim * size + 4 * Dim
+ // h2x_h1x (size * Dim) + h3x_h4x (size * Dim )
+ // + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 2 * Dim * size + 4 * Dim
+// NOG const MSKint32t numcon = 12 * size + 12;
+ const MSKint32t numcon = Dim * 2 * size + 4 * Dim; // TODO
+
+ //this gives us the matrix A of size numcon * numvaar
+ MatrixX a = MatrixX::Zero(numcon, numvar);
+ a.block(0 , 0, size * Dim, numvar) = h2x_h1x;
+ a.block(size * Dim , 0, size * Dim, numvar) = h3x_h4x;
+ a.block(size * Dim * 2 , 0, Dim, numvar) = x0p_0 ;
+ a.block(size * Dim * 2 + Dim , 0, Dim, numvar) = xtp_0 ;
+ a.block(size * Dim * 2 + Dim * 2 , 0, Dim, numvar) = x0_x0 ;
+ a.block(size * Dim * 2 + Dim * 3 , 0, Dim, numvar) = xt_xt ;
+
+ //convert to sparse representation
+ Eigen::SparseMatrix<Numeric> spA;
+ spA = a.sparseView();
+
+ //convert to sparse representation using column
+ // http://docs.mosek.com/7.0/capi/Conventions_employed_in_the_API.html#sec-intro-subsubsec-cmo-rmo-matrix
+
+ int nonZeros = spA.nonZeros();
+
+ /* Below is the sparse representation of the A
+ matrix stored by column. */
+ double* aval = new double[nonZeros];
+ MSKint32t* asub = new MSKint32t[nonZeros];
+ MSKint32t* aptrb = new MSKint32t[numvar];
+ MSKint32t* aptre = new MSKint32t[numvar];
+
+ int currentIndex = 0;
+ for(int j=0; j<numvar; ++j)
+ {
+ bool nonZeroAtThisCol = false;
+ for(int i=0; i<numcon; ++i)
+ {
+ if(a(i,j) != 0)
+ {
+ if(!nonZeroAtThisCol)
+ {
+ aptrb[j] = currentIndex;
+ nonZeroAtThisCol = true;
+ }
+ aval[currentIndex] = a(i,j);
+ asub[currentIndex] = i;
+ aptre[j] = currentIndex + 1; //overriding previous value
+ ++currentIndex;
+ }
+ }
+ }
+
+ /*Q looks like this
+ 0 0 0 0 0 0 | -> size * 3
+ 0 0 2 0 0 0 | -> size *3
+ 0 0 0 0 0 0
+ 0 0 0 0 2 0
+ 0 0 0 0 0 0
+ */
+ /* Number of non-zeros in Q.*/
+ const MSKint32t numqz = size * Dim * 2;
+ MSKint32t* qsubi = new MSKint32t[numqz];
+ MSKint32t* qsubj = new MSKint32t[numqz];
+ double* qval = new double [numqz];
+ for(int id = 0; id < numqz; ++id)
+ {
+ qsubi[id] = id + ptOff; // we want the x.
+ qsubj[id] = id + ptOff;
+ qval[id] = 2;
+ }
+
+ /* Bounds on constraints. */
+ MSKboundkeye* bkc = new MSKboundkeye[numcon];
+
+ double* blc = new double[numcon];
+ double* buc = new double[numcon];
+
+ for(int i = 0; i < numcon - Dim * 2 ; ++i)
+ {
+ bkc[i] = MSK_BK_FX;
+ blc[i] = 0;
+ buc[i] = 0;
+ }
+ for(int i = numcon - Dim * 2; i < numcon - Dim ; ++i) // x0 = x^0
+ {
+ bkc[i] = MSK_BK_FX;
+ blc[i] = wayPointsBegin->second(i - (numcon - Dim * 2) );
+ buc[i] = wayPointsBegin->second(i - (numcon - Dim * 2) );
+ }
+ In last(wayPointsEnd);
+ --last;
+ for(int i = numcon - 3; i < numcon ; ++i) // xT = x^T
+ {
+ bkc[i] = MSK_BK_FX;
+ blc[i] = last->second(i - (numcon - Dim) );
+ buc[i] = last->second(i - (numcon - Dim) );
+ }
+
+ ///*No Bounds on variables. */
+ MSKboundkeye* bkx = new MSKboundkeye[numvar];
+
+ double* blx = new double[numvar];
+ double* bux = new double[numvar];
+
+ for(int i = 0; i < numvar; ++i)
+ {
+ bkx[i] = MSK_BK_FR;
+ blx[i] = -MSK_INFINITY;
+ bux[i] = +MSK_INFINITY;
+ }
+
+ MSKrescodee r;
+ MSKtask_t task = NULL;
+ /* Create the optimization task. */
+ r = MSK_maketask(env_,numcon,numvar,&task);
+
+ /* Directs the log task stream to the 'printstr' function. */
+ /*if ( r==MSK_RES_OK )
+ r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
+
+ /* Append 'numcon' empty constraints.
+ The constraints will initially have no bounds. */
+ if ( r == MSK_RES_OK )
+ r = MSK_appendcons(task,numcon);
+
+ /* Append 'numvar' variables.
+ The variables will initially be fixed at zero (x=0). */
+ if ( r == MSK_RES_OK )
+ r = MSK_appendvars(task,numvar);
+
+ for(int j=0; j<numvar && r == MSK_RES_OK; ++j)
+ {
+ /* Set the linear term c_j in the objective.*/
+ if(r == MSK_RES_OK)
+ r = MSK_putcj(task,j,0);
+
+ /* Set the bounds on variable j.
+ blx[j] <= x_j <= bux[j] */
+ if(r == MSK_RES_OK)
+ r = MSK_putvarbound(task,
+ j, /* Index of variable.*/
+ bkx[j], /* Bound key.*/
+ blx[j], /* Numerical value of lower bound.*/
+ bux[j]); /* Numerical value of upper bound.*/
+
+ /* Input column j of A */
+ if(r == MSK_RES_OK)
+ r = MSK_putacol(task,
+ j, /* Variable (column) index.*/
+ aptre[j]-aptrb[j], /* Number of non-zeros in column j.*/
+ asub+aptrb[j], /* Pointer to row indexes of column j.*/
+ aval+aptrb[j]); /* Pointer to Values of column j.*/
+ }
+
+ /* Set the bounds on constraints.
+ for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
+ for(int i=0; i<numcon && r == MSK_RES_OK; ++i)
+ r = MSK_putconbound(task,
+ i, /* Index of constraint.*/
+ bkc[i], /* Bound key.*/
+ blc[i], /* Numerical value of lower bound.*/
+ buc[i]); /* Numerical value of upper bound.*/
+
+ /* Maximize objective function. */
+ if (r == MSK_RES_OK)
+ r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
+
+
+ if ( r==MSK_RES_OK )
+ {
+ /* Input the Q for the objective. */
+
+ r = MSK_putqobj(task,numqz,qsubi,qsubj,qval);
+ }
+
+ if ( r==MSK_RES_OK )
+ {
+ MSKrescodee trmcode;
+
+ /* Run optimizer */
+ r = MSK_optimizetrm(task,&trmcode);
+ if ( r==MSK_RES_OK )
+ {
+ double *xx = (double*) calloc(numvar,sizeof(double));
+ if ( xx )
+ {
+ /* Request the interior point solution. */
+ MSK_getxx(task, MSK_SOL_ITR, xx);
+ T_waypoints_t nwaypoints;
+ In begin(wayPointsBegin);
+ for(int i=0; i< size; i = ++i, ++begin)
+ {
+ point_t target;
+ for(int j=0; j< Dim; ++ j)
+ {
+ target(j) = xx[i + j*size];
+ }
+ nwaypoints.push_back(std::make_pair(begin->first, target));
+ }
+ res = new exact_cubic_t(nwaypoints.begin(), nwaypoints.end());
+ free(xx);
+ }
+ }
+ }
+ /* Delete the task and the associated data. */
+ MSK_deletetask(&task);
+ return res;
+}
+
+} // namespace spline
+#endif //_CLASS_SPLINEOPTIMIZER
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 7f9da71e92274bfcfd9ca5481d0f84d75410e935..3fef0ca27ea228bb8b04394e7fde678195f6d5aa 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -15,8 +15,8 @@ typedef Eigen::Vector3d point_t;
typedef cubic_function<double, double, 3, true, point_t> cubic_function_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
-typedef typename std::pair<double, point_t> Waypoint;
-typedef typename std::vector<Waypoint> T_Waypoint;
+typedef std::pair<double, point_t> Waypoint;
+typedef std::vector<Waypoint> T_Waypoint;
bool QuasiEqual(const double a, const double b, const float margin)
{