GCC Code Coverage Report


Directory: ./
File: include/hpp/bezier-com-traj/solve_end_effector.hh
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 0 224 0.0%
Branches: 0 670 0.0%

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