Main MRPT website > C++ reference for MRPT 1.9.9
CPTG_Holo_Blend.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "nav-precomp.h" // Precomp header
12 #include <mrpt/math/types_math.h>
14 #include <mrpt/core/round.h>
16 #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
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 
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 
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, double tolerance_dist) const
311 {
313 
314  MRPT_UNUSED_PARAM(tolerance_dist);
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  Eigen::Vector3d 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:
341  Eigen::Vector3d r;
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
360  Eigen::Matrix3d J;
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 
383  Eigen::Vector3d q_incr = J.householderQr().solve(r);
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 
427  cmd->vel = internal_get_v(dir_local);
428  cmd->dir_local = dir_local;
429  cmd->ramp_time = internal_get_T_ramp(dir_local);
430  cmd->rot_speed = mrpt::signWithZero(dir_local) * internal_get_w(dir_local);
431 
433 }
434 
436 {
437  if (m_pathStepCountCache.size() > k && m_pathStepCountCache[k] > 0)
438  return m_pathStepCountCache[k];
439 
440  uint32_t step;
441  if (!getPathStepForDist(k, this->refDistance, step))
442  {
444  "Could not solve closed-form distance for k=%u",
445  static_cast<unsigned>(k));
446  }
447  ASSERT_(step > 0);
448  if (m_pathStepCountCache.size() != m_alphaValuesCount)
449  {
450  m_pathStepCountCache.assign(m_alphaValuesCount, -1);
451  }
452  m_pathStepCountCache[k] = step;
453  return step;
454 }
455 
457  uint16_t k, uint32_t step, mrpt::math::TPose2D& p) const
458 {
459  const double t = PATH_TIME_STEP * step;
462  const double wf = mrpt::signWithZero(dir) * this->internal_get_w(dir);
463  const double TR2_ = 1.0 / (2 * T_ramp);
464 
465  // Translational part:
466  if (t < T_ramp)
467  {
468  p.x = vxi * t + t * t * TR2_ * (vxf - vxi);
469  p.y = vyi * t + t * t * TR2_ * (vyf - vyi);
470  }
471  else
472  {
473  p.x = T_ramp * 0.5 * (vxi + vxf) + (t - T_ramp) * vxf;
474  p.y = T_ramp * 0.5 * (vyi + vyf) + (t - T_ramp) * vyf;
475  }
476 
477  // Rotational part:
478  const double wi = m_nav_dyn_state.curVelLocal.omega;
479 
480  if (t < T_ramp)
481  {
482  // Time required to align completed?
483  const double a = TR2_ * (wf - wi), b = (wi), c = -dir;
484 
485  // Solves equation `a*x^2 + b*x + c = 0`.
486  double r1, r2;
487  int nroots = mrpt::math::solve_poly2(a, b, c, r1, r2);
488  if (nroots != 2)
489  {
490  p.phi = .0; // typical case: wi=wf=0
491  }
492  else
493  {
494  const double t_solve = std::max(r1, r2);
495  if (t > t_solve)
496  p.phi = dir;
497  else
498  p.phi = wi * t + t * t * TR2_ * (wf - wi);
499  }
500  }
501  else
502  {
503  // Time required to align completed?
504  const double t_solve = (dir - T_ramp * 0.5 * (wi + wf)) / wf + T_ramp;
505  if (t > t_solve)
506  p.phi = dir;
507  else
508  p.phi = T_ramp * 0.5 * (wi + wf) + (t - T_ramp) * wf;
509  }
510 }
511 
513 {
514  const double t = PATH_TIME_STEP * step;
516 
518  const double TR2_ = 1.0 / (2 * T_ramp);
519 
520  const double k2 = (vxf - vxi) * TR2_;
521  const double k4 = (vyf - vyi) * TR2_;
522 
523  if (t < T_ramp)
524  {
525  return calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, t);
526  }
527  else
528  {
529  const double dist_trans =
530  (t - T_ramp) * V_MAX +
531  calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
532  return dist_trans;
533  }
534 }
535 
537  uint16_t k, double dist, uint32_t& out_step) const
538 {
540 
543 
544  const double TR2_ = 1.0 / (2 * T_ramp);
545 
546  const double k2 = (vxf - vxi) * TR2_;
547  const double k4 = (vyf - vyi) * TR2_;
548 
549  // --------------------------------------
550  // Solution within t >= T_ramp ??
551  // --------------------------------------
552  const double dist_trans_T_ramp =
553  calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
554  double t_solved = -1;
555 
556  if (dist >= dist_trans_T_ramp)
557  {
558  // Good solution:
559  t_solved = T_ramp + (dist - dist_trans_T_ramp) / V_MAX;
560  }
561  else
562  {
563  // ------------------------------------
564  // Solutions within t < T_ramp
565  //
566  // Cases:
567  // 1) k2=k4=0 --> vi=vf. Path is straight line
568  // 2) b=c=0 -> vi=0
569  // 3) Otherwise, general case
570  // ------------------------------------
571  if (std::abs(k2) < eps && std::abs(k4) < eps)
572  {
573  // Case 1
574  t_solved = (dist) / V_MAX;
575  }
576  else
577  {
578  const double a = ((k2 * k2) * 4.0 + (k4 * k4) * 4.0);
579  const double b = (k2 * vxi * 4.0 + k4 * vyi * 4.0);
580  const double c = (vxi * vxi + vyi * vyi);
581 
582  // Numerically-ill case: b=c=0 (initial vel=0)
583  if (std::abs(b) < eps && std::abs(c) < eps)
584  {
585  // Case 2:
586  t_solved = sqrt(2.0) * 1.0 / pow(a, 1.0 / 4.0) * sqrt(dist);
587  }
588  else
589  {
590  // Case 3: general case with non-linear equation:
591  // dist = (t/2 + b/(4*a))*(a*t^2 + b*t + c)^(1/2) -
592  // (b*c^(1/2))/(4*a) + (log((b/2 + a*t)/a^(1/2) + (a*t^2 + b*t +
593  // c)^(1/2))*(- b^2/4 + a*c))/(2*a^(3/2)) - (log((b +
594  // 2*a^(1/2)*c^(1/2))/(2*a^(1/2)))*(- b^2/4 + a*c))/(2*a^(3/2))
595  // dist =
596  // (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);
597 
598  // We must solve this by iterating:
599  // Newton method:
600  // Minimize f(t)-dist = 0
601  // with: f(t)=calc_trans_distance_t_below_Tramp_abc(t)
602  // and: f'(t) = sqrt(a*t^2+b*t+c)
603 
604  t_solved = T_ramp * 0.6; // Initial value for starting
605  // interation inside the valid domain
606  // of the function t=[0,T_ramp]
607  for (int iters = 0; iters < 10; iters++)
608  {
609  double err = calc_trans_distance_t_below_Tramp_abc(
610  t_solved, a, b, c) -
611  dist;
612  const double diff =
613  std::sqrt(a * t_solved * t_solved + b * t_solved + c);
614  ASSERT_(std::abs(diff) > 1e-40);
615  t_solved -= (err) / diff;
616  if (t_solved < 0) t_solved = .0;
617  if (std::abs(err) < 1e-3) break; // Good enough!
618  }
619  }
620  }
621  }
622  if (t_solved >= 0)
623  {
624  out_step = mrpt::round(t_solved / PATH_TIME_STEP);
625  return true;
626  }
627  else
628  return false;
629 }
630 
632  double ox, double oy, uint16_t k, double& tp_obstacle_k) const
633 {
634  const double R = m_robotRadius;
637 
638  const double TR2_ = 1.0 / (2 * T_ramp);
639  const double TR_2 = T_ramp * 0.5;
640  const double T_ramp_thres099 = T_ramp * 0.99;
641  const double T_ramp_thres101 = T_ramp * 1.01;
642 
643  double sol_t = -1.0; // candidate solution for shortest time to collision
644 
645  // Note: It's tempting to try to solve first for t>T_ramp because it has
646  // simpler (faster) equations,
647  // but there are cases in which we will have valid collisions for t>T_ramp
648  // but other valid ones
649  // for t<T_ramp as well, so the only SAFE way to detect shortest distances
650  // is to check over increasing values of "t".
651 
652  // Try to solve first for t<T_ramp:
653  const double k2 = (vxf - vxi) * TR2_;
654  const double k4 = (vyf - vyi) * TR2_;
655 
656  // equation: a*t^4 + b*t^3 + c*t^2 + d*t + e = 0
657  const double a = (k2 * k2 + k4 * k4);
658  const double b = (k2 * vxi * 2.0 + k4 * vyi * 2.0);
659  const double c = -(k2 * ox * 2.0 + k4 * oy * 2.0 - vxi * vxi - vyi * vyi);
660  const double d = -(ox * vxi * 2.0 + oy * vyi * 2.0);
661  const double e = -R * R + ox * ox + oy * oy;
662 
663  double roots[4];
664  int num_real_sols = 0;
665  if (std::abs(a) > eps)
666  {
667  // General case: 4th order equation
668  // a * x^4 + b * x^3 + c * x^2 + d * x + e
669  num_real_sols =
670  mrpt::math::solve_poly4(roots, b / a, c / a, d / a, e / a);
671  }
672  else if (std::abs(b) > eps)
673  {
674  // Special case: k2=k4=0 (straight line path, no blend)
675  // 3rd order equation:
676  // b * x^3 + c * x^2 + d * x + e
677  num_real_sols = mrpt::math::solve_poly3(roots, c / b, d / b, e / b);
678  }
679  else
680  {
681  // Special case: 2nd order equation (a=b=0)
682  const double discr = d * d - 4 * c * e; // c*t^2 + d*t + e = 0
683  if (discr >= 0)
684  {
685  num_real_sols = 2;
686  roots[0] = (-d + sqrt(discr)) / (2 * c);
687  roots[1] = (-d - sqrt(discr)) / (2 * c);
688  }
689  else
690  {
691  num_real_sols = 0;
692  }
693  }
694 
695  for (int i = 0; i < num_real_sols; i++)
696  {
697  if (roots[i] == roots[i] && // not NaN
698  std::isfinite(roots[i]) && roots[i] >= .0 &&
699  roots[i] <= T_ramp * 1.01)
700  {
701  if (sol_t < 0)
702  sol_t = roots[i];
703  else
704  mrpt::keep_min(sol_t, roots[i]);
705  }
706  }
707 
708  // Invalid with these equations?
709  if (sol_t < 0 || sol_t > T_ramp_thres101)
710  {
711  // Now, attempt to solve with the equations for t>T_ramp:
712  sol_t = -1.0;
713 
714  const double c1 = TR_2 * (vxi - vxf) - ox;
715  const double c2 = TR_2 * (vyi - vyf) - oy;
716 
717  const double a = vf_mod * vf_mod;
718  const double b = 2 * (c1 * vxf + c2 * vyf);
719  const double c = c1 * c1 + c2 * c2 - R * R;
720 
721  const double discr = b * b - 4 * a * c;
722  if (discr >= 0)
723  {
724  const double sol_t0 = (-b + sqrt(discr)) / (2 * a);
725  const double sol_t1 = (-b - sqrt(discr)) / (2 * a);
726 
727  // Identify the shortest valid collision time:
728  if (sol_t0 < T_ramp && sol_t1 < T_ramp)
729  sol_t = -1.0;
730  else if (sol_t0 < T_ramp && sol_t1 >= T_ramp_thres099)
731  sol_t = sol_t1;
732  else if (sol_t1 < T_ramp && sol_t0 >= T_ramp_thres099)
733  sol_t = sol_t0;
734  else if (sol_t1 >= T_ramp_thres099 && sol_t0 >= T_ramp_thres099)
735  sol_t = std::min(sol_t0, sol_t1);
736  }
737  }
738 
739  // Valid solution?
740  if (sol_t < 0) return;
741  // Compute the transversed distance:
742  double dist;
743 
744  if (sol_t < T_ramp)
745  dist = calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, sol_t);
746  else
747  dist = (sol_t - T_ramp) * V_MAX +
748  calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
749 
750  // Store in the output variable:
751  internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
752 }
753 
755  double ox, double oy, std::vector<double>& tp_obstacles) const
756 {
758 
759  for (unsigned int k = 0; k < m_alphaValuesCount; k++)
760  {
761  updateTPObstacleSingle(ox, oy, k, tp_obstacles[k]);
762  } // end for each "k" alpha
763 }
764 
766 {
767  // Nothing to do in a closed-form PTG.
768 }
769 
772 {
775 }
776 
777 bool CPTG_Holo_Blend::supportVelCmdNOP() const { return true; }
778 double CPTG_Holo_Blend::maxTimeInVelCmdNOP(int path_k) const
779 {
780  // const double dir_local =
781  // CParameterizedTrajectoryGenerator::index2alpha(path_k);
782 
783  const size_t nSteps = getPathStepCount(path_k);
784  const double max_t =
785  PATH_TIME_STEP *
786  (nSteps *
787  0.7 /* leave room for obstacle detection ahead when we are far down the predicted PTG path */);
788  return max_t;
789 }
790 
791 double CPTG_Holo_Blend::getPathStepDuration() const { return PATH_TIME_STEP; }
793  : T_ramp_max(-1.0), V_MAX(-1.0), W_MAX(-1.0), turningRadiusReference(0.30)
794 {
796 }
797 
799  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
800  : turningRadiusReference(0.30)
801 {
803  this->loadFromConfigFile(cfg, sSection);
804 }
805 
808 {
809  std::map<std::string, double*> symbols;
810  symbols["dir"] = &m_expr_dir;
811  symbols["V_MAX"] = &V_MAX;
812  symbols["W_MAX"] = &W_MAX;
813  symbols["T_ramp_max"] = &T_ramp_max;
814  symbols["T_ramp_max"] = &T_ramp_max;
815 
819 
820  // Default expressions (can be overloaded by values in a config file)
821  expr_V = "V_MAX";
822  expr_W = "W_MAX";
823  expr_T_ramp = "T_ramp_max";
824 }
825 
826 double CPTG_Holo_Blend::internal_get_v(const double dir) const
827 {
828  const_cast<double&>(m_expr_dir) = dir;
829  return std::abs(m_expr_v.eval());
830 }
831 double CPTG_Holo_Blend::internal_get_w(const double dir) const
832 {
833  const_cast<double&>(m_expr_dir) = dir;
834  return std::abs(m_expr_w.eval());
835 }
836 double CPTG_Holo_Blend::internal_get_T_ramp(const double dir) const
837 {
838  const_cast<double&>(m_expr_dir) = dir;
839  return m_expr_T_ramp.eval();
840 }
841 
843  const std::string& cacheFilename, const bool verbose)
844 {
845  // No need to initialize anything, just do some params sanity checks:
846  ASSERT_(T_ramp_max > 0);
847  ASSERT_(V_MAX > 0);
848  ASSERT_(W_MAX > 0);
850  ASSERT_(m_robotRadius > 0);
851 
852  // Compile user-given expressions:
853  m_expr_v.compile(expr_V, std::map<std::string, double>(), "expr_V");
854  m_expr_w.compile(expr_W, std::map<std::string, double>(), "expr_w");
856  expr_T_ramp, std::map<std::string, double>(), "expr_T_ramp");
857 
858 #ifdef DO_PERFORMANCE_BENCHMARK
859  tl.dumpAllStats();
860 #endif
861 
862  m_pathStepCountCache.clear();
863 }
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:124
mrpt::nav::CParameterizedTrajectoryGenerator::loadDefaultParams
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
Definition: CParameterizedTrajectoryGenerator.cpp:40
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:573
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:47
mrpt::nav::CPTG_Holo_Blend::maxTimeInVelCmdNOP
virtual 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:778
mrpt::nav::CPTG_Holo_Blend::T_ramp_max
double T_ramp_max
Definition: CPTG_Holo_Blend.h:82
mrpt::nav::CPTG_Holo_Blend::supportVelCmdNOP
virtual bool supportVelCmdNOP() const override
Returns true if it is possible to stop sending velocity commands to the robot and,...
Definition: CPTG_Holo_Blend.cpp:777
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:536
mrpt::nav::CPTG_Holo_Blend::m_expr_v
mrpt::expr::CRuntimeCompiledExpression m_expr_v
Definition: CPTG_Holo_Blend.h:90
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
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:85
t
GLdouble GLdouble t
Definition: glext.h:3689
uint16_t
unsigned __int16 uint16_t
Definition: rptypes.h:44
mrpt::nav::CPTG_Holo_Blend::m_pathStepCountCache
std::vector< int > m_pathStepCountCache
Definition: CPTG_Holo_Blend.h:87
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:282
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:29
CPTG_Holo_Blend.h
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:43
mrpt::nav::CPTG_Holo_Blend::eps
static double eps
Mathematical "epsilon", to detect ill-conditioned situations (e.g.
Definition: CPTG_Holo_Blend.h:79
mrpt::nav::CParameterizedTrajectoryGenerator::internal_readFromStream
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
Definition: CParameterizedTrajectoryGenerator.cpp:148
c
const GLubyte * c
Definition: glext.h:6313
mrpt::nav::CPTG_Holo_Blend::W_MAX
double W_MAX
Definition: CPTG_Holo_Blend.h:83
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:456
mrpt::nav::CPTG_Holo_Blend::saveToConfigFile
virtual 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:90
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:631
mrpt::kinematics::CVehicleVelCmd_Holo::vel
double vel
speed(m / s)
Definition: CVehicleVelCmd_Holo.h:26
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::signWithZero
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
Definition: core/include/mrpt/core/bits_math.h:75
mrpt::nav::CPTG_Holo_Blend::CPTG_Holo_Blend
CPTG_Holo_Blend()
Definition: CPTG_Holo_Blend.cpp:792
mrpt::nav
Definition: CAbstractHolonomicReactiveMethod.h:23
mrpt::nav::CPTG_Holo_Blend::getPathStepDuration
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".
Definition: CPTG_Holo_Blend.cpp:791
mrpt::nav::CPTG_Holo_Blend::V_MAX
double V_MAX
Definition: CPTG_Holo_Blend.h:83
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
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:435
uint8_t
unsigned char uint8_t
Definition: rptypes.h:41
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:88
mrpt::nav::CPTG_Holo_Blend::loadDefaultParams
virtual 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:138
mrpt::nav::CPTG_Holo_Blend::m_expr_dir
double m_expr_dir
Definition: CPTG_Holo_Blend.h:91
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
alpha
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
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
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::RAD2DEG
double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:48
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:118
mrpt::nav::CParameterizedTrajectoryGenerator::index2alpha
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
Definition: CParameterizedTrajectoryGenerator.cpp:201
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:330
mrpt::nav::CPTG_Holo_Blend::getSupportedKinematicVelocityCommand
virtual 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:771
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:216
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:38
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
mrpt::kinematics::CVehicleVelCmd_Holo::ramp_time
double ramp_time
: Blending time between current and target time.
Definition: CVehicleVelCmd_Holo.h:31
mrpt::nav::CPTG_Holo_Blend::expr_V
std::string expr_V
Definition: CPTG_Holo_Blend.h:86
eps
const double eps
Definition: distributions_unittest.cpp:19
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:754
MRPT_LOAD_HERE_CONFIG_VAR
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:324
mrpt::nav::CPTG_Holo_Blend::internal_construct_exprs
void internal_construct_exprs()
Definition: CPTG_Holo_Blend.cpp:807
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:836
mrpt::kinematics::CVehicleVelCmd_Holo::dir_local
double dir_local
: direction, relative to the current robot heading (radians).
Definition: CVehicleVelCmd_Holo.h:29
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
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::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::nav::CParameterizedTrajectoryGenerator::m_alphaValuesCount
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
Definition: CParameterizedTrajectoryGenerator.h:449
b
GLubyte GLubyte b
Definition: glext.h:6279
mrpt::nav::CPTG_Holo_Blend::~CPTG_Holo_Blend
virtual ~CPTG_Holo_Blend()
Definition: CPTG_Holo_Blend.cpp:806
round.h
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: lightweight_geom_data.h:186
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:765
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:401
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:357
mrpt::nav::CParameterizedTrajectoryGenerator::internal_writeToStream
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
Definition: CParameterizedTrajectoryGenerator.cpp:183
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:28
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Definition: CSerializable.h:114
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:76
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:826
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:94
mrpt::kinematics::CVehicleVelCmd::Ptr
std::shared_ptr< CVehicleVelCmd > Ptr
Definition: CVehicleVelCmd.h:24
mrpt::nav::CPTG_Holo_Blend::expr_W
std::string expr_W
Definition: CPTG_Holo_Blend.h:86
PERFORMANCE_BENCHMARK
#define PERFORMANCE_BENCHMARK
mrpt::nav::CPTG_Holo_Blend
A PTG for circular-shaped robots with holonomic kinematics.
Definition: CPTG_Holo_Blend.h:28
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
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:254
CTimeLogger.h
mrpt::nav::CParameterizedTrajectoryGenerator::saveToConfigFile
virtual 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:94
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::nav::CPTG_Holo_Blend::directionToMotionCommand
virtual 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
mrpt::expr::CRuntimeCompiledExpression::eval
double eval() const
Evaluates the current value of the precompiled formula.
Definition: CRuntimeCompiledExpression.cpp:82
mrpt::nav::CPTG_Holo_Blend::internal_get_w
double internal_get_w(const double dir) const
Evals expr_w.
Definition: CPTG_Holo_Blend.cpp:831
mrpt::kinematics::CVehicleVelCmd_Holo
Kinematic model for.
Definition: CVehicleVelCmd_Holo.h:21
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
in
GLuint in
Definition: glext.h:7274
string
GLsizei const GLchar ** string
Definition: glext.h:4101
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:512
mrpt::kinematics::CVehicleVelCmd_Holo::rot_speed
double rot_speed
: (rad/s) rotational speed for rotating such as the robot slowly faces forward.
Definition: CVehicleVelCmd_Holo.h:34
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::nav::CPTG_Holo_Blend::loadFromConfigFile
virtual 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:90
mrpt::nav::CPTG_RobotShape_Circular::internal_shape_loadFromStream
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
Definition: CPTG_RobotShape_Circular.cpp:78
COMMON_PTG_DESIGN_PARAMS
#define COMMON_PTG_DESIGN_PARAMS
Definition: CPTG_Holo_Blend.cpp:55
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::nav::CPTG_Holo_Blend::expr_T_ramp
std::string expr_T_ramp
Definition: CPTG_Holo_Blend.h:86
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:842
types_math.h
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:47
mrpt::nav::CParameterizedTrajectoryGenerator
This is the base class for any user-defined PTG.
Definition: CParameterizedTrajectoryGenerator.h:76
x
GLenum GLint x
Definition: glext.h:3538
mrpt::nav::CParameterizedTrajectoryGenerator::loadFromConfigFile
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
Definition: CParameterizedTrajectoryGenerator.cpp:58
mrpt::system
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
Definition: math_frwds.h:25
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
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
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST