hpp-bezier-com-traj  4.12.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 =
16 static const size_t DIM_VAR = 3 * 5;
17 static const size_t DIM_POINT = 3;
21 
28 inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
29  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
30  const double t2 = t * t;
31  const double t3 = t2 * t;
32  const double t4 = t3 * t;
33  const double t5 = t4 * t;
34  const double t6 = t5 * t;
35  const double t7 = t6 * t;
36  const double t8 = t7 * t;
37  const double t9 = t8 * t;
38  const double t10 = t9 * t;
39  const double t11 = t10 * t;
40  const double t12 = t11 * t;
41 
42  // equation found with sympy
43  wp.first.block<3, 3>(0, 0) =
44  Matrix3::Identity() * (+495.0 * t4 - 3960.0 * t5 + 13860.0 * t6 - 27720.0 * t7 + 34650.0 * t8 - 27720.0 * t9 +
45  13860.0 * t10 - 3960.0 * t11 + 495.0 * t12); // x0
46  wp.first.block<3, 3>(0, 3) =
47  Matrix3::Identity() * (+792.0 * t5 - 5544.0 * t6 + 16632.0 * t7 - 27720.0 * t8 + 27720.0 * t9 - 16632.0 * t10 +
48  5544.0 * t11 - 792.0 * t12); // x1
49  wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * (+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 +
50  13860.0 * t10 - 5544.0 * t11 + 924.0 * t12); // x2
51  wp.first.block<3, 3>(0, 9) = Matrix3::Identity() * (+792.0 * t7 - 3960.0 * t8 + 7920.0 * t9 - 7920.0 * t10 +
52  3960.0 * t11 - 792.0 * t12); // x3
53  wp.first.block<3, 3>(0, 12) =
54  Matrix3::Identity() * (+495.0 * t8 - 1980.0 * t9 + 2970.0 * t10 - 1980.0 * t11 + 495.0 * t12); // x4
55 
56  wp.second = 1.0 * pi[0] + t * (-12.0 * pi[0] + 12.0 * pi[1]) + t2 * (66.0 * pi[0] - 132.0 * pi[1] + 66.0 * pi[2]) +
57  t3 * (-220.0 * pi[0] + 660.0 * pi[1] - 660.0 * pi[2] + 220.0 * pi[3]) +
58  t4 * (495.0 * pi[0] - 1980.0 * pi[1] + 2970.0 * pi[2] - 1980.0 * pi[3]) +
59  t5 * (-792.0 * pi[0] + 3960.0 * pi[1] - 7920.0 * pi[2] + 7920.0 * pi[3]) +
60  t6 * (924.0 * pi[0] - 5544.0 * pi[1] + 13860.0 * pi[2] - 18480.0 * pi[3]) +
61  t7 * (-792.0 * pi[0] + 5544.0 * pi[1] - 16632.0 * pi[2] + 27720.0 * pi[3]) +
62  t8 * (495.0 * pi[0] - 3960.0 * pi[1] + 13860.0 * pi[2] - 27720.0 * pi[3]) +
63  t9 * (-220.0 * pi[0] + 1980.0 * pi[1] - 7920.0 * pi[2] + 18480.0 * pi[3] + 220.0 * pi[9]) +
64  t10 * (66.0 * pi[0] + 66.0 * pi[10] - 660.0 * pi[1] + 2970.0 * pi[2] - 7920.0 * pi[3] - 660.0 * pi[9]) +
65  t11 * (-12.0 * pi[0] - 132.0 * pi[10] + 12.0 * pi[11] + 132.0 * pi[1] - 660.0 * pi[2] + 1980.0 * pi[3] +
66  660.0 * pi[9]) +
67  t12 * (1.0 * pi[0] + 66.0 * pi[10] - 12.0 * pi[11] + 1.0 * pi[12] - 12.0 * pi[1] + 66.0 * pi[2] -
68  220.0 * pi[3] - 220.0 * pi[9]);
69  return wp;
70 }
71 
72 // TODO
73 inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
74  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
75  std::cout << "NOT IMPLEMENTED YET" << std::endl;
76 
77  const double alpha = 1. / (T);
78  const double t2 = t * t;
79  const double t3 = t2 * t;
80  const double t4 = t3 * t;
81  const double t5 = t4 * t;
82  const double t6 = t5 * t;
83  const double t7 = t6 * t;
84  const double t8 = t7 * t;
85  const double t9 = t8 * t;
86 
87  // equation found with sympy
88  wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha; // x0
89  wp.first.block<3, 3>(0, 3) = Matrix3::Identity() * alpha; // x1
90  wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha; // x2
91  wp.first.block<3, 3>(0, 9) = Matrix3::Identity() * alpha; // x3
92  wp.first.block<3, 3>(0, 12) = Matrix3::Identity() * alpha; // x4
93 
94  wp.second = (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) + t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
95  t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
96  t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
97  810.0 * pi[8] + 90.0 * pi[9])) +
98  t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
99  t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
100  1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
101  t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
102  t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
103  t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
104  t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
105  360.0 * pi[8]))) *
106  alpha;
107  return wp;
108 }
109 
110 // TODO
111 inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
112  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
113  std::cout << "NOT IMPLEMENTED YET" << std::endl;
114 
115  const double alpha = 1. / (T * T);
116  const double t2 = t * t;
117  const double t3 = t2 * t;
118  const double t4 = t3 * t;
119  const double t5 = t4 * t;
120  const double t6 = t5 * t;
121  const double t7 = t6 * t;
122  const double t8 = t7 * t;
123  // equation found with sympy
124  wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
125  (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
126  25200.0 * t3 + 2520.0 * t2)); // x0
127  wp.first.block<3, 3>(0, 3) =
128  Matrix3::Identity() * alpha *
129  (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3)); // x1
130  wp.first.block<3, 3>(0, 6) =
131  Matrix3::Identity() * alpha *
132  (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4)); // x2
133  wp.second =
134  (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
135  t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
136  t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
137  t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] + 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
138  4050.0 * pi[8] - 900.0 * pi[9])) +
139  t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
140  t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
141  t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
142  t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
143  2520.0 * pi[8])) +
144  t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
145  6480.0 * pi[8] + 720.0 * pi[9]))) *
146  alpha;
147  return wp;
148 }
149 
150 // TODO
151 inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
152  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
153  std::cout << "NOT IMPLEMENTED YET" << std::endl;
154 
155  const double alpha = 1. / (T * T * T);
156  const double t2 = t * t;
157  const double t3 = t2 * t;
158  const double t4 = t3 * t;
159  const double t5 = t4 * t;
160  const double t6 = t5 * t;
161  const double t7 = t6 * t;
162  // equation found with sympy
163  wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
164  (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
165  75600.0 * t2 + 5040.0 * t)); // x0
166  wp.first.block<3, 3>(0, 3) =
167  Matrix3::Identity() * alpha *
168  (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2)); // x1
169  wp.first.block<3, 3>(0, 6) =
170  Matrix3::Identity() * alpha *
171  (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3)); // x2
172  wp.second =
173  (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
174  t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
175  t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
176  t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
177  t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
178  t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
179  15120.0 * pi[8])) +
180  t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
181  45360.0 * pi[8] + 5040.0 * pi[9])) +
182  t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
183  86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
184  alpha;
185  return wp;
186 }
187 
188 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
189  // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
190  // waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
191  double n = 12.;
192  std::vector<point_t> pi;
193  pi.push_back(pData.c0_);
194  pi.push_back((pData.dc0_ * T / n) + pData.c0_);
195  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
196  pData.c0_); // * T because derivation make a T appear
197  pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
198  (3 * pData.dc0_ * T / n) + pData.c0_);
199  pi.push_back(point_t::Zero());
200  pi.push_back(point_t::Zero());
201  pi.push_back(point_t::Zero());
202  pi.push_back(point_t::Zero());
203  pi.push_back(point_t::Zero());
204  pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
205  (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
206  pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
207  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
208  pi.push_back(pData.c1_);
209  return pi;
210 }
211 
212 // TODO
213 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
214  bezier_wp_t::t_point_t wps;
215  //const int DIM_POINT = 6;
216  //const int DIM_VAR = 15;
217  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
218  std::vector<Matrix3> Cpi;
219  for (std::size_t i = 0; i < pi.size(); ++i) {
220  Cpi.push_back(skew(pi[i]));
221  }
222  //const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
223  //const Matrix3 Cg = skew(g);
224  //const double T2 = T * T;
225  //const double alpha = 1 / (T2);
226  std::cout << "NOT IMPLEMENTED YET" << std::endl;
227  return wps;
228 }
229 
230 std::vector<waypoint_t> computeVelocityWaypoints(
231  const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
232  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
233 
234  std::vector<waypoint_t> wps;
235  assert(pi.size() == 13);
236 
237  double alpha = 1. / (T);
238  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
239 
240  // assign w0:
241  w.second = alpha * 12 * (-pi[0] + pi[1]);
242  wps.push_back(w);
243  w = initwp(DIM_POINT, DIM_VAR);
244  // assign w1:
245  w.second = alpha * 12 * (-pi[1] + pi[2]);
246  wps.push_back(w);
247  w = initwp(DIM_POINT, DIM_VAR);
248  // assign w2:
249  w.second = alpha * 12 * (-pi[2] + pi[3]);
250  wps.push_back(w);
251  w = initwp(DIM_POINT, DIM_VAR);
252  // assign w3:
253  w.first.block<3, 3>(0, 0) = 12 * alpha * Matrix3::Identity(); // x0
254  w.second = alpha * 12 * (-pi[3]);
255  wps.push_back(w);
256  w = initwp(DIM_POINT, DIM_VAR);
257  // assign w4:
258  w.first.block<3, 3>(0, 0) = -12 * alpha * Matrix3::Identity(); // x0
259  w.first.block<3, 3>(0, 3) = 12 * alpha * Matrix3::Identity(); // x1
260  wps.push_back(w);
261  w = initwp(DIM_POINT, DIM_VAR);
262  // assign w5:
263  w.first.block<3, 3>(0, 3) = -12 * alpha * Matrix3::Identity(); // x1
264  w.first.block<3, 3>(0, 6) = 12 * alpha * Matrix3::Identity(); // x2
265  wps.push_back(w);
266  w = initwp(DIM_POINT, DIM_VAR);
267  // assign w6:
268  w.first.block<3, 3>(0, 6) = -12 * alpha * Matrix3::Identity(); // x2
269  w.first.block<3, 3>(0, 9) = 12 * alpha * Matrix3::Identity(); // x3
270  wps.push_back(w);
271  w = initwp(DIM_POINT, DIM_VAR);
272  // assign w7:
273  w.first.block<3, 3>(0, 9) = -12 * alpha * Matrix3::Identity(); // x3
274  w.first.block<3, 3>(0, 12) = 12 * alpha * Matrix3::Identity(); // x4
275  wps.push_back(w);
276  w = initwp(DIM_POINT, DIM_VAR);
277  // assign w8:
278  w.first.block<3, 3>(0, 12) = -12 * alpha * Matrix3::Identity(); // x4
279  w.second = alpha * 12 * pi[9];
280  wps.push_back(w);
281  w = initwp(DIM_POINT, DIM_VAR);
282  // assign w9:
283  w.second = alpha * 12 * (-pi[9] + pi[10]);
284  wps.push_back(w);
285  w = initwp(DIM_POINT, DIM_VAR);
286  // assign w10:
287  w.second = alpha * 12 * (-pi[10] + pi[11]);
288  wps.push_back(w);
289  w = initwp(DIM_POINT, DIM_VAR);
290  // assign w11:
291  w.second = alpha * 12 * (-pi[11] + pi[12]);
292  wps.push_back(w);
293  return wps;
294 }
295 
296 std::vector<waypoint_t> computeAccelerationWaypoints(
297  const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
298  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
299 
300  std::vector<waypoint_t> wps;
301  assert(pi.size() == 13);
302  double alpha = 1. / (T * T);
303 
304  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
305 
306  // assign w0:
307  w.second = alpha * 132 * (pi[0] - 2 * pi[1] + pi[2]);
308  wps.push_back(w);
309  w = initwp(DIM_POINT, DIM_VAR);
310  // assign w1:
311  w.second = alpha * 132 * (pi[1] - 2 * pi[2] + pi[3]);
312  wps.push_back(w);
313  w = initwp(DIM_POINT, DIM_VAR);
314  // assign w2:
315  w.first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity(); // x0
316  w.second = alpha * 132 * (pi[2] - pi[3]);
317  wps.push_back(w);
318  w = initwp(DIM_POINT, DIM_VAR);
319  // assign w3:
320  w.first.block<3, 3>(0, 0) = -264 * alpha * Matrix3::Identity(); // x0
321  w.first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity(); // x1
322  w.second = alpha * 132 * pi[3];
323  wps.push_back(w);
324  w = initwp(DIM_POINT, DIM_VAR);
325  // assign w4:
326  w.first.block<3, 3>(0, 0) = 132 * alpha * Matrix3::Identity(); // x0
327  w.first.block<3, 3>(0, 3) = -264 * alpha * Matrix3::Identity(); // x1
328  w.first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity(); // x2
329  wps.push_back(w);
330  w = initwp(DIM_POINT, DIM_VAR);
331  // assign w5:
332  w.first.block<3, 3>(0, 3) = 132 * alpha * Matrix3::Identity(); // x1
333  w.first.block<3, 3>(0, 6) = -264 * alpha * Matrix3::Identity(); // x2
334  w.first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity(); // x3
335  wps.push_back(w);
336  w = initwp(DIM_POINT, DIM_VAR);
337  // assign w6:
338  w.first.block<3, 3>(0, 6) = 132 * alpha * Matrix3::Identity(); // x2
339  w.first.block<3, 3>(0, 9) = -264 * alpha * Matrix3::Identity(); // x3
340  w.first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity(); // x4
341  wps.push_back(w);
342  w = initwp(DIM_POINT, DIM_VAR);
343  // assign w7:
344  w.first.block<3, 3>(0, 9) = 132 * alpha * Matrix3::Identity(); // x3
345  w.first.block<3, 3>(0, 12) = -264 * alpha * Matrix3::Identity(); // x4
346  w.second = alpha * 132 * pi[7];
347  wps.push_back(w);
348  w = initwp(DIM_POINT, DIM_VAR);
349  // assign w8:
350  w.first.block<3, 3>(0, 12) = 132 * alpha * Matrix3::Identity(); // x4
351  w.second = alpha * 132 * (-2 * pi[9] + pi[10]);
352  wps.push_back(w);
353  w = initwp(DIM_POINT, DIM_VAR);
354  // assign w9:
355  w.second = alpha * 132 * (pi[9] - 2 * pi[10] + pi[11]);
356  wps.push_back(w);
357  wps.push_back(w);
358  w = initwp(DIM_POINT, DIM_VAR);
359  // assign w10:
360  w.second = alpha * 132 * (pi[12] + pi[10] - 2 * pi[11]);
361  wps.push_back(w);
362  return wps;
363 }
364 
365 std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
366  std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
367  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
368 
369  std::vector<waypoint_t> wps;
370  assert(pi.size() == 13);
371 
372  double alpha = 1. / (T * T * T);
373 
374  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
375 
376  // assign w0:
377  w.second = alpha * 1320 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
378  wps.push_back(w);
379  w = initwp(DIM_POINT, DIM_VAR);
380  // assign w1:
381  w.first.block<3, 3>(0, 0) = 1320 * alpha * Matrix3::Identity(); // x0
382  w.second = alpha * 1320 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
383  wps.push_back(w);
384  w = initwp(DIM_POINT, DIM_VAR);
385  // assign w2:
386  w.first.block<3, 3>(0, 0) = 1320 * -3 * alpha * Matrix3::Identity(); // x0
387  w.first.block<3, 3>(0, 3) = 1320 * alpha * Matrix3::Identity(); // x1
388  w.second = alpha * 1320 * (-pi[2] + 3 * pi[3]);
389  wps.push_back(w);
390  w = initwp(DIM_POINT, DIM_VAR);
391  // assign w3:
392  w.first.block<3, 3>(0, 0) = 1320 * 3 * alpha * Matrix3::Identity(); // x0
393  w.first.block<3, 3>(0, 3) = 1320 * -3 * alpha * Matrix3::Identity(); // x1
394  w.first.block<3, 3>(0, 6) = 1320 * alpha * Matrix3::Identity(); // x2
395  w.second = alpha * 1320 * (-pi[3]);
396  wps.push_back(w);
397  w = initwp(DIM_POINT, DIM_VAR);
398  // assign w4:
399  w.first.block<3, 3>(0, 0) = -1320 * alpha * Matrix3::Identity(); // x0
400  w.first.block<3, 3>(0, 3) = 1320 * 3 * alpha * Matrix3::Identity(); // x1
401  w.first.block<3, 3>(0, 6) = 1320 * -3 * alpha * Matrix3::Identity(); // x2
402  w.first.block<3, 3>(0, 9) = 1320 * alpha * Matrix3::Identity(); // x3
403  wps.push_back(w);
404  w = initwp(DIM_POINT, DIM_VAR);
405  // assign w5:
406  w.first.block<3, 3>(0, 3) = -1320 * alpha * Matrix3::Identity(); // x1
407  w.first.block<3, 3>(0, 6) = 1320 * 3 * alpha * Matrix3::Identity(); // x2
408  w.first.block<3, 3>(0, 9) = 1320 * -3 * alpha * Matrix3::Identity(); // x3
409  w.first.block<3, 3>(0, 12) = 1320 * alpha * Matrix3::Identity(); // x4
410  wps.push_back(w);
411  w = initwp(DIM_POINT, DIM_VAR);
412  // assign w4:
413  w.first.block<3, 3>(0, 6) = -1320 * alpha * Matrix3::Identity(); // x2
414  w.first.block<3, 3>(0, 9) = 1320 * 3 * alpha * Matrix3::Identity(); // x3
415  w.first.block<3, 3>(0, 12) = 1320 * -3 * alpha * Matrix3::Identity(); // x4
416  w.second = alpha * 1320 * pi[9];
417  wps.push_back(w);
418  w = initwp(DIM_POINT, DIM_VAR);
419  // assign w5:
420  w.first.block<3, 3>(0, 9) = -1320 * alpha * Matrix3::Identity(); // x3
421  w.first.block<3, 3>(0, 12) = 1320 * 3 * alpha * Matrix3::Identity(); // x4
422  w.second = alpha * 1320 * (-3 * pi[9] + pi[10]);
423  wps.push_back(w);
424  w = initwp(DIM_POINT, DIM_VAR);
425  // assign w5:
426  w.first.block<3, 3>(0, 12) = -1320 * alpha * Matrix3::Identity(); // x6
427  w.second = alpha * 1320 * (3 * pi[9] - 3 * pi[10] + pi[11]);
428  wps.push_back(w);
429  w = initwp(DIM_POINT, DIM_VAR);
430  // assign w6:
431  w.second = alpha * 1320 * (pi[12] - pi[9] + 3 * pi[10] - 3 * pi[11]);
432  wps.push_back(w);
433  return wps;
434 }
435 
436 // TODO
437 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
438  coefs_t v;
439  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
440  // equation found with sympy
441  v.first = 0.;
442  v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
443  return v;
444 }
445 
446 // TODO
447 inline std::pair<MatrixXX, VectorX> computeVelocityCost(
448  const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
449  MatrixXX H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
450  VectorX g = VectorX::Zero(DIM_VAR);
451  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
452 
453  g.segment<3>(0) = ((-17.8646615739593 * pi[0] - 4.24835843773412 * pi[10] - 1.80981866649436 * pi[11] -
454  0.408668730537654 * pi[12] - 13.8947369836412 * pi[1] + 2.56878303943036 * pi[2] +
455  16.0548432515434 * pi[3] - 6.66893486967885 * pi[9])) /
456  (2 * T); // x0
457  g.segment<3>(3) =
458  ((-7.93984965058761 * pi[0] - 7.0641309185535 * pi[10] - 4.08668730511085 * pi[11] - 1.22600619206665 * pi[12] -
459  12.1432972410894 * pi[1] - 7.06413670827152 * pi[2] + 3.85315840674136 * pi[3] - 7.50872663647158 * pi[9])) /
460  (2 * T); // x1
461  g.segment<3>(6) =
462  (-3.26934980716442 * pi[0] - 8.9907120599184 * pi[10] - 7.76470588258269 * pi[11] - 3.26934984520124 * pi[12] -
463  7.76470597007188 * pi[1] - 8.99071211730055 * pi[2] - 4.49535589801788 * pi[3] - 4.49535607858364 * pi[9]) /
464  (2 * T); // x2
465  g.segment<3>(9) =
466  (-1.22600620726636 * pi[0] - 7.06413092270385 * pi[10] - 12.1432994250704 * pi[11] - 7.93984962409094 * pi[12] -
467  4.08668774398579 * pi[1] - 7.0641311269266 * pi[2] - 7.50872489092664 * pi[3] + 3.85316232209763 * pi[9]) /
468  (2 * T); // x3
469  g.segment<3>(12) =
470  (-0.408668732514974 * pi[0] + 2.56877487851457 * pi[10] - 13.8947368423667 * pi[11] - 17.8646616541281 * pi[12] -
471  1.80981880873492 * pi[1] - 4.2483587965255 * pi[2] - 6.66893350792178 * pi[3] + 16.0548429731073 * pi[9]) /
472  (2 * T); // x4
473 
474  H.block<3, 3>(0, 0) = Matrix3::Identity() * 9.63290527229048 / (T); // x0^2
475  H.block<3, 3>(3, 3) = Matrix3::Identity() * 8.29911962311903 / (T); // x1^2
476  H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.92188615942945 / (T); // x2^2
477  H.block<3, 3>(9, 9) = Matrix3::Identity() * 8.29911871865983 / (T); // x3^2
478  H.block<3, 3>(12, 12) = Matrix3::Identity() * 9.63290582796267 / (T); // x4^2
479 
480  H.block<3, 3>(0, 3) = Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
481  H.block<3, 3>(3, 0) = Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
482  H.block<3, 3>(0, 6) = Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
483  H.block<3, 3>(6, 0) = Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
484  H.block<3, 3>(0, 9) = Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
485  H.block<3, 3>(9, 0) = Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
486  H.block<3, 3>(0, 12) = Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
487  H.block<3, 3>(12, 0) = Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
488 
489  H.block<3, 3>(3, 6) = Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
490  H.block<3, 3>(6, 3) = Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
491  H.block<3, 3>(3, 9) = Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
492  H.block<3, 3>(9, 3) = Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
493  H.block<3, 3>(3, 12) = Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
494  H.block<3, 3>(12, 3) = Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
495 
496  H.block<3, 3>(6, 9) = Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
497  H.block<3, 3>(9, 6) = Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
498  H.block<3, 3>(6, 12) = Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
499  H.block<3, 3>(12, 6) = Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
500 
501  H.block<3, 3>(9, 12) = Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
502  H.block<3, 3>(12, 9) = Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
503 
504  double norm = H.norm();
505  H /= norm;
506  g /= norm;
507 
508  return std::make_pair(H, g);
509 }
510 
511 } // namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1
512 
513 } // namespace bezier_com_traj
514 
515 #endif // WAYPOINTS_C0_DC0_DDC0_J0_J1_DDC1_DC1_C1_HH
FIVE_FREE_VAR
Definition: flags.hh:32
point_t j0_
Definition: data.hh:103
point_t ddc0_
Definition: data.hh:103
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:296
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:151
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:365
VectorX second
Definition: utils.hh:29
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:437
INIT_VEL
Definition: flags.hh:21
Definition: utils.hh:27
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:188
centroidal_dynamics::VectorX VectorX
Definition: definitions.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_x5_j1_ddc1_dc1_c1.hh:28
END_ACC
Definition: flags.hh:25
point_t c0_
Definition: data.hh:103
MatrixXX first
Definition: utils.hh:28
END_POS
Definition: flags.hh:23
point_t dc1_
Definition: data.hh:103
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:61
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:447
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:103
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh:213
END_JERK
Definition: flags.hh:27
point_t ddc1_
Definition: data.hh:103
point_t j1_
Definition: data.hh:103
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:111
END_VEL
Definition: flags.hh:24
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:20
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:73
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:88
Definition: common_solve_methods.hh:16
INIT_POS
Definition: flags.hh:20
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition: utils.cpp:56
point_t c1_
Definition: data.hh:103
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:230
const int DIM_POINT
Definition: solve_end_effector.hh:15