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 12 namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1 {
14 static const ConstraintFlag flag =
16 static const size_t DIM_VAR = 3 * 5;
30 const double t2 = t * t;
31 const double t3 = t2 * t;
32 const double t4 = t3 * t;
33 const double t5 = t4 * t;
34 const double t6 = t5 * t;
35 const double t7 = t6 * t;
36 const double t8 = t7 * t;
37 const double t9 = t8 * t;
38 const double t10 = t9 * t;
39 const double t11 = t10 * t;
40 const double t12 = t11 * t;
43 wp.
first.block<3, 3>(0, 0) =
44 Matrix3::Identity() * (+495.0 * t4 - 3960.0 * t5 + 13860.0 * t6 - 27720.0 * t7 + 34650.0 * t8 - 27720.0 * t9 +
45 13860.0 * t10 - 3960.0 * t11 + 495.0 * t12);
46 wp.
first.block<3, 3>(0, 3) =
47 Matrix3::Identity() * (+792.0 * t5 - 5544.0 * t6 + 16632.0 * t7 - 27720.0 * t8 + 27720.0 * t9 - 16632.0 * t10 +
48 5544.0 * t11 - 792.0 * t12);
49 wp.
first.block<3, 3>(0, 6) = Matrix3::Identity() * (+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 +
50 13860.0 * t10 - 5544.0 * t11 + 924.0 * t12);
51 wp.
first.block<3, 3>(0, 9) = Matrix3::Identity() * (+792.0 * t7 - 3960.0 * t8 + 7920.0 * t9 - 7920.0 * t10 +
52 3960.0 * t11 - 792.0 * t12);
53 wp.
first.block<3, 3>(0, 12) =
54 Matrix3::Identity() * (+495.0 * t8 - 1980.0 * t9 + 2970.0 * t10 - 1980.0 * t11 + 495.0 * t12);
56 wp.
second = 1.0 * pi[0] + t * (-12.0 * pi[0] + 12.0 * pi[1]) + t2 * (66.0 * pi[0] - 132.0 * pi[1] + 66.0 * pi[2]) +
57 t3 * (-220.0 * pi[0] + 660.0 * pi[1] - 660.0 * pi[2] + 220.0 * pi[3]) +
58 t4 * (495.0 * pi[0] - 1980.0 * pi[1] + 2970.0 * pi[2] - 1980.0 * pi[3]) +
59 t5 * (-792.0 * pi[0] + 3960.0 * pi[1] - 7920.0 * pi[2] + 7920.0 * pi[3]) +
60 t6 * (924.0 * pi[0] - 5544.0 * pi[1] + 13860.0 * pi[2] - 18480.0 * pi[3]) +
61 t7 * (-792.0 * pi[0] + 5544.0 * pi[1] - 16632.0 * pi[2] + 27720.0 * pi[3]) +
62 t8 * (495.0 * pi[0] - 3960.0 * pi[1] + 13860.0 * pi[2] - 27720.0 * pi[3]) +
63 t9 * (-220.0 * pi[0] + 1980.0 * pi[1] - 7920.0 * pi[2] + 18480.0 * pi[3] + 220.0 * pi[9]) +
64 t10 * (66.0 * pi[0] + 66.0 * pi[10] - 660.0 * pi[1] + 2970.0 * pi[2] - 7920.0 * pi[3] - 660.0 * pi[9]) +
65 t11 * (-12.0 * pi[0] - 132.0 * pi[10] + 12.0 * pi[11] + 132.0 * pi[1] - 660.0 * pi[2] + 1980.0 * pi[3] +
67 t12 * (1.0 * pi[0] + 66.0 * pi[10] - 12.0 * pi[11] + 1.0 * pi[12] - 12.0 * pi[1] + 66.0 * pi[2] -
68 220.0 * pi[3] - 220.0 * pi[9]);
75 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
77 const double alpha = 1. / (T);
78 const double t2 = t * t;
79 const double t3 = t2 * t;
80 const double t4 = t3 * t;
81 const double t5 = t4 * t;
82 const double t6 = t5 * t;
83 const double t7 = t6 * t;
84 const double t8 = t7 * t;
85 const double t9 = t8 * t;
88 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha;
89 wp.
first.block<3, 3>(0, 3) = Matrix3::Identity() * alpha;
90 wp.
first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha;
91 wp.
first.block<3, 3>(0, 9) = Matrix3::Identity() * alpha;
92 wp.
first.block<3, 3>(0, 12) = Matrix3::Identity() * alpha;
94 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])) +
95 t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
96 t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
97 810.0 * pi[8] + 90.0 * pi[9])) +
98 t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
99 t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
100 1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
101 t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
102 t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
103 t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
104 t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
113 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
115 const double alpha = 1. / (T * T);
116 const double t2 = t * t;
117 const double t3 = t2 * t;
118 const double t4 = t3 * t;
119 const double t5 = t4 * t;
120 const double t6 = t5 * t;
121 const double t7 = t6 * t;
122 const double t8 = t7 * t;
124 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
125 (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
126 25200.0 * t3 + 2520.0 * t2));
127 wp.
first.block<3, 3>(0, 3) =
128 Matrix3::Identity() * alpha *
129 (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3));
130 wp.
first.block<3, 3>(0, 6) =
131 Matrix3::Identity() * alpha *
132 (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4));
134 (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
135 t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
136 t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
137 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] +
138 4050.0 * pi[8] - 900.0 * pi[9])) +
139 t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
140 t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
141 t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
142 t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
144 t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
145 6480.0 * pi[8] + 720.0 * pi[9]))) *
153 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
155 const double alpha = 1. / (T * T * T);
156 const double t2 = t * t;
157 const double t3 = t2 * t;
158 const double t4 = t3 * t;
159 const double t5 = t4 * t;
160 const double t6 = t5 * t;
161 const double t7 = t6 * t;
163 wp.
first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
164 (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
165 75600.0 * t2 + 5040.0 * t));
166 wp.
first.block<3, 3>(0, 3) =
167 Matrix3::Identity() * alpha *
168 (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2));
169 wp.
first.block<3, 3>(0, 6) =
170 Matrix3::Identity() * alpha *
171 (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3));
173 (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
174 t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
175 t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
176 t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
177 t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
178 t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
180 t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
181 45360.0 * pi[8] + 5040.0 * pi[9])) +
182 t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
183 86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
192 std::vector<point_t> pi;
193 pi.push_back(pData.
c0_);
194 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
195 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) + (2 * pData.
dc0_ * T / n) +
197 pi.push_back((pData.
j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.
ddc0_ * T * T / (n * (n - 1))) +
198 (3 * pData.
dc0_ * T / n) + pData.
c0_);
199 pi.push_back(point_t::Zero());
200 pi.push_back(point_t::Zero());
201 pi.push_back(point_t::Zero());
202 pi.push_back(point_t::Zero());
203 pi.push_back(point_t::Zero());
204 pi.push_back((-pData.
j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.
ddc1_ * T * T / (n * (n - 1))) -
205 (3 * pData.
dc1_ * T / n) + pData.
c1_);
206 pi.push_back((pData.
ddc1_ * T * T / (n * (n - 1))) - (2 * pData.
dc1_ * T / n) + pData.
c1_);
207 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
208 pi.push_back(pData.
c1_);
214 bezier_wp_t::t_point_t wps;
218 std::vector<Matrix3> Cpi;
219 for (std::size_t i = 0; i < pi.size(); ++i) {
220 Cpi.push_back(
skew(pi[i]));
226 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
231 const ProblemData& pData,
const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
234 std::vector<waypoint_t> wps;
235 assert(pi.size() == 13);
237 double alpha = 1. / (T);
241 w.
second = alpha * 12 * (-pi[0] + pi[1]);
243 w =
initwp(DIM_POINT, DIM_VAR);
245 w.
second = alpha * 12 * (-pi[1] + pi[2]);
247 w =
initwp(DIM_POINT, DIM_VAR);
249 w.
second = alpha * 12 * (-pi[2] + pi[3]);
251 w =
initwp(DIM_POINT, DIM_VAR);
253 w.
first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity();
254 w.
second = alpha * 12 * (-pi[3]);
256 w =
initwp(DIM_POINT, DIM_VAR);
258 w.
first.block<3, 3>(0, 0) = -12 * alpha * Matrix3::Identity();
259 w.
first.block<3, 3>(0, 3) = 12 * alpha * Matrix3::Identity();
261 w =
initwp(DIM_POINT, DIM_VAR);
263 w.
first.block<3, 3>(0, 3) = -12 * alpha * Matrix3::Identity();
264 w.
first.block<3, 3>(0, 6) = 12 * alpha * Matrix3::Identity();
266 w =
initwp(DIM_POINT, DIM_VAR);
268 w.
first.block<3, 3>(0, 6) = -12 * alpha * Matrix3::Identity();
269 w.
first.block<3, 3>(0, 9) = 12 * alpha * Matrix3::Identity();
271 w =
initwp(DIM_POINT, DIM_VAR);
273 w.
first.block<3, 3>(0, 9) = -12 * alpha * Matrix3::Identity();
274 w.
first.block<3, 3>(0, 12) = 12 * alpha * Matrix3::Identity();
276 w =
initwp(DIM_POINT, DIM_VAR);
278 w.
first.block<3, 3>(0, 12) = -12 * alpha * Matrix3::Identity();
279 w.
second = alpha * 12 * pi[9];
281 w =
initwp(DIM_POINT, DIM_VAR);
283 w.
second = alpha * 12 * (-pi[9] + pi[10]);
285 w =
initwp(DIM_POINT, DIM_VAR);
287 w.
second = alpha * 12 * (-pi[10] + pi[11]);
289 w =
initwp(DIM_POINT, DIM_VAR);
291 w.
second = alpha * 12 * (-pi[11] + pi[12]);
297 const ProblemData& pData,
const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
300 std::vector<waypoint_t> wps;
301 assert(pi.size() == 13);
302 double alpha = 1. / (T * T);
307 w.
second = alpha * 132 * (pi[0] - 2 * pi[1] + pi[2]);
309 w =
initwp(DIM_POINT, DIM_VAR);
311 w.
second = alpha * 132 * (pi[1] - 2 * pi[2] + pi[3]);
313 w =
initwp(DIM_POINT, DIM_VAR);
315 w.
first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity();
316 w.
second = alpha * 132 * (pi[2] - pi[3]);
318 w =
initwp(DIM_POINT, DIM_VAR);
320 w.
first.block<3, 3>(0, 0) = -264 * alpha * Matrix3::Identity();
321 w.
first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity();
322 w.
second = alpha * 132 * pi[3];
324 w =
initwp(DIM_POINT, DIM_VAR);
326 w.
first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity();
327 w.
first.block<3, 3>(0, 3) = -264 * alpha * Matrix3::Identity();
328 w.
first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity();
330 w =
initwp(DIM_POINT, DIM_VAR);
332 w.
first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity();
333 w.
first.block<3, 3>(0, 6) = -264 * alpha * Matrix3::Identity();
334 w.
first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity();
336 w =
initwp(DIM_POINT, DIM_VAR);
338 w.
first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity();
339 w.
first.block<3, 3>(0, 9) = -264 * alpha * Matrix3::Identity();
340 w.
first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity();
342 w =
initwp(DIM_POINT, DIM_VAR);
344 w.
first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity();
345 w.
first.block<3, 3>(0, 12) = -264 * alpha * Matrix3::Identity();
346 w.
second = alpha * 132 * pi[7];
348 w =
initwp(DIM_POINT, DIM_VAR);
350 w.
first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity();
351 w.
second = alpha * 132 * (-2 * pi[9] + pi[10]);
353 w =
initwp(DIM_POINT, DIM_VAR);
355 w.
second = alpha * 132 * (pi[9] - 2 * pi[10] + pi[11]);
358 w =
initwp(DIM_POINT, DIM_VAR);
360 w.
second = alpha * 132 * (pi[12] + pi[10] - 2 * pi[11]);
366 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
369 std::vector<waypoint_t> wps;
370 assert(pi.size() == 13);
372 double alpha = 1. / (T * T * T);
377 w.
second = alpha * 1320 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
379 w =
initwp(DIM_POINT, DIM_VAR);
381 w.
first.block<3, 3>(0, 0) = 1320 * alpha * Matrix3::Identity();
382 w.
second = alpha * 1320 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
384 w =
initwp(DIM_POINT, DIM_VAR);
386 w.
first.block<3, 3>(0, 0) = 1320 * -3 * alpha * Matrix3::Identity();
387 w.
first.block<3, 3>(0, 3) = 1320 * alpha * Matrix3::Identity();
388 w.
second = alpha * 1320 * (-pi[2] + 3 * pi[3]);
390 w =
initwp(DIM_POINT, DIM_VAR);
392 w.
first.block<3, 3>(0, 0) = 1320 * 3 * alpha * Matrix3::Identity();
393 w.
first.block<3, 3>(0, 3) = 1320 * -3 * alpha * Matrix3::Identity();
394 w.
first.block<3, 3>(0, 6) = 1320 * alpha * Matrix3::Identity();
395 w.
second = alpha * 1320 * (-pi[3]);
397 w =
initwp(DIM_POINT, DIM_VAR);
399 w.
first.block<3, 3>(0, 0) = -1320 * alpha * Matrix3::Identity();
400 w.
first.block<3, 3>(0, 3) = 1320 * 3 * alpha * Matrix3::Identity();
401 w.
first.block<3, 3>(0, 6) = 1320 * -3 * alpha * Matrix3::Identity();
402 w.
first.block<3, 3>(0, 9) = 1320 * alpha * Matrix3::Identity();
404 w =
initwp(DIM_POINT, DIM_VAR);
406 w.
first.block<3, 3>(0, 3) = -1320 * alpha * Matrix3::Identity();
407 w.
first.block<3, 3>(0, 6) = 1320 * 3 * alpha * Matrix3::Identity();
408 w.
first.block<3, 3>(0, 9) = 1320 * -3 * alpha * Matrix3::Identity();
409 w.
first.block<3, 3>(0, 12) = 1320 * alpha * Matrix3::Identity();
411 w =
initwp(DIM_POINT, DIM_VAR);
413 w.
first.block<3, 3>(0, 6) = -1320 * alpha * Matrix3::Identity();
414 w.
first.block<3, 3>(0, 9) = 1320 * 3 * alpha * Matrix3::Identity();
415 w.
first.block<3, 3>(0, 12) = 1320 * -3 * alpha * Matrix3::Identity();
416 w.
second = alpha * 1320 * pi[9];
418 w =
initwp(DIM_POINT, DIM_VAR);
420 w.
first.block<3, 3>(0, 9) = -1320 * alpha * Matrix3::Identity();
421 w.
first.block<3, 3>(0, 12) = 1320 * 3 * alpha * Matrix3::Identity();
422 w.
second = alpha * 1320 * (-3 * pi[9] + pi[10]);
424 w =
initwp(DIM_POINT, DIM_VAR);
426 w.
first.block<3, 3>(0, 12) = -1320 * alpha * Matrix3::Identity();
427 w.
second = alpha * 1320 * (3 * pi[9] - 3 * pi[10] + pi[11]);
429 w =
initwp(DIM_POINT, DIM_VAR);
431 w.
second = alpha * 1320 * (pi[12] - pi[9] + 3 * pi[10] - 3 * pi[11]);
442 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
448 const ProblemData& pData,
double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
449 MatrixXX H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
450 VectorX g = VectorX::Zero(DIM_VAR);
453 g.segment<3>(0) = ((-17.8646615739593 * pi[0] - 4.24835843773412 * pi[10] - 1.80981866649436 * pi[11] -
454 0.408668730537654 * pi[12] - 13.8947369836412 * pi[1] + 2.56878303943036 * pi[2] +
455 16.0548432515434 * pi[3] - 6.66893486967885 * pi[9])) /
458 ((-7.93984965058761 * pi[0] - 7.0641309185535 * pi[10] - 4.08668730511085 * pi[11] - 1.22600619206665 * pi[12] -
459 12.1432972410894 * pi[1] - 7.06413670827152 * pi[2] + 3.85315840674136 * pi[3] - 7.50872663647158 * pi[9])) /
462 (-3.26934980716442 * pi[0] - 8.9907120599184 * pi[10] - 7.76470588258269 * pi[11] - 3.26934984520124 * pi[12] -
463 7.76470597007188 * pi[1] - 8.99071211730055 * pi[2] - 4.49535589801788 * pi[3] - 4.49535607858364 * pi[9]) /
466 (-1.22600620726636 * pi[0] - 7.06413092270385 * pi[10] - 12.1432994250704 * pi[11] - 7.93984962409094 * pi[12] -
467 4.08668774398579 * pi[1] - 7.0641311269266 * pi[2] - 7.50872489092664 * pi[3] + 3.85316232209763 * pi[9]) /
470 (-0.408668732514974 * pi[0] + 2.56877487851457 * pi[10] - 13.8947368423667 * pi[11] - 17.8646616541281 * pi[12] -
471 1.80981880873492 * pi[1] - 4.2483587965255 * pi[2] - 6.66893350792178 * pi[3] + 16.0548429731073 * pi[9]) /
474 H.block<3, 3>(0, 0) = Matrix3::Identity() * 9.63290527229048 / (T);
475 H.block<3, 3>(3, 3) = Matrix3::Identity() * 8.29911962311903 / (T);
476 H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.92188615942945 / (T);
477 H.block<3, 3>(9, 9) = Matrix3::Identity() * 8.29911871865983 / (T);
478 H.block<3, 3>(12, 12) = Matrix3::Identity() * 9.63290582796267 / (T);
480 H.block<3, 3>(0, 3) = Matrix3::Identity() * 13.4860690009623 / (2 * T);
481 H.block<3, 3>(3, 0) = Matrix3::Identity() * 13.4860690009623 / (2 * T);
482 H.block<3, 3>(0, 6) = Matrix3::Identity() * 4.14955180440231 / (2 * T);
483 H.block<3, 3>(6, 0) = Matrix3::Identity() * 4.14955180440231 / (2 * T);
484 H.block<3, 3>(0, 9) = Matrix3::Identity() * -3.55676093144659 / (2 * T);
485 H.block<3, 3>(9, 0) = Matrix3::Identity() * -3.55676093144659 / (2 * T);
486 H.block<3, 3>(0, 12) = Matrix3::Identity() * -7.07311260219052 / (2 * T);
487 H.block<3, 3>(12, 0) = Matrix3::Identity() * -7.07311260219052 / (2 * T);
489 H.block<3, 3>(3, 6) = Matrix3::Identity() * 12.4486856197374 / (2 * T);
490 H.block<3, 3>(6, 3) = Matrix3::Identity() * 12.4486856197374 / (2 * T);
491 H.block<3, 3>(3, 9) = Matrix3::Identity() * 4.20345048607838 / (2 * T);
492 H.block<3, 3>(9, 3) = Matrix3::Identity() * 4.20345048607838 / (2 * T);
493 H.block<3, 3>(3, 12) = Matrix3::Identity() * -3.55676456195318 / (2 * T);
494 H.block<3, 3>(12, 3) = Matrix3::Identity() * -3.55676456195318 / (2 * T);
496 H.block<3, 3>(6, 9) = Matrix3::Identity() * 12.448679688301 / (2 * T);
497 H.block<3, 3>(9, 6) = Matrix3::Identity() * 12.448679688301 / (2 * T);
498 H.block<3, 3>(6, 12) = Matrix3::Identity() * 4.149559164651 / (2 * T);
499 H.block<3, 3>(12, 6) = Matrix3::Identity() * 4.149559164651 / (2 * T);
501 H.block<3, 3>(9, 12) = Matrix3::Identity() * 13.4860680294621 / (2 * T);
502 H.block<3, 3>(12, 9) = Matrix3::Identity() * 13.4860680294621 / (2 * T);
504 double norm = H.norm();
508 return std::make_pair(H, g);
515 #endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH FIVE_FREE_VAR
Definition: flags.hh:32
point_t j0_
Definition: data.hh:103
point_t ddc0_
Definition: data.hh:103
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:296
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:151
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:365
VectorX second
Definition: utils.hh:29
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:437
INIT_VEL
Definition: flags.hh:21
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:188
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:23
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:28
END_ACC
Definition: flags.hh:25
point_t c0_
Definition: data.hh:103
MatrixXX first
Definition: utils.hh:28
END_POS
Definition: flags.hh:23
point_t dc1_
Definition: data.hh:103
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:61
INIT_JERK
Definition: flags.hh:26
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:447
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:103
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:213
END_JERK
Definition: flags.hh:27
point_t ddc1_
Definition: data.hh:103
point_t j1_
Definition: data.hh:103
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:111
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:20
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:73
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:88
Definition: common_solve_methods.hh:16
INIT_POS
Definition: flags.hh:20
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:56
point_t c1_
Definition: data.hh:103
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:230
const int DIM_POINT
Definition: solve_end_effector.hh:15