hpp-bezier-com-traj  4.12.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 =
16 static const size_t DIM_VAR = 9;
17 static const size_t DIM_POINT = 3;
21 
28 // TODO
29 inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
30  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
31  const double t2 = t * t;
32  const double t3 = t2 * t;
33  const double t4 = t3 * t;
34  const double t5 = t4 * t;
35  const double t6 = t5 * t;
36  const double t7 = t6 * t;
37  const double t8 = t7 * t;
38  const double t9 = t8 * t;
39  const double t10 = t9 * t;
40 
41  // equation found with sympy
42  wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * (t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 +
43  3150.0 * t8 - 1260.0 * t9 + 210.0 * t10); // x0
44  wp.first.block<3, 3>(0, 3) =
45  Matrix3::Identity() * (252.0 * t5 - 1260.0 * t6 + 2520.0 * t7 - 2520.0 * t8 + 1260.0 * t9 - 252.0 * t10); // x1
46  wp.first.block<3, 3>(0, 6) =
47  Matrix3::Identity() * (210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10); // x2
48  wp.second = 1.0 * pi[0] + t * (-10.0 * pi[0] + 10.0 * pi[1]) + t2 * (45.0 * pi[0] - 90.0 * pi[1] + 45.0 * pi[2]) +
49  t3 * (-120.0 * pi[0] + 360.0 * pi[1] - 360.0 * pi[2] + 120.0 * pi[3]) +
50  t4 * (210.0 * pi[0] - 840.0 * pi[1] + 1260.0 * pi[2] - 840.0 * pi[3]) +
51  t5 * (-252.0 * pi[0] + 1260.0 * pi[1] - 2520.0 * pi[2] + 2520.0 * pi[3]) +
52  t6 * (210.0 * pi[0] - 1260.0 * pi[1] + 3150.0 * pi[2] - 4200.0 * pi[3]) +
53  t7 * (-120.0 * pi[0] + 840.0 * pi[1] - 2520.0 * pi[2] + 4200.0 * pi[3] + 120.0 * pi[7]) +
54  t8 * (45.0 * pi[0] - 360.0 * pi[1] + 1260.0 * pi[2] - 2520.0 * pi[3] - 360.0 * pi[7] + 45.0 * pi[8]) +
55  t9 * (-10.0 * pi[0] + 90.0 * pi[1] - 360.0 * pi[2] + 840.0 * pi[3] + 360.0 * pi[7] - 90.0 * pi[8] +
56  10.0 * pi[9]) +
57  t10 * (1.0 * pi[0] + 1.0 * pi[10] - 10.0 * pi[1] + 45.0 * pi[2] - 120.0 * pi[3] - 120.0 * pi[7] +
58  45.0 * pi[8] - 10.0 * pi[9]);
59  return wp;
60 }
61 
62 // TODO
63 inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
64  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
65  const double alpha = 1. / (T);
66  const double t2 = t * t;
67  const double t3 = t2 * t;
68  const double t4 = t3 * t;
69  const double t5 = t4 * t;
70  const double t6 = t5 * t;
71  const double t7 = t6 * t;
72  const double t8 = t7 * t;
73  const double t9 = t8 * t;
74  // equation found with sympy
75  wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
76  (1.0 * (2100.0 * t9 - 11340.0 * t8 + 25200.0 * t7 - 29400.0 * t6 + 18900.0 * t5 -
77  6300.0 * t4 + 840.0 * t3)); // x0
78  wp.first.block<3, 3>(0, 3) =
79  Matrix3::Identity() * alpha *
80  (1.0 * (-2520.0 * t9 + 11340.0 * t8 - 20160.0 * t7 + 17640.0 * t6 - 7560.0 * t5 + 1260.0 * t4)); // x1
81  wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha *
82  (1.0 * (2100.0 * t9 - 7560.0 * t8 + 10080.0 * t7 - 5880.0 * t6 + 1260.0 * t5)); // x2
83  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])) +
84  t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
85  t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
86  810.0 * pi[8] + 90.0 * pi[9])) +
87  t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
88  t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
89  1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
90  t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
91  t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
92  t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
93  t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
94  360.0 * pi[8]))) *
95  alpha;
96  return wp;
97 }
98 
99 // TODO
100 inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
101  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
102  const double alpha = 1. / (T * T);
103  const double t2 = t * t;
104  const double t3 = t2 * t;
105  const double t4 = t3 * t;
106  const double t5 = t4 * t;
107  const double t6 = t5 * t;
108  const double t7 = t6 * t;
109  const double t8 = t7 * t;
110  // equation found with sympy
111  wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
112  (1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
113  25200.0 * t3 + 2520.0 * t2)); // x0
114  wp.first.block<3, 3>(0, 3) =
115  Matrix3::Identity() * alpha *
116  (1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3)); // x1
117  wp.first.block<3, 3>(0, 6) =
118  Matrix3::Identity() * alpha *
119  (1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4)); // x2
120  wp.second =
121  (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
122  t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
123  t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
124  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] +
125  4050.0 * pi[8] - 900.0 * pi[9])) +
126  t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
127  t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
128  t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
129  t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
130  2520.0 * pi[8])) +
131  t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
132  6480.0 * pi[8] + 720.0 * pi[9]))) *
133  alpha;
134  return wp;
135 }
136 
137 // TODO
138 inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
139  waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
140  const double alpha = 1. / (T * T * T);
141  const double t2 = t * t;
142  const double t3 = t2 * t;
143  const double t4 = t3 * t;
144  const double t5 = t4 * t;
145  const double t6 = t5 * t;
146  const double t7 = t6 * t;
147  // equation found with sympy
148  wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
149  (1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
150  75600.0 * t2 + 5040.0 * t)); // x0
151  wp.first.block<3, 3>(0, 3) =
152  Matrix3::Identity() * alpha *
153  (1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2)); // x1
154  wp.first.block<3, 3>(0, 6) =
155  Matrix3::Identity() * alpha *
156  (1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3)); // x2
157  wp.second =
158  (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
159  t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
160  t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
161  t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
162  t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
163  t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
164  15120.0 * pi[8])) +
165  t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
166  45360.0 * pi[8] + 5040.0 * pi[9])) +
167  t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
168  86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
169  alpha;
170  return wp;
171 }
172 
173 inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
174  // equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
175  // waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
176  double n = 10.;
177  std::vector<point_t> pi;
178  pi.push_back(pData.c0_);
179  pi.push_back((pData.dc0_ * T / n) + pData.c0_);
180  pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
181  pData.c0_); // * T because derivation make a T appear
182  pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
183  (3 * pData.dc0_ * T / n) + pData.c0_);
184  pi.push_back(point_t::Zero());
185  pi.push_back(point_t::Zero());
186  pi.push_back(point_t::Zero());
187  pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
188  (3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
189  pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
190  pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
191  pi.push_back(pData.c1_);
192  return pi;
193 }
194 
195 // TODO
196 inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
197  bezier_wp_t::t_point_t wps;
198  //const int DIM_POINT = 6;
199  //const int DIM_VAR = 9;
200  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
201  std::vector<Matrix3> Cpi;
202  for (std::size_t i = 0; i < pi.size(); ++i) {
203  Cpi.push_back(skew(pi[i]));
204  }
205  //const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
206  //const Matrix3 Cg = skew(g);
207  //const double T2 = T * T;
208  //const double alpha = 1 / (T2);
209  std::cout << "NOT IMPLEMENTED YET" << std::endl;
210  return wps;
211 }
212 
213 std::vector<waypoint_t> computeVelocityWaypoints(
214  const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
215  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
216 
217  std::vector<waypoint_t> wps;
218  assert(pi.size() == 11);
219 
220  double alpha = 1. / (T);
221  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
222 
223  // assign w0:
224  w.second = alpha * 10 * (-pi[0] + pi[1]);
225  wps.push_back(w);
226  w = initwp(DIM_POINT, DIM_VAR);
227  // assign w1:
228  w.second = alpha * 10 * (-pi[1] + pi[2]);
229  wps.push_back(w);
230  w = initwp(DIM_POINT, DIM_VAR);
231  // assign w2:
232  w.second = alpha * 10 * (-pi[2] + pi[3]);
233  wps.push_back(w);
234  w = initwp(DIM_POINT, DIM_VAR);
235  // assign w3:
236  w.first.block<3, 3>(0, 0) = 10 * alpha * Matrix3::Identity(); // x0
237  w.second = alpha * 10 * (-pi[3]);
238  wps.push_back(w);
239  w = initwp(DIM_POINT, DIM_VAR);
240  // assign w4:
241  w.first.block<3, 3>(0, 0) = -10 * alpha * Matrix3::Identity(); // x0
242  w.first.block<3, 3>(0, 3) = 10 * alpha * Matrix3::Identity(); // x1
243  wps.push_back(w);
244  w = initwp(DIM_POINT, DIM_VAR);
245  // assign w5:
246  w.first.block<3, 3>(0, 3) = -10 * alpha * Matrix3::Identity(); // x1
247  w.first.block<3, 3>(0, 6) = 10 * alpha * Matrix3::Identity(); // x2
248  wps.push_back(w);
249  w = initwp(DIM_POINT, DIM_VAR);
250  // assign w6:
251  w.first.block<3, 3>(0, 6) = -10 * alpha * Matrix3::Identity(); // x2
252  w.second = alpha * 10 * pi[7];
253  wps.push_back(w);
254  w = initwp(DIM_POINT, DIM_VAR);
255  // assign w7:
256  w.second = alpha * 10 * (-pi[7] + pi[8]);
257  wps.push_back(w);
258  w = initwp(DIM_POINT, DIM_VAR);
259  // assign w8:
260  w.second = alpha * 10 * (-pi[8] + pi[9]);
261  wps.push_back(w);
262  w = initwp(DIM_POINT, DIM_VAR);
263  // assign w9:
264  w.second = alpha * 10 * (pi[10] - pi[9]);
265  wps.push_back(w);
266  return wps;
267 }
268 
269 std::vector<waypoint_t> computeAccelerationWaypoints(
270  const ProblemData& pData, const double T, 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  double alpha = 1. / (T * T);
276 
277  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
278 
279  // assign w0:
280  w.second = alpha * 90 * (pi[0] - 2 * pi[1] + pi[2]);
281  wps.push_back(w);
282  w = initwp(DIM_POINT, DIM_VAR);
283  // assign w1:
284  w.second = alpha * 90 * (pi[1] - 2 * pi[2] + pi[3]);
285  wps.push_back(w);
286  w = initwp(DIM_POINT, DIM_VAR);
287  // assign w2:
288  w.first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity(); // x0
289  w.second = alpha * 90 * (pi[2] - pi[3]);
290  wps.push_back(w);
291  w = initwp(DIM_POINT, DIM_VAR);
292  // assign w3:
293  w.first.block<3, 3>(0, 0) = -180 * alpha * Matrix3::Identity(); // x0
294  w.first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity(); // x1
295  w.second = alpha * 90 * pi[3];
296  wps.push_back(w);
297  w = initwp(DIM_POINT, DIM_VAR);
298  // assign w4:
299  w.first.block<3, 3>(0, 0) = 90 * alpha * Matrix3::Identity(); // x0
300  w.first.block<3, 3>(0, 3) = -180 * alpha * Matrix3::Identity(); // x1
301  w.first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity(); // x2
302  wps.push_back(w);
303  w = initwp(DIM_POINT, DIM_VAR);
304  // assign w5:
305  w.first.block<3, 3>(0, 3) = 90 * alpha * Matrix3::Identity(); // x1
306  w.first.block<3, 3>(0, 6) = -180 * alpha * Matrix3::Identity(); // x2
307  w.second = alpha * 90 * pi[7];
308  wps.push_back(w);
309  w = initwp(DIM_POINT, DIM_VAR);
310  // assign w6:
311  w.first.block<3, 3>(0, 6) = 90 * alpha * Matrix3::Identity(); // x2
312  w.second = alpha * 90 * (-2 * pi[7] + pi[8]);
313  wps.push_back(w);
314  w = initwp(DIM_POINT, DIM_VAR);
315  // assign w7:
316  w.second = alpha * 90 * (pi[7] - 2 * pi[8] + pi[9]);
317  wps.push_back(w);
318  wps.push_back(w);
319  w = initwp(DIM_POINT, DIM_VAR);
320  // assign w8:
321  w.second = alpha * 90 * (pi[10] + pi[8] - 2 * pi[9]);
322  wps.push_back(w);
323  return wps;
324 }
325 
326 std::vector<waypoint_t> computeJerkWaypoints(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 
333  double alpha = 1. / (T * T * T);
334 
335  waypoint_t w = initwp(DIM_POINT, DIM_VAR);
336 
337  // assign w0:
338  w.second = alpha * 720 * (-pi[0] + 3 * pi[1] - 3 * pi[2] + pi[3]);
339  wps.push_back(w);
340  w = initwp(DIM_POINT, DIM_VAR);
341  // assign w1:
342  w.first.block<3, 3>(0, 0) = 720 * alpha * Matrix3::Identity(); // x0
343  w.second = alpha * 720 * (-pi[1] + 3 * pi[2] - 3 * pi[3]);
344  wps.push_back(w);
345  w = initwp(DIM_POINT, DIM_VAR);
346  // assign w2:
347  w.first.block<3, 3>(0, 0) = 720 * -3 * alpha * Matrix3::Identity(); // x0
348  w.first.block<3, 3>(0, 3) = 720 * alpha * Matrix3::Identity(); // x1
349  w.second = alpha * 720 * (-pi[2] + 3 * pi[3]);
350  wps.push_back(w);
351  w = initwp(DIM_POINT, DIM_VAR);
352  // assign w3:
353  w.first.block<3, 3>(0, 0) = 720 * 3 * alpha * Matrix3::Identity(); // x0
354  w.first.block<3, 3>(0, 3) = 720 * -3 * alpha * Matrix3::Identity(); // x1
355  w.first.block<3, 3>(0, 6) = 720 * alpha * Matrix3::Identity(); // x2
356  w.second = alpha * 720 * (-pi[3]);
357  wps.push_back(w);
358  w = initwp(DIM_POINT, DIM_VAR);
359  // assign w4:
360  w.first.block<3, 3>(0, 0) = -720 * alpha * Matrix3::Identity(); // x0
361  w.first.block<3, 3>(0, 3) = 720 * 3 * alpha * Matrix3::Identity(); // x1
362  w.first.block<3, 3>(0, 6) = 720 * -3 * alpha * Matrix3::Identity(); // x2
363  w.second = alpha * 720 * pi[7];
364  wps.push_back(w);
365  w = initwp(DIM_POINT, DIM_VAR);
366  // assign w5:
367  w.first.block<3, 3>(0, 3) = -720 * alpha * Matrix3::Identity(); // x1
368  w.first.block<3, 3>(0, 6) = 720 * 3 * alpha * Matrix3::Identity(); // x2
369  w.second = alpha * 720 * (-3 * pi[7] + pi[8]);
370  wps.push_back(w);
371  w = initwp(DIM_POINT, DIM_VAR);
372  // assign w5:
373  w.first.block<3, 3>(0, 6) = -720 * alpha * Matrix3::Identity(); // x2
374  w.second = alpha * 720 * (3 * pi[7] - 3 * pi[8] + pi[9]);
375  wps.push_back(w);
376  w = initwp(DIM_POINT, DIM_VAR);
377  // assign w6:
378  w.second = alpha * 720 * (pi[10] - pi[7] + 3 * pi[8] - 3 * pi[9]);
379  wps.push_back(w);
380  return wps;
381 }
382 
383 // TODO
384 inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
385  coefs_t v;
386  std::vector<point_t> pi = computeConstantWaypoints(pData, T);
387  // equation found with sympy
388  v.first = 0.;
389  v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
390  return v;
391 }
392 
393 inline std::pair<MatrixXX, VectorX> computeVelocityCost(
394  const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
395  MatrixXX H = MatrixXX::Zero(9, 9);
396  VectorX g = VectorX::Zero(9);
397  if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
398 
399  g.segment<3>(0) =
400  (-12.352941184069 * pi[0] - 2.03619909502433 * pi[10] - 10.5882353430148 * pi[1] + 1.2217194516605 * pi[2] +
401  12.2171947000329 * pi[3] - 4.66474701697538 * pi[7] - 7.21925133730399 * pi[8] - 5.42986425333795 * pi[9]) /
402  (2 * T); // x0
403  g.segment<3>(3) =
404  (-5.29411764601331 * pi[0] - 5.29411764705762 * pi[10] - 8.95927605247282 * pi[1] - 6.10859723220821 * pi[2] +
405  2.2213080007358 * pi[3] + 2.22130810120924 * pi[7] - 6.10859728485633 * pi[8] - 8.95927601808432 * pi[9]) /
406  (2 * T); // x1
407  g.segment<3>(6) =
408  (-2.03619909557297 * pi[0] - 12.3529411764706 * pi[10] - 5.42986425052241 * pi[1] - 7.21925133714926 * pi[2] -
409  4.66474700749421 * pi[3] + 12.2171945706055 * pi[7] + 1.22171945695766 * pi[8] - 10.5882352941172 * pi[9]) /
410  (2 * T); // x2
411 
412  H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T); // x0^2
413  H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T); // x1^2
414  H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T); // x2^2
415  H.block<3, 3>(0, 3) = Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
416  H.block<3, 3>(3, 0) = Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
417  H.block<3, 3>(0, 6) = Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
418  H.block<3, 3>(6, 0) = Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
419  H.block<3, 3>(3, 6) = Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
420  H.block<3, 3>(6, 3) = Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
421 
422  double norm = H.norm();
423  H /= norm;
424  g /= norm;
425 
426  return std::make_pair(H, g);
427 }
428 
429 } // namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1
430 
431 } // namespace bezier_com_traj
432 
433 #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:100
point_t j0_
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_x3_j1_ddc1_dc1_c1.hh:213
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:384
point_t ddc0_
Definition: data.hh:103
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:63
VectorX second
Definition: utils.hh:29
INIT_VEL
Definition: flags.hh:21
Definition: utils.hh:27
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:23
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:269
THREE_FREE_VAR
Definition: flags.hh:30
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
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:29
point_t dc1_
Definition: data.hh:103
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:61
INIT_JERK
Definition: flags.hh:26
INIT_ACC
Definition: flags.hh:22
point_t dc0_
Definition: data.hh:103
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition: waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh:173
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:196
point_t ddc1_
Definition: data.hh:103
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:138
point_t j1_
Definition: data.hh:103
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:393
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:326
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:20
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
const int DIM_POINT
Definition: solve_end_effector.hh:15