hpp-bezier-com-traj 4.14.0
Multi contact trajectory generation for the COM using Bezier curves
Loading...
Searching...
No Matches
waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
Go to the documentation of this file.
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
3
4/*
5 * Copyright 2018, LAAS-CNRS
6 * Author: Pierre Fernbach
7 */
8
10
11namespace bezier_com_traj {
12namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1 {
13
14static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
17static const size_t DIM_VAR = 3 * 5;
18static const size_t DIM_POINT = 3;
22
31inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
32 double t) {
33 waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
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;
42 const double t10 = t9 * t;
43 const double t11 = t10 * t;
44 const double t12 = t11 * t;
45
46 // equation found with sympy
47 wp.first.block<3, 3>(0, 0) =
48 Matrix3::Identity() *
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); // x0
51 wp.first.block<3, 3>(0, 3) =
52 Matrix3::Identity() *
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); // x1
55 wp.first.block<3, 3>(0, 6) =
56 Matrix3::Identity() *
57 (+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 + 13860.0 * t10 -
58 5544.0 * t11 + 924.0 * t12); // x2
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); // x3
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); // x4
65
66 wp.second =
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]) +
72 t6 *
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] +
75 27720.0 * pi[3]) +
76 t8 *
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] +
79 220.0 * pi[9]) +
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]);
86 return wp;
87}
88
89// TODO
91 const std::vector<point_t>& pi, double T, double t) {
92 waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
93 std::cout << "NOT IMPLEMENTED YET" << std::endl;
94
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;
104
105 // equation found with sympy
106 wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha; // x0
107 wp.first.block<3, 3>(0, 3) = Matrix3::Identity() * alpha; // x1
108 wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha; // x2
109 wp.first.block<3, 3>(0, 9) = Matrix3::Identity() * alpha; // x3
110 wp.first.block<3, 3>(0, 12) = Matrix3::Identity() * alpha; // x4
111
112 wp.second =
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] +
116 360.0 * pi[3])) +
117 t3 * (1.0 *
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] -
121 3360.0 * pi[3])) +
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] +
126 12600.0 * pi[3])) +
127 t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
128 25200.0 * pi[3])) +
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]))) *
133 alpha;
134 return wp;
135}
136
137// TODO
139 const std::vector<point_t>& pi, double T, double t) {
140 waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
141 std::cout << "NOT IMPLEMENTED YET" << std::endl;
142
143 const double alpha = 1. / (T * T);
144 const double t2 = t * t;
145 const double t3 = t2 * t;
146 const double t4 = t3 * t;
147 const double t5 = t4 * t;
148 const double t6 = t5 * t;
149 const double t7 = t6 * t;
150 const double t8 = t7 * t;
151 // equation found with sympy
152 wp.first.block<3, 3>(0, 0) =
153 Matrix3::Identity() * alpha *
154 (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 +
155 94500.0 * t4 - 25200.0 * t3 + 2520.0 * t2)); // x0
156 wp.first.block<3, 3>(0, 3) =
157 Matrix3::Identity() * alpha *
158 (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 -
159 37800.0 * t4 + 5040.0 * t3)); // x1
160 wp.first.block<3, 3>(0, 6) =
161 Matrix3::Identity() * alpha *
162 (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 +
163 6300.0 * t4)); // x2
164 wp.second =
165 (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
166 t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] +
167 720.0 * pi[3])) +
168 t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
169 10080.0 * pi[3])) +
170 t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] +
171 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
172 4050.0 * pi[8] - 900.0 * pi[9])) +
173 t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] +
174 50400.0 * pi[3])) +
175 t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
176 126000.0 * pi[3])) +
177 t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] +
178 176400.0 * pi[3] + 5040.0 * pi[7])) +
179 t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] -
180 141120.0 * pi[3] - 20160.0 * pi[7] + 2520.0 * pi[8])) +
181 t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] +
182 60480.0 * pi[3] + 25920.0 * pi[7] - 6480.0 * pi[8] +
183 720.0 * pi[9]))) *
184 alpha;
185 return wp;
186}
187
188// TODO
190 const std::vector<point_t>& pi, double T, double t) {
191 waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
192 std::cout << "NOT IMPLEMENTED YET" << std::endl;
193
194 const double alpha = 1. / (T * T * T);
195 const double t2 = t * t;
196 const double t3 = t2 * t;
197 const double t4 = t3 * t;
198 const double t5 = t4 * t;
199 const double t6 = t5 * t;
200 const double t7 = t6 * t;
201 // equation found with sympy
202 wp.first.block<3, 3>(0, 0) =
203 Matrix3::Identity() * alpha *
204 (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 +
205 378000.0 * t3 - 75600.0 * t2 + 5040.0 * t)); // x0
206 wp.first.block<3, 3>(0, 3) =
207 Matrix3::Identity() * alpha *
208 (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 -
209 151200.0 * t3 + 15120.0 * t2)); // x1
210 wp.first.block<3, 3>(0, 6) =
211 Matrix3::Identity() * alpha *
212 (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 +
213 25200.0 * t3)); // x2
214 wp.second =
215 (1.0 *
216 (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
217 t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] -
218 20160.0 * pi[3])) +
219 t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
220 151200.0 * pi[3])) +
221 t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
222 504000.0 * pi[3])) +
223 t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] +
224 882000.0 * pi[3] + 25200.0 * pi[7])) +
225 t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] -
226 846720.0 * pi[3] - 120960.0 * pi[7] + 15120.0 * pi[8])) +
227 t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] +
228 423360.0 * pi[3] + 181440.0 * pi[7] - 45360.0 * pi[8] +
229 5040.0 * pi[9])) +
230 t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] +
231 32400.0 * pi[2] - 86400.0 * pi[3] - 86400.0 * pi[7] +
232 32400.0 * pi[8] - 7200.0 * pi[9]))) *
233 alpha;
234 return wp;
235}
236
237inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
238 double T) {
239 // equation for constraint on initial and final position and velocity and
240 // initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
241 // first, compute the constant waypoints that only depend on pData :
242 double n = 12.;
243 std::vector<point_t> pi;
244 pi.push_back(pData.c0_);
245 pi.push_back((pData.dc0_ * T / n) + pData.c0_);
246 pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
247 (2 * pData.dc0_ * T / n) +
248 pData.c0_); // * T because derivation make a T appear
249 pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
250 (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
251 (3 * pData.dc0_ * T / n) + pData.c0_);
252 pi.push_back(point_t::Zero());
253 pi.push_back(point_t::Zero());
254 pi.push_back(point_t::Zero());
255 pi.push_back(point_t::Zero());
256 pi.push_back(point_t::Zero());
257 pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
258 (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
259 (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
260 pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
261 (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
262 pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
263 pi.push_back(pData.c1_);
264 return pi;
265}
266
267// TODO
268inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
269 double T) {
270 bezier_wp_t::t_point_t wps;
271 // const int DIM_POINT = 6;
272 // const int DIM_VAR = 15;
273 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
274 std::vector<Matrix3> Cpi;
275 for (std::size_t i = 0; i < pi.size(); ++i) {
276 Cpi.push_back(skew(pi[i]));
277 }
278 // const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
279 // const Matrix3 Cg = skew(g);
280 // const double T2 = T * T;
281 // const double alpha = 1 / (T2);
282 std::cout << "NOT IMPLEMENTED YET" << std::endl;
283 return wps;
284}
285
286std::vector<waypoint_t> computeVelocityWaypoints(
287 const ProblemData& pData, const double T,
288 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
289 if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
290
291 std::vector<waypoint_t> wps;
292 assert(pi.size() == 13);
293
294 double alpha = 1. / (T);
295 waypoint_t w = initwp(DIM_POINT, DIM_VAR);
296
297 // assign w0:
298 w.second = alpha * 12 * (-pi[0] + pi[1]);
299 wps.push_back(w);
300 w = initwp(DIM_POINT, DIM_VAR);
301 // assign w1:
302 w.second = alpha * 12 * (-pi[1] + pi[2]);
303 wps.push_back(w);
304 w = initwp(DIM_POINT, DIM_VAR);
305 // assign w2:
306 w.second = alpha * 12 * (-pi[2] + pi[3]);
307 wps.push_back(w);
308 w = initwp(DIM_POINT, DIM_VAR);
309 // assign w3:
310 w.first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity(); // x0
311 w.second = alpha * 12 * (-pi[3]);
312 wps.push_back(w);
313 w = initwp(DIM_POINT, DIM_VAR);
314 // assign w4:
315 w.first.block<3, 3>(0, 0) = -12 * alpha * Matrix3::Identity(); // x0
316 w.first.block<3, 3>(0, 3) = 12 * alpha * Matrix3::Identity(); // x1
317 wps.push_back(w);
318 w = initwp(DIM_POINT, DIM_VAR);
319 // assign w5:
320 w.first.block<3, 3>(0, 3) = -12 * alpha * Matrix3::Identity(); // x1
321 w.first.block<3, 3>(0, 6) = 12 * alpha * Matrix3::Identity(); // x2
322 wps.push_back(w);
323 w = initwp(DIM_POINT, DIM_VAR);
324 // assign w6:
325 w.first.block<3, 3>(0, 6) = -12 * alpha * Matrix3::Identity(); // x2
326 w.first.block<3, 3>(0, 9) = 12 * alpha * Matrix3::Identity(); // x3
327 wps.push_back(w);
328 w = initwp(DIM_POINT, DIM_VAR);
329 // assign w7:
330 w.first.block<3, 3>(0, 9) = -12 * alpha * Matrix3::Identity(); // x3
331 w.first.block<3, 3>(0, 12) = 12 * alpha * Matrix3::Identity(); // x4
332 wps.push_back(w);
333 w = initwp(DIM_POINT, DIM_VAR);
334 // assign w8:
335 w.first.block<3, 3>(0, 12) = -12 * alpha * Matrix3::Identity(); // x4
336 w.second = alpha * 12 * pi[9];
337 wps.push_back(w);
338 w = initwp(DIM_POINT, DIM_VAR);
339 // assign w9:
340 w.second = alpha * 12 * (-pi[9] + pi[10]);
341 wps.push_back(w);
342 w = initwp(DIM_POINT, DIM_VAR);
343 // assign w10:
344 w.second = alpha * 12 * (-pi[10] + pi[11]);
345 wps.push_back(w);
346 w = initwp(DIM_POINT, DIM_VAR);
347 // assign w11:
348 w.second = alpha * 12 * (-pi[11] + pi[12]);
349 wps.push_back(w);
350 return wps;
351}
352
353std::vector<waypoint_t> computeAccelerationWaypoints(
354 const ProblemData& pData, const double T,
355 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
356 if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
357
358 std::vector<waypoint_t> wps;
359 assert(pi.size() == 13);
360 double alpha = 1. / (T * T);
361
362 waypoint_t w = initwp(DIM_POINT, DIM_VAR);
363
364 // assign w0:
365 w.second = alpha * 132 * (pi[0] - 2 * pi[1] + pi[2]);
366 wps.push_back(w);
367 w = initwp(DIM_POINT, DIM_VAR);
368 // assign w1:
369 w.second = alpha * 132 * (pi[1] - 2 * pi[2] + pi[3]);
370 wps.push_back(w);
371 w = initwp(DIM_POINT, DIM_VAR);
372 // assign w2:
373 w.first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity(); // x0
374 w.second = alpha * 132 * (pi[2] - pi[3]);
375 wps.push_back(w);
376 w = initwp(DIM_POINT, DIM_VAR);
377 // assign w3:
378 w.first.block<3, 3>(0, 0) = -264 * alpha * Matrix3::Identity(); // x0
379 w.first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity(); // x1
380 w.second = alpha * 132 * pi[3];
381 wps.push_back(w);
382 w = initwp(DIM_POINT, DIM_VAR);
383 // assign w4:
384 w.first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity(); // x0
385 w.first.block<3, 3>(0, 3) = -264 * alpha * Matrix3::Identity(); // x1
386 w.first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity(); // x2
387 wps.push_back(w);
388 w = initwp(DIM_POINT, DIM_VAR);
389 // assign w5:
390 w.first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity(); // x1
391 w.first.block<3, 3>(0, 6) = -264 * alpha * Matrix3::Identity(); // x2
392 w.first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity(); // x3
393 wps.push_back(w);
394 w = initwp(DIM_POINT, DIM_VAR);
395 // assign w6:
396 w.first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity(); // x2
397 w.first.block<3, 3>(0, 9) = -264 * alpha * Matrix3::Identity(); // x3
398 w.first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity(); // x4
399 wps.push_back(w);
400 w = initwp(DIM_POINT, DIM_VAR);
401 // assign w7:
402 w.first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity(); // x3
403 w.first.block<3, 3>(0, 12) = -264 * alpha * Matrix3::Identity(); // x4
404 w.second = alpha * 132 * pi[7];
405 wps.push_back(w);
406 w = initwp(DIM_POINT, DIM_VAR);
407 // assign w8:
408 w.first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity(); // x4
409 w.second = alpha * 132 * (-2 * pi[9] + pi[10]);
410 wps.push_back(w);
411 w = initwp(DIM_POINT, DIM_VAR);
412 // assign w9:
413 w.second = alpha * 132 * (pi[9] - 2 * pi[10] + pi[11]);
414 wps.push_back(w);
415 wps.push_back(w);
416 w = initwp(DIM_POINT, DIM_VAR);
417 // assign w10:
418 w.second = alpha * 132 * (pi[12] + pi[10] - 2 * pi[11]);
419 wps.push_back(w);
420 return wps;
421}
422
423std::vector<waypoint_t> computeJerkWaypoints(
424 const ProblemData& pData, const double T,
425 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
426 if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
427
428 std::vector<waypoint_t> wps;
429 assert(pi.size() == 13);
430
431 double alpha = 1. / (T * T * T);
432
433 waypoint_t w = initwp(DIM_POINT, DIM_VAR);
434
435 // assign w0:
436 w.second = alpha * 1320 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
437 wps.push_back(w);
438 w = initwp(DIM_POINT, DIM_VAR);
439 // assign w1:
440 w.first.block<3, 3>(0, 0) = 1320 * alpha * Matrix3::Identity(); // x0
441 w.second = alpha * 1320 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
442 wps.push_back(w);
443 w = initwp(DIM_POINT, DIM_VAR);
444 // assign w2:
445 w.first.block<3, 3>(0, 0) = 1320 * -3 * alpha * Matrix3::Identity(); // x0
446 w.first.block<3, 3>(0, 3) = 1320 * alpha * Matrix3::Identity(); // x1
447 w.second = alpha * 1320 * (-pi[2] + 3 * pi[3]);
448 wps.push_back(w);
449 w = initwp(DIM_POINT, DIM_VAR);
450 // assign w3:
451 w.first.block<3, 3>(0, 0) = 1320 * 3 * alpha * Matrix3::Identity(); // x0
452 w.first.block<3, 3>(0, 3) = 1320 * -3 * alpha * Matrix3::Identity(); // x1
453 w.first.block<3, 3>(0, 6) = 1320 * alpha * Matrix3::Identity(); // x2
454 w.second = alpha * 1320 * (-pi[3]);
455 wps.push_back(w);
456 w = initwp(DIM_POINT, DIM_VAR);
457 // assign w4:
458 w.first.block<3, 3>(0, 0) = -1320 * alpha * Matrix3::Identity(); // x0
459 w.first.block<3, 3>(0, 3) = 1320 * 3 * alpha * Matrix3::Identity(); // x1
460 w.first.block<3, 3>(0, 6) = 1320 * -3 * alpha * Matrix3::Identity(); // x2
461 w.first.block<3, 3>(0, 9) = 1320 * alpha * Matrix3::Identity(); // x3
462 wps.push_back(w);
463 w = initwp(DIM_POINT, DIM_VAR);
464 // assign w5:
465 w.first.block<3, 3>(0, 3) = -1320 * alpha * Matrix3::Identity(); // x1
466 w.first.block<3, 3>(0, 6) = 1320 * 3 * alpha * Matrix3::Identity(); // x2
467 w.first.block<3, 3>(0, 9) = 1320 * -3 * alpha * Matrix3::Identity(); // x3
468 w.first.block<3, 3>(0, 12) = 1320 * alpha * Matrix3::Identity(); // x4
469 wps.push_back(w);
470 w = initwp(DIM_POINT, DIM_VAR);
471 // assign w4:
472 w.first.block<3, 3>(0, 6) = -1320 * alpha * Matrix3::Identity(); // x2
473 w.first.block<3, 3>(0, 9) = 1320 * 3 * alpha * Matrix3::Identity(); // x3
474 w.first.block<3, 3>(0, 12) = 1320 * -3 * alpha * Matrix3::Identity(); // x4
475 w.second = alpha * 1320 * pi[9];
476 wps.push_back(w);
477 w = initwp(DIM_POINT, DIM_VAR);
478 // assign w5:
479 w.first.block<3, 3>(0, 9) = -1320 * alpha * Matrix3::Identity(); // x3
480 w.first.block<3, 3>(0, 12) = 1320 * 3 * alpha * Matrix3::Identity(); // x4
481 w.second = alpha * 1320 * (-3 * pi[9] + pi[10]);
482 wps.push_back(w);
483 w = initwp(DIM_POINT, DIM_VAR);
484 // assign w5:
485 w.first.block<3, 3>(0, 12) = -1320 * alpha * Matrix3::Identity(); // x6
486 w.second = alpha * 1320 * (3 * pi[9] - 3 * pi[10] + pi[11]);
487 wps.push_back(w);
488 w = initwp(DIM_POINT, DIM_VAR);
489 // assign w6:
490 w.second = alpha * 1320 * (pi[12] - pi[9] + 3 * pi[10] - 3 * pi[11]);
491 wps.push_back(w);
492 return wps;
493}
494
495// TODO
496inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
497 coefs_t v;
498 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
499 // equation found with sympy
500 v.first = 0.;
501 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
502 return v;
503}
504
505// TODO
506inline std::pair<MatrixXX, VectorX> computeVelocityCost(
507 const ProblemData& pData, double T,
508 std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
509 MatrixXX H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
510 VectorX g = VectorX::Zero(DIM_VAR);
511 if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
512
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])) /
517 (2 * T); // x0
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])) /
522 (2 * T); // x1
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]) /
527 (2 * T); // x2
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]) /
532 (2 * T); // x3
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]) /
537 (2 * T); // x4
538
539 H.block<3, 3>(0, 0) = Matrix3::Identity() * 9.63290527229048 / (T); // x0^2
540 H.block<3, 3>(3, 3) = Matrix3::Identity() * 8.29911962311903 / (T); // x1^2
541 H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.92188615942945 / (T); // x2^2
542 H.block<3, 3>(9, 9) = Matrix3::Identity() * 8.29911871865983 / (T); // x3^2
543 H.block<3, 3>(12, 12) = Matrix3::Identity() * 9.63290582796267 / (T); // x4^2
544
545 H.block<3, 3>(0, 3) =
546 Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
547 H.block<3, 3>(3, 0) =
548 Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
549 H.block<3, 3>(0, 6) =
550 Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
551 H.block<3, 3>(6, 0) =
552 Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
553 H.block<3, 3>(0, 9) =
554 Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
555 H.block<3, 3>(9, 0) =
556 Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
557 H.block<3, 3>(0, 12) =
558 Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
559 H.block<3, 3>(12, 0) =
560 Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
561
562 H.block<3, 3>(3, 6) =
563 Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
564 H.block<3, 3>(6, 3) =
565 Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
566 H.block<3, 3>(3, 9) =
567 Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
568 H.block<3, 3>(9, 3) =
569 Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
570 H.block<3, 3>(3, 12) =
571 Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
572 H.block<3, 3>(12, 3) =
573 Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
574
575 H.block<3, 3>(6, 9) =
576 Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
577 H.block<3, 3>(9, 6) =
578 Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
579 H.block<3, 3>(6, 12) =
580 Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
581 H.block<3, 3>(12, 6) =
582 Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
583
584 H.block<3, 3>(9, 12) =
585 Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
586 H.block<3, 3>(12, 9) =
587 Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
588
589 double norm = H.norm();
590 H /= norm;
591 g /= norm;
592
593 return std::make_pair(H, g);
594}
595
596} // namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1
597
598} // namespace bezier_com_traj
599
600#endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
END_ACC
Definition: flags.hh:25
INIT_VEL
Definition: flags.hh:21
END_VEL
Definition: flags.hh:24
END_POS
Definition: flags.hh:23
FIVE_FREE_VAR
Definition: flags.hh:32
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::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:423
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:90
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:286
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:496
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:237
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:189
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:353
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:268
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:138
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:506
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:31
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
Definition: utils.hh:25
VectorX second
Definition: utils.hh:27
MatrixXX first
Definition: utils.hh:26