1 #ifndef BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_X3_J1_DDC1_DC1_C1_HH
2 #define BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_X3_J1_DDC1_DC1_C1_HH
12 namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1 {
14 static const ConstraintFlag flag =
16 static const size_t DIM_VAR = 9;
31 const double t2 = t * t;
32 const double t3 = t2 * t;
33 const double t4 = t3 * t;
34 const double t5 = t4 * t;
35 const double t6 = t5 * t;
36 const double t7 = t6 * t;
37 const double t8 = t7 * t;
38 const double t9 = t8 * t;
39 const double t10 = t9 * t;
42 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * (t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 +
43 3150.0 * t8 - 1260.0 * t9 + 210.0 * t10);
44 wp.
first.block<3, 3>(0, 3) =
45 Matrix3::Identity() * (252.0 * t5 - 1260.0 * t6 + 2520.0 * t7 - 2520.0 * t8 + 1260.0 * t9 - 252.0 * t10);
46 wp.
first.block<3, 3>(0, 6) =
47 Matrix3::Identity() * (210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10);
48 wp.
second = 1.0 * pi[0] + t * (-10.0 * pi[0] + 10.0 * pi[1]) + t2 * (45.0 * pi[0] - 90.0 * pi[1] + 45.0 * pi[2]) +
49 t3 * (-120.0 * pi[0] + 360.0 * pi[1] - 360.0 * pi[2] + 120.0 * pi[3]) +
50 t4 * (210.0 * pi[0] - 840.0 * pi[1] + 1260.0 * pi[2] - 840.0 * pi[3]) +
51 t5 * (-252.0 * pi[0] + 1260.0 * pi[1] - 2520.0 * pi[2] + 2520.0 * pi[3]) +
52 t6 * (210.0 * pi[0] - 1260.0 * pi[1] + 3150.0 * pi[2] - 4200.0 * pi[3]) +
53 t7 * (-120.0 * pi[0] + 840.0 * pi[1] - 2520.0 * pi[2] + 4200.0 * pi[3] + 120.0 * pi[7]) +
54 t8 * (45.0 * pi[0] - 360.0 * pi[1] + 1260.0 * pi[2] - 2520.0 * pi[3] - 360.0 * pi[7] + 45.0 * pi[8]) +
55 t9 * (-10.0 * pi[0] + 90.0 * pi[1] - 360.0 * pi[2] + 840.0 * pi[3] + 360.0 * pi[7] - 90.0 * pi[8] +
57 t10 * (1.0 * pi[0] + 1.0 * pi[10] - 10.0 * pi[1] + 45.0 * pi[2] - 120.0 * pi[3] - 120.0 * pi[7] +
58 45.0 * pi[8] - 10.0 * pi[9]);
65 const double alpha = 1. / (T);
66 const double t2 = t * t;
67 const double t3 = t2 * t;
68 const double t4 = t3 * t;
69 const double t5 = t4 * t;
70 const double t6 = t5 * t;
71 const double t7 = t6 * t;
72 const double t8 = t7 * t;
73 const double t9 = t8 * t;
75 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
76 (1.0 * (2100.0 * t9 - 11340.0 * t8 + 25200.0 * t7 - 29400.0 * t6 + 18900.0 * t5 -
77 6300.0 * t4 + 840.0 * t3));
78 wp.
first.block<3, 3>(0, 3) =
79 Matrix3::Identity() * alpha *
80 (1.0 * (-2520.0 * t9 + 11340.0 * t8 - 20160.0 * t7 + 17640.0 * t6 - 7560.0 * t5 + 1260.0 * t4));
81 wp.
first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha *
82 (1.0 * (2100.0 * t9 - 7560.0 * t8 + 10080.0 * t7 - 5880.0 * t6 + 1260.0 * t5));
83 wp.
second = (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) + t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
84 t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
85 t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
86 810.0 * pi[8] + 90.0 * pi[9])) +
87 t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
88 t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
89 1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
90 t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
91 t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
92 t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
93 t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
102 const double alpha = 1. / (T * T);
103 const double t2 = t * t;
104 const double t3 = t2 * t;
105 const double t4 = t3 * t;
106 const double t5 = t4 * t;
107 const double t6 = t5 * t;
108 const double t7 = t6 * t;
109 const double t8 = t7 * t;
111 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
112 (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
113 25200.0 * t3 + 2520.0 * t2));
114 wp.
first.block<3, 3>(0, 3) =
115 Matrix3::Identity() * alpha *
116 (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3));
117 wp.
first.block<3, 3>(0, 6) =
118 Matrix3::Identity() * alpha *
119 (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4));
121 (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
122 t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
123 t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
124 t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] + 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
125 4050.0 * pi[8] - 900.0 * pi[9])) +
126 t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
127 t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
128 t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
129 t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
131 t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
132 6480.0 * pi[8] + 720.0 * pi[9]))) *
140 const double alpha = 1. / (T * T * T);
141 const double t2 = t * t;
142 const double t3 = t2 * t;
143 const double t4 = t3 * t;
144 const double t5 = t4 * t;
145 const double t6 = t5 * t;
146 const double t7 = t6 * t;
148 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
149 (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
150 75600.0 * t2 + 5040.0 * t));
151 wp.
first.block<3, 3>(0, 3) =
152 Matrix3::Identity() * alpha *
153 (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2));
154 wp.
first.block<3, 3>(0, 6) =
155 Matrix3::Identity() * alpha *
156 (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3));
158 (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
159 t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
160 t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
161 t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
162 t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
163 t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
165 t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
166 45360.0 * pi[8] + 5040.0 * pi[9])) +
167 t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
168 86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
177 std::vector<point_t> pi;
178 pi.push_back(pData.
c0_);
179 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
180 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) + (2 * pData.
dc0_ * T / n) +
182 pi.push_back((pData.
j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.
ddc0_ * T * T / (n * (n - 1))) +
183 (3 * pData.
dc0_ * T / n) + pData.
c0_);
184 pi.push_back(point_t::Zero());
185 pi.push_back(point_t::Zero());
186 pi.push_back(point_t::Zero());
187 pi.push_back((-pData.
j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.
ddc1_ * T * T / (n * (n - 1))) -
188 (3 * pData.
dc1_ * T / n) + pData.
c1_);
189 pi.push_back((pData.
ddc1_ * T * T / (n * (n - 1))) - (2 * pData.
dc1_ * T / n) + pData.
c1_);
190 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
191 pi.push_back(pData.
c1_);
197 bezier_wp_t::t_point_t wps;
201 std::vector<Matrix3> Cpi;
202 for (std::size_t i = 0; i < pi.size(); ++i) {
203 Cpi.push_back(
skew(pi[i]));
209 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
214 const ProblemData& pData,
const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
217 std::vector<waypoint_t> wps;
218 assert(pi.size() == 11);
220 double alpha = 1. / (T);
224 w.
second = alpha * 10 * (-pi[0] + pi[1]);
228 w.
second = alpha * 10 * (-pi[1] + pi[2]);
232 w.
second = alpha * 10 * (-pi[2] + pi[3]);
236 w.
first.block<3, 3>(0, 0) = 10 * alpha * Matrix3::Identity();
237 w.
second = alpha * 10 * (-pi[3]);
241 w.
first.block<3, 3>(0, 0) = -10 * alpha * Matrix3::Identity();
242 w.
first.block<3, 3>(0, 3) = 10 * alpha * Matrix3::Identity();
246 w.
first.block<3, 3>(0, 3) = -10 * alpha * Matrix3::Identity();
247 w.
first.block<3, 3>(0, 6) = 10 * alpha * Matrix3::Identity();
251 w.
first.block<3, 3>(0, 6) = -10 * alpha * Matrix3::Identity();
252 w.
second = alpha * 10 * pi[7];
256 w.
second = alpha * 10 * (-pi[7] + pi[8]);
260 w.
second = alpha * 10 * (-pi[8] + pi[9]);
264 w.
second = alpha * 10 * (pi[10] - pi[9]);
270 const ProblemData& pData,
const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
273 std::vector<waypoint_t> wps;
274 assert(pi.size() == 11);
275 double alpha = 1. / (T * T);
280 w.
second = alpha * 90 * (pi[0] - 2 * pi[1] + pi[2]);
284 w.
second = alpha * 90 * (pi[1] - 2 * pi[2] + pi[3]);
288 w.
first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity();
289 w.
second = alpha * 90 * (pi[2] - pi[3]);
293 w.
first.block<3, 3>(0, 0) = -180 * alpha * Matrix3::Identity();
294 w.
first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity();
295 w.
second = alpha * 90 * pi[3];
299 w.
first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity();
300 w.
first.block<3, 3>(0, 3) = -180 * alpha * Matrix3::Identity();
301 w.
first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity();
305 w.
first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity();
306 w.
first.block<3, 3>(0, 6) = -180 * alpha * Matrix3::Identity();
307 w.
second = alpha * 90 * pi[7];
311 w.
first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity();
312 w.
second = alpha * 90 * (-2 * pi[7] + pi[8]);
316 w.
second = alpha * 90 * (pi[7] - 2 * pi[8] + pi[9]);
321 w.
second = alpha * 90 * (pi[10] + pi[8] - 2 * pi[9]);
327 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
330 std::vector<waypoint_t> wps;
331 assert(pi.size() == 11);
333 double alpha = 1. / (T * T * T);
338 w.
second = alpha * 720 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
342 w.
first.block<3, 3>(0, 0) = 720 * alpha * Matrix3::Identity();
343 w.
second = alpha * 720 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
347 w.
first.block<3, 3>(0, 0) = 720 * -3 * alpha * Matrix3::Identity();
348 w.
first.block<3, 3>(0, 3) = 720 * alpha * Matrix3::Identity();
349 w.
second = alpha * 720 * (-pi[2] + 3 * pi[3]);
353 w.
first.block<3, 3>(0, 0) = 720 * 3 * alpha * Matrix3::Identity();
354 w.
first.block<3, 3>(0, 3) = 720 * -3 * alpha * Matrix3::Identity();
355 w.
first.block<3, 3>(0, 6) = 720 * alpha * Matrix3::Identity();
356 w.
second = alpha * 720 * (-pi[3]);
360 w.
first.block<3, 3>(0, 0) = -720 * alpha * Matrix3::Identity();
361 w.
first.block<3, 3>(0, 3) = 720 * 3 * alpha * Matrix3::Identity();
362 w.
first.block<3, 3>(0, 6) = 720 * -3 * alpha * Matrix3::Identity();
363 w.
second = alpha * 720 * pi[7];
367 w.
first.block<3, 3>(0, 3) = -720 * alpha * Matrix3::Identity();
368 w.
first.block<3, 3>(0, 6) = 720 * 3 * alpha * Matrix3::Identity();
369 w.
second = alpha * 720 * (-3 * pi[7] + pi[8]);
373 w.
first.block<3, 3>(0, 6) = -720 * alpha * Matrix3::Identity();
374 w.
second = alpha * 720 * (3 * pi[7] - 3 * pi[8] + pi[9]);
378 w.
second = alpha * 720 * (pi[10] - pi[7] + 3 * pi[8] - 3 * pi[9]);
389 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
394 const ProblemData& pData,
double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
400 (-12.352941184069 * pi[0] - 2.03619909502433 * pi[10] - 10.5882353430148 * pi[1] + 1.2217194516605 * pi[2] +
401 12.2171947000329 * pi[3] - 4.66474701697538 * pi[7] - 7.21925133730399 * pi[8] - 5.42986425333795 * pi[9]) /
404 (-5.29411764601331 * pi[0] - 5.29411764705762 * pi[10] - 8.95927605247282 * pi[1] - 6.10859723220821 * pi[2] +
405 2.2213080007358 * pi[3] + 2.22130810120924 * pi[7] - 6.10859728485633 * pi[8] - 8.95927601808432 * pi[9]) /
408 (-2.03619909557297 * pi[0] - 12.3529411764706 * pi[10] - 5.42986425052241 * pi[1] - 7.21925133714926 * pi[2] -
409 4.66474700749421 * pi[3] + 12.2171945706055 * pi[7] + 1.22171945695766 * pi[8] - 10.5882352941172 * pi[9]) /
412 H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T);
413 H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T);
414 H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T);
415 H.block<3, 3>(0, 3) = Matrix3::Identity() * 10.8844097406652 / (2 * T);
416 H.block<3, 3>(3, 0) = Matrix3::Identity() * 10.8844097406652 / (2 * T);
417 H.block<3, 3>(0, 6) = Matrix3::Identity() * 2.41875768460934 / (2 * T);
418 H.block<3, 3>(6, 0) = Matrix3::Identity() * 2.41875768460934 / (2 * T);
419 H.block<3, 3>(3, 6) = Matrix3::Identity() * 10.8844097036619 / (2 * T);
420 H.block<3, 3>(6, 3) = Matrix3::Identity() * 10.8844097036619 / (2 * T);
422 double norm = H.norm();
426 return std::make_pair(H, g);
433 #endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH