12 #ifndef _CLASS_SPLINEOPTIMIZER 13 #define _CLASS_SPLINEOPTIMIZER 15 #include "spline/MathDefs.h" 16 #include "spline/exact_cubic.h" 18 #include "mosek/mosek.h" 19 #include <Eigen/SparseCore> 26 template <
typename Time = double,
typename Numeric =
Time, Eigen::Index Dim = 3,
bool Safe =
false,
27 typename Point = Eigen::Matrix<Numeric, Dim, 1> >
29 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
MatrixX;
40 MSKrescodee r_ = MSK_makeenv(&env_, NULL);
41 assert(r_ == MSK_RES_OK);
57 template <
typename In>
62 template <
typename In>
71 typedef std::pair<time_t, Point> waypoint_t;
72 typedef std::vector<waypoint_t> T_waypoints_t;
75 template <
typename Time,
typename Numeric, Eigen::Index Dim,
bool Safe,
typename Po
int>
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,
80 std::size_t
const size(std::distance(wayPointsBegin, wayPointsEnd));
81 assert((!Safe) || (size > 1));
83 In it(wayPointsBegin), next(wayPointsBegin);
85 Numeric t_previous((*it).first);
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);
91 h3(i, i) = -3 / dTi_sqr;
92 h3(i, i + 1) = 3 / dTi_sqr;
94 h4(i, i + 1) = -1 / dTi;
98 num_t
const dTi_1((*it2).first - (*next).first);
99 num_t
const dTi_1sqr(dTi_1 * dTi_1);
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;
111 template <
typename Time,
typename Numeric, std::
size_t Dim,
bool Safe,
typename Po
int>
112 template <
typename In>
113 inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
116 int const size((
int)std::distance(wayPointsBegin, wayPointsEnd));
117 if (Safe && size < 1) {
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);
132 ComputeHMatrices(wayPointsBegin, wayPointsEnd, h1, h2, h3, h4);
138 const MSKint32t numvar = (size)*Dim * 3;
151 int ptOff = (int)Dim * size;
152 int ptptOff = (int)Dim * 2 * size;
153 int prOff = (int)3 * ptOff;
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) {
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);
180 MatrixX g1x_g2x = MatrixX::Zero(size * Dim, numvar);
207 MatrixX g3x_g4x = MatrixX::Zero(size * Dim, numvar);
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) {
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;
257 MatrixX x0_x0 = MatrixX::Zero(Dim, numvar);
258 for (
int j = 0; j < Dim; ++j) {
261 x0_x0(2, size * 2) = 1;
265 MatrixX x0p_0 = MatrixX::Zero(Dim, numvar);
266 for (
int j = 0; j < Dim; ++j) {
268 x0p_0(1, ptOff + size) = 1;
269 x0p_0(2, ptOff + size * 2) = 1;
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;
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;
297 const MSKint32t numcon = Dim * 2 * size + 4 * Dim;
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;
309 Eigen::SparseMatrix<Numeric> spA;
310 spA = a.sparseView();
315 int nonZeros = spA.nonZeros();
319 double* aval =
new double[nonZeros];
320 MSKint32t* asub =
new MSKint32t[nonZeros];
321 MSKint32t* aptrb =
new MSKint32t[numvar];
322 MSKint32t* aptre =
new MSKint32t[numvar];
324 int currentIndex = 0;
325 for (
int j = 0; j < numvar; ++j) {
326 bool nonZeroAtThisCol =
false;
327 for (
int i = 0; i < numcon; ++i) {
329 if (!nonZeroAtThisCol) {
330 aptrb[j] = currentIndex;
331 nonZeroAtThisCol =
true;
333 aval[currentIndex] = a(i, j);
334 asub[currentIndex] = i;
335 aptre[j] = currentIndex + 1;
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;
355 qsubj[id] =
id + ptOff;
360 MSKboundkeye* bkc =
new MSKboundkeye[numcon];
362 double* blc =
new double[numcon];
363 double* buc =
new double[numcon];
365 for (
int i = 0; i < numcon - Dim * 2; ++i) {
370 for (
int i = numcon - Dim * 2; i < numcon - Dim; ++i)
373 blc[i] = wayPointsBegin->second(i - (numcon - Dim * 2));
374 buc[i] = wayPointsBegin->second(i - (numcon - Dim * 2));
376 In last(wayPointsEnd);
378 for (
int i = numcon - 3; i < numcon; ++i)
381 blc[i] = last->second(i - (numcon - Dim));
382 buc[i] = last->second(i - (numcon - Dim));
386 MSKboundkeye* bkx =
new MSKboundkeye[numvar];
388 double* blx =
new double[numvar];
389 double* bux =
new double[numvar];
391 for (
int i = 0; i < numvar; ++i) {
393 blx[i] = -MSK_INFINITY;
394 bux[i] = +MSK_INFINITY;
398 MSKtask_t task = NULL;
400 r = MSK_maketask(env_, numcon, numvar, &task);
408 if (r == MSK_RES_OK) r = MSK_appendcons(task, numcon);
412 if (r == MSK_RES_OK) r = MSK_appendvars(task, numvar);
414 for (
int j = 0; j < numvar && r == MSK_RES_OK; ++j) {
416 if (r == MSK_RES_OK) r = MSK_putcj(task, j, 0);
421 r = MSK_putvarbound(task, j,
428 r = MSK_putacol(task, j,
436 for (
int i = 0; i < numcon && r == MSK_RES_OK; ++i)
437 r = MSK_putconbound(task, i,
443 if (r == MSK_RES_OK) r = MSK_putobjsense(task, MSK_OBJECTIVE_SENSE_MINIMIZE);
445 if (r == MSK_RES_OK) {
448 r = MSK_putqobj(task, numqz, qsubi, qsubj, qval);
451 if (r == MSK_RES_OK) {
455 r = MSK_optimizetrm(task, &trmcode);
456 if (r == MSK_RES_OK) {
457 double* xx = (
double*)calloc(numvar,
sizeof(
double));
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) {
465 for (
int j = 0; j < Dim; ++j) {
466 target(j) = xx[i + j * size];
468 nwaypoints.push_back(std::make_pair(begin->first, target));
470 res =
new exact_cubic_t(nwaypoints.begin(), nwaypoints.end());
476 MSK_deletetask(&task);
481 #endif //_CLASS_SPLINEOPTIMIZER SplineOptimizer()
Initializes optimizer environment.
Definition: OptimizeSpline.h:39
Numeric num_t
Definition: OptimizeSpline.h:32
Definition: bernstein.h:20
Time time_t
Definition: OptimizeSpline.h:31
~SplineOptimizer()
Destructor.
Definition: OptimizeSpline.h:45
double Time
Definition: effector_spline.h:27
double Numeric
Definition: effector_spline.h:26
SplineOptimizer< time_t, Numeric, Dim, Safe, Point > splineOptimizer_t
Definition: OptimizeSpline.h:34
Point point_t
Definition: OptimizeSpline.h:30
exact_cubic_t * GenerateOptimizedCurve(In wayPointsBegin, In wayPointsEnd) const
Starts an optimization loop to create curve.
Mosek connection to produce optimized splines.
Definition: OptimizeSpline.h:28
exact_cubic< time_t, Numeric, Dim, Safe, Point > exact_cubic_t
Definition: OptimizeSpline.h:33
spline_deriv_constraint_t::exact_cubic_t exact_cubic_t
Definition: effector_spline.h:34
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: OptimizeSpline.h:29
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28