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