hpp-bezier-com-traj  4.13.0
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
Go to the documentation of this file.
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
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_x3_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 = 9;
18 static const size_t DIM_POINT = 3;
22 
31 // TODO
32 inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
33  double t) {
34  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
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;
44 
45  // equation found with sympy
46  wp.first.block<3, 3>(0, 0) =
47  Matrix3::Identity() *
48  (t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 + 3150.0 * t8 -
49  1260.0 * t9 + 210.0 * t10); // x0
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); // x1
53  wp.first.block<3, 3>(0, 6) =
54  Matrix3::Identity() *
55  (210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10); // x2
56  wp.second =
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] +
64  120.0 * pi[7]) +
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]);
71  return wp;
72 }
73 
74 // TODO
76  const std::vector<point_t>& pi, double T, double t) {
77  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
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;
87  // equation found with sympy
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)); // x0
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)); // x1
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 +
99  1260.0 * t5)); // x2
100  wp.second =
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] +
104  360.0 * pi[3])) +
105  t3 * (1.0 *
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] -
109  3360.0 * pi[3])) +
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] +
114  12600.0 * pi[3])) +
115  t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
116  25200.0 * pi[3])) +
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]))) *
121  alpha;
122  return wp;
123 }
124 
125 // TODO
127  const std::vector<point_t>& pi, double T, double t) {
128  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
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;
137  // equation found with sympy
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)); // x0
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)); // x1
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 +
149  6300.0 * t4)); // x2
150  wp.second =
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] +
153  720.0 * pi[3])) +
154  t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
155  10080.0 * pi[3])) +
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] +
160  50400.0 * pi[3])) +
161  t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
162  126000.0 * pi[3])) +
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] +
169  720.0 * pi[9]))) *
170  alpha;
171  return wp;
172 }
173 
174 // TODO
176  const std::vector<point_t>& pi, double T, double t) {
177  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
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;
185  // equation found with sympy
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)); // x0
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)); // x1
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 +
197  25200.0 * t3)); // x2
198  wp.second =
199  (1.0 *
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] -
202  20160.0 * pi[3])) +
203  t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
204  151200.0 * pi[3])) +
205  t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
206  504000.0 * pi[3])) +
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] +
213  5040.0 * pi[9])) +
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]))) *
217  alpha;
218  return wp;
219 }
220 
221 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
222  double T) {
223  // equation for constraint on initial and final position and velocity and
224  // initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
225  // first, compute the constant waypoints that only depend on pData :
226  double n = 10.;
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) +
232  pData.c0_); // * T because derivation make a T appear
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_); // * T ??
242  pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
243  (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
244  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
245  pi.push_back(pData.c1_);
246  return pi;
247 }
248 
249 // TODO
250 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
251  double T) {
252  bezier_wp_t::t_point_t wps;
253  // const int DIM_POINT = 6;
254  // const int DIM_VAR = 9;
255  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
256  std::vector<Matrix3> Cpi;
257  for (std::size_t i = 0; i < pi.size(); ++i) {
258  Cpi.push_back(skew(pi[i]));
259  }
260  // const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
261  // const Matrix3 Cg = skew(g);
262  // const double T2 = T * T;
263  // const double alpha = 1 / (T2);
264  std::cout << "NOT IMPLEMENTED YET" << std::endl;
265  return wps;
266 }
267 
268 std::vector<waypoint_t> computeVelocityWaypoints(
269  const ProblemData& pData, const double T,
270  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
271  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
272 
273  std::vector<waypoint_t> wps;
274  assert(pi.size() == 11);
275 
276  double alpha = 1. / (T);
277  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
278 
279  // assign w0:
280  w.second = alpha * 10 * (-pi[0] + pi[1]);
281  wps.push_back(w);
282  w = initwp(DIM_POINT, DIM_VAR);
283  // assign w1:
284  w.second = alpha * 10 * (-pi[1] + pi[2]);
285  wps.push_back(w);
286  w = initwp(DIM_POINT, DIM_VAR);
287  // assign w2:
288  w.second = alpha * 10 * (-pi[2] + pi[3]);
289  wps.push_back(w);
290  w = initwp(DIM_POINT, DIM_VAR);
291  // assign w3:
292  w.first.block<3, 3>(0, 0) = 10 * alpha * Matrix3::Identity(); // x0
293  w.second = alpha * 10 * (-pi[3]);
294  wps.push_back(w);
295  w = initwp(DIM_POINT, DIM_VAR);
296  // assign w4:
297  w.first.block<3, 3>(0, 0) = -10 * alpha * Matrix3::Identity(); // x0
298  w.first.block<3, 3>(0, 3) = 10 * alpha * Matrix3::Identity(); // x1
299  wps.push_back(w);
300  w = initwp(DIM_POINT, DIM_VAR);
301  // assign w5:
302  w.first.block<3, 3>(0, 3) = -10 * alpha * Matrix3::Identity(); // x1
303  w.first.block<3, 3>(0, 6) = 10 * alpha * Matrix3::Identity(); // x2
304  wps.push_back(w);
305  w = initwp(DIM_POINT, DIM_VAR);
306  // assign w6:
307  w.first.block<3, 3>(0, 6) = -10 * alpha * Matrix3::Identity(); // x2
308  w.second = alpha * 10 * pi[7];
309  wps.push_back(w);
310  w = initwp(DIM_POINT, DIM_VAR);
311  // assign w7:
312  w.second = alpha * 10 * (-pi[7] + pi[8]);
313  wps.push_back(w);
314  w = initwp(DIM_POINT, DIM_VAR);
315  // assign w8:
316  w.second = alpha * 10 * (-pi[8] + pi[9]);
317  wps.push_back(w);
318  w = initwp(DIM_POINT, DIM_VAR);
319  // assign w9:
320  w.second = alpha * 10 * (pi[10] - pi[9]);
321  wps.push_back(w);
322  return wps;
323 }
324 
325 std::vector<waypoint_t> computeAccelerationWaypoints(
326  const ProblemData& pData, const double T,
327  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
328  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
329 
330  std::vector<waypoint_t> wps;
331  assert(pi.size() == 11);
332  double alpha = 1. / (T * T);
333 
334  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
335 
336  // assign w0:
337  w.second = alpha * 90 * (pi[0] - 2 * pi[1] + pi[2]);
338  wps.push_back(w);
339  w = initwp(DIM_POINT, DIM_VAR);
340  // assign w1:
341  w.second = alpha * 90 * (pi[1] - 2 * pi[2] + pi[3]);
342  wps.push_back(w);
343  w = initwp(DIM_POINT, DIM_VAR);
344  // assign w2:
345  w.first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity(); // x0
346  w.second = alpha * 90 * (pi[2] - pi[3]);
347  wps.push_back(w);
348  w = initwp(DIM_POINT, DIM_VAR);
349  // assign w3:
350  w.first.block<3, 3>(0, 0) = -180 * alpha * Matrix3::Identity(); // x0
351  w.first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity(); // x1
352  w.second = alpha * 90 * pi[3];
353  wps.push_back(w);
354  w = initwp(DIM_POINT, DIM_VAR);
355  // assign w4:
356  w.first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity(); // x0
357  w.first.block<3, 3>(0, 3) = -180 * alpha * Matrix3::Identity(); // x1
358  w.first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity(); // x2
359  wps.push_back(w);
360  w = initwp(DIM_POINT, DIM_VAR);
361  // assign w5:
362  w.first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity(); // x1
363  w.first.block<3, 3>(0, 6) = -180 * alpha * Matrix3::Identity(); // x2
364  w.second = alpha * 90 * pi[7];
365  wps.push_back(w);
366  w = initwp(DIM_POINT, DIM_VAR);
367  // assign w6:
368  w.first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity(); // x2
369  w.second = alpha * 90 * (-2 * pi[7] + pi[8]);
370  wps.push_back(w);
371  w = initwp(DIM_POINT, DIM_VAR);
372  // assign w7:
373  w.second = alpha * 90 * (pi[7] - 2 * pi[8] + pi[9]);
374  wps.push_back(w);
375  wps.push_back(w);
376  w = initwp(DIM_POINT, DIM_VAR);
377  // assign w8:
378  w.second = alpha * 90 * (pi[10] + pi[8] - 2 * pi[9]);
379  wps.push_back(w);
380  return wps;
381 }
382 
383 std::vector<waypoint_t> computeJerkWaypoints(
384  const ProblemData& pData, const double T,
385  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
386  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
387 
388  std::vector<waypoint_t> wps;
389  assert(pi.size() == 11);
390 
391  double alpha = 1. / (T * T * T);
392 
393  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
394 
395  // assign w0:
396  w.second = alpha * 720 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
397  wps.push_back(w);
398  w = initwp(DIM_POINT, DIM_VAR);
399  // assign w1:
400  w.first.block<3, 3>(0, 0) = 720 * alpha * Matrix3::Identity(); // x0
401  w.second = alpha * 720 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
402  wps.push_back(w);
403  w = initwp(DIM_POINT, DIM_VAR);
404  // assign w2:
405  w.first.block<3, 3>(0, 0) = 720 * -3 * alpha * Matrix3::Identity(); // x0
406  w.first.block<3, 3>(0, 3) = 720 * alpha * Matrix3::Identity(); // x1
407  w.second = alpha * 720 * (-pi[2] + 3 * pi[3]);
408  wps.push_back(w);
409  w = initwp(DIM_POINT, DIM_VAR);
410  // assign w3:
411  w.first.block<3, 3>(0, 0) = 720 * 3 * alpha * Matrix3::Identity(); // x0
412  w.first.block<3, 3>(0, 3) = 720 * -3 * alpha * Matrix3::Identity(); // x1
413  w.first.block<3, 3>(0, 6) = 720 * alpha * Matrix3::Identity(); // x2
414  w.second = alpha * 720 * (-pi[3]);
415  wps.push_back(w);
416  w = initwp(DIM_POINT, DIM_VAR);
417  // assign w4:
418  w.first.block<3, 3>(0, 0) = -720 * alpha * Matrix3::Identity(); // x0
419  w.first.block<3, 3>(0, 3) = 720 * 3 * alpha * Matrix3::Identity(); // x1
420  w.first.block<3, 3>(0, 6) = 720 * -3 * alpha * Matrix3::Identity(); // x2
421  w.second = alpha * 720 * pi[7];
422  wps.push_back(w);
423  w = initwp(DIM_POINT, DIM_VAR);
424  // assign w5:
425  w.first.block<3, 3>(0, 3) = -720 * alpha * Matrix3::Identity(); // x1
426  w.first.block<3, 3>(0, 6) = 720 * 3 * alpha * Matrix3::Identity(); // x2
427  w.second = alpha * 720 * (-3 * pi[7] + pi[8]);
428  wps.push_back(w);
429  w = initwp(DIM_POINT, DIM_VAR);
430  // assign w5:
431  w.first.block<3, 3>(0, 6) = -720 * alpha * Matrix3::Identity(); // x2
432  w.second = alpha * 720 * (3 * pi[7] - 3 * pi[8] + pi[9]);
433  wps.push_back(w);
434  w = initwp(DIM_POINT, DIM_VAR);
435  // assign w6:
436  w.second = alpha * 720 * (pi[10] - pi[7] + 3 * pi[8] - 3 * pi[9]);
437  wps.push_back(w);
438  return wps;
439 }
440 
441 // TODO
442 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
443  coefs_t v;
444  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
445  // equation found with sympy
446  v.first = 0.;
447  v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
448  return v;
449 }
450 
451 inline std::pair<MatrixXX, VectorX> computeVelocityCost(
452  const ProblemData& pData, double T,
453  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
454  MatrixXX H = MatrixXX::Zero(9, 9);
455  VectorX g = VectorX::Zero(9);
456  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
457 
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]) /
462  (2 * T); // x0
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]) /
467  (2 * T); // x1
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]) /
472  (2 * T); // x2
473 
474  H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T); // x0^2
475  H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T); // x1^2
476  H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T); // x2^2
477  H.block<3, 3>(0, 3) =
478  Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
479  H.block<3, 3>(3, 0) =
480  Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
481  H.block<3, 3>(0, 6) =
482  Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
483  H.block<3, 3>(6, 0) =
484  Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
485  H.block<3, 3>(3, 6) =
486  Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
487  H.block<3, 3>(6, 3) =
488  Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
489 
490  double norm = H.norm();
491  H /= norm;
492  g /= norm;
493 
494  return std::make_pair(H, g);
495 }
496 
497 } // namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1
498 
499 } // namespace bezier_com_traj
500 
501 #endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
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
point_t j0_
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_x3_j1_ddc1_dc1_c1.hh:268
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:442
point_t ddc0_
Definition: data.hh:107
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
VectorX second
Definition: utils.hh:27
INIT_VEL
Definition: flags.hh:21
Definition: utils.hh:25
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:24
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
THREE_FREE_VAR
Definition: flags.hh:30
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
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
point_t dc1_
Definition: data.hh:107
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:62
INIT_JERK
Definition: flags.hh:26
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:107
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:221
END_JERK
Definition: flags.hh:27
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
point_t ddc1_
Definition: data.hh:107
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
point_t j1_
Definition: data.hh:107
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
END_VEL
Definition: flags.hh:24
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
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
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
const int DIM_POINT
Definition: solve_end_effector.hh:15