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