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;
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]))) *
288 std::vector<bezier_t::point_t>
pi = std::vector<bezier_t::point_t>()) {
291 std::vector<waypoint_t>
wps;
310 w.first.block<3, 3>(0, 0) = 12 *
alpha * Matrix3::Identity();
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();
355 std::vector<bezier_t::point_t>
pi = std::vector<bezier_t::point_t>()) {
358 std::vector<waypoint_t>
wps;
373 w.first.block<3, 3>(0, 0) = 132 *
alpha * Matrix3::Identity();
378 w.first.block<3, 3>(0, 0) = -264 *
alpha * Matrix3::Identity();
379 w.first.block<3, 3>(0, 3) = 132 *
alpha * Matrix3::Identity();
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();
408 w.first.block<3, 3>(0, 12) = 132 *
alpha * Matrix3::Identity();
409 w.second =
alpha * 132 * (-2 *
pi[9] +
pi[10]);
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;
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();
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();
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]);
508 std::vector<bezier_t::point_t>
pi = std::vector<bezier_t::point_t>()) {
509 MatrixXX H = MatrixXX::Zero(DIM_VAR, 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);