GCC Code Coverage Report

Line	Branch	Exec	Source
1			/*
2			* Copyright 2017, LAAS-CNRS
3			* Author: Pierre Fernbach
4			*/
5
6			#include <hpp/bezier-com-traj/common_solve_methods.hh>
7			#include <hpp/bezier-com-traj/solve.hh>
8			#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
9			#include <limits>
10
11			using namespace bezier_com_traj;
12
13			namespace bezier_com_traj {
14			typedef std::pair<double, point3_t> coefs_t;
15			const int DIM_POINT = 3;
16			// const int NUM_DISCRETIZATION = 11;
17			const bool verbose = false;
18
19			/**
20			* @brief solveEndEffector Tries to produce a trajectory represented as a bezier
21			* curve that satisfy position, velocity and acceleration constraint for the
22			* initial and final point and that follow as close as possible the input
23			* trajectory
24			* @param pData problem Data.
25			* @param path the path to follow, the class Path must implement the operator
26			* (double t) , t \in [0,1] return a Vector3 that give the position on the path
27			* for a given time
28			* @param T time lenght of the trajectory
29			* @param timeStep time that the solver has to stop
30			* @return ResultData a struct containing the resulting trajectory, if success
31			* is true.
32			*/
33			template <typename Path>
34			ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path,
35			const double T, const double weightDistance,
36			bool useVelCost = true);
37
38		✗	coefs_t initCoefs() {
39		✗	coefs_t c;
40		✗	c.first = 0;
41		✗	c.second = point3_t::Zero();
42		✗	return c;
43			}
44
45			// up to jerk second derivativ constraints for init, pos vel and acc constraint
46			// for goal
47		✗	std::vector<bezier_t::point_t> computeConstantWaypointsInitPredef(
48			const ProblemData& pData, double T) {
49		✗	const double n = 4;
50		✗	std::vector<bezier_t::point_t> pts;
51		✗	pts.push_back(pData.c0_); // c0
52		✗	pts.push_back((pData.dc0_ * T / n) + pData.c0_); // dc0
53		✗	pts.push_back(
54		✗	(n * n * pData.c0_ - n * pData.c0_ + 2 * n * pData.dc0_ * T -
55		✗	2 * pData.dc0_ * T + pData.ddc0_ * T * T) /
56		✗	(n * (n - 1))); // ddc0 // * T because derivation make a T appear
57		✗	pts.push_back((n * n * pData.c0_ - n * pData.c0_ + 3 * n * pData.dc0_ * T -
58		✗	3 * pData.dc0_ * T + 3 * pData.ddc0_ * T * T) /
59		✗	(n * (n - 1))); // j0
60			// pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 4*n*pData.dc0_*T -
61			// 4*pData.dc0_ *T+ 6*pData.ddc0_*T*T)/(n*(n - 1))) ; //dj0
62			// pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 5*n*pData.dc0_*T -
63			// 5*pData.dc0_ *T+ 10*pData.ddc0_*T*T)/(n*(n - 1))) ; //ddj0
64
65			// pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 2*n*pData.dc1_*T +
66			// 2*pData.dc1_*T + pData.ddc1_*T*T)/(n*(n - 1))) ;
67			// //ddc1 // * T ?? pts.push_back((-pData.dc1_ * T / n) + pData.c1_); // dc1
68		✗	pts.push_back(pData.c1_); // c1
69		✗	return pts;
70		✗	}
71
72			// up to jerk second derivativ constraints for goal, pos vel and acc constraint
73			// for init
74		✗	std::vector<bezier_t::point_t> computeConstantWaypointsGoalPredef(
75			const ProblemData& pData, double T) {
76		✗	const double n = 4;
77		✗	std::vector<bezier_t::point_t> pts;
78		✗	pts.push_back(pData.c0_); // c0
79			// pts.push_back((pData.dc0_ * T / n )+ pData.c0_); //dc0
80			// pts.push_back((n*n*pData.c0_ - n*pData.c0_ + 2*n*pData.dc0_*T -
81			// 2*pData.dc0_*T + pData.ddc0_*T*T)/(n*(n - 1)));
82			// //ddc0 // * T because derivation make a T appear
83
84			// pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 5*n*pData.dc1_*T +
85			// 5*pData.dc1_*T + 10*pData.ddc1_*T*T)/(n*(n - 1))) ; //ddj1
86			// pts.push_back((n*n*pData.c1_ - n*pData.c1_ - 4*n*pData.dc1_*T +
87			// 4*pData.dc1_*T + 6*pData.ddc1_*T*T)/(n*(n - 1))) ; //dj1
88		✗	pts.push_back((n * n * pData.c1_ - n * pData.c1_ - 3 * n * pData.dc1_ * T +
89		✗	3 * pData.dc1_ * T + 3 * pData.ddc1_ * T * T) /
90		✗	(n * (n - 1))); // j1
91		✗	pts.push_back((n * n * pData.c1_ - n * pData.c1_ - 2 * n * pData.dc1_ * T +
92		✗	2 * pData.dc1_ * T + pData.ddc1_ * T * T) /
93		✗	(n * (n - 1))); // ddc1 * T ??
94		✗	pts.push_back((-pData.dc1_ * T / n) + pData.c1_); // dc1
95		✗	pts.push_back(pData.c1_); // c1
96		✗	return pts;
97		✗	}
98
99		✗	void computeConstraintsMatrix(
100			const ProblemData& pData, const std::vector<waypoint_t>& wps_acc,
101			const std::vector<waypoint_t>& wps_vel, const VectorX& acc_bounds,
102			const VectorX& vel_bounds, MatrixXX& A, VectorX& b,
103			const std::vector<waypoint_t>& wps_jerk = std::vector<waypoint_t>(),
104			const VectorX& jerk_bounds = VectorX(DIM_POINT)) {
105		✗	assert(acc_bounds.size() == DIM_POINT &&
106			"Acceleration bounds should have the same dimension as the points");
107		✗	assert(vel_bounds.size() == DIM_POINT &&
108			"Velocity bounds should have the same dimension as the points");
109		✗	assert(jerk_bounds.size() == DIM_POINT &&
110			"Jerk bounds should have the same dimension as the points");
111		✗	const int DIM_VAR = dimVar(pData);
112		✗	int empty_acc = 0;
113		✗	int empty_vel = 0;
114		✗	int empty_jerk = 0;
115		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin();
116		✗	wpcit != wps_acc.end(); ++wpcit) {
117		✗	if (wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
118		✗	empty_acc++;
119			}
120		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin();
121		✗	wpcit != wps_vel.end(); ++wpcit) {
122		✗	if (wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
123		✗	empty_vel++;
124			}
125		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin();
126		✗	wpcit != wps_jerk.end(); ++wpcit) {
127		✗	if (wpcit->first.isZero(std::numeric_limits<double>::epsilon()))
128		✗	empty_jerk++;
129			}
130
131		✗	A = MatrixXX::Zero(
132		✗	(2 * DIM_POINT *
133		✗	(wps_acc.size() - empty_acc + wps_vel.size() - empty_vel +
134		✗	wps_jerk.size() - empty_jerk)) +
135		✗	DIM_VAR,
136		✗	DIM_VAR); // *2 because we have to put the lower and upper bound for each
137			// one, +DIM_VAR for the constraint on x[z]
138		✗	b = VectorX::Zero(A.rows());
139		✗	int i = 0;
140
141			// upper acc bounds
142		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin();
143		✗	wpcit != wps_acc.end(); ++wpcit) {
144		✗	if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
145		✗	A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = wpcit->first;
146		✗	b.segment<DIM_POINT>(i * DIM_POINT) = acc_bounds - wpcit->second;
147		✗	++i;
148			}
149			}
150			// lower acc bounds
151		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_acc.begin();
152		✗	wpcit != wps_acc.end(); ++wpcit) {
153		✗	if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
154		✗	A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = -wpcit->first;
155		✗	b.segment<DIM_POINT>(i * DIM_POINT) = acc_bounds + wpcit->second;
156		✗	++i;
157			}
158			}
159
160			// upper velocity bounds
161		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin();
162		✗	wpcit != wps_vel.end(); ++wpcit) {
163		✗	if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
164		✗	A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = wpcit->first;
165		✗	b.segment<DIM_POINT>(i * DIM_POINT) = vel_bounds - wpcit->second;
166		✗	++i;
167			}
168			}
169			// lower velocity bounds
170		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_vel.begin();
171		✗	wpcit != wps_vel.end(); ++wpcit) {
172		✗	if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
173		✗	A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = -wpcit->first;
174		✗	b.segment<DIM_POINT>(i * DIM_POINT) = vel_bounds + wpcit->second;
175		✗	++i;
176			}
177			}
178
179			// upper jerk bounds
180		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin();
181		✗	wpcit != wps_jerk.end(); ++wpcit) {
182		✗	if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
183		✗	A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = wpcit->first;
184		✗	b.segment<DIM_POINT>(i * DIM_POINT) = vel_bounds - wpcit->second;
185		✗	++i;
186			}
187			}
188			// lower jerk bounds
189		✗	for (std::vector<waypoint_t>::const_iterator wpcit = wps_jerk.begin();
190		✗	wpcit != wps_jerk.end(); ++wpcit) {
191		✗	if (!wpcit->first.isZero(std::numeric_limits<double>::epsilon())) {
192		✗	A.block(i * DIM_POINT, 0, DIM_POINT, DIM_VAR) = -wpcit->first;
193		✗	b.segment<DIM_POINT>(i * DIM_POINT) = jerk_bounds + wpcit->second;
194		✗	++i;
195			}
196			}
197
198			// test : constraint x[z] to be always higher than init[z] and goal[z].
199			// TODO replace z with the direction of the contact normal ... need to change
200			// the API
201		✗	MatrixXX mxz = MatrixXX::Zero(DIM_VAR, DIM_VAR);
202		✗	int j = DIM_POINT - 1;
203		✗	VectorX nxz = VectorX::Zero(DIM_VAR);
204		✗	while (j < (DIM_VAR)) {
205		✗	mxz(j, j) = -1;
206		✗	nxz[j] = -std::min(pData.c0_[2], pData.c1_[2]);
207		✗	j += DIM_POINT;
208			}
209		✗	A.block(i * DIM_POINT, 0, DIM_VAR, DIM_VAR) = mxz;
210		✗	b.segment(i * DIM_POINT, DIM_VAR) = nxz;
211
212			// std::cout<<"(i*DIM_POINT + DIM_VAR) = " << (i*DIM_POINT +
213			// DIM_VAR)<<std::endl; std::cout<<"A rows = "<<A.rows()<<std::endl;
214		✗	assert((i * DIM_POINT + DIM_VAR) == A.rows() &&
215			"Constraints matrix were not correctly initialized");
216			// TEST :
217			/* A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = Matrix3::Identity();
218			b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);
219			i++;
220			A.block<DIM_POINT,DIM_POINT>(i*DIM_POINT,0) = -Matrix3::Identity();
221			b.segment<DIM_POINT>(i*DIM_POINT) = Vector3(10,10,10);*/
222		✗	}
223
224		✗	std::pair<MatrixXX, VectorX> computeDistanceCostFunction(
225			size_t numPoints, const ProblemData& pData, double T,
226			std::vector<point3_t> pts_path) {
227		✗	assert(numPoints == pts_path.size() &&
228			"Pts_path size must be equal to numPoints");
229		✗	double step = 1. / (double)(numPoints - 1);
230		✗	std::vector<point_t> pi = computeConstantWaypoints(pData, T);
231		✗	waypoint_t c_wp;
232		✗	MatrixXX H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
233		✗	VectorX g = VectorX::Zero(dimVar(pData));
234		✗	point3_t pk;
235		✗	for (size_t i = 0; i < numPoints; ++i) {
236		✗	c_wp = evaluateCurveWaypointAtTime(pData, pi, (double)i * step);
237		✗	pk = pts_path[i];
238			// std::cout<<"pk = "<<pk.transpose()<<std::endl;
239			// std::cout<<"coef First : "<<ckcit->first<<std::endl;
240			// std::cout<<"coef second : "<<ckcit->second.transpose()<<std::endl;
241		✗	H += (c_wp.first.transpose() * c_wp.first);
242		✗	g += ((c_wp.second - pk).transpose() * c_wp.first).transpose();
243			}
244		✗	double norm = H.norm();
245		✗	H /= norm;
246		✗	g /= norm;
247		✗	return std::make_pair(H, g);
248		✗	}
249
250			template <typename Path>
251		✗	std::pair<MatrixXX, VectorX> computeDistanceCostFunction(
252			size_t numPoints, const ProblemData& pData, double T, const Path& path) {
253		✗	double step = 1. / (double)(numPoints - 1);
254		✗	std::vector<point3_t> pts_path;
255		✗	for (size_t i = 0; i < numPoints; ++i)
256		✗	pts_path.push_back(path(((double)i * step)));
257		✗	return computeDistanceCostFunction(numPoints, pData, T, pts_path);
258		✗	}
259
260			// TODO
261		✗	void computeC_of_T(const ProblemData& pData, double T, ResultDataCOMTraj& res) {
262		✗	std::vector<Vector3> wps = computeConstantWaypoints(pData, T);
263		✗	if (dimVar(pData) == 3)
264		✗	wps[4] = res.x; // FIXME : compute id from constraints
265		✗	else if (dimVar(pData) == 9) {
266		✗	wps[4] = res.x.segment<3>(0);
267		✗	wps[5] = res.x.segment<3>(3);
268		✗	wps[6] = res.x.segment<3>(6);
269		✗	} else if (dimVar(pData) == 15) {
270		✗	wps[4] = res.x.segment<3>(0);
271		✗	wps[5] = res.x.segment<3>(3);
272		✗	wps[6] = res.x.segment<3>(6);
273		✗	wps[7] = res.x.segment<3>(9);
274		✗	wps[8] = res.x.segment<3>(12);
275			}
276		✗	res.c_of_t_ = bezier_t(wps.begin(), wps.end(), 0, T);
277			if (verbose)
278			std::cout << "bezier curve created, size = " << res.c_of_t_.size_
279			<< std::endl;
280		✗	}
281
282		✗	void computeVelCostFunctionDiscretized(int numPoints, const ProblemData& pData,
283			double T, MatrixXX& H, VectorX& g) {
284		✗	double step = 1. / (numPoints - 1);
285		✗	std::vector<waypoint_t> cks;
286		✗	std::vector<point_t> pi = computeConstantWaypoints(pData, T);
287		✗	for (int i = 0; i < numPoints; ++i) {
288		✗	cks.push_back(evaluateVelocityCurveWaypointAtTime(pData, T, pi, i * step));
289			}
290		✗	H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
291		✗	g = VectorX::Zero(dimVar(pData));
292		✗	for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin();
293		✗	ckcit != cks.end(); ++ckcit) {
294			// H+=(ckcit->first.transpose() * ckcit->first);
295			// g+=ckcit->second.transpose() * ckcit->first;
296		✗	for (int i = 0; i < (dimVar(pData) / 3); ++i) {
297		✗	H.block<3, 3>(i * 3, i * 3) +=
298		✗	Matrix3::Identity() * ckcit->first(0, i * 3) * ckcit->first(0, i * 3);
299		✗	g.segment<3>(i * 3) +=
300		✗	ckcit->second.segment<3>(0) * ckcit->first(0, i * 3);
301			}
302			}
303			// TEST : don't consider z axis for minimum acceleration cost
304			// H(2,2) = 1e-6;
305			// g[2] = 1e-6 ;
306			// normalize :
307			// double norm=H.norm(); // because H is always diagonal
308			// H /= norm;
309			// g /= norm;
310		✗	}
311
312		✗	void computeAccelerationCostFunctionDiscretized(int numPoints,
313			const ProblemData& pData,
314			double T, MatrixXX& H,
315			VectorX& g) {
316		✗	double step = 1. / (numPoints - 1);
317		✗	std::vector<waypoint_t> cks;
318		✗	std::vector<point_t> pi = computeConstantWaypoints(pData, T);
319		✗	for (int i = 0; i < numPoints; ++i) {
320		✗	cks.push_back(
321		✗	evaluateAccelerationCurveWaypointAtTime(pData, T, pi, i * step));
322			}
323		✗	H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
324		✗	g = VectorX::Zero(dimVar(pData));
325		✗	for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin();
326		✗	ckcit != cks.end(); ++ckcit) {
327		✗	H += (ckcit->first.transpose() * ckcit->first);
328		✗	g += ckcit->first.transpose() * ckcit->second;
329			}
330			// TEST : don't consider z axis for minimum acceleration cost
331			// H(2,2) = 1e-6;
332			// g[2] = 1e-6 ;
333			// normalize :
334			// double norm=H.norm(); // because H is always diagonal
335			// H /= norm;
336			// g /= norm;
337		✗	}
338
339		✗	void computeJerkCostFunctionDiscretized(int numPoints, const ProblemData& pData,
340			double T, MatrixXX& H, VectorX& g) {
341		✗	double step = 1. / (numPoints - 1);
342
343		✗	std::vector<waypoint_t> cks;
344		✗	std::vector<point_t> pi = computeConstantWaypoints(pData, T);
345		✗	for (int i = 0; i < numPoints; ++i) {
346		✗	cks.push_back(evaluateJerkCurveWaypointAtTime(pData, T, pi, i * step));
347			}
348		✗	H = MatrixXX::Zero(dimVar(pData), dimVar(pData));
349		✗	g = VectorX::Zero(dimVar(pData));
350		✗	for (std::vector<waypoint_t>::const_iterator ckcit = cks.begin();
351		✗	ckcit != cks.end(); ++ckcit) {
352		✗	H += (ckcit->first.transpose() * ckcit->first);
353		✗	g += ckcit->first.transpose() * ckcit->second;
354			}
355			// TEST : don't consider z axis for minimum acceleration cost
356			// H(2,2) = 1e-6;
357			// g[2] = 1e-6 ;
358			// normalize :
359			// double norm=H.norm(); // because H is always diagonal
360			// H /= norm;
361			// g /= norm;
362		✗	}
363
364		✗	std::pair<MatrixXX, VectorX> computeEndEffectorConstraints(
365			const ProblemData& pData, const double T,
366			std::vector<bezier_t::point_t> pi) {
367		✗	std::vector<waypoint_t> wps_jerk = computeJerkWaypoints(pData, T, pi);
368		✗	std::vector<waypoint_t> wps_acc = computeAccelerationWaypoints(pData, T, pi);
369		✗	std::vector<waypoint_t> wps_vel = computeVelocityWaypoints(pData, T, pi);
370			// stack the constraint for each waypoint :
371			Vector3 jerk_bounds(10000, 10000,
372		✗	10000); // TODO : read it from somewhere (ProblemData ?)
373		✗	Vector3 acc_bounds(10000, 10000, 10000);
374		✗	Vector3 vel_bounds(10000, 10000, 10000);
375		✗	MatrixXX A;
376		✗	VectorX b;
377		✗	computeConstraintsMatrix(pData, wps_acc, wps_vel, acc_bounds, vel_bounds, A,
378			b, wps_jerk, jerk_bounds);
379		✗	return std::make_pair(A, b);
380		✗	}
381
382			template <typename Path>
383		✗	std::pair<MatrixXX, VectorX> computeEndEffectorCost(
384			const ProblemData& pData, const Path& path, const double T,
385			const double weightDistance, bool /*useVelCost*/,
386			std::vector<bezier_t::point_t> pi) {
387		✗	assert(weightDistance >= 0. && weightDistance <= 1. &&
388			"WeightDistance must be between 0 and 1");
389		✗	double weightSmooth = 1. - weightDistance;
390		✗	const int DIM_VAR = dimVar(pData);
391			// compute distance cost function (discrete integral under the curve defined
392			// by 'path')
393		✗	MatrixXX H;
394		✗	VectorX g;
395		✗	std::pair<MatrixXX, VectorX> Hg_smooth, Hg_rrt;
396
397		✗	if (weightDistance > 0)
398		✗	Hg_rrt = computeDistanceCostFunction<Path>(50, pData, T, path);
399			else {
400		✗	Hg_rrt.first = MatrixXX::Zero(DIM_VAR, DIM_VAR);
401		✗	Hg_rrt.second = VectorX::Zero(DIM_VAR);
402			}
403
404		✗	Hg_smooth = computeVelocityCost(pData, T, pi);
405
406			/* std::cout<<"End eff H_rrt = "<<std::endl<<H_rrt<<std::endl;
407			std::cout<<"End eff g_rrt = "<<std::endl<<g_rrt<<std::endl;
408			std::cout<<"End eff H_acc = "<<std::endl<<H_acc<<std::endl;
409			std::cout<<"End eff g_acc = "<<std::endl<<g_acc<<std::endl;
410			*/
411			// add the costs :
412		✗	H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
413		✗	g = VectorX::Zero(DIM_VAR);
414		✗	H = weightSmooth * (Hg_smooth.first) + weightDistance * Hg_rrt.first;
415		✗	g = weightSmooth * (Hg_smooth.second) + weightDistance * Hg_rrt.second;
416			// H = Hg_smooth.first;
417			// g = Hg_smooth.second;
418
419		✗	return std::make_pair(H, g);
420		✗	}
421
422			template <typename Path>
423		✗	ResultDataCOMTraj solveEndEffector(const ProblemData& pData, const Path& path,
424			const double T, const double weightDistance,
425			bool useVelCost) {
426			if (verbose) std::cout << "solve end effector, T = " << T << std::endl;
427		✗	std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, T);
428		✗	std::pair<MatrixXX, VectorX> Ab = computeEndEffectorConstraints(pData, T, pi);
429		✗	std::pair<MatrixXX, VectorX> Hg =
430			computeEndEffectorCost(pData, path, T, weightDistance, useVelCost, pi);
431			if (verbose) {
432			std::cout << "End eff A = " << std::endl << Ab.first << std::endl;
433			std::cout << "End eff b = " << std::endl << Ab.second << std::endl;
434			std::cout << "End eff H = " << std::endl << Hg.first << std::endl;
435			std::cout << "End eff g = " << std::endl << Hg.second << std::endl;
436			std::cout << "Dim Var = " << dimVar(pData) << std::endl;
437			std::cout << "Dim H = " << Hg.first.rows() << " x " << Hg.first.cols()
438			<< std::endl;
439			std::cout << "Dim g = " << Hg.second.rows() << std::endl;
440			std::cout << "Dim A = " << Ab.first.rows() << " x " << Ab.first.cols()
441			<< std::endl;
442			std::cout << "Dim b = " << Ab.first.rows() << std::endl;
443			}
444
445		✗	VectorX init = VectorX(dimVar(pData));
446			// init = (pData.c0_ + pData.c1_)/2.;
447			// init =pData.c0_;
448			if (verbose)
449			std::cout << "Init = " << std::endl << init.transpose() << std::endl;
450
451		✗	ResultData resQp = solve(Ab, Hg, init);
452
453		✗	ResultDataCOMTraj res;
454		✗	if (resQp.success_) {
455		✗	res.success_ = true;
456		✗	res.x = resQp.x;
457			// computeRealCost(pData, res);
458		✗	computeC_of_T(pData, T, res);
459			// computedL_of_T(pData,Ts,res);
460			}
461			if (verbose) {
462			std::cout << "Solved, success = " << res.success_
463			<< " x = " << res.x.transpose() << std::endl;
464			std::cout << "Final cost : " << resQp.cost_ << std::endl;
465			}
466		✗	return res;
467		✗	}
468
469			} // namespace bezier_com_traj
470