MRPT  2.0.4
CPTG_Holo_Blend.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
11 
12 #include <mrpt/core/round.h>
14 #include <mrpt/math/CVectorFixed.h>
15 #include <mrpt/math/poly_roots.h>
19 
20 using namespace mrpt::nav;
21 using namespace mrpt::system;
22 
25 
26 /*
27 Closed-form PTG. Parameters:
28 - Initial velocity vector (xip, yip)
29 - Target velocity vector depends on \alpha: xfp = V_MAX*cos(alpha), yfp =
30 V_MAX*sin(alpha)
31 - T_ramp_max max time for velocity interpolation (xip,yip) -> (xfp, yfp)
32 - W_MAX: Rotational velocity for robot heading forwards.
33 
34 Number of steps "d" for each PTG path "k":
35 - Step = time increment PATH_TIME_STEP
36 
37 */
38 
39 // Uncomment only for benchmarking during development
40 //#define DO_PERFORMANCE_BENCHMARK
41 
42 #ifdef DO_PERFORMANCE_BENCHMARK
43 mrpt::system::CTimeLogger tl_holo("CPTG_Holo_Blend");
44 #define PERFORMANCE_BENCHMARK \
45  CTimeLoggerEntry tle(tl_holo, __CURRENT_FUNCTION_NAME__);
46 #else
47 #define PERFORMANCE_BENCHMARK
48 #endif
49 
50 double CPTG_Holo_Blend::PATH_TIME_STEP = 10e-3; // 10 ms
51 double CPTG_Holo_Blend::eps = 1e-4; // epsilon for detecting 1/0 situation
52 
53 // As a macro instead of a function (uglier) to allow for const variables
54 // (safer)
55 #define COMMON_PTG_DESIGN_PARAMS \
56  const double vxi = m_nav_dyn_state.curVelLocal.vx, \
57  vyi = m_nav_dyn_state.curVelLocal.vy; \
58  const double vf_mod = internal_get_v(dir); \
59  const double vxf = vf_mod * cos(dir), vyf = vf_mod * sin(dir); \
60  const double T_ramp = internal_get_T_ramp(dir);
61 
62 #if 0
63 static double calc_trans_distance_t_below_Tramp_abc_analytic(double t, double a, double b, double c)
64 {
66 
67  ASSERT_(t >= 0);
68  if (t == 0.0) return .0;
69 
70  double dist;
71  // Handle special case: degenerate sqrt(a*t^2+b*t+c) = sqrt((t-r)^2) = |t-r|
72  const double discr = b*b - 4 * a*c;
73  if (std::abs(discr)<1e-6)
74  {
75  const double r = -b / (2 * a);
76  // dist= definite integral [0,t] of: |t-r| dt
77  dist = r*std::abs(r)*0.5 - (r - t)*std::abs(r - t)*0.5;
78  }
79  else
80  {
81  // General case:
82  // Indefinite integral of sqrt(a*t^2+b*t+c):
83  const double int_t = (t*(1.0 / 2.0) + (b*(1.0 / 4.0)) / a)*sqrt(c + b*t + a*(t*t)) + 1.0 / pow(a, 3.0 / 2.0)*log(1.0 / sqrt(a)*(b*(1.0 / 2.0) + a*t) + sqrt(c + b*t + a*(t*t)))*(a*c - (b*b)*(1.0 / 4.0))*(1.0 / 2.0);
84  // Limit when t->0:
85  const double int_t0 = (b*sqrt(c)*(1.0 / 4.0)) / a + 1.0 / pow(a, 3.0 / 2.0)*log(1.0 / sqrt(a)*(b + sqrt(a)*sqrt(c)*2.0)*(1.0 / 2.0))*(a*c - (b*b)*(1.0 / 4.0))*(1.0 / 2.0);
86  dist = int_t - int_t0;// Definite integral [0,t]
87  }
88 #ifdef _DEBUG
89  using namespace mrpt;
91  ASSERT_(dist >= .0);
92 #endif
93  return dist;
94 }
95 #endif
96 
97 // Numeric integration of: sqrt(a*t^2+b*t+c) for t=[0,T]
99  double T, double a, double b, double c)
100 {
102 
103  double d = .0;
104  const unsigned int NUM_STEPS = 15;
105 
106  ASSERT_(a >= .0);
107  ASSERT_(c >= .0);
108  double feval_t = std::sqrt(c); // t (initial: t=0)
109  double feval_tp1; // t+1
110 
111  const double At = T / (NUM_STEPS);
112  double t = .0;
113  for (unsigned int i = 0; i < NUM_STEPS; i++)
114  {
115  // Eval function at t+1:
116  t += At;
117  double dd = a * t * t + b * t + c;
118 
119  // handle numerical innacuracies near t=T_ramp:
120  ASSERT_(dd > -1e-5);
121  if (dd < 0) dd = .0;
122 
123  feval_tp1 = sqrt(dd);
124 
125  // Trapezoidal rule:
126  d += At * (feval_t + feval_tp1) * 0.5;
127 
128  // for next step:
129  feval_t = feval_tp1;
130  }
131 
132  return d;
133 }
134 
135 // Axiliary function for calc_trans_distance_t_below_Tramp() and others:
137  double t, double a, double b, double c)
138 {
139 // JLB (29 Jan 2017): it turns out that numeric integration is *faster* and more
140 // accurate (does not have "special cases")...
141 #if 0
142  double ret = calc_trans_distance_t_below_Tramp_abc_analytic(t, a, b, c);
143 #else
144  double ret = calc_trans_distance_t_below_Tramp_abc_numeric(t, a, b, c);
145 #endif
146 
147  return ret;
148 }
149 
150 // Axiliary function for computing the line-integral distance along the
151 // trajectory, handling special cases of 1/0:
153  double k2, double k4, double vxi, double vyi, double t)
154 {
155  /*
156  dd = sqrt( (4*k2^2 + 4*k4^2)*t^2 + (4*k2*vxi + 4*k4*vyi)*t + vxi^2 + vyi^2 )
157  dt
158  a t^2 + b t + c
159  */
160  const double c = (vxi * vxi + vyi * vyi);
161  if (std::abs(k2) > eps || std::abs(k4) > eps)
162  {
163  const double a = ((k2 * k2) * 4.0 + (k4 * k4) * 4.0);
164  const double b = (k2 * vxi * 4.0 + k4 * vyi * 4.0);
165 
166  // Numerically-ill case: b=c=0 (initial vel=0)
167  if (std::abs(b) < eps && std::abs(c) < eps)
168  {
169  // Indefinite integral of simplified case: sqrt(a)*t
170  const double int_t = sqrt(a) * (t * t) * 0.5;
171  return int_t; // Definite integral [0,t]
172  }
173  else
174  {
175  return calc_trans_distance_t_below_Tramp_abc(t, a, b, c);
176  }
177  }
178  else
179  {
180  return std::sqrt(c) * t;
181  }
182 }
183 
185 {
186  m_pathStepCountCache.assign(m_alphaValuesCount, -1); // mark as invalid
187 }
188 
190 {
193 
194  m_alphaValuesCount = 31;
195  T_ramp_max = 0.9;
196  V_MAX = 1.0;
197  W_MAX = mrpt::DEG2RAD(40);
198 }
199 
201  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
202 {
205 
207  T_ramp_max, double, T_ramp_max, cfg, sSection);
209  v_max_mps, double, V_MAX, cfg, sSection);
211  w_max_dps, double, W_MAX, cfg, sSection);
212  MRPT_LOAD_CONFIG_VAR(turningRadiusReference, double, cfg, sSection);
213 
214  MRPT_LOAD_HERE_CONFIG_VAR(expr_V, string, expr_V, cfg, sSection);
215  MRPT_LOAD_HERE_CONFIG_VAR(expr_W, string, expr_W, cfg, sSection);
216  MRPT_LOAD_HERE_CONFIG_VAR(expr_T_ramp, string, expr_T_ramp, cfg, sSection);
217 }
219  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
220 {
221  MRPT_START
222  const int WN = 25, WV = 30;
223 
225 
226  cfg.write(
227  sSection, "T_ramp_max", T_ramp_max, WN, WV,
228  "Max duration of the velocity interpolation since a vel_cmd is issued "
229  "[s].");
230  cfg.write(
231  sSection, "v_max_mps", V_MAX, WN, WV,
232  "Maximum linear velocity for trajectories [m/s].");
233  cfg.write(
234  sSection, "w_max_dps", mrpt::RAD2DEG(W_MAX), WN, WV,
235  "Maximum angular velocity for trajectories [deg/s].");
236  cfg.write(
237  sSection, "turningRadiusReference", turningRadiusReference, WN, WV,
238  "An approximate dimension of the robot (not a critical parameter) "
239  "[m].");
240 
241  cfg.write(
242  sSection, "expr_V", expr_V, WN, WV,
243  "Math expr for |V| as a function of "
244  "`dir`,`V_MAX`,`W_MAX`,`T_ramp_max`.");
245  cfg.write(
246  sSection, "expr_W", expr_W, WN, WV,
247  "Math expr for |omega| (disregarding the sign, only the module) as a "
248  "function of `dir`,`V_MAX`,`W_MAX`,`T_ramp_max`.");
249  cfg.write(
250  sSection, "expr_T_ramp", expr_T_ramp, WN, WV,
251  "Math expr for `T_ramp` as a function of "
252  "`dir`,`V_MAX`,`W_MAX`,`T_ramp_max`.");
253 
255 
256  MRPT_END
257 }
258 
260 {
261  return mrpt::format(
262  "PTG_Holo_Blend_Tramp=%.03f_Vmax=%.03f_Wmax=%.03f", T_ramp_max, V_MAX,
263  W_MAX);
264 }
265 
267  mrpt::serialization::CArchive& in, uint8_t version)
268 {
270 
271  switch (version)
272  {
273  case 0:
274  case 1:
275  case 2:
276  case 3:
277  case 4:
278  if (version >= 1)
279  {
281  }
282 
283  in >> T_ramp_max >> V_MAX >> W_MAX >> turningRadiusReference;
284  if (version == 2)
285  {
286  double dummy_maxAllowedDirAngle; // removed in v3
287  in >> dummy_maxAllowedDirAngle;
288  }
289  if (version >= 4)
290  {
291  in >> expr_V >> expr_W >> expr_T_ramp;
292  }
293  break;
294  default:
296  };
297 }
298 
299 uint8_t CPTG_Holo_Blend::serializeGetVersion() const { return 4; }
301 {
304 
305  out << T_ramp_max << V_MAX << W_MAX << turningRadiusReference;
306  out << expr_V << expr_W << expr_T_ramp;
307 }
308 
310  double x, double y, int& out_k, double& out_d,
311  [[maybe_unused]] double tolerance_dist) const
312 {
314 
315  ASSERT_(x != 0 || y != 0);
316 
317  const double err_threshold = 1e-3;
318  const double T_ramp = T_ramp_max;
319  const double vxi = m_nav_dyn_state.curVelLocal.vx,
320  vyi = m_nav_dyn_state.curVelLocal.vy;
321 
322  // Use a Newton iterative non-linear optimizer to find the "exact" solution
323  // for (t,alpha)
324  // in each case: (1) t<T_ramp and (2) t>T_ramp
325 
326  // Initial value:
327  mrpt::math::CVectorFixed<double, 3> q; // [t vxf vyf]
328  q[0] = T_ramp_max * 1.1;
329  q[1] = V_MAX * x / sqrt(x * x + y * y);
330  q[2] = V_MAX * y / sqrt(x * x + y * y);
331 
332  // Iterate: case (2) t > T_ramp
333  double err_mod = 1e7;
334  bool sol_found = false;
335  for (int iters = 0; !sol_found && iters < 25; iters++)
336  {
337  const double TR_ = 1.0 / (T_ramp);
338  const double TR2_ = 1.0 / (2 * T_ramp);
339 
340  // Eval residual:
342  if (q[0] >= T_ramp)
343  {
344  r[0] = 0.5 * T_ramp * (vxi + q[1]) + (q[0] - T_ramp) * q[1] - x;
345  r[1] = 0.5 * T_ramp * (vyi + q[2]) + (q[0] - T_ramp) * q[2] - y;
346  }
347  else
348  {
349  r[0] = vxi * q[0] + q[0] * q[0] * TR2_ * (q[1] - vxi) - x;
350  r[1] = vyi * q[0] + q[0] * q[0] * TR2_ * (q[2] - vyi) - y;
351  }
352  const double alpha = atan2(q[2], q[1]);
353  const double V_MAXsq = mrpt::square(this->internal_get_v(alpha));
354  r[2] = q[1] * q[1] + q[2] * q[2] - V_MAXsq;
355 
356  // Jacobian: q=[t vxf vyf] q0=t q1=vxf q2=vyf
357  // dx/dt dx/dvxf dx/dvyf
358  // dy/dt dy/dvxf dy/dvyf
359  // dVF/dt dVF/dvxf dVF/dvyf
361  if (q[0] >= T_ramp)
362  {
363  J(0, 0) = q[1];
364  J(0, 1) = 0.5 * T_ramp + q[0];
365  J(0, 2) = 0.0;
366  J(1, 0) = q[2];
367  J(1, 1) = 0.0;
368  J(1, 2) = 0.5 * T_ramp + q[0];
369  }
370  else
371  {
372  J(0, 0) = vxi + q[0] * TR_ * (q[1] - vxi);
373  J(0, 1) = TR2_ * q[0] * q[0];
374  J(0, 2) = 0.0;
375  J(1, 0) = vyi + q[0] * TR_ * (q[2] - vyi);
376  J(1, 1) = 0.0;
377  J(1, 2) = TR2_ * q[0] * q[0];
378  }
379  J(2, 0) = 0.0;
380  J(2, 1) = 2 * q[1];
381  J(2, 2) = 2 * q[2];
382 
384  q -= q_incr;
385 
386  err_mod = r.norm();
387  sol_found = (err_mod < err_threshold);
388  }
389 
390  if (sol_found && q[0] >= .0)
391  {
392  const double alpha = atan2(q[2], q[1]);
394 
395  const double solved_t = q[0];
396  const unsigned int solved_step = solved_t / PATH_TIME_STEP;
397  const double found_dist = this->getPathDist(out_k, solved_step);
398 
399  out_d = found_dist / this->refDistance;
400  return true;
401  }
402  else
403  {
404  return false;
405  }
406 }
407 
408 bool CPTG_Holo_Blend::PTG_IsIntoDomain(double x, double y) const
409 {
410  int k;
411  double d;
412  return inverseMap_WS2TP(x, y, k, d);
413 }
414 
416 {
417  // Nothing to do in a closed-form PTG.
418 }
419 
421  uint16_t k) const
422 {
423  const double dir_local = CParameterizedTrajectoryGenerator::index2alpha(k);
424 
426  cmd->vel = internal_get_v(dir_local);
427  cmd->dir_local = dir_local;
428  cmd->ramp_time = internal_get_T_ramp(dir_local);
429  cmd->rot_speed = mrpt::signWithZero(dir_local) * internal_get_w(dir_local);
430 
432 }
433 
434 size_t CPTG_Holo_Blend::getPathStepCount(uint16_t k) const
435 {
436  if (m_pathStepCountCache.size() > k && m_pathStepCountCache[k] > 0)
437  return m_pathStepCountCache[k];
438 
439  uint32_t step;
440  if (!getPathStepForDist(k, this->refDistance, step))
441  {
443  "Could not solve closed-form distance for k=%u",
444  static_cast<unsigned>(k));
445  }
446  ASSERT_(step > 0);
447  if (m_pathStepCountCache.size() != m_alphaValuesCount)
448  {
449  m_pathStepCountCache.assign(m_alphaValuesCount, -1);
450  }
451  m_pathStepCountCache[k] = step;
452  return step;
453 }
454 
456  uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const
457 {
458  const double t = PATH_TIME_STEP * step;
461  const double wf = mrpt::signWithZero(dir) * this->internal_get_w(dir);
462  const double TR2_ = 1.0 / (2 * T_ramp);
463 
464  // Translational part:
465  if (t < T_ramp)
466  {
467  p.x = vxi * t + t * t * TR2_ * (vxf - vxi);
468  p.y = vyi * t + t * t * TR2_ * (vyf - vyi);
469  }
470  else
471  {
472  p.x = T_ramp * 0.5 * (vxi + vxf) + (t - T_ramp) * vxf;
473  p.y = T_ramp * 0.5 * (vyi + vyf) + (t - T_ramp) * vyf;
474  }
475 
476  // Rotational part:
477  const double wi = m_nav_dyn_state.curVelLocal.omega;
478 
479  if (t < T_ramp)
480  {
481  // Time required to align completed?
482  const double a = TR2_ * (wf - wi), b = (wi), c = -dir;
483 
484  // Solves equation `a*x^2 + b*x + c = 0`.
485  double r1, r2;
486  int nroots = mrpt::math::solve_poly2(a, b, c, r1, r2);
487  if (nroots != 2)
488  {
489  p.phi = .0; // typical case: wi=wf=0
490  }
491  else
492  {
493  const double t_solve = std::max(r1, r2);
494  if (t > t_solve)
495  p.phi = dir;
496  else
497  p.phi = wi * t + t * t * TR2_ * (wf - wi);
498  }
499  }
500  else
501  {
502  // Time required to align completed?
503  const double t_solve = (dir - T_ramp * 0.5 * (wi + wf)) / wf + T_ramp;
504  if (t > t_solve)
505  p.phi = dir;
506  else
507  p.phi = T_ramp * 0.5 * (wi + wf) + (t - T_ramp) * wf;
508  }
509 }
510 
511 double CPTG_Holo_Blend::getPathDist(uint16_t k, uint32_t step) const
512 {
513  const double t = PATH_TIME_STEP * step;
515 
517  const double TR2_ = 1.0 / (2 * T_ramp);
518 
519  const double k2 = (vxf - vxi) * TR2_;
520  const double k4 = (vyf - vyi) * TR2_;
521 
522  if (t < T_ramp)
523  {
524  return calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, t);
525  }
526  else
527  {
528  const double dist_trans =
529  (t - T_ramp) * V_MAX +
530  calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
531  return dist_trans;
532  }
533 }
534 
536  uint16_t k, double dist, uint32_t& out_step) const
537 {
539 
542 
543  const double TR2_ = 1.0 / (2 * T_ramp);
544 
545  const double k2 = (vxf - vxi) * TR2_;
546  const double k4 = (vyf - vyi) * TR2_;
547 
548  // --------------------------------------
549  // Solution within t >= T_ramp ??
550  // --------------------------------------
551  const double dist_trans_T_ramp =
552  calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
553  double t_solved = -1;
554 
555  if (dist >= dist_trans_T_ramp)
556  {
557  // Good solution:
558  t_solved = T_ramp + (dist - dist_trans_T_ramp) / V_MAX;
559  }
560  else
561  {
562  // ------------------------------------
563  // Solutions within t < T_ramp
564  //
565  // Cases:
566  // 1) k2=k4=0 --> vi=vf. Path is straight line
567  // 2) b=c=0 -> vi=0
568  // 3) Otherwise, general case
569  // ------------------------------------
570  if (std::abs(k2) < eps && std::abs(k4) < eps)
571  {
572  // Case 1
573  t_solved = (dist) / V_MAX;
574  }
575  else
576  {
577  const double a = ((k2 * k2) * 4.0 + (k4 * k4) * 4.0);
578  const double b = (k2 * vxi * 4.0 + k4 * vyi * 4.0);
579  const double c = (vxi * vxi + vyi * vyi);
580 
581  // Numerically-ill case: b=c=0 (initial vel=0)
582  if (std::abs(b) < eps && std::abs(c) < eps)
583  {
584  // Case 2:
585  t_solved = sqrt(2.0) * 1.0 / pow(a, 1.0 / 4.0) * sqrt(dist);
586  }
587  else
588  {
589  // Case 3: general case with non-linear equation:
590  // dist = (t/2 + b/(4*a))*(a*t^2 + b*t + c)^(1/2) -
591  // (b*c^(1/2))/(4*a) + (log((b/2 + a*t)/a^(1/2) + (a*t^2 + b*t +
592  // c)^(1/2))*(- b^2/4 + a*c))/(2*a^(3/2)) - (log((b +
593  // 2*a^(1/2)*c^(1/2))/(2*a^(1/2)))*(- b^2/4 + a*c))/(2*a^(3/2))
594  // dist =
595  // (t*(1.0/2.0)+(b*(1.0/4.0))/a)*sqrt(c+b*t+a*(t*t))-(b*sqrt(c)*(1.0/4.0))/a+1.0/pow(a,3.0/2.0)*log(1.0/sqrt(a)*(b*(1.0/2.0)+a*t)+sqrt(c+b*t+a*(t*t)))*(a*c-(b*b)*(1.0/4.0))*(1.0/2.0)-1.0/pow(a,3.0/2.0)*log(1.0/sqrt(a)*(b+sqrt(a)*sqrt(c)*2.0)*(1.0/2.0))*(a*c-(b*b)*(1.0/4.0))*(1.0/2.0);
596 
597  // We must solve this by iterating:
598  // Newton method:
599  // Minimize f(t)-dist = 0
600  // with: f(t)=calc_trans_distance_t_below_Tramp_abc(t)
601  // and: f'(t) = sqrt(a*t^2+b*t+c)
602 
603  t_solved = T_ramp * 0.6; // Initial value for starting
604  // interation inside the valid domain
605  // of the function t=[0,T_ramp]
606  for (int iters = 0; iters < 10; iters++)
607  {
608  double err = calc_trans_distance_t_below_Tramp_abc(
609  t_solved, a, b, c) -
610  dist;
611  const double diff =
612  std::sqrt(a * t_solved * t_solved + b * t_solved + c);
613  ASSERT_(std::abs(diff) > 1e-40);
614  t_solved -= (err) / diff;
615  if (t_solved < 0) t_solved = .0;
616  if (std::abs(err) < 1e-3) break; // Good enough!
617  }
618  }
619  }
620  }
621  if (t_solved >= 0)
622  {
623  out_step = mrpt::round(t_solved / PATH_TIME_STEP);
624  return true;
625  }
626  else
627  return false;
628 }
629 
631  double ox, double oy, uint16_t k, double& tp_obstacle_k) const
632 {
633  const double R = m_robotRadius;
636 
637  const double TR2_ = 1.0 / (2 * T_ramp);
638  const double TR_2 = T_ramp * 0.5;
639  const double T_ramp_thres099 = T_ramp * 0.99;
640  const double T_ramp_thres101 = T_ramp * 1.01;
641 
642  double sol_t = -1.0; // candidate solution for shortest time to collision
643 
644  // Note: It's tempting to try to solve first for t>T_ramp because it has
645  // simpler (faster) equations,
646  // but there are cases in which we will have valid collisions for t>T_ramp
647  // but other valid ones
648  // for t<T_ramp as well, so the only SAFE way to detect shortest distances
649  // is to check over increasing values of "t".
650 
651  // Try to solve first for t<T_ramp:
652  const double k2 = (vxf - vxi) * TR2_;
653  const double k4 = (vyf - vyi) * TR2_;
654 
655  // equation: a*t^4 + b*t^3 + c*t^2 + d*t + e = 0
656  const double a = (k2 * k2 + k4 * k4);
657  const double b = (k2 * vxi * 2.0 + k4 * vyi * 2.0);
658  const double c = -(k2 * ox * 2.0 + k4 * oy * 2.0 - vxi * vxi - vyi * vyi);
659  const double d = -(ox * vxi * 2.0 + oy * vyi * 2.0);
660  const double e = -R * R + ox * ox + oy * oy;
661 
662  double roots[4];
663  int num_real_sols = 0;
664  if (std::abs(a) > eps)
665  {
666  // General case: 4th order equation
667  // a * x^4 + b * x^3 + c * x^2 + d * x + e
668  num_real_sols =
669  mrpt::math::solve_poly4(roots, b / a, c / a, d / a, e / a);
670  }
671  else if (std::abs(b) > eps)
672  {
673  // Special case: k2=k4=0 (straight line path, no blend)
674  // 3rd order equation:
675  // b * x^3 + c * x^2 + d * x + e
676  num_real_sols = mrpt::math::solve_poly3(roots, c / b, d / b, e / b);
677  }
678  else
679  {
680  // Special case: 2nd order equation (a=b=0)
681  const double discr = d * d - 4 * c * e; // c*t^2 + d*t + e = 0
682  if (discr >= 0)
683  {
684  num_real_sols = 2;
685  roots[0] = (-d + sqrt(discr)) / (2 * c);
686  roots[1] = (-d - sqrt(discr)) / (2 * c);
687  }
688  else
689  {
690  num_real_sols = 0;
691  }
692  }
693 
694  for (int i = 0; i < num_real_sols; i++)
695  {
696  if (roots[i] == roots[i] && // not NaN
697  std::isfinite(roots[i]) && roots[i] >= .0 &&
698  roots[i] <= T_ramp * 1.01)
699  {
700  if (sol_t < 0)
701  sol_t = roots[i];
702  else
703  mrpt::keep_min(sol_t, roots[i]);
704  }
705  }
706 
707  // Invalid with these equations?
708  if (sol_t < 0 || sol_t > T_ramp_thres101)
709  {
710  // Now, attempt to solve with the equations for t>T_ramp:
711  sol_t = -1.0;
712 
713  const double c1 = TR_2 * (vxi - vxf) - ox;
714  const double c2 = TR_2 * (vyi - vyf) - oy;
715 
716  const double xa = vf_mod * vf_mod;
717  const double xb = 2 * (c1 * vxf + c2 * vyf);
718  const double xc = c1 * c1 + c2 * c2 - R * R;
719 
720  const double discr = xb * xb - 4 * xa * xc;
721  if (discr >= 0)
722  {
723  const double sol_t0 = (-xb + sqrt(discr)) / (2 * xa);
724  const double sol_t1 = (-xb - sqrt(discr)) / (2 * xa);
725 
726  // Identify the shortest valid collision time:
727  if (sol_t0 < T_ramp && sol_t1 < T_ramp)
728  sol_t = -1.0;
729  else if (sol_t0 < T_ramp && sol_t1 >= T_ramp_thres099)
730  sol_t = sol_t1;
731  else if (sol_t1 < T_ramp && sol_t0 >= T_ramp_thres099)
732  sol_t = sol_t0;
733  else if (sol_t1 >= T_ramp_thres099 && sol_t0 >= T_ramp_thres099)
734  sol_t = std::min(sol_t0, sol_t1);
735  }
736  }
737 
738  // Valid solution?
739  if (sol_t < 0) return;
740  // Compute the transversed distance:
741  double dist;
742 
743  if (sol_t < T_ramp)
744  dist = calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, sol_t);
745  else
746  dist = (sol_t - T_ramp) * V_MAX +
747  calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
748 
749  // Store in the output variable:
750  internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
751 }
752 
754  double ox, double oy, std::vector<double>& tp_obstacles) const
755 {
757 
758  for (unsigned int k = 0; k < m_alphaValuesCount; k++)
759  {
760  updateTPObstacleSingle(ox, oy, k, tp_obstacles[k]);
761  } // end for each "k" alpha
762 }
763 
765 {
766  // Nothing to do in a closed-form PTG.
767 }
768 
771 {
774 }
775 
776 bool CPTG_Holo_Blend::supportVelCmdNOP() const { return true; }
777 double CPTG_Holo_Blend::maxTimeInVelCmdNOP(int path_k) const
778 {
779  // const double dir_local =
780  // CParameterizedTrajectoryGenerator::index2alpha(path_k);
781 
782  const size_t nSteps = getPathStepCount(path_k);
783  const double max_t =
784  PATH_TIME_STEP *
785  (nSteps *
786  0.7 /* leave room for obstacle detection ahead when we are far down the predicted PTG path */);
787  return max_t;
788 }
789 
790 double CPTG_Holo_Blend::getPathStepDuration() const { return PATH_TIME_STEP; }
791 CPTG_Holo_Blend::CPTG_Holo_Blend() { internal_construct_exprs(); }
793  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
794  : turningRadiusReference(0.30)
795 {
797  this->loadFromConfigFile(cfg, sSection);
798 }
799 
802 {
803  std::map<std::string, double*> symbols;
804  symbols["dir"] = &m_expr_dir;
805  symbols["V_MAX"] = &V_MAX;
806  symbols["W_MAX"] = &W_MAX;
807  symbols["T_ramp_max"] = &T_ramp_max;
808 
812 
813  // Default expressions (can be overloaded by values in a config file)
814  expr_V = "V_MAX";
815  expr_W = "W_MAX";
816  expr_T_ramp = "T_ramp_max";
817 }
818 
819 double CPTG_Holo_Blend::internal_get_v(const double dir) const
820 {
821  const_cast<double&>(m_expr_dir) = dir;
822  return std::abs(m_expr_v.eval());
823 }
824 double CPTG_Holo_Blend::internal_get_w(const double dir) const
825 {
826  const_cast<double&>(m_expr_dir) = dir;
827  return std::abs(m_expr_w.eval());
828 }
829 double CPTG_Holo_Blend::internal_get_T_ramp(const double dir) const
830 {
831  const_cast<double&>(m_expr_dir) = dir;
832  return m_expr_T_ramp.eval();
833 }
834 
836  const std::string& cacheFilename, const bool verbose)
837 {
838  // No need to initialize anything, just do some params sanity checks:
839  ASSERT_(T_ramp_max > 0);
840  ASSERT_(V_MAX > 0);
841  ASSERT_(W_MAX > 0);
843  ASSERT_(m_robotRadius > 0);
844 
845  // Compile user-given expressions:
846  m_expr_v.compile(expr_V, std::map<std::string, double>(), "expr_V");
847  m_expr_w.compile(expr_W, std::map<std::string, double>(), "expr_w");
849  expr_T_ramp, std::map<std::string, double>(), "expr_T_ramp");
850 
851 #ifdef DO_PERFORMANCE_BENCHMARK
852  tl.dumpAllStats();
853 #endif
854 
855  m_pathStepCountCache.clear();
856 }
mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
Definition: CParameterizedTrajectoryGenerator.cpp:45
nav-precomp.h
mrpt::nav::CPTG_Holo_Blend::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CPTG_Holo_Blend.cpp:266
mrpt::nav::CPTG_Holo_Blend::inverseMap_WS2TP
bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const override
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
Definition: CPTG_Holo_Blend.cpp:309
CVehicleVelCmd_Holo.h
mrpt::nav::CPTG_RobotShape_Circular::m_robotRadius
double m_robotRadius
Definition: CParameterizedTrajectoryGenerator.h:590
mrpt::expr::CRuntimeCompiledExpression::compile
void compile(const std::string &expression, const std::map< std::string, double > &variables=std::map< std::string, double >(), const std::string &expr_name_for_error_reporting=std::string())
Initializes the object by compiling an expression.
Definition: CRuntimeCompiledExpression.cpp:74
mrpt::nav::CPTG_Holo_Blend::maxTimeInVelCmdNOP
double maxTimeInVelCmdNOP(int path_k) const override
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
Definition: CPTG_Holo_Blend.cpp:777
mrpt::nav::CPTG_Holo_Blend::T_ramp_max
double T_ramp_max
Definition: CPTG_Holo_Blend.h:80
mrpt::nav::CPTG_Holo_Blend::supportVelCmdNOP
bool supportVelCmdNOP() const override
Returns true if it is possible to stop sending velocity commands to the robot and,...
Definition: CPTG_Holo_Blend.cpp:776
mrpt::nav::CPTG_Holo_Blend::getPathStepForDist
bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
Definition: CPTG_Holo_Blend.cpp:535
mrpt::nav::CPTG_Holo_Blend::m_expr_v
mrpt::expr::CRuntimeCompiledExpression m_expr_v
Definition: CPTG_Holo_Blend.h:88
mrpt::config::CConfigFileBase::write
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
Definition: config/CConfigFileBase.h:107
verbose
params verbose
Definition: chessboard_stereo_camera_calib_unittest.cpp:59
mrpt::nav::CPTG_Holo_Blend::m_pathStepCountCache
std::vector< int > m_pathStepCountCache
Definition: CPTG_Holo_Blend.h:85
MRPT_LOAD_CONFIG_VAR
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
Definition: config/CConfigFileBase.h:306
mrpt::nav::CPTG_Holo_Blend::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CPTG_Holo_Blend.cpp:300
mrpt::nav::CPTG_RobotShape_Circular::loadShapeFromConfigFile
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
Definition: CPTG_RobotShape_Circular.cpp:28
CPTG_Holo_Blend.h
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::system::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: system/CTimeLogger.h:51
mrpt::nav::CPTG_Holo_Blend::eps
static double eps
Mathematical "epsilon", to detect ill-conditioned situations (e.g.
Definition: CPTG_Holo_Blend.h:77
mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
Definition: CParameterizedTrajectoryGenerator.cpp:155
mrpt::nav::CPTG_Holo_Blend::W_MAX
double W_MAX
Definition: CPTG_Holo_Blend.h:81
poly_roots.h
mrpt::nav::CPTG_Holo_Blend::getPathPose
void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
Definition: CPTG_Holo_Blend.cpp:455
mrpt::nav::CPTG_Holo_Blend::saveToConfigFile
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
Definition: CPTG_Holo_Blend.cpp:218
mrpt::nav::CPTG_Holo_Blend::m_expr_w
mrpt::expr::CRuntimeCompiledExpression m_expr_w
Definition: CPTG_Holo_Blend.h:88
mrpt::nav::CPTG_Holo_Blend::updateTPObstacleSingle
void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const override
Like updateTPObstacle() but for one direction only (k) in TP-Space.
Definition: CPTG_Holo_Blend.cpp:630
mrpt::nav::CPTG_Holo_Blend::CPTG_Holo_Blend
CPTG_Holo_Blend()
Definition: CPTG_Holo_Blend.cpp:791
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:21
mrpt::math::TPose2D::y
double y
Definition: TPose2D.h:30
mrpt::nav::CPTG_Holo_Blend::getPathStepDuration
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".
Definition: CPTG_Holo_Blend.cpp:790
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::nav::CPTG_Holo_Blend::V_MAX
double V_MAX
Definition: CPTG_Holo_Blend.h:81
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:16
mrpt::nav::CPTG_Holo_Blend::getPathStepCount
size_t getPathStepCount(uint16_t k) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
Definition: CPTG_Holo_Blend.cpp:434
mrpt::expr::CRuntimeCompiledExpression::register_symbol_table
void register_symbol_table(const std::map< std::string, double * > &variables)
Can be used before calling compile() to register additional variables by means of pointers instead of...
Definition: CRuntimeCompiledExpression.cpp:115
mrpt::nav::CPTG_Holo_Blend::loadDefaultParams
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Definition: CPTG_Holo_Blend.cpp:189
R
const float R
Definition: CKinematicChain.cpp:137
mrpt::nav::CPTG_Holo_Blend::m_expr_dir
double m_expr_dir
Definition: CPTG_Holo_Blend.h:89
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::math::solve_poly3
int solve_poly3(double *x, double a, double b, double c) noexcept
Solves cubic equation x^3 + a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:29
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise.
Definition: exceptions.h:125
mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
Definition: CParameterizedTrajectoryGenerator.cpp:208
mrpt::math::MatrixVectorBase< T, CMatrixFixed< T, ROWS, COLS > >::norm
T norm() const
Compute the L2 norm of a vector/array/matrix (the Euclidean distance to the origin,...
Definition: MatrixVectorBase_impl.h:505
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:55
MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:358
mrpt::nav::CPTG_Holo_Blend::getSupportedKinematicVelocityCommand
mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
Definition: CPTG_Holo_Blend.cpp:770
mrpt::nav::CPTG_Holo_Blend::PTG_IsIntoDomain
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
Definition: CPTG_Holo_Blend.cpp:408
mrpt::nav::CParameterizedTrajectoryGenerator::alpha2index
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
Definition: CParameterizedTrajectoryGenerator.cpp:223
mrpt::nav::CPTG_RobotShape_Circular::saveToConfigFile
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
Definition: CPTG_RobotShape_Circular.cpp:37
mrpt::nav::CPTG_Holo_Blend::expr_V
std::string expr_V
Definition: CPTG_Holo_Blend.h:84
CVectorFixed.h
eps
const double eps
Definition: distributions_unittest.cpp:21
mrpt::nav::CPTG_Holo_Blend::updateTPObstacle
void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const override
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,...
Definition: CPTG_Holo_Blend.cpp:753
MRPT_LOAD_HERE_CONFIG_VAR
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:352
mrpt::nav::CPTG_Holo_Blend::internal_construct_exprs
void internal_construct_exprs()
Definition: CPTG_Holo_Blend.cpp:801
mrpt::nav::CPTG_Holo_Blend::internal_get_T_ramp
double internal_get_T_ramp(const double dir) const
Evals expr_T_ramp.
Definition: CPTG_Holo_Blend.cpp:829
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:34
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:45
mrpt::nav::CPTG_Holo_Blend::getDescription
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
Definition: CPTG_Holo_Blend.cpp:259
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
Definition: CParameterizedTrajectoryGenerator.h:466
mrpt::math::CMatrixFixed::lu_solve
CMatrixFixed< T, ROWS, 1 > lu_solve(const CMatrixFixed< T, ROWS, 1 > &b) const
Solves the linear system Ax=b, returns x, with A this asymmetric matrix.
Definition: CMatrixFixed_impl.h:49
round.h
cmd
TCLAP::CmdLine cmd("system_control_rate_timer_example")
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:24
mrpt::nav::CPTG_Holo_Blend::internal_processNewRobotShape
void internal_processNewRobotShape() override
Will be called whenever the robot shape is set / updated.
Definition: CPTG_Holo_Blend.cpp:764
mrpt::math::solve_poly2
int solve_poly2(double a, double b, double c, double &r1, double &r2) noexcept
Solves equation a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:394
MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:382
mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
Definition: CParameterizedTrajectoryGenerator.cpp:190
mrpt::keep_min
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value.
Definition: core/include/mrpt/core/bits_math.h:145
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::nav::CPTG_Holo_Blend::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CPTG_Holo_Blend.cpp:299
mrpt::nav::CPTG_RobotShape_Circular::loadDefaultParams
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Definition: CPTG_RobotShape_Circular.cpp:27
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
calc_trans_distance_t_below_Tramp_abc_numeric
static double calc_trans_distance_t_below_Tramp_abc_numeric(double T, double a, double b, double c)
Definition: CPTG_Holo_Blend.cpp:98
mrpt::nav::CPTG_Holo_Blend::PATH_TIME_STEP
static double PATH_TIME_STEP
Duration of each PTG "step" (default: 10e-3=10 ms)
Definition: CPTG_Holo_Blend.h:74
mrpt::nav::CPTG_Holo_Blend::onNewNavDynamicState
void onNewNavDynamicState() override
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
Definition: CPTG_Holo_Blend.cpp:184
mrpt::nav::CPTG_Holo_Blend::internal_get_v
double internal_get_v(const double dir) const
Evals expr_v.
Definition: CPTG_Holo_Blend.cpp:819
mrpt::nav::CPTG_Holo_Blend::internal_deinitialize
void internal_deinitialize() override
This must be called to de-initialize the PTG if some parameter is to be changed.
Definition: CPTG_Holo_Blend.cpp:415
mrpt::nav::CPTG_RobotShape_Circular::internal_shape_saveToStream
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
Definition: CPTG_RobotShape_Circular.cpp:93
mrpt::kinematics::CVehicleVelCmd::Ptr
std::shared_ptr< CVehicleVelCmd > Ptr
Definition: CVehicleVelCmd.h:22
mrpt::nav::CPTG_Holo_Blend::expr_W
std::string expr_W
Definition: CPTG_Holo_Blend.h:84
PERFORMANCE_BENCHMARK
#define PERFORMANCE_BENCHMARK
mrpt::nav::CPTG_Holo_Blend::~CPTG_Holo_Blend
~CPTG_Holo_Blend() override
mrpt::signWithZero
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
Definition: core/include/mrpt/core/bits_math.h:97
mrpt::nav::CPTG_Holo_Blend
A PTG for circular-shaped robots with holonomic kinematics.
Definition: CPTG_Holo_Blend.h:27
mrpt::math::solve_poly4
int solve_poly4(double *x, double a, double b, double c, double d) noexcept
Solves quartic equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method.
Definition: poly_roots.cpp:247
CTimeLogger.h
mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile
void saveToConfigFile(mrpt::config::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list.
Definition: CParameterizedTrajectoryGenerator.cpp:101
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::nav::CPTG_Holo_Blend::directionToMotionCommand
mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const override
Converts a discretized "alpha" value into a feasible motion command or action.
Definition: CPTG_Holo_Blend.cpp:420
dir
const auto dir
Definition: chessboard_stereo_camera_calib_unittest.cpp:28
mrpt::expr::CRuntimeCompiledExpression::eval
double eval() const
Evaluates the current value of the precompiled formula.
Definition: CRuntimeCompiledExpression.cpp:107
mrpt::nav::CPTG_Holo_Blend::internal_get_w
double internal_get_w(const double dir) const
Evals expr_w.
Definition: CPTG_Holo_Blend.cpp:824
mrpt::kinematics::CVehicleVelCmd_Holo
Kinematic model for.
Definition: CVehicleVelCmd_Holo.h:20
mrpt::nav::CPTG_Holo_Blend::calc_trans_distance_t_below_Tramp
static double calc_trans_distance_t_below_Tramp(double k2, double k4, double vxi, double vyi, double t)
Axiliary function for computing the line-integral distance along the trajectory, handling special cas...
Definition: CPTG_Holo_Blend.cpp:152
mrpt::nav::CPTG_Holo_Blend::getPathDist
double getPathDist(uint16_t k, uint32_t step) const override
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
Definition: CPTG_Holo_Blend.cpp:511
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::nav::CPTG_Holo_Blend::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
Definition: CPTG_Holo_Blend.cpp:200
mrpt::nav::CPTG_Holo_Blend::m_expr_T_ramp
mrpt::expr::CRuntimeCompiledExpression m_expr_T_ramp
Definition: CPTG_Holo_Blend.h:88
mrpt::nav::CPTG_RobotShape_Circular::internal_shape_loadFromStream
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
Definition: CPTG_RobotShape_Circular.cpp:77
COMMON_PTG_DESIGN_PARAMS
#define COMMON_PTG_DESIGN_PARAMS
Definition: CPTG_Holo_Blend.cpp:55
mrpt::nav::CPTG_Holo_Blend::expr_T_ramp
std::string expr_T_ramp
Definition: CPTG_Holo_Blend.h:84
mrpt::nav::CPTG_Holo_Blend::internal_initialize
void internal_initialize(const std::string &cacheFilename=std::string(), const bool verbose=true) override
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...
Definition: CPTG_Holo_Blend.cpp:835
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: TPose2D.h:30
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:81
mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
Definition: CParameterizedTrajectoryGenerator.cpp:63
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::system
Definition: backtrace.h:15
mrpt::nav::CPTG_Holo_Blend::calc_trans_distance_t_below_Tramp_abc
static double calc_trans_distance_t_below_Tramp_abc(double t, double a, double b, double c)
Axiliary function for calc_trans_distance_t_below_Tramp() and others.
Definition: CPTG_Holo_Blend.cpp:136



Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020