hpp-spline  4.10.0
template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics.
OptimizeSpline.h
Go to the documentation of this file.
1 
12 #ifndef _CLASS_SPLINEOPTIMIZER
13 #define _CLASS_SPLINEOPTIMIZER
14 
15 #include "spline/MathDefs.h"
16 #include "spline/exact_cubic.h"
17 
18 #include "mosek/mosek.h"
19 #include <Eigen/SparseCore>
20 
21 #include <utility>
22 
23 namespace spline {
26 template <typename Time = double, typename Numeric = Time, std::size_t Dim = 3, bool Safe = false,
27  typename Point = Eigen::Matrix<Numeric, Dim, 1> >
29  typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
30  typedef Point point_t;
31  typedef Time time_t;
32  typedef Numeric num_t;
35 
36  /* Constructors - destructors */
37  public:
40  MSKrescodee r_ = MSK_makeenv(&env_, NULL);
41  assert(r_ == MSK_RES_OK);
42  }
43 
45  ~SplineOptimizer() { MSK_deleteenv(&env_); }
46 
47  private:
49  SplineOptimizer& operator=(const SplineOptimizer&);
50  /* Constructors - destructors */
51 
52  /*Operations*/
53  public:
57  template <typename In>
58  exact_cubic_t* GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const;
59  /*Operations*/
60 
61  private:
62  template <typename In>
63  void ComputeHMatrices(In wayPointsBegin, In wayPointsEnd, MatrixX& h1, MatrixX& h2, MatrixX& h3, MatrixX& h4) const;
64 
65  /*Attributes*/
66  private:
67  MSKenv_t env_;
68  /*Attributes*/
69 
70  private:
71  typedef std::pair<time_t, Point> waypoint_t;
72  typedef std::vector<waypoint_t> T_waypoints_t;
73 };
74 
75 template <typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
76 template <typename In>
77 inline void SplineOptimizer<Time, Numeric, Dim, Safe, Point>::ComputeHMatrices(In wayPointsBegin, In wayPointsEnd,
78  MatrixX& h1, MatrixX& h2, MatrixX& h3,
79  MatrixX& h4) const {
80  std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
81  assert((!Safe) || (size > 1));
82 
83  In it(wayPointsBegin), next(wayPointsBegin);
84  ++next;
85  Numeric t_previous((*it).first);
86 
87  for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
88  num_t const dTi((*next).first - (*it).first);
89  num_t const dTi_sqr(dTi * dTi);
90  // filling matrices values
91  h3(i, i) = -3 / dTi_sqr;
92  h3(i, i + 1) = 3 / dTi_sqr;
93  h4(i, i) = -2 / dTi;
94  h4(i, i + 1) = -1 / dTi;
95  if (i + 2 < size) {
96  In it2(next);
97  ++it2;
98  num_t const dTi_1((*it2).first - (*next).first);
99  num_t const dTi_1sqr(dTi_1 * dTi_1);
100  // this can be optimized but let's focus on clarity as long as not needed
101  h1(i + 1, i) = 2 / dTi;
102  h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
103  h1(i + 1, i + 2) = 2 / dTi_1;
104  h2(i + 1, i) = -6 / dTi_sqr;
105  h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
106  h2(i + 1, i + 2) = 6 / dTi_1sqr;
107  }
108  }
109 }
110 
111 template <typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
112 template <typename In>
113 inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
115  exact_cubic_t* res = 0;
116  int const size((int)std::distance(wayPointsBegin, wayPointsEnd));
117  if (Safe && size < 1) {
118  throw; // TODO
119  }
120  // refer to the paper to understand all this.
121  MatrixX h1 = MatrixX::Zero(size, size);
122  MatrixX h2 = MatrixX::Zero(size, size);
123  MatrixX h3 = MatrixX::Zero(size, size);
124  MatrixX h4 = MatrixX::Zero(size, size);
125 
126  // remove this for the time being
127  /*MatrixX g1 = MatrixX::Zero(size, size);
128  MatrixX g2 = MatrixX::Zero(size, size);
129  MatrixX g3 = MatrixX::Zero(size, size);
130  MatrixX g4 = MatrixX::Zero(size, size);*/
131 
132  ComputeHMatrices(wayPointsBegin, wayPointsEnd, h1, h2, h3, h4);
133 
134  // number of Waypoints : T + 1 => T mid points. Dim variables per points, + acceleration + derivations
135  // (T * t+ 1 ) * Dim * 3 = nb var
136 
137  // NOG const MSKint32t numvar = ( size + size - 1) * 3 * 3;
138  const MSKint32t numvar = (size)*Dim * 3;
139  /*
140  We store the variables in that order to simplifly matrix computation ( see later )
141 // NOG [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn.. x0' --- zn..' ] T
142  [ x0 x1 --- xn y0 --- y z0 --- zn x0. --- zn. x0..--- zn..] T
143  */
144 
145  /*the first constraint is H1x. = H2x => H2x - H1x. = 0
146  this will give us size * 3 inequalities constraints
147  So this line of A will be writen
148  H2 -H1 0 0 0 0
149  */
150 
151  int ptOff = (int)Dim * size; // . offest
152  int ptptOff = (int)Dim * 2 * size; // .. offest
153  int prOff = (int)3 * ptOff; // ' offest
154  // NOG int prptOff = (int) prOff + ptOff; // '. offest
155  // NOG int prptptOff = (int) prptOff + ptOff; // '.. offest
156 
157  MatrixX h2x_h1x = MatrixX::Zero(size * Dim, numvar);
167  for (int i = 0; i < size * Dim; i = i + 3) {
168  for (int j = 0; j < Dim; ++j) {
169  int id = i + j;
170  h2x_h1x.block(id, j * size, 1, size) = h2.row(i % 3);
171  h2x_h1x.block(id, j * size + ptOff, 1, size) -= h1.row(i % 3);
172  }
173  }
174 
175  /*the second constraint is x' = G1x + G2x. => G1x + G2x. - x' = 0
176  this will give us size * 3 inequalities constraints
177  So this line of A will be writen
178  H2 -H1 0 0 0 0
179  */
180  MatrixX g1x_g2x = MatrixX::Zero(size * Dim, numvar);
190  // NOG
191  /*for(int j = 0; j<3; ++j)
192  {
193  for(int j =0; j<3; ++j)
194  {
195  int id = i + j;
196  g1x_g2x.block(id, j*size , 1, size) = g1.row(i);
197  g1x_g2x.block(id, j*size + ptOff, 1, size) = g2.row(i);
198  g1x_g2x.block(id, j*size + prOff, 1, size) -= MatrixX::Ones(1, size);
199  }
200  }*/
201 
202  /*the third constraint is x.' = G3x + G4x. => G3x + G4x. - x.' = 0
203  this will give us size * 3 inequalities constraints
204  So this line of A will be writen
205  H2 -H1 0 0 0 0
206  */
207  MatrixX g3x_g4x = MatrixX::Zero(size * Dim, numvar);
217  // NOG
218  /*for(int j = 0; j<3; ++j)
219  {
220  for(int j =0; j<3; ++j)
221  {
222  int id = i + j;
223  g3x_g4x.block(id, j*size , 1, size) = g3.row(i);
224  g3x_g4x.block(id, j*size + ptOff, 1, size) = g4.row(i);
225  g3x_g4x.block(id, j*size + prptOff, 1, size) -= MatrixX::Ones(1, size);
226  }
227  }
228 */
229  /*the fourth constraint is x.. = 1/2(H3x + H4x.) => 1/2(H3x + H4x.) - x.. = 0
230  => H3x + H4x. - 2x.. = 0
231  this will give us size * 3 inequalities constraints
232  So this line of A will be writen
233  H2 -H1 0 0 0 0
234  */
235  MatrixX h3x_h4x = MatrixX::Zero(size * Dim, numvar);
245  for (int i = 0; i < size * Dim; i = i + Dim) {
246  for (int j = 0; j < Dim; ++j) {
247  int id = i + j;
248  h3x_h4x.block(id, j * size, 1, size) = h3.row(i % 3);
249  h3x_h4x.block(id, j * size + ptOff, 1, size) = h4.row(i % 3);
250  h3x_h4x.block(id, j * size + ptptOff, 1, size) = MatrixX::Ones(1, size) * -2;
251  }
252  }
253 
254  /*the following constraints are easy to understand*/
255 
256  /*x0,: = x^0*/
257  MatrixX x0_x0 = MatrixX::Zero(Dim, numvar);
258  for (int j = 0; j < Dim; ++j) {
259  x0_x0(0, 0) = 1;
260  x0_x0(1, size) = 1;
261  x0_x0(2, size * 2) = 1;
262  }
263 
264  /*x0.,: = 0*/
265  MatrixX x0p_0 = MatrixX::Zero(Dim, numvar);
266  for (int j = 0; j < Dim; ++j) {
267  x0p_0(0, ptOff) = 1;
268  x0p_0(1, ptOff + size) = 1;
269  x0p_0(2, ptOff + size * 2) = 1;
270  }
271 
272  /*xt,: = x^t*/
273  MatrixX xt_xt = MatrixX::Zero(Dim, numvar);
274  for (int j = 0; j < Dim; ++j) {
275  xt_xt(0, size - 1) = 1;
276  xt_xt(1, 2 * size - 1) = 1;
277  xt_xt(2, 3 * size - 1) = 1;
278  }
279 
280  /*xT.,: = 0*/
281  MatrixX xtp_0 = MatrixX::Zero(Dim, numvar);
282  for (int j = 0; j < Dim; ++j) {
283  xtp_0(0, ptOff + size - 1) = 1;
284  xtp_0(1, ptOff + size + size - 1) = 1;
285  xtp_0(2, ptOff + size * 2 + size - 1) = 1;
286  }
287 
288  // skipping constraints on x and y accelerations for the time being
289  // to compute A i'll create an eigen matrix, then i'll convert it to a sparse one and fill those tables
290 
291  // total number of constraints
292  // NOG h2x_h1x (size * Dim) + h3x_h4x (size * Dim ) + g1x_g2x (size * Dim ) + g3x_g4x (size*Dim)
293  // NOG + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 4 * Dim * size + 4 * Dim
294  // h2x_h1x (size * Dim) + h3x_h4x (size * Dim )
295  // + x0_x0 (Dim ) + x0p_0 (Dim) + xt_xt (Dim) + xtp_0 (Dim) = 2 * Dim * size + 4 * Dim
296  // NOG const MSKint32t numcon = 12 * size + 12;
297  const MSKint32t numcon = Dim * 2 * size + 4 * Dim; // TODO
298 
299  // this gives us the matrix A of size numcon * numvaar
300  MatrixX a = MatrixX::Zero(numcon, numvar);
301  a.block(0, 0, size * Dim, numvar) = h2x_h1x;
302  a.block(size * Dim, 0, size * Dim, numvar) = h3x_h4x;
303  a.block(size * Dim * 2, 0, Dim, numvar) = x0p_0;
304  a.block(size * Dim * 2 + Dim, 0, Dim, numvar) = xtp_0;
305  a.block(size * Dim * 2 + Dim * 2, 0, Dim, numvar) = x0_x0;
306  a.block(size * Dim * 2 + Dim * 3, 0, Dim, numvar) = xt_xt;
307 
308  // convert to sparse representation
309  Eigen::SparseMatrix<Numeric> spA;
310  spA = a.sparseView();
311 
312  // convert to sparse representation using column
313  // http://docs.mosek.com/7.0/capi/Conventions_employed_in_the_API.html#sec-intro-subsubsec-cmo-rmo-matrix
314 
315  int nonZeros = spA.nonZeros();
316 
317  /* Below is the sparse representation of the A
318  matrix stored by column. */
319  double* aval = new double[nonZeros];
320  MSKint32t* asub = new MSKint32t[nonZeros];
321  MSKint32t* aptrb = new MSKint32t[numvar];
322  MSKint32t* aptre = new MSKint32t[numvar];
323 
324  int currentIndex = 0;
325  for (int j = 0; j < numvar; ++j) {
326  bool nonZeroAtThisCol = false;
327  for (int i = 0; i < numcon; ++i) {
328  if (a(i, j) != 0) {
329  if (!nonZeroAtThisCol) {
330  aptrb[j] = currentIndex;
331  nonZeroAtThisCol = true;
332  }
333  aval[currentIndex] = a(i, j);
334  asub[currentIndex] = i;
335  aptre[j] = currentIndex + 1; // overriding previous value
336  ++currentIndex;
337  }
338  }
339  }
340 
341  /*Q looks like this
342  0 0 0 0 0 0 | -> size * 3
343  0 0 2 0 0 0 | -> size *3
344  0 0 0 0 0 0
345  0 0 0 0 2 0
346  0 0 0 0 0 0
347  */
348  /* Number of non-zeros in Q.*/
349  const MSKint32t numqz = size * Dim * 2;
350  MSKint32t* qsubi = new MSKint32t[numqz];
351  MSKint32t* qsubj = new MSKint32t[numqz];
352  double* qval = new double[numqz];
353  for (int id = 0; id < numqz; ++id) {
354  qsubi[id] = id + ptOff; // we want the x.
355  qsubj[id] = id + ptOff;
356  qval[id] = 2;
357  }
358 
359  /* Bounds on constraints. */
360  MSKboundkeye* bkc = new MSKboundkeye[numcon];
361 
362  double* blc = new double[numcon];
363  double* buc = new double[numcon];
364 
365  for (int i = 0; i < numcon - Dim * 2; ++i) {
366  bkc[i] = MSK_BK_FX;
367  blc[i] = 0;
368  buc[i] = 0;
369  }
370  for (int i = numcon - Dim * 2; i < numcon - Dim; ++i) // x0 = x^0
371  {
372  bkc[i] = MSK_BK_FX;
373  blc[i] = wayPointsBegin->second(i - (numcon - Dim * 2));
374  buc[i] = wayPointsBegin->second(i - (numcon - Dim * 2));
375  }
376  In last(wayPointsEnd);
377  --last;
378  for (int i = numcon - 3; i < numcon; ++i) // xT = x^T
379  {
380  bkc[i] = MSK_BK_FX;
381  blc[i] = last->second(i - (numcon - Dim));
382  buc[i] = last->second(i - (numcon - Dim));
383  }
384 
386  MSKboundkeye* bkx = new MSKboundkeye[numvar];
387 
388  double* blx = new double[numvar];
389  double* bux = new double[numvar];
390 
391  for (int i = 0; i < numvar; ++i) {
392  bkx[i] = MSK_BK_FR;
393  blx[i] = -MSK_INFINITY;
394  bux[i] = +MSK_INFINITY;
395  }
396 
397  MSKrescodee r;
398  MSKtask_t task = NULL;
399  /* Create the optimization task. */
400  r = MSK_maketask(env_, numcon, numvar, &task);
401 
402  /* Directs the log task stream to the 'printstr' function. */
403  /*if ( r==MSK_RES_OK )
404  r = MSK_linkfunctotaskstream(task,MSK_STREAM_LOG,NULL,printstr);*/
405 
406  /* Append 'numcon' empty constraints.
407  The constraints will initially have no bounds. */
408  if (r == MSK_RES_OK) r = MSK_appendcons(task, numcon);
409 
410  /* Append 'numvar' variables.
411  The variables will initially be fixed at zero (x=0). */
412  if (r == MSK_RES_OK) r = MSK_appendvars(task, numvar);
413 
414  for (int j = 0; j < numvar && r == MSK_RES_OK; ++j) {
415  /* Set the linear term c_j in the objective.*/
416  if (r == MSK_RES_OK) r = MSK_putcj(task, j, 0);
417 
418  /* Set the bounds on variable j.
419  blx[j] <= x_j <= bux[j] */
420  if (r == MSK_RES_OK)
421  r = MSK_putvarbound(task, j, /* Index of variable.*/
422  bkx[j], /* Bound key.*/
423  blx[j], /* Numerical value of lower bound.*/
424  bux[j]); /* Numerical value of upper bound.*/
425 
426  /* Input column j of A */
427  if (r == MSK_RES_OK)
428  r = MSK_putacol(task, j, /* Variable (column) index.*/
429  aptre[j] - aptrb[j], /* Number of non-zeros in column j.*/
430  asub + aptrb[j], /* Pointer to row indexes of column j.*/
431  aval + aptrb[j]); /* Pointer to Values of column j.*/
432  }
433 
434  /* Set the bounds on constraints.
435  for i=1, ...,numcon : blc[i] <= constraint i <= buc[i] */
436  for (int i = 0; i < numcon && r == MSK_RES_OK; ++i)
437  r = MSK_putconbound(task, i, /* Index of constraint.*/
438  bkc[i], /* Bound key.*/
439  blc[i], /* Numerical value of lower bound.*/
440  buc[i]); /* Numerical value of upper bound.*/
441 
442  /* Maximize objective function. */
443  if (r == MSK_RES_OK) r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
444 
445  if (r == MSK_RES_OK) {
446  /* Input the Q for the objective. */
447 
448  r = MSK_putqobj(task, numqz, qsubi, qsubj, qval);
449  }
450 
451  if (r == MSK_RES_OK) {
452  MSKrescodee trmcode;
453 
454  /* Run optimizer */
455  r = MSK_optimizetrm(task, &trmcode);
456  if (r == MSK_RES_OK) {
457  double* xx = (double*)calloc(numvar, sizeof(double));
458  if (xx) {
459  /* Request the interior point solution. */
460  MSK_getxx(task, MSK_SOL_ITR, xx);
461  T_waypoints_t nwaypoints;
462  In begin(wayPointsBegin);
463  for (int i = 0; i < size; i = ++i, ++begin) {
464  point_t target;
465  for (int j = 0; j < Dim; ++j) {
466  target(j) = xx[i + j * size];
467  }
468  nwaypoints.push_back(std::make_pair(begin->first, target));
469  }
470  res = new exact_cubic_t(nwaypoints.begin(), nwaypoints.end());
471  free(xx);
472  }
473  }
474  }
475  /* Delete the task and the associated data. */
476  MSK_deletetask(&task);
477  return res;
478 }
479 
480 } // namespace spline
481 #endif //_CLASS_SPLINEOPTIMIZER
spline::SplineOptimizer
Mosek connection to produce optimized splines.
Definition: OptimizeSpline.h:28
spline::SplineOptimizer::~SplineOptimizer
~SplineOptimizer()
Destructor.
Definition: OptimizeSpline.h:45
spline::SplineOptimizer::point_t
Point point_t
Definition: OptimizeSpline.h:30
spline::SplineOptimizer::num_t
Numeric num_t
Definition: OptimizeSpline.h:32
spline::SplineOptimizer::GenerateOptimizedCurve
exact_cubic_t * GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
Starts an optimization loop to create curve.
spline::SplineOptimizer::exact_cubic_t
exact_cubic< time_t, Numeric, Dim, Safe, Point > exact_cubic_t
Definition: OptimizeSpline.h:33
spline::SplineOptimizer::SplineOptimizer
SplineOptimizer()
Initializes optimizer environment.
Definition: OptimizeSpline.h:39
exact_cubic.h
class allowing to create an Exact cubic spline.
spline::SplineOptimizer::splineOptimizer_t
SplineOptimizer< time_t, Numeric, Dim, Safe, Point > splineOptimizer_t
Definition: OptimizeSpline.h:34
spline::helpers::Numeric
double Numeric
Definition: effector_spline.h:26
spline::helpers::Time
double Time
Definition: effector_spline.h:27
spline::SplineOptimizer::MatrixX
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: OptimizeSpline.h:29
spline::SplineOptimizer::time_t
Time time_t
Definition: OptimizeSpline.h:31
spline::helpers::Point
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
MathDefs.h
spline::helpers::exact_cubic_t
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
spline
Definition: bernstein.h:20
spline::exact_cubic
Definition: exact_cubic.h:40