1 #ifndef BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH 2 #define BEZIER_COM_TRAJ_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH 12 namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1 {
15 static const size_t DIM_VAR = 3;
28 const double t2 = t * t;
29 const double t3 = t2 * t;
30 const double t4 = t3 * t;
31 const double t5 = t4 * t;
32 const double t6 = t5 * t;
33 const double t7 = t6 * t;
34 const double t8 = t7 * t;
36 wp.first = 70.0 * t8 - 280.0 * t7 + 420.0 * t6 - 280.0 * t5 + 70.0 * t4;
37 wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 - 56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 -
38 56.0 * pi[8] * t3 + 28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] - 8.0 * pi[1] * t8 +
39 56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 + 280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
40 56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 - 168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 -
41 560.0 * pi[2] * t5 + 420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
42 56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 - 560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 -
43 280.0 * pi[3] * t4 + 56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 - 168.0 * pi[5] * t6 +
44 56.0 * pi[5] * pow(t, 5) + 28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 - 8.0 * pi[7] * t8 +
45 8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
51 const double alpha = 1. / (T);
52 const double t2 = t * t;
53 const double t3 = t2 * t;
54 const double t4 = t3 * t;
55 const double t5 = t4 * t;
56 const double t6 = t5 * t;
57 const double t7 = t6 * t;
59 wp.first = (560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) * alpha;
61 (8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 - 280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 -
62 168.0 * pi[8] * t2 + 56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 + 392.0 * pi[1] * t6 -
63 1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 - 1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
64 8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 + 2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 +
65 1680.0 * pi[2] * t3 - 504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 + 1960.0 * pi[3] * t6 -
66 3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 - 1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
67 1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 + 224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 +
68 168.0 * pi[6] * t5 - 64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
75 const double alpha = 1. / (T * T);
76 const double t2 = t * t;
77 const double t3 = t2 * t;
78 const double t4 = t3 * t;
79 const double t5 = t4 * t;
80 const double t6 = t5 * t;
82 wp.first = ((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) * alpha;
84 (56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 - 1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 -
85 336.0 * pi[8] * t + 56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 - 5040.0 * pi[1] * t4 +
86 5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 + 1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
87 7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 - 11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
88 56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 - 16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
89 3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 + 7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 +
90 1120.0 * pi[5] * t3 + 1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 - 448.0 * pi[7] * t6 +
91 336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
98 const double alpha = 1. / (T * T * T);
99 const double t2 = t * t;
100 const double t3 = t2 * t;
101 const double t4 = t3 * t;
102 const double t5 = t4 * t;
104 wp.first = (23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) * alpha;
107 (336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 - 3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t -
108 336.0 * pi[0] - 2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 + 16800.0 * pi[1] * t2 -
109 6720.0 * pi[1] * t + 1008.0 * pi[1] + 9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
110 33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] - 18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 -
111 67200.0 * pi[3] * t3 + 33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] - 18816.0 * pi[5] * t5 +
112 35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 + 3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
113 3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 + 336.0 * pi[8] * t5) *
120 wp.
first = Matrix3::Identity() * coef.first;
127 wp.
first = Matrix3::Identity() * coef.first;
134 wp.
first = Matrix3::Identity() * coef.first;
142 wp.
first = Matrix3::Identity() * coef.first;
151 std::vector<point_t> pi;
152 pi.push_back(pData.
c0_);
153 pi.push_back((pData.
dc0_ * T / n) + pData.
c0_);
154 pi.push_back((pData.
ddc0_ * T * T / (n * (n - 1))) + (2 * pData.
dc0_ * T / n) +
156 pi.push_back((pData.
j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.
ddc0_ * T * T / (n * (n - 1))) +
157 (3 * pData.
dc0_ * T / n) + pData.
c0_);
158 pi.push_back(point_t::Zero());
159 pi.push_back((-pData.
j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.
ddc1_ * T * T / (n * (n - 1))) -
160 (3 * pData.
dc1_ * T / n) + pData.
c1_);
161 pi.push_back((pData.
ddc1_ * T * T / (n * (n - 1))) - (2 * pData.
dc1_ * T / n) + pData.
c1_);
162 pi.push_back((-pData.
dc1_ * T / n) + pData.
c1_);
163 pi.push_back(pData.
c1_);
168 bezier_wp_t::t_point_t wps;
169 const int DIM_POINT = 6;
170 const int DIM_VAR = 3;
172 std::vector<Matrix3> Cpi;
173 for (std::size_t i = 0; i < pi.size(); ++i) {
174 Cpi.push_back(
skew(pi[i]));
178 const double T2 = T * T;
179 const double alpha = 1 / (T2);
183 w0.
second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
184 w0.
second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) * alpha;
187 w1.
first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
188 w1.
first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
189 w1.
second.head<3>() = 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
190 w1.
second.tail<3>() = 1.0 *
191 (0.333333333333333 * Cg * T2 * pi[0] + 0.666666666666667 * Cg * T2 * pi[1] -
192 30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
196 w2.
first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
197 w2.
first.block<3, 3>(3, 0) = 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
198 w2.
second.head<3>() = 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
199 w2.
second.tail<3>() = 1.0 *
200 (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
201 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] - 20.0 * Cpi[1] * pi[2]) *
205 w3.
first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
206 w3.
first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] + 14.2857142857143 * Cpi[2]) * alpha;
207 w3.
second.head<3>() = 1.0 *
208 (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] - 14.2857142857143 * pi[2] +
209 7.85714285714286 * pi[4] + 1.42857142857143 * pi[5]) *
211 w3.
second.tail<3>() = 1.0 *
212 (0.0119047619047619 * Cg * T2 * pi[0] + 0.214285714285714 * Cg * T2 * pi[1] +
213 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
214 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
218 w4.
first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
219 w4.
first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
220 w4.
second.head<3>() = 1.0 *
221 (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] - 3.57142857142857 * pi[2] + 5.0 * pi[4] +
222 4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
226 (0.0476190476190471 * Cg * T2 * pi[1] + 0.357142857142857 * Cg * T2 * pi[2] +
227 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] + 0.238095238095238 * Cpi[0] * pi[6] -
228 12.8571428571429 * Cpi[1] * pi[4] + 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
232 w5.
first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
233 w5.
first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
234 w5.
second.head<3>() = 1.0 *
235 (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] + 5.0 * pi[2] -
236 3.57142857142857 * pi[4] + 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
240 (+0.11904761904762 * Cg * T2 * pi[2] + 0.357142857142857 * Cg * T2 * pi[4] +
241 0.0476190476190476 * Cg * T2 * pi[5] - 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
242 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] + 12.8571428571429 * Cpi[2] * pi[5]) *
246 w6.
first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
247 w6.
first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 + 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
248 w6.
second.head<3>() = 1.0 *
249 (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] - 14.2857142857143 * pi[4] +
250 7.14285714285715 * pi[5] + 3.57142857142857 * pi[6]) *
252 w6.
second.tail<3>() = 1.0 *
253 (0.535714285714286 * Cg * T2 * pi[4] + 0.214285714285714 * Cg * T2 * pi[5] +
254 0.0119047619047619 * Cg * T2 * pi[6] - 1.42857142857143 * Cpi[1] * pi[6] -
255 12.8571428571429 * Cpi[2] * pi[5] + 5.0 * Cpi[2] * pi[6]) *
259 w7.
first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
260 w7.
first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
261 w7.
second.head<3>() = 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
262 w7.
second.tail<3>() = 1.0 *
263 (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
264 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] + 20.0 * Cpi[4] * pi[5]) *
268 w8.
first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
269 w8.
first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
270 w8.
second.head<3>() = 1.0 * (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) * alpha;
271 w8.
second.tail<3>() = 1.0 *
272 (0.666666666666667 * Cg * T2 * pi[5] + 0.333333333333333 * Cg * T2 * pi[6] -
273 20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
277 w9.
second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
278 w9.
second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) * alpha;
284 const ProblemData& pData,
const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
287 std::vector<waypoint_t> wps;
288 assert(pi.size() == 9);
290 double alpha = 1. / (T);
293 w.
second = alpha * 8 * (-pi[0] + pi[1]);
295 w =
initwp(DIM_POINT, DIM_VAR);
297 w.
second = alpha * 8 * (-pi[1] + pi[2]);
299 w =
initwp(DIM_POINT, DIM_VAR);
301 w.
second = alpha * 8 * (-pi[2] + pi[3]);
303 w =
initwp(DIM_POINT, DIM_VAR);
305 w.
first = 8 * alpha * Matrix3::Identity();
306 w.
second = alpha * -8 * pi[3];
308 w =
initwp(DIM_POINT, DIM_VAR);
310 w.
first = -8 * alpha * Matrix3::Identity();
311 w.
second = alpha * 8 * pi[5];
313 w =
initwp(DIM_POINT, DIM_VAR);
315 w.
second = alpha * 8 * (-pi[5] + pi[6]);
317 w =
initwp(DIM_POINT, DIM_VAR);
319 w.
second = alpha * 8 * (-pi[6] + pi[7]);
321 w =
initwp(DIM_POINT, DIM_VAR);
323 w.
second = alpha * 8 * (-pi[7] + pi[8]);
329 const ProblemData& pData,
const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
332 std::vector<waypoint_t> wps;
333 assert(pi.size() == 9);
334 double alpha = 1. / (T * T);
339 w.
second = 56 * alpha * (pi[0] - 2 * pi[1] + pi[2]);
341 w =
initwp(DIM_POINT, DIM_VAR);
343 w.
second = 56 * alpha * (pi[1] - 2 * pi[2] + pi[3]);
345 w =
initwp(DIM_POINT, DIM_VAR);
347 w.
first = 56 * alpha * Matrix3::Identity();
348 w.
second = (56 * pi[2] - 112 * pi[3]) * alpha;
350 w =
initwp(DIM_POINT, DIM_VAR);
352 w.
first = -112 * alpha * Matrix3::Identity();
353 w.
second = (56 * pi[3] + 56 * pi[8]) * alpha;
355 w =
initwp(DIM_POINT, DIM_VAR);
357 w.
first = 56 * alpha * Matrix3::Identity();
358 w.
second = (-112 * pi[5] + 56 * pi[6]) * alpha;
360 w =
initwp(DIM_POINT, DIM_VAR);
362 w.
second = 56 * alpha * (pi[5] - 2 * pi[6] + pi[7]);
364 w =
initwp(DIM_POINT, DIM_VAR);
366 w.
second = 56 * alpha * (pi[6] - 2 * pi[7] + pi[8]);
372 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
375 std::vector<waypoint_t> wps;
376 assert(pi.size() == 9);
378 double alpha = 1. / (T * T * T);
383 w.
second = 336 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]) * alpha;
385 w =
initwp(DIM_POINT, DIM_VAR);
387 w.
first = 336 * alpha * Matrix3::Identity();
388 w.
second = 336 * (-pi[1] + 3 * pi[2] - 3 * pi[3]) * alpha;
390 w =
initwp(DIM_POINT, DIM_VAR);
392 w.
first = -3 * 336 * alpha * Matrix3::Identity();
393 w.
second = 336 * (-pi[2] + 3 * pi[3] + pi[5]) * alpha;
395 w =
initwp(DIM_POINT, DIM_VAR);
397 w.
first = 3 * 336 * alpha * Matrix3::Identity();
398 w.
second = 336 * (-pi[3] - 3 * pi[5] + pi[6]) * alpha;
400 w =
initwp(DIM_POINT, DIM_VAR);
402 w.
first = -336 * alpha * Matrix3::Identity();
403 w.
second = 336 * (3 * pi[5] - 3 * pi[6] + pi[7]) * alpha;
405 w =
initwp(DIM_POINT, DIM_VAR);
407 w.
second = 336 * (-pi[5] + 3 * pi[6] - 3 * pi[7] + pi[8]) * alpha;
417 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
422 const ProblemData& pData,
double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
427 g = (-7.8321678321748 * pi[0] - 7.83216783237586 * pi[1] + 9.13752913728184 * pi[3] + 9.13752913758454 * pi[5] -
428 7.83216783216697 * pi[7] - 7.83216783216777 * pi[8]) /
430 H = Matrix3::Identity() * 6.52680652684107 / (T);
432 double norm = H.norm();
436 return std::make_pair(H, g);
443 #endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH coefs_t evaluateJerkCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:96
point_t j0_
Definition: data.hh:103
waypoint_t evaluateCurveWaypointAtTime(const std::vector< point_t > &pi, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:117
point_t ddc0_
Definition: data.hh:103
waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha)
Definition: solve_0_step.cpp:32
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:21
waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha)
Definition: solve_0_step.cpp:22
VectorX second
Definition: utils.hh:29
waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:131
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:73
waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha)
Definition: solve_0_step.cpp:42
INIT_VEL
Definition: flags.hh:21
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:23
waypoint6_t w4(point_t_tC, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha)
Definition: solve_0_step.cpp:52
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:412
waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:139
END_ACC
Definition: flags.hh:25
point_t c0_
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_j1_ddc1_dc1_c1.hh:328
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_j1_ddc1_dc1_c1.hh:283
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
coefs_t evaluateVelocityCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:49
INIT_JERK
Definition: flags.hh:26
coefs_t evaluateCurveAtTime(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_j1_ddc1_dc1_c1.hh:26
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:103
END_JERK
Definition: flags.hh:27
waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &p0X, const Matrix3 &, const Matrix3 &, const double alpha)
Definition: solve_0_step.cpp:12
point_t ddc1_
Definition: data.hh:103
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_j1_ddc1_dc1_c1.hh:421
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:167
point_t j1_
Definition: data.hh:103
waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:124
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:16
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:20
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
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:147
std::vector< ContactData > contacts_
Definition: data.hh:102
point_t c1_
Definition: data.hh:103
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_j1_ddc1_dc1_c1.hh:371
const int DIM_POINT
Definition: solve_end_effector.hh:15