GCC Code Coverage Report

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