1#ifndef BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_X5_J1_DDC1_DC1_C1_HH
2#define BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_X5_J1_DDC1_DC1_C1_HH
12namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1 {
17static const size_t DIM_VAR = 3 * 5;
34 const double t2 = t * t;
35 const double t3 = t2 * t;
36 const double t4 = t3 * t;
37 const double t5 = t4 * t;
38 const double t6 = t5 * t;
39 const double t7 = t6 * t;
40 const double t8 = t7 * t;
41 const double t9 = t8 * t;
42 const double t10 = t9 * t;
43 const double t11 = t10 * t;
44 const double t12 = t11 * t;
47 wp.
first.block<3, 3>(0, 0) =
49 (+495.0 * t4 - 3960.0 * t5 + 13860.0 * t6 - 27720.0 * t7 + 34650.0 * t8 -
50 27720.0 * t9 + 13860.0 * t10 - 3960.0 * t11 + 495.0 * t12);
51 wp.
first.block<3, 3>(0, 3) =
53 (+792.0 * t5 - 5544.0 * t6 + 16632.0 * t7 - 27720.0 * t8 + 27720.0 * t9 -
54 16632.0 * t10 + 5544.0 * t11 - 792.0 * t12);
55 wp.
first.block<3, 3>(0, 6) =
57 (+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 + 13860.0 * t10 -
58 5544.0 * t11 + 924.0 * t12);
59 wp.
first.block<3, 3>(0, 9) =
60 Matrix3::Identity() * (+792.0 * t7 - 3960.0 * t8 + 7920.0 * t9 -
61 7920.0 * t10 + 3960.0 * t11 - 792.0 * t12);
62 wp.
first.block<3, 3>(0, 12) =
63 Matrix3::Identity() * (+495.0 * t8 - 1980.0 * t9 + 2970.0 * t10 -
64 1980.0 * t11 + 495.0 * t12);
67 1.0 * pi[0] + t * (-12.0 * pi[0] + 12.0 * pi[1]) +
68 t2 * (66.0 * pi[0] - 132.0 * pi[1] + 66.0 * pi[2]) +
69 t3 * (-220.0 * pi[0] + 660.0 * pi[1] - 660.0 * pi[2] + 220.0 * pi[3]) +
70 t4 * (495.0 * pi[0] - 1980.0 * pi[1] + 2970.0 * pi[2] - 1980.0 * pi[3]) +
71 t5 * (-792.0 * pi[0] + 3960.0 * pi[1] - 7920.0 * pi[2] + 7920.0 * pi[3]) +
73 (924.0 * pi[0] - 5544.0 * pi[1] + 13860.0 * pi[2] - 18480.0 * pi[3]) +
74 t7 * (-792.0 * pi[0] + 5544.0 * pi[1] - 16632.0 * pi[2] +
77 (495.0 * pi[0] - 3960.0 * pi[1] + 13860.0 * pi[2] - 27720.0 * pi[3]) +
78 t9 * (-220.0 * pi[0] + 1980.0 * pi[1] - 7920.0 * pi[2] + 18480.0 * pi[3] +
80 t10 * (66.0 * pi[0] + 66.0 * pi[10] - 660.0 * pi[1] + 2970.0 * pi[2] -
81 7920.0 * pi[3] - 660.0 * pi[9]) +
82 t11 * (-12.0 * pi[0] - 132.0 * pi[10] + 12.0 * pi[11] + 132.0 * pi[1] -
83 660.0 * pi[2] + 1980.0 * pi[3] + 660.0 * pi[9]) +
84 t12 * (1.0 * pi[0] + 66.0 * pi[10] - 12.0 * pi[11] + 1.0 * pi[12] -
85 12.0 * pi[1] + 66.0 * pi[2] - 220.0 * pi[3] - 220.0 * pi[9]);
91 const std::vector<point_t>& pi,
double T,
double t) {
93 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
95 const double alpha = 1. / (T);
96 const double t2 = t * t;
97 const double t3 = t2 * t;
98 const double t4 = t3 * t;
99 const double t5 = t4 * t;
100 const double t6 = t5 * t;
101 const double t7 = t6 * t;
102 const double t8 = t7 * t;
103 const double t9 = t8 * t;
106 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha;
107 wp.
first.block<3, 3>(0, 3) = Matrix3::Identity() * alpha;
108 wp.
first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha;
109 wp.
first.block<3, 3>(0, 9) = Matrix3::Identity() * alpha;
110 wp.
first.block<3, 3>(0, 12) = Matrix3::Identity() * alpha;
113 (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) +
114 t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
115 t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] +
118 (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] +
119 3240.0 * pi[7] - 810.0 * pi[8] + 90.0 * pi[9])) +
120 t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] -
122 t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] +
123 450.0 * pi[2] - 1200.0 * pi[3] - 1200.0 * pi[7] +
124 450.0 * pi[8] - 100.0 * pi[9])) +
125 t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] +
127 t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
129 t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] +
130 29400.0 * pi[3] + 840.0 * pi[7])) +
131 t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] -
132 20160.0 * pi[3] - 2880.0 * pi[7] + 360.0 * pi[8]))) *
139 const std::vector<point_t>& pi,
double T,
double t) {
141 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
143 const double alpha = 1. / (T * T);
144 const double t2 = t * t;
145 const double t3 = t2 * t;
146 const double t4 = t3 * t;
147 const double t5 = t4 * t;
148 const double t6 = t5 * t;
149 const double t7 = t6 * t;
150 const double t8 = t7 * t;
152 wp.
first.block<3, 3>(0, 0) =
153 Matrix3::Identity() * alpha *
154 (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 +
155 94500.0 * t4 - 25200.0 * t3 + 2520.0 * t2));
156 wp.
first.block<3, 3>(0, 3) =
157 Matrix3::Identity() * alpha *
158 (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 -
159 37800.0 * t4 + 5040.0 * t3));
160 wp.
first.block<3, 3>(0, 6) =
161 Matrix3::Identity() * alpha *
162 (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 +
165 (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
166 t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] +
168 t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
170 t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] +
171 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
172 4050.0 * pi[8] - 900.0 * pi[9])) +
173 t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] +
175 t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
177 t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] +
178 176400.0 * pi[3] + 5040.0 * pi[7])) +
179 t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] -
180 141120.0 * pi[3] - 20160.0 * pi[7] + 2520.0 * pi[8])) +
181 t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] +
182 60480.0 * pi[3] + 25920.0 * pi[7] - 6480.0 * pi[8] +
190 const std::vector<point_t>& pi,
double T,
double t) {
192 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
194 const double alpha = 1. / (T * T * T);
195 const double t2 = t * t;
196 const double t3 = t2 * t;
197 const double t4 = t3 * t;
198 const double t5 = t4 * t;
199 const double t6 = t5 * t;
200 const double t7 = t6 * t;
202 wp.
first.block<3, 3>(0, 0) =
203 Matrix3::Identity() * alpha *
204 (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 +
205 378000.0 * t3 - 75600.0 * t2 + 5040.0 * t));
206 wp.
first.block<3, 3>(0, 3) =
207 Matrix3::Identity() * alpha *
208 (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 -
209 151200.0 * t3 + 15120.0 * t2));
210 wp.
first.block<3, 3>(0, 6) =
211 Matrix3::Identity() * alpha *
212 (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 +
216 (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
217 t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] -
219 t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
221 t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
223 t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] +
224 882000.0 * pi[3] + 25200.0 * pi[7])) +
225 t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] -
226 846720.0 * pi[3] - 120960.0 * pi[7] + 15120.0 * pi[8])) +
227 t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] +
228 423360.0 * pi[3] + 181440.0 * pi[7] - 45360.0 * pi[8] +
230 t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] +
231 32400.0 * pi[2] - 86400.0 * pi[3] - 86400.0 * pi[7] +
232 32400.0 * pi[8] - 7200.0 * pi[9]))) *
243 std::vector<point_t> pi;
244 pi.push_back(pData.
c0_);
245 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
246 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) +
247 (2 * pData.
dc0_ * T / n) +
249 pi.push_back((pData.
j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
250 (3 * pData.
ddc0_ * T * T / (n * (n - 1))) +
251 (3 * pData.
dc0_ * T / n) + pData.
c0_);
252 pi.push_back(point_t::Zero());
253 pi.push_back(point_t::Zero());
254 pi.push_back(point_t::Zero());
255 pi.push_back(point_t::Zero());
256 pi.push_back(point_t::Zero());
257 pi.push_back((-pData.
j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
258 (3 * pData.
ddc1_ * T * T / (n * (n - 1))) -
259 (3 * pData.
dc1_ * T / n) + pData.
c1_);
260 pi.push_back((pData.
ddc1_ * T * T / (n * (n - 1))) -
261 (2 * pData.
dc1_ * T / n) + pData.
c1_);
262 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
263 pi.push_back(pData.
c1_);
270 bezier_wp_t::t_point_t wps;
274 std::vector<Matrix3> Cpi;
275 for (std::size_t i = 0; i < pi.size(); ++i) {
276 Cpi.push_back(
skew(pi[i]));
282 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
288 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
291 std::vector<waypoint_t> wps;
292 assert(pi.size() == 13);
294 double alpha = 1. / (T);
298 w.
second = alpha * 12 * (-pi[0] + pi[1]);
302 w.
second = alpha * 12 * (-pi[1] + pi[2]);
306 w.
second = alpha * 12 * (-pi[2] + pi[3]);
310 w.
first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity();
311 w.
second = alpha * 12 * (-pi[3]);
315 w.
first.block<3, 3>(0, 0) = -12 * alpha * Matrix3::Identity();
316 w.
first.block<3, 3>(0, 3) = 12 * alpha * Matrix3::Identity();
320 w.
first.block<3, 3>(0, 3) = -12 * alpha * Matrix3::Identity();
321 w.
first.block<3, 3>(0, 6) = 12 * alpha * Matrix3::Identity();
325 w.
first.block<3, 3>(0, 6) = -12 * alpha * Matrix3::Identity();
326 w.
first.block<3, 3>(0, 9) = 12 * alpha * Matrix3::Identity();
330 w.
first.block<3, 3>(0, 9) = -12 * alpha * Matrix3::Identity();
331 w.
first.block<3, 3>(0, 12) = 12 * alpha * Matrix3::Identity();
335 w.
first.block<3, 3>(0, 12) = -12 * alpha * Matrix3::Identity();
336 w.
second = alpha * 12 * pi[9];
340 w.
second = alpha * 12 * (-pi[9] + pi[10]);
344 w.
second = alpha * 12 * (-pi[10] + pi[11]);
348 w.
second = alpha * 12 * (-pi[11] + pi[12]);
355 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
358 std::vector<waypoint_t> wps;
359 assert(pi.size() == 13);
360 double alpha = 1. / (T * T);
365 w.
second = alpha * 132 * (pi[0] - 2 * pi[1] + pi[2]);
369 w.
second = alpha * 132 * (pi[1] - 2 * pi[2] + pi[3]);
373 w.
first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity();
374 w.
second = alpha * 132 * (pi[2] - pi[3]);
378 w.
first.block<3, 3>(0, 0) = -264 * alpha * Matrix3::Identity();
379 w.
first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity();
380 w.
second = alpha * 132 * pi[3];
384 w.
first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity();
385 w.
first.block<3, 3>(0, 3) = -264 * alpha * Matrix3::Identity();
386 w.
first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity();
390 w.
first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity();
391 w.
first.block<3, 3>(0, 6) = -264 * alpha * Matrix3::Identity();
392 w.
first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity();
396 w.
first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity();
397 w.
first.block<3, 3>(0, 9) = -264 * alpha * Matrix3::Identity();
398 w.
first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity();
402 w.
first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity();
403 w.
first.block<3, 3>(0, 12) = -264 * alpha * Matrix3::Identity();
404 w.
second = alpha * 132 * pi[7];
408 w.
first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity();
409 w.
second = alpha * 132 * (-2 * pi[9] + pi[10]);
413 w.
second = alpha * 132 * (pi[9] - 2 * pi[10] + pi[11]);
418 w.
second = alpha * 132 * (pi[12] + pi[10] - 2 * pi[11]);
425 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
428 std::vector<waypoint_t> wps;
429 assert(pi.size() == 13);
431 double alpha = 1. / (T * T * T);
436 w.
second = alpha * 1320 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
440 w.
first.block<3, 3>(0, 0) = 1320 * alpha * Matrix3::Identity();
441 w.
second = alpha * 1320 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
445 w.
first.block<3, 3>(0, 0) = 1320 * -3 * alpha * Matrix3::Identity();
446 w.
first.block<3, 3>(0, 3) = 1320 * alpha * Matrix3::Identity();
447 w.
second = alpha * 1320 * (-pi[2] + 3 * pi[3]);
451 w.
first.block<3, 3>(0, 0) = 1320 * 3 * alpha * Matrix3::Identity();
452 w.
first.block<3, 3>(0, 3) = 1320 * -3 * alpha * Matrix3::Identity();
453 w.
first.block<3, 3>(0, 6) = 1320 * alpha * Matrix3::Identity();
454 w.
second = alpha * 1320 * (-pi[3]);
458 w.
first.block<3, 3>(0, 0) = -1320 * alpha * Matrix3::Identity();
459 w.
first.block<3, 3>(0, 3) = 1320 * 3 * alpha * Matrix3::Identity();
460 w.
first.block<3, 3>(0, 6) = 1320 * -3 * alpha * Matrix3::Identity();
461 w.
first.block<3, 3>(0, 9) = 1320 * alpha * Matrix3::Identity();
465 w.
first.block<3, 3>(0, 3) = -1320 * alpha * Matrix3::Identity();
466 w.
first.block<3, 3>(0, 6) = 1320 * 3 * alpha * Matrix3::Identity();
467 w.
first.block<3, 3>(0, 9) = 1320 * -3 * alpha * Matrix3::Identity();
468 w.
first.block<3, 3>(0, 12) = 1320 * alpha * Matrix3::Identity();
472 w.
first.block<3, 3>(0, 6) = -1320 * alpha * Matrix3::Identity();
473 w.
first.block<3, 3>(0, 9) = 1320 * 3 * alpha * Matrix3::Identity();
474 w.
first.block<3, 3>(0, 12) = 1320 * -3 * alpha * Matrix3::Identity();
475 w.
second = alpha * 1320 * pi[9];
479 w.
first.block<3, 3>(0, 9) = -1320 * alpha * Matrix3::Identity();
480 w.
first.block<3, 3>(0, 12) = 1320 * 3 * alpha * Matrix3::Identity();
481 w.
second = alpha * 1320 * (-3 * pi[9] + pi[10]);
485 w.
first.block<3, 3>(0, 12) = -1320 * alpha * Matrix3::Identity();
486 w.
second = alpha * 1320 * (3 * pi[9] - 3 * pi[10] + pi[11]);
490 w.
second = alpha * 1320 * (pi[12] - pi[9] + 3 * pi[10] - 3 * pi[11]);
501 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
508 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
509 MatrixXX H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
510 VectorX g = VectorX::Zero(DIM_VAR);
513 g.segment<3>(0) = ((-17.8646615739593 * pi[0] - 4.24835843773412 * pi[10] -
514 1.80981866649436 * pi[11] - 0.408668730537654 * pi[12] -
515 13.8947369836412 * pi[1] + 2.56878303943036 * pi[2] +
516 16.0548432515434 * pi[3] - 6.66893486967885 * pi[9])) /
518 g.segment<3>(3) = ((-7.93984965058761 * pi[0] - 7.0641309185535 * pi[10] -
519 4.08668730511085 * pi[11] - 1.22600619206665 * pi[12] -
520 12.1432972410894 * pi[1] - 7.06413670827152 * pi[2] +
521 3.85315840674136 * pi[3] - 7.50872663647158 * pi[9])) /
523 g.segment<3>(6) = (-3.26934980716442 * pi[0] - 8.9907120599184 * pi[10] -
524 7.76470588258269 * pi[11] - 3.26934984520124 * pi[12] -
525 7.76470597007188 * pi[1] - 8.99071211730055 * pi[2] -
526 4.49535589801788 * pi[3] - 4.49535607858364 * pi[9]) /
528 g.segment<3>(9) = (-1.22600620726636 * pi[0] - 7.06413092270385 * pi[10] -
529 12.1432994250704 * pi[11] - 7.93984962409094 * pi[12] -
530 4.08668774398579 * pi[1] - 7.0641311269266 * pi[2] -
531 7.50872489092664 * pi[3] + 3.85316232209763 * pi[9]) /
533 g.segment<3>(12) = (-0.408668732514974 * pi[0] + 2.56877487851457 * pi[10] -
534 13.8947368423667 * pi[11] - 17.8646616541281 * pi[12] -
535 1.80981880873492 * pi[1] - 4.2483587965255 * pi[2] -
536 6.66893350792178 * pi[3] + 16.0548429731073 * pi[9]) /
539 H.block<3, 3>(0, 0) = Matrix3::Identity() * 9.63290527229048 / (T);
540 H.block<3, 3>(3, 3) = Matrix3::Identity() * 8.29911962311903 / (T);
541 H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.92188615942945 / (T);
542 H.block<3, 3>(9, 9) = Matrix3::Identity() * 8.29911871865983 / (T);
543 H.block<3, 3>(12, 12) = Matrix3::Identity() * 9.63290582796267 / (T);
545 H.block<3, 3>(0, 3) =
546 Matrix3::Identity() * 13.4860690009623 / (2 * T);
547 H.block<3, 3>(3, 0) =
548 Matrix3::Identity() * 13.4860690009623 / (2 * T);
549 H.block<3, 3>(0, 6) =
550 Matrix3::Identity() * 4.14955180440231 / (2 * T);
551 H.block<3, 3>(6, 0) =
552 Matrix3::Identity() * 4.14955180440231 / (2 * T);
553 H.block<3, 3>(0, 9) =
554 Matrix3::Identity() * -3.55676093144659 / (2 * T);
555 H.block<3, 3>(9, 0) =
556 Matrix3::Identity() * -3.55676093144659 / (2 * T);
557 H.block<3, 3>(0, 12) =
558 Matrix3::Identity() * -7.07311260219052 / (2 * T);
559 H.block<3, 3>(12, 0) =
560 Matrix3::Identity() * -7.07311260219052 / (2 * T);
562 H.block<3, 3>(3, 6) =
563 Matrix3::Identity() * 12.4486856197374 / (2 * T);
564 H.block<3, 3>(6, 3) =
565 Matrix3::Identity() * 12.4486856197374 / (2 * T);
566 H.block<3, 3>(3, 9) =
567 Matrix3::Identity() * 4.20345048607838 / (2 * T);
568 H.block<3, 3>(9, 3) =
569 Matrix3::Identity() * 4.20345048607838 / (2 * T);
570 H.block<3, 3>(3, 12) =
571 Matrix3::Identity() * -3.55676456195318 / (2 * T);
572 H.block<3, 3>(12, 3) =
573 Matrix3::Identity() * -3.55676456195318 / (2 * T);
575 H.block<3, 3>(6, 9) =
576 Matrix3::Identity() * 12.448679688301 / (2 * T);
577 H.block<3, 3>(9, 6) =
578 Matrix3::Identity() * 12.448679688301 / (2 * T);
579 H.block<3, 3>(6, 12) =
580 Matrix3::Identity() * 4.149559164651 / (2 * T);
581 H.block<3, 3>(12, 6) =
582 Matrix3::Identity() * 4.149559164651 / (2 * T);
584 H.block<3, 3>(9, 12) =
585 Matrix3::Identity() * 13.4860680294621 / (2 * T);
586 H.block<3, 3>(12, 9) =
587 Matrix3::Identity() * 13.4860680294621 / (2 * T);
589 double norm = H.norm();
593 return std::make_pair(H, g);
END_ACC
Definition: flags.hh:25
INIT_VEL
Definition: flags.hh:21
END_VEL
Definition: flags.hh:24
END_POS
Definition: flags.hh:23
FIVE_FREE_VAR
Definition: flags.hh:32
INIT_ACC
Definition: flags.hh:22
INIT_POS
Definition: flags.hh:20
INIT_JERK
Definition: flags.hh:26
END_JERK
Definition: flags.hh:27
std::vector< waypoint_t > computeJerkWaypoints(const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:423
waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:90
std::vector< waypoint_t > computeVelocityWaypoints(const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:286
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:496
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:237
waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:189
std::vector< waypoint_t > computeAccelerationWaypoints(const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:353
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:268
waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:138
std::pair< MatrixXX, VectorX > computeVelocityCost(const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:506
waypoint_t evaluateCurveWaypointAtTime(const std::vector< point_t > &pi, double t)
evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi...
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:31
Definition: common_solve_methods.hh:15
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:24
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:62
const int DIM_POINT
Definition: solve_end_effector.hh:15
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:62
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:21
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:92
point_t ddc1_
Definition: data.hh:107
point_t j1_
Definition: data.hh:107
point_t dc0_
Definition: data.hh:107
point_t ddc0_
Definition: data.hh:107
point_t dc1_
Definition: data.hh:107
point_t j0_
Definition: data.hh:107
point_t c0_
Definition: data.hh:107
point_t c1_
Definition: data.hh:107
VectorX second
Definition: utils.hh:27
MatrixXX first
Definition: utils.hh:26