hpp-bezier-com-traj  4.13.0
Multi contact trajectory generation for the COM using Bezier curves
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 
11 namespace bezier_com_traj {
12 namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1 {
13 
14 static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
17 static const size_t DIM_VAR = 3 * 5;
18 static const size_t DIM_POINT = 3;
22 
31 inline 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 
237 inline 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
268 inline 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 
286 std::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 
353 std::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 
423 std::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
496 inline 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
506 inline 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
FIVE_FREE_VAR
Definition: flags.hh:32
point_t j0_
Definition: data.hh:107
point_t ddc0_
Definition: data.hh:107
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
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 > 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
VectorX second
Definition: utils.hh:27
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:496
INIT_VEL
Definition: flags.hh:21
Definition: utils.hh:25
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:237
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:24
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
END_ACC
Definition: flags.hh:25
point_t c0_
Definition: data.hh:107
MatrixXX first
Definition: utils.hh:26
END_POS
Definition: flags.hh:23
point_t dc1_
Definition: data.hh:107
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:62
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:506
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:107
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
END_JERK
Definition: flags.hh:27
point_t ddc1_
Definition: data.hh:107
point_t j1_
Definition: data.hh:107
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
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:21
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
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:92
Definition: common_solve_methods.hh:15
INIT_POS
Definition: flags.hh:20
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:62
point_t c1_
Definition: data.hh:107
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
const int DIM_POINT
Definition: solve_end_effector.hh:15