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