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
12namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1 {
16static const size_t DIM_VAR = 3;
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;
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 -
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;
70 (560.0 *
t7 - 1960.0 *
t6 + 2520.0 *
t5 - 1400.0 *
t4 + 280.0 *
t3) *
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) *
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;
100 ((3920.0 *
t6 - 11760.0 *
t5 + 12600.0 *
t4 - 5600.0 *
t3 + 840.0 *
t2)) *
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) *
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;
129 (23520.0 *
t5 - 58800.0 *
t4 + 50400.0 *
t3 - 16800.0 *
t2 + 1680.0 *
t) *
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) *
157 const std::vector<point_t>&
pi,
double T,
double t) {
165 const std::vector<point_t>&
pi,
double T,
double t) {
174 const std::vector<point_t>&
pi,
double T,
double t) {
188 std::vector<point_t>
pi;
191 pi.push_back((
pData.ddc0_ *
T *
T / (
n * (
n - 1))) +
194 pi.push_back((
pData.j0_ *
T *
T *
T / (
n * (
n - 1) * (
n - 2))) +
195 (3 *
pData.ddc0_ *
T *
T / (
n * (
n - 1))) +
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))) -
201 pi.push_back((
pData.ddc1_ *
T *
T / (
n * (
n - 1))) -
210 bezier_wp_t::t_point_t
wps;
212 const int DIM_VAR = 3;
214 std::vector<Matrix3>
Cpi;
215 for (std::size_t
i = 0;
i <
pi.size(); ++
i) {
218 const Vector3 g =
pData.contacts_.front().contactPhase_->m_gravity;
220 const double T2 =
T *
T;
225 w0.second.head<3>() = (30 *
pi[0] - 60 *
pi[1] + 30 *
pi[2]) *
alpha;
226 w0.second.tail<3>() =
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]) *
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>() =
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]) *
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]) *
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]) *
266 w3.second.tail<3>() =
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]) *
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]) *
283 w4.second.tail<3>() =
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]) *
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]) *
301 w5.second.tail<3>() =
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]) *
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]) *
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]) *
322 w6.second.tail<3>() =
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]) *
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>() =
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]) *
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>() =
349 (-9.99999999999999 *
pi[4] - 20.0 *
pi[5] + 16.6666666666667 *
pi[6]) *
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]) *
358 w9.second.head<3>() = (30 *
pi[4] - 60 *
pi[5] + 30 *
pi[6]) *
alpha;
359 w9.second.tail<3>() =
369 std::vector<bezier_t::point_t>
pi = std::vector<bezier_t::point_t>()) {
372 std::vector<waypoint_t>
wps;
390 w.first = 8 *
alpha * Matrix3::Identity();
395 w.first = -8 *
alpha * Matrix3::Identity();
415 std::vector<bezier_t::point_t>
pi = std::vector<bezier_t::point_t>()) {
418 std::vector<waypoint_t>
wps;
433 w.first = 56 *
alpha * Matrix3::Identity();
434 w.second = (56 *
pi[2] - 112 *
pi[3]) *
alpha;
438 w.first = -112 *
alpha * Matrix3::Identity();
443 w.first = 56 *
alpha * Matrix3::Identity();
444 w.second = (-112 *
pi[5] + 56 *
pi[6]) *
alpha;
459 std::vector<bezier_t::point_t>
pi = std::vector<bezier_t::point_t>()) {
462 std::vector<waypoint_t>
wps;
470 w.second = 336 * (-
pi[0] + 3 *
pi[1] - 3 *
pi[2] +
pi[3]) *
alpha;
474 w.first = 336 *
alpha * Matrix3::Identity();
475 w.second = 336 * (-
pi[1] + 3 *
pi[2] - 3 *
pi[3]) *
alpha;
479 w.first = -3 * 336 *
alpha * Matrix3::Identity();
484 w.first = 3 * 336 *
alpha * Matrix3::Identity();
489 w.first = -336 *
alpha * Matrix3::Identity();
490 w.second = 336 * (3 *
pi[5] - 3 *
pi[6] +
pi[7]) *
alpha;
494 w.second = 336 * (-
pi[5] + 3 *
pi[6] - 3 *
pi[7] +
pi[8]) *
alpha;
504 v.second = (-6.0 *
pi[5] + 6.0 *
pi[6]) /
T;
510 std::vector<bezier_t::point_t>
pi = std::vector<bezier_t::point_t>()) {
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]) /
519 H = Matrix3::Identity() * 6.52680652684107 / (
T);
521 double norm =
H.norm();
525 return std::make_pair(
H,
g);
END_ACC
Definition flags.hh:25
INIT_VEL
Definition flags.hh:21
END_VEL
Definition flags.hh:24
END_POS
Definition flags.hh:23
INIT_ACC
Definition flags.hh:22
INIT_POS
Definition flags.hh:20
INIT_JERK
Definition flags.hh:26
END_JERK
Definition flags.hh:27
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
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
waypoint_t evaluateCurveWaypointAtTime(const std::vector< point_t > &pi, double t)
Definition waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:148
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
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:208
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
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:182
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
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
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
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
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
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh:499
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
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
Definition common_solve_methods.hh:15
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
centroidal_dynamics::VectorX VectorX
Definition definitions.hh:24
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition utils.cpp:62
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition definitions.hh:17
const int DIM_POINT
Definition solve_end_effector.hh:15
centroidal_dynamics::Vector3 Vector3
Definition definitions.hh:22
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
std::pair< double, point3_t > coefs_t
Definition definitions.hh:62
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
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
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
std::pair< MatrixXX, VectorX > computeDistanceCostFunction(size_t numPoints, const ProblemData &pData, double T, std::vector< point3_t > pts_path)
Definition solve_end_effector.hh:224
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
MatrixXX first
Definition utils.hh:26