hpp-bezier-com-traj  4.13.0
Multi contact trajectory generation for the COM using Bezier curves
waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
Go to the documentation of this file.
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
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_j1_ddc1_dc1_c1 {
13 
14 static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
16 static const size_t DIM_VAR = 3;
17 static const size_t DIM_POINT = 3;
21 
30 inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
31  coefs_t wp;
32  const double t2 = t * t;
33  const double t3 = t2 * t;
34  const double t4 = t3 * t;
35  const double t5 = t4 * t;
36  const double t6 = t5 * t;
37  const double t7 = t6 * t;
38  const double t8 = t7 * t;
39  // equation found with sympy
40  wp.first = 70.0 * t8 - 280.0 * t7 + 420.0 * t6 - 280.0 * t5 + 70.0 * t4;
41  wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 -
42  56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 - 56.0 * pi[8] * t3 +
43  28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] -
44  8.0 * pi[1] * t8 + 56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 +
45  280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
46  56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 -
47  168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 - 560.0 * pi[2] * t5 +
48  420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
49  56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 -
50  560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 - 280.0 * pi[3] * t4 +
51  56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 -
52  168.0 * pi[5] * t6 + 56.0 * pi[5] * pow(t, 5) +
53  28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 -
54  8.0 * pi[7] * t8 + 8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
55  return wp;
56 }
57 
58 inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi,
59  double T, double t) {
60  coefs_t wp;
61  const double alpha = 1. / (T);
62  const double t2 = t * t;
63  const double t3 = t2 * t;
64  const double t4 = t3 * t;
65  const double t5 = t4 * t;
66  const double t6 = t5 * t;
67  const double t7 = t6 * t;
68  // equation found with sympy
69  wp.first =
70  (560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) *
71  alpha;
72  wp.second = (8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 -
73  280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 - 168.0 * pi[8] * t2 +
74  56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 +
75  392.0 * pi[1] * t6 - 1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 -
76  1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
77  8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 +
78  2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 + 1680.0 * pi[2] * t3 -
79  504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 +
80  1960.0 * pi[3] * t6 - 3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 -
81  1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
82  1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 +
83  224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 + 168.0 * pi[6] * t5 -
84  64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
85  alpha;
86  return wp;
87 }
88 
89 inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
90  double T, double t) {
91  coefs_t wp;
92  const double alpha = 1. / (T * T);
93  const double t2 = t * t;
94  const double t3 = t2 * t;
95  const double t4 = t3 * t;
96  const double t5 = t4 * t;
97  const double t6 = t5 * t;
98  // equation found with sympy
99  wp.first =
100  ((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) *
101  alpha;
102  wp.second = (56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 -
103  1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 - 336.0 * pi[8] * t +
104  56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 -
105  5040.0 * pi[1] * t4 + 5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 +
106  1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
107  7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 -
108  11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
109  56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 -
110  16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
111  3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 +
112  7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 + 1120.0 * pi[5] * t3 +
113  1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 -
114  448.0 * pi[7] * t6 + 336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
115  alpha;
116  return wp;
117 }
118 
119 inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T,
120  double t) {
121  coefs_t wp;
122  const double alpha = 1. / (T * T * T);
123  const double t2 = t * t;
124  const double t3 = t2 * t;
125  const double t4 = t3 * t;
126  const double t5 = t4 * t;
127  // equation found with sympy
128  wp.first =
129  (23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) *
130  alpha;
131  wp.second =
132  1.0 *
133  (336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 -
134  3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t - 336.0 * pi[0] -
135  2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 +
136  16800.0 * pi[1] * t2 - 6720.0 * pi[1] * t + 1008.0 * pi[1] +
137  9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
138  33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] -
139  18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 - 67200.0 * pi[3] * t3 +
140  33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] -
141  18816.0 * pi[5] * t5 + 35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 +
142  3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
143  3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 +
144  336.0 * pi[8] * t5) *
145  alpha;
146  return wp;
147 }
148 inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
149  double t) {
150  coefs_t coef = evaluateCurveAtTime(pi, t);
151  waypoint_t wp;
152  wp.first = Matrix3::Identity() * coef.first;
153  wp.second = coef.second;
154  return wp;
155 }
157  const std::vector<point_t>& pi, double T, double t) {
158  coefs_t coef = evaluateVelocityCurveAtTime(pi, T, t);
159  waypoint_t wp;
160  wp.first = Matrix3::Identity() * coef.first;
161  wp.second = coef.second;
162  return wp;
163 }
165  const std::vector<point_t>& pi, double T, double t) {
166  coefs_t coef = evaluateAccelerationCurveAtTime(pi, T, t);
167  waypoint_t wp;
168  wp.first = Matrix3::Identity() * coef.first;
169  wp.second = coef.second;
170  return wp;
171 }
172 
174  const std::vector<point_t>& pi, double T, double t) {
175  coefs_t coef = evaluateJerkCurveAtTime(pi, T, t);
176  waypoint_t wp;
177  wp.first = Matrix3::Identity() * coef.first;
178  wp.second = coef.second;
179  return wp;
180 }
181 
182 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
183  double T) {
184  // equation for constraint on initial and final position and velocity and
185  // initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
186  // first, compute the constant waypoints that only depend on pData :
187  double n = 8.;
188  std::vector<point_t> pi;
189  pi.push_back(pData.c0_);
190  pi.push_back((pData.dc0_ * T / n) + pData.c0_);
191  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
192  (2 * pData.dc0_ * T / n) +
193  pData.c0_); // * T because derivation make a T appear
194  pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
195  (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
196  (3 * pData.dc0_ * T / n) + pData.c0_);
197  pi.push_back(point_t::Zero());
198  pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
199  (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
200  (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
201  pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
202  (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
203  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
204  pi.push_back(pData.c1_);
205  return pi;
206 }
207 
208 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
209  double T) {
210  bezier_wp_t::t_point_t wps;
211  const int DIM_POINT = 6;
212  const int DIM_VAR = 3;
213  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
214  std::vector<Matrix3> Cpi;
215  for (std::size_t i = 0; i < pi.size(); ++i) {
216  Cpi.push_back(skew(pi[i]));
217  }
218  const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
219  const Matrix3 Cg = skew(g);
220  const double T2 = T * T;
221  const double alpha = 1 / (T2);
222 
223  // equation of waypoints for curve w found with sympy
224  waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
225  w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
226  w0.second.tail<3>() =
227  1.0 *
228  (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
229  alpha;
230  wps.push_back(w0);
231  waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
232  w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
233  w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
234  w1.second.head<3>() =
235  1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
236  w1.second.tail<3>() = 1.0 *
237  (0.333333333333333 * Cg * T2 * pi[0] +
238  0.666666666666667 * Cg * T2 * pi[1] -
239  30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
240  alpha;
241  wps.push_back(w1);
242  waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
243  w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
244  w2.first.block<3, 3>(3, 0) =
245  1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
246  w2.second.head<3>() =
247  1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
248  w2.second.tail<3>() =
249  1.0 *
250  (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
251  0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
252  20.0 * Cpi[1] * pi[2]) *
253  alpha;
254  wps.push_back(w2);
255  waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
256  w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
257  w3.first.block<3, 3>(3, 0) = 1.0 *
258  (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
259  14.2857142857143 * Cpi[2]) *
260  alpha;
261  w3.second.head<3>() = 1.0 *
262  (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
263  14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
264  1.42857142857143 * pi[5]) *
265  alpha;
266  w3.second.tail<3>() =
267  1.0 *
268  (0.0119047619047619 * Cg * T2 * pi[0] +
269  0.214285714285714 * Cg * T2 * pi[1] +
270  0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
271  1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
272  alpha;
273  wps.push_back(w3);
274  waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
275  w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
276  w4.first.block<3, 3>(3, 0) =
277  1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
278  w4.second.head<3>() = 1.0 *
279  (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
280  3.57142857142857 * pi[2] + 5.0 * pi[4] +
281  4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
282  alpha;
283  w4.second.tail<3>() =
284  1.0 *
285  (0.0476190476190471 * Cg * T2 * pi[1] +
286  0.357142857142857 * Cg * T2 * pi[2] +
287  0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
288  0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
289  5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
290  alpha;
291  wps.push_back(w4);
292  waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
293  w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
294  w5.first.block<3, 3>(3, 0) =
295  1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
296  w5.second.head<3>() = 1.0 *
297  (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
298  5.0 * pi[2] - 3.57142857142857 * pi[4] +
299  7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
300  alpha;
301  w5.second.tail<3>() =
302  1.0 *
303  (+0.11904761904762 * Cg * T2 * pi[2] +
304  0.357142857142857 * Cg * T2 * pi[4] +
305  0.0476190476190476 * Cg * T2 * pi[5] -
306  0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
307  1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
308  12.8571428571429 * Cpi[2] * pi[5]) *
309  alpha;
310  wps.push_back(w5);
311  waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
312  w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
313  w6.first.block<3, 3>(3, 0) = 1.0 *
314  (0.238095238095238 * Cg * T2 +
315  14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
316  alpha;
317  w6.second.head<3>() = 1.0 *
318  (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
319  14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
320  3.57142857142857 * pi[6]) *
321  alpha;
322  w6.second.tail<3>() =
323  1.0 *
324  (0.535714285714286 * Cg * T2 * pi[4] +
325  0.214285714285714 * Cg * T2 * pi[5] +
326  0.0119047619047619 * Cg * T2 * pi[6] -
327  1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
328  5.0 * Cpi[2] * pi[6]) *
329  alpha;
330  wps.push_back(w6);
331  waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
332  w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
333  w7.first.block<3, 3>(3, 0) =
334  1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
335  w7.second.head<3>() =
336  1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
337  w7.second.tail<3>() =
338  1.0 *
339  (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
340  0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
341  20.0 * Cpi[4] * pi[5]) *
342  alpha;
343  wps.push_back(w7);
344  waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
345  w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
346  w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
347  w8.second.head<3>() =
348  1.0 *
349  (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
350  alpha;
351  w8.second.tail<3>() = 1.0 *
352  (0.666666666666667 * Cg * T2 * pi[5] +
353  0.333333333333333 * Cg * T2 * pi[6] -
354  20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
355  alpha;
356  wps.push_back(w8);
357  waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
358  w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
359  w9.second.tail<3>() =
360  1.0 *
361  (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
362  alpha;
363  wps.push_back(w9);
364  return wps;
365 }
366 
367 std::vector<waypoint_t> computeVelocityWaypoints(
368  const ProblemData& pData, const double T,
369  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
370  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
371 
372  std::vector<waypoint_t> wps;
373  assert(pi.size() == 9);
374 
375  double alpha = 1. / (T);
376  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
377  // assign w0:
378  w.second = alpha * 8 * (-pi[0] + pi[1]);
379  wps.push_back(w);
380  w = initwp(DIM_POINT, DIM_VAR);
381  // assign w1:
382  w.second = alpha * 8 * (-pi[1] + pi[2]);
383  wps.push_back(w);
384  w = initwp(DIM_POINT, DIM_VAR);
385  // assign w2:
386  w.second = alpha * 8 * (-pi[2] + pi[3]);
387  wps.push_back(w);
388  w = initwp(DIM_POINT, DIM_VAR);
389  // assign w3:
390  w.first = 8 * alpha * Matrix3::Identity();
391  w.second = alpha * -8 * pi[3];
392  wps.push_back(w);
393  w = initwp(DIM_POINT, DIM_VAR);
394  // assign w4:
395  w.first = -8 * alpha * Matrix3::Identity();
396  w.second = alpha * 8 * pi[5];
397  wps.push_back(w);
398  w = initwp(DIM_POINT, DIM_VAR);
399  // assign w5:
400  w.second = alpha * 8 * (-pi[5] + pi[6]);
401  wps.push_back(w);
402  w = initwp(DIM_POINT, DIM_VAR);
403  // assign w6:
404  w.second = alpha * 8 * (-pi[6] + pi[7]);
405  wps.push_back(w);
406  w = initwp(DIM_POINT, DIM_VAR);
407  // assign w7:
408  w.second = alpha * 8 * (-pi[7] + pi[8]);
409  wps.push_back(w);
410  return wps;
411 }
412 
413 std::vector<waypoint_t> computeAccelerationWaypoints(
414  const ProblemData& pData, const double T,
415  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
416  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
417 
418  std::vector<waypoint_t> wps;
419  assert(pi.size() == 9);
420  double alpha = 1. / (T * T);
421 
422  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
423 
424  // assign w0:
425  w.second = 56 * alpha * (pi[0] - 2 * pi[1] + pi[2]);
426  wps.push_back(w);
427  w = initwp(DIM_POINT, DIM_VAR);
428  // assign w1:
429  w.second = 56 * alpha * (pi[1] - 2 * pi[2] + pi[3]);
430  wps.push_back(w);
431  w = initwp(DIM_POINT, DIM_VAR);
432  // assign w2:
433  w.first = 56 * alpha * Matrix3::Identity();
434  w.second = (56 * pi[2] - 112 * pi[3]) * alpha;
435  wps.push_back(w);
436  w = initwp(DIM_POINT, DIM_VAR);
437  // assign w3:
438  w.first = -112 * alpha * Matrix3::Identity();
439  w.second = (56 * pi[3] + 56 * pi[8]) * alpha;
440  wps.push_back(w);
441  w = initwp(DIM_POINT, DIM_VAR);
442  // assign w4:
443  w.first = 56 * alpha * Matrix3::Identity();
444  w.second = (-112 * pi[5] + 56 * pi[6]) * alpha;
445  wps.push_back(w);
446  w = initwp(DIM_POINT, DIM_VAR);
447  // assign w5:
448  w.second = 56 * alpha * (pi[5] - 2 * pi[6] + pi[7]);
449  wps.push_back(w);
450  w = initwp(DIM_POINT, DIM_VAR);
451  // assign w5:
452  w.second = 56 * alpha * (pi[6] - 2 * pi[7] + pi[8]);
453  wps.push_back(w);
454  return wps;
455 }
456 
457 std::vector<waypoint_t> computeJerkWaypoints(
458  const ProblemData& pData, const double T,
459  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
460  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
461 
462  std::vector<waypoint_t> wps;
463  assert(pi.size() == 9);
464 
465  double alpha = 1. / (T * T * T);
466 
467  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
468 
469  // assign w0:
470  w.second = 336 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]) * alpha;
471  wps.push_back(w);
472  w = initwp(DIM_POINT, DIM_VAR);
473  // assign w1:
474  w.first = 336 * alpha * Matrix3::Identity();
475  w.second = 336 * (-pi[1] + 3 * pi[2] - 3 * pi[3]) * alpha;
476  wps.push_back(w);
477  w = initwp(DIM_POINT, DIM_VAR);
478  // assign w2:
479  w.first = -3 * 336 * alpha * Matrix3::Identity();
480  w.second = 336 * (-pi[2] + 3 * pi[3] + pi[5]) * alpha;
481  wps.push_back(w);
482  w = initwp(DIM_POINT, DIM_VAR);
483  // assign w3:
484  w.first = 3 * 336 * alpha * Matrix3::Identity();
485  w.second = 336 * (-pi[3] - 3 * pi[5] + pi[6]) * alpha;
486  wps.push_back(w);
487  w = initwp(DIM_POINT, DIM_VAR);
488  // assign w4:
489  w.first = -336 * alpha * Matrix3::Identity();
490  w.second = 336 * (3 * pi[5] - 3 * pi[6] + pi[7]) * alpha;
491  wps.push_back(w);
492  w = initwp(DIM_POINT, DIM_VAR);
493  // assign w5:
494  w.second = 336 * (-pi[5] + 3 * pi[6] - 3 * pi[7] + pi[8]) * alpha;
495  wps.push_back(w);
496  return wps;
497 }
498 
499 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
500  coefs_t v;
501  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
502  // equation found with sympy
503  v.first = 0.;
504  v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
505  return v;
506 }
507 
508 inline std::pair<MatrixXX, VectorX> computeVelocityCost(
509  const ProblemData& pData, double T,
510  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
511  MatrixXX H = MatrixXX::Zero(3, 3);
512  VectorX g = VectorX::Zero(3);
513  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
514 
515  g = (-7.8321678321748 * pi[0] - 7.83216783237586 * pi[1] +
516  9.13752913728184 * pi[3] + 9.13752913758454 * pi[5] -
517  7.83216783216697 * pi[7] - 7.83216783216777 * pi[8]) /
518  (2 * T);
519  H = Matrix3::Identity() * 6.52680652684107 / (T);
520 
521  double norm = H.norm();
522  H /= norm;
523  g /= norm;
524 
525  return std::make_pair(H, g);
526 }
527 
528 } // namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1
529 
530 } // namespace bezier_com_traj
531 
532 #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:119
point_t j0_
Definition: data.hh:107
waypoint_t evaluateCurveWaypointAtTime(const std::vector< point_t > &pi, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:148
point_t ddc0_
Definition: data.hh:107
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:34
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:22
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:23
VectorX second
Definition: utils.hh:27
waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:164
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:89
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:45
INIT_VEL
Definition: flags.hh:21
Definition: utils.hh:25
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:24
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:56
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:499
waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:173
END_ACC
Definition: flags.hh:25
point_t c0_
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_j1_ddc1_dc1_c1.hh:413
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:367
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
coefs_t evaluateVelocityCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:58
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:30
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:107
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: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_j1_ddc1_dc1_c1.hh:508
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:208
point_t j1_
Definition: data.hh:107
waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector< point_t > &pi, double T, double t)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:156
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition: definitions.hh:17
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
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:182
std::vector< ContactData > contacts_
Definition: data.hh:106
point_t c1_
Definition: data.hh:107
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:457
const int DIM_POINT
Definition: solve_end_effector.hh:15