hpp-bezier-com-traj  4.15.1
Multi contact trajectory generation for the COM using Bezier curves
solve_end_effector.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2017, LAAS-CNRS
3  * Author: Pierre Fernbach
4  */
5 
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 
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 
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 
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
const bool verbose
Definition: solve_end_effector.hh:17
point_t ddc0_
Definition: data.hh:107
Vector3 point3_t
Definition: definitions.hh:40
BEZIER_COM_TRAJ_DLLAPI ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds, const solvers::SolverType solver=solvers::SOLVER_QUADPROG)
solve x&#39; h x + 2 g&#39; x, subject to A*x <= b using quadprog
void computeVelCostFunctionDiscretized(int numPoints, const ProblemData &pData, double T, MatrixXX &H, VectorX &g)
Definition: solve_end_effector.hh:282
centroidal_dynamics::Vector3 Vector3
Definition: definitions.hh:22
bool success_
Definition: solver-abstract.hpp:68
std::vector< waypoint_t > computeVelocityWaypoints(const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
computeWwaypoints compute the constant waypoints of dc(t) defined by the constraints on initial and f...
Definition: waypoints_definition.cpp:335
VectorX second
Definition: utils.hh:27
waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData &pData, const double T, const std::vector< point_t > &pi, double t)
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint ...
Definition: waypoints_definition.cpp:173
std::vector< bezier_t::point_t > computeConstantWaypointsInitPredef(const ProblemData &pData, double T)
Definition: solve_end_effector.hh:47
void computeConstraintsMatrix(const ProblemData &pData, const std::vector< waypoint_t > &wps_acc, const std::vector< waypoint_t > &wps_vel, const VectorX &acc_bounds, const VectorX &vel_bounds, MatrixXX &A, VectorX &b, const std::vector< waypoint_t > &wps_jerk=std::vector< waypoint_t >(), const VectorX &jerk_bounds=VectorX(DIM_POINT))
Definition: solve_end_effector.hh:99
std::vector< waypoint_t > computeAccelerationWaypoints(const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
computeWwaypoints compute the constant waypoints of ddc(t) defined by the constraints on initial and ...
Definition: waypoints_definition.cpp:365
coefs_t initCoefs()
Definition: solve_end_effector.hh:38
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
void computeJerkCostFunctionDiscretized(int numPoints, const ProblemData &pData, double T, MatrixXX &H, VectorX &g)
Definition: solve_end_effector.hh:339
std::vector< waypoint_t > computeJerkWaypoints(const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
computeWwaypoints compute the constant waypoints of dddc(t) defined by the constraints on initial and...
Definition: waypoints_definition.cpp:395
Definition: utils.hh:25
centroidal_dynamics::VectorX VectorX
Definition: definitions.hh:24
waypoint_t evaluateCurveWaypointAtTime(const ProblemData &pData, const std::vector< point_t > &pi, double t)
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint ...
Definition: waypoints_definition.cpp:138
std::pair< MatrixXX, VectorX > computeEndEffectorConstraints(const ProblemData &pData, const double T, std::vector< bezier_t::point_t > pi)
Definition: solve_end_effector.hh:364
ndcurves::bezier_curve< double, double, true, point_t > bezier_t
Definition: definitions.hh:53
ResultDataCOMTraj solveEndEffector(const ProblemData &pData, const Path &path, const double T, const double weightDistance, bool useVelCost=true)
solveEndEffector Tries to produce a trajectory represented as a bezier curve that satisfy position...
Definition: solve_end_effector.hh:423
void computeAccelerationCostFunctionDiscretized(int numPoints, const ProblemData &pData, double T, MatrixXX &H, VectorX &g)
Definition: solve_end_effector.hh:312
point_t c0_
Definition: data.hh:107
MatrixXX first
Definition: utils.hh:26
std::vector< bezier_t::point_t > computeConstantWaypointsGoalPredef(const ProblemData &pData, double T)
Definition: solve_end_effector.hh:74
point_t dc1_
Definition: data.hh:107
std::pair< double, point3_t > coefs_t
Definition: definitions.hh:62
std::pair< MatrixXX, VectorX > computeEndEffectorCost(const ProblemData &pData, const Path &path, const double T, const double weightDistance, bool, std::vector< bezier_t::point_t > pi)
Definition: solve_end_effector.hh:383
Specialized ResultData that computes the Bezier curves corresponding to the computed trajectory...
Definition: data.hh:121
Struct used to return the results of the trajectory generation problem.
Definition: solver-abstract.hpp:52
point_t dc0_
Definition: data.hh:107
point_t ddc1_
Definition: data.hh:107
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
computeConstantWaypoints compute the constant waypoints of c(t) defined by the constraints on initial...
Definition: waypoints_definition.cpp:286
double cost_
Definition: solver-abstract.hpp:69
waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData &pData, const double T, const std::vector< point_t > &pi, double t)
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint ...
Definition: waypoints_definition.cpp:246
VectorXd x
Definition: solver-abstract.hpp:70
std::pair< MatrixXX, VectorX > computeVelocityCost(const ProblemData &pData, double T, std::vector< bezier_t::point_t > pi=std::vector< bezier_t::point_t >())
computeVelocityCost the matrices H and g defining a cost that minimise the integral of the squared ve...
Definition: waypoints_definition.cpp:479
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: definitions.hh:21
waypoint_t evaluateAccelerationCurveWaypointAtTime(const ProblemData &pData, const double T, const std::vector< point_t > &pi, double t)
evaluateCurveAtTime compute the expression of the point on the curve c at t, defined by the waypoint ...
Definition: waypoints_definition.cpp:211
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition: data.hh:92
Definition: common_solve_methods.hh:15
int dimVar(const ProblemData &pData)
Definition: waypoints_definition.cpp:30
bezier_t c_of_t_
Definition: data.hh:130
point_t c1_
Definition: data.hh:107
void computeC_of_T(const ProblemData &pData, double T, ResultDataCOMTraj &res)
Definition: solve_end_effector.hh:261
const int DIM_POINT
Definition: solve_end_effector.hh:15