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
12namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1 {
17static const size_t DIM_VAR = 9;
35 const double t2 = t * t;
36 const double t3 = t2 * t;
37 const double t4 = t3 * t;
38 const double t5 = t4 * t;
39 const double t6 = t5 * t;
40 const double t7 = t6 * t;
41 const double t8 = t7 * t;
42 const double t9 = t8 * t;
43 const double t10 = t9 * t;
46 wp.
first.block<3, 3>(0, 0) =
48 (t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 + 3150.0 * t8 -
49 1260.0 * t9 + 210.0 * t10);
50 wp.
first.block<3, 3>(0, 3) =
51 Matrix3::Identity() * (252.0 * t5 - 1260.0 * t6 + 2520.0 * t7 -
52 2520.0 * t8 + 1260.0 * t9 - 252.0 * t10);
53 wp.
first.block<3, 3>(0, 6) =
55 (210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10);
57 1.0 * pi[0] + t * (-10.0 * pi[0] + 10.0 * pi[1]) +
58 t2 * (45.0 * pi[0] - 90.0 * pi[1] + 45.0 * pi[2]) +
59 t3 * (-120.0 * pi[0] + 360.0 * pi[1] - 360.0 * pi[2] + 120.0 * pi[3]) +
60 t4 * (210.0 * pi[0] - 840.0 * pi[1] + 1260.0 * pi[2] - 840.0 * pi[3]) +
61 t5 * (-252.0 * pi[0] + 1260.0 * pi[1] - 2520.0 * pi[2] + 2520.0 * pi[3]) +
62 t6 * (210.0 * pi[0] - 1260.0 * pi[1] + 3150.0 * pi[2] - 4200.0 * pi[3]) +
63 t7 * (-120.0 * pi[0] + 840.0 * pi[1] - 2520.0 * pi[2] + 4200.0 * pi[3] +
65 t8 * (45.0 * pi[0] - 360.0 * pi[1] + 1260.0 * pi[2] - 2520.0 * pi[3] -
66 360.0 * pi[7] + 45.0 * pi[8]) +
67 t9 * (-10.0 * pi[0] + 90.0 * pi[1] - 360.0 * pi[2] + 840.0 * pi[3] +
68 360.0 * pi[7] - 90.0 * pi[8] + 10.0 * pi[9]) +
69 t10 * (1.0 * pi[0] + 1.0 * pi[10] - 10.0 * pi[1] + 45.0 * pi[2] -
70 120.0 * pi[3] - 120.0 * pi[7] + 45.0 * pi[8] - 10.0 * pi[9]);
76 const std::vector<point_t>& pi,
double T,
double t) {
78 const double alpha = 1. / (T);
79 const double t2 = t * t;
80 const double t3 = t2 * t;
81 const double t4 = t3 * t;
82 const double t5 = t4 * t;
83 const double t6 = t5 * t;
84 const double t7 = t6 * t;
85 const double t8 = t7 * t;
86 const double t9 = t8 * t;
88 wp.
first.block<3, 3>(0, 0) =
89 Matrix3::Identity() * alpha *
90 (1.0 * (2100.0 * t9 - 11340.0 * t8 + 25200.0 * t7 - 29400.0 * t6 +
91 18900.0 * t5 - 6300.0 * t4 + 840.0 * t3));
92 wp.
first.block<3, 3>(0, 3) =
93 Matrix3::Identity() * alpha *
94 (1.0 * (-2520.0 * t9 + 11340.0 * t8 - 20160.0 * t7 + 17640.0 * t6 -
95 7560.0 * t5 + 1260.0 * t4));
96 wp.
first.block<3, 3>(0, 6) =
97 Matrix3::Identity() * alpha *
98 (1.0 * (2100.0 * t9 - 7560.0 * t8 + 10080.0 * t7 - 5880.0 * t6 +
101 (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) +
102 t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
103 t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] +
106 (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] +
107 3240.0 * pi[7] - 810.0 * pi[8] + 90.0 * pi[9])) +
108 t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] -
110 t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] +
111 450.0 * pi[2] - 1200.0 * pi[3] - 1200.0 * pi[7] +
112 450.0 * pi[8] - 100.0 * pi[9])) +
113 t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] +
115 t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
117 t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] +
118 29400.0 * pi[3] + 840.0 * pi[7])) +
119 t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] -
120 20160.0 * pi[3] - 2880.0 * pi[7] + 360.0 * pi[8]))) *
127 const std::vector<point_t>& pi,
double T,
double t) {
129 const double alpha = 1. / (T * T);
130 const double t2 = t * t;
131 const double t3 = t2 * t;
132 const double t4 = t3 * t;
133 const double t5 = t4 * t;
134 const double t6 = t5 * t;
135 const double t7 = t6 * t;
136 const double t8 = t7 * t;
138 wp.
first.block<3, 3>(0, 0) =
139 Matrix3::Identity() * alpha *
140 (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 +
141 94500.0 * t4 - 25200.0 * t3 + 2520.0 * t2));
142 wp.
first.block<3, 3>(0, 3) =
143 Matrix3::Identity() * alpha *
144 (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 -
145 37800.0 * t4 + 5040.0 * t3));
146 wp.
first.block<3, 3>(0, 6) =
147 Matrix3::Identity() * alpha *
148 (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 +
151 (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
152 t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] +
154 t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
156 t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] +
157 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
158 4050.0 * pi[8] - 900.0 * pi[9])) +
159 t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] +
161 t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
163 t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] +
164 176400.0 * pi[3] + 5040.0 * pi[7])) +
165 t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] -
166 141120.0 * pi[3] - 20160.0 * pi[7] + 2520.0 * pi[8])) +
167 t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] +
168 60480.0 * pi[3] + 25920.0 * pi[7] - 6480.0 * pi[8] +
176 const std::vector<point_t>& pi,
double T,
double t) {
178 const double alpha = 1. / (T * T * T);
179 const double t2 = t * t;
180 const double t3 = t2 * t;
181 const double t4 = t3 * t;
182 const double t5 = t4 * t;
183 const double t6 = t5 * t;
184 const double t7 = t6 * t;
186 wp.
first.block<3, 3>(0, 0) =
187 Matrix3::Identity() * alpha *
188 (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 +
189 378000.0 * t3 - 75600.0 * t2 + 5040.0 * t));
190 wp.
first.block<3, 3>(0, 3) =
191 Matrix3::Identity() * alpha *
192 (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 -
193 151200.0 * t3 + 15120.0 * t2));
194 wp.
first.block<3, 3>(0, 6) =
195 Matrix3::Identity() * alpha *
196 (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 +
200 (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
201 t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] -
203 t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
205 t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
207 t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] +
208 882000.0 * pi[3] + 25200.0 * pi[7])) +
209 t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] -
210 846720.0 * pi[3] - 120960.0 * pi[7] + 15120.0 * pi[8])) +
211 t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] +
212 423360.0 * pi[3] + 181440.0 * pi[7] - 45360.0 * pi[8] +
214 t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] +
215 32400.0 * pi[2] - 86400.0 * pi[3] - 86400.0 * pi[7] +
216 32400.0 * pi[8] - 7200.0 * pi[9]))) *
227 std::vector<point_t> pi;
228 pi.push_back(pData.
c0_);
229 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
230 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) +
231 (2 * pData.
dc0_ * T / n) +
233 pi.push_back((pData.
j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
234 (3 * pData.
ddc0_ * T * T / (n * (n - 1))) +
235 (3 * pData.
dc0_ * T / n) + pData.
c0_);
236 pi.push_back(point_t::Zero());
237 pi.push_back(point_t::Zero());
238 pi.push_back(point_t::Zero());
239 pi.push_back((-pData.
j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
240 (3 * pData.
ddc1_ * T * T / (n * (n - 1))) -
241 (3 * pData.
dc1_ * T / n) + pData.
c1_);
242 pi.push_back((pData.
ddc1_ * T * T / (n * (n - 1))) -
243 (2 * pData.
dc1_ * T / n) + pData.
c1_);
244 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
245 pi.push_back(pData.
c1_);
252 bezier_wp_t::t_point_t wps;
256 std::vector<Matrix3> Cpi;
257 for (std::size_t i = 0; i < pi.size(); ++i) {
258 Cpi.push_back(
skew(pi[i]));
264 std::cout <<
"NOT IMPLEMENTED YET" << std::endl;
270 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
273 std::vector<waypoint_t> wps;
274 assert(pi.size() == 11);
276 double alpha = 1. / (T);
280 w.
second = alpha * 10 * (-pi[0] + pi[1]);
284 w.
second = alpha * 10 * (-pi[1] + pi[2]);
288 w.
second = alpha * 10 * (-pi[2] + pi[3]);
292 w.
first.block<3, 3>(0, 0) = 10 * alpha * Matrix3::Identity();
293 w.
second = alpha * 10 * (-pi[3]);
297 w.
first.block<3, 3>(0, 0) = -10 * alpha * Matrix3::Identity();
298 w.
first.block<3, 3>(0, 3) = 10 * alpha * Matrix3::Identity();
302 w.
first.block<3, 3>(0, 3) = -10 * alpha * Matrix3::Identity();
303 w.
first.block<3, 3>(0, 6) = 10 * alpha * Matrix3::Identity();
307 w.
first.block<3, 3>(0, 6) = -10 * alpha * Matrix3::Identity();
308 w.
second = alpha * 10 * pi[7];
312 w.
second = alpha * 10 * (-pi[7] + pi[8]);
316 w.
second = alpha * 10 * (-pi[8] + pi[9]);
320 w.
second = alpha * 10 * (pi[10] - 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);
332 double alpha = 1. / (T * T);
337 w.
second = alpha * 90 * (pi[0] - 2 * pi[1] + pi[2]);
341 w.
second = alpha * 90 * (pi[1] - 2 * pi[2] + pi[3]);
345 w.
first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity();
346 w.
second = alpha * 90 * (pi[2] - pi[3]);
350 w.
first.block<3, 3>(0, 0) = -180 * alpha * Matrix3::Identity();
351 w.
first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity();
352 w.
second = alpha * 90 * pi[3];
356 w.
first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity();
357 w.
first.block<3, 3>(0, 3) = -180 * alpha * Matrix3::Identity();
358 w.
first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity();
362 w.
first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity();
363 w.
first.block<3, 3>(0, 6) = -180 * alpha * Matrix3::Identity();
364 w.
second = alpha * 90 * pi[7];
368 w.
first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity();
369 w.
second = alpha * 90 * (-2 * pi[7] + pi[8]);
373 w.
second = alpha * 90 * (pi[7] - 2 * pi[8] + pi[9]);
378 w.
second = alpha * 90 * (pi[10] + pi[8] - 2 * pi[9]);
385 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
388 std::vector<waypoint_t> wps;
389 assert(pi.size() == 11);
391 double alpha = 1. / (T * T * T);
396 w.
second = alpha * 720 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
400 w.
first.block<3, 3>(0, 0) = 720 * alpha * Matrix3::Identity();
401 w.
second = alpha * 720 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
405 w.
first.block<3, 3>(0, 0) = 720 * -3 * alpha * Matrix3::Identity();
406 w.
first.block<3, 3>(0, 3) = 720 * alpha * Matrix3::Identity();
407 w.
second = alpha * 720 * (-pi[2] + 3 * pi[3]);
411 w.
first.block<3, 3>(0, 0) = 720 * 3 * alpha * Matrix3::Identity();
412 w.
first.block<3, 3>(0, 3) = 720 * -3 * alpha * Matrix3::Identity();
413 w.
first.block<3, 3>(0, 6) = 720 * alpha * Matrix3::Identity();
414 w.
second = alpha * 720 * (-pi[3]);
418 w.
first.block<3, 3>(0, 0) = -720 * alpha * Matrix3::Identity();
419 w.
first.block<3, 3>(0, 3) = 720 * 3 * alpha * Matrix3::Identity();
420 w.
first.block<3, 3>(0, 6) = 720 * -3 * alpha * Matrix3::Identity();
421 w.
second = alpha * 720 * pi[7];
425 w.
first.block<3, 3>(0, 3) = -720 * alpha * Matrix3::Identity();
426 w.
first.block<3, 3>(0, 6) = 720 * 3 * alpha * Matrix3::Identity();
427 w.
second = alpha * 720 * (-3 * pi[7] + pi[8]);
431 w.
first.block<3, 3>(0, 6) = -720 * alpha * Matrix3::Identity();
432 w.
second = alpha * 720 * (3 * pi[7] - 3 * pi[8] + pi[9]);
436 w.
second = alpha * 720 * (pi[10] - pi[7] + 3 * pi[8] - 3 * pi[9]);
447 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
453 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
458 g.segment<3>(0) = (-12.352941184069 * pi[0] - 2.03619909502433 * pi[10] -
459 10.5882353430148 * pi[1] + 1.2217194516605 * pi[2] +
460 12.2171947000329 * pi[3] - 4.66474701697538 * pi[7] -
461 7.21925133730399 * pi[8] - 5.42986425333795 * pi[9]) /
463 g.segment<3>(3) = (-5.29411764601331 * pi[0] - 5.29411764705762 * pi[10] -
464 8.95927605247282 * pi[1] - 6.10859723220821 * pi[2] +
465 2.2213080007358 * pi[3] + 2.22130810120924 * pi[7] -
466 6.10859728485633 * pi[8] - 8.95927601808432 * pi[9]) /
468 g.segment<3>(6) = (-2.03619909557297 * pi[0] - 12.3529411764706 * pi[10] -
469 5.42986425052241 * pi[1] - 7.21925133714926 * pi[2] -
470 4.66474700749421 * pi[3] + 12.2171945706055 * pi[7] +
471 1.22171945695766 * pi[8] - 10.5882352941172 * pi[9]) /
474 H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T);
475 H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T);
476 H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T);
477 H.block<3, 3>(0, 3) =
478 Matrix3::Identity() * 10.8844097406652 / (2 * T);
479 H.block<3, 3>(3, 0) =
480 Matrix3::Identity() * 10.8844097406652 / (2 * T);
481 H.block<3, 3>(0, 6) =
482 Matrix3::Identity() * 2.41875768460934 / (2 * T);
483 H.block<3, 3>(6, 0) =
484 Matrix3::Identity() * 2.41875768460934 / (2 * T);
485 H.block<3, 3>(3, 6) =
486 Matrix3::Identity() * 10.8844097036619 / (2 * T);
487 H.block<3, 3>(6, 3) =
488 Matrix3::Identity() * 10.8844097036619 / (2 * T);
490 double norm = H.norm();
494 return std::make_pair(H, g);
END_ACC
Definition: flags.hh:25
THREE_FREE_VAR
Definition: flags.hh:30
INIT_VEL
Definition: flags.hh:21
END_VEL
Definition: flags.hh:24
END_POS
Definition: flags.hh:23
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::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_x3_j1_ddc1_dc1_c1.hh:451
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_x3_j1_ddc1_dc1_c1.hh:325
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_x3_j1_ddc1_dc1_c1.hh:32
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:442
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:250
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:221
waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:175
waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:75
waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:126
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_x3_j1_ddc1_dc1_c1.hh:268
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_x3_j1_ddc1_dc1_c1.hh:383
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