Skip to content

GitLab

Commit 03578173 authored by steve tonneau's avatar steve tonneau
Browse files

cleaning + added optimization

parent 7cca5327
Show whitespace changes
Inline Side-by-side
include/spline/cubic_function.h
View file @ 03578173
......@@ -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");
}
......
include/spline/optimization/OptimizeSpline.h 0 → 100644
View file @ 03578173
/**
* \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
src/tests/spline_test/Main.cpp
View file @ 03578173
......@@ -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)
{
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment