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 }
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
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...
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble t
Definition: glext.h:3689
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:44
double RAD2DEG(const double x)
Radians to degrees.
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. ...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
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.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double DEG2RAD(const double x)
Degrees to radians.
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 < ...
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
void internal_shape_saveToStream(mrpt::serialization::CArchive &out) const
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
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...
void internal_deinitialize() override
This must be called to de-initialize the PTG if some parameter is to be changed.
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...
virtual double maxTimeInVelCmdNOP(int path_k) const override
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
T square(const T x)
Inline function for the square of a number.
double internal_get_v(const double dir) const
Evals expr_v.
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.
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
double ramp_time
: Blending time between current and target time.
This is the base class for any user-defined PTG.
This class allows loading and storing values and vectors of different types from a configuration text...
A PTG for circular-shaped robots with holonomic kinematics.
#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
double dir_local
: direction, relative to the current robot heading (radians).
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".
mrpt::expr::CRuntimeCompiledExpression m_expr_v
const GLubyte * c
Definition: glext.h:6313
std::vector< int > m_pathStepCountCache
double internal_get_w(const double dir) const
Evals expr_w.
virtual void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
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
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLubyte GLubyte b
Definition: glext.h:6279
const double eps
void onNewNavDynamicState() override
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
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,oy)
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
virtual bool supportVelCmdNOP() const override
Returns true if it is possible to stop sending velocity commands to the robot and, still, the robot controller will be able to keep following the last sent trajectory ("NOP" velocity commands).
GLsizei const GLchar ** string
Definition: glext.h:4101
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())
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.
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...
#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...
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.
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static double calc_trans_distance_t_below_Tramp_abc_numeric(double T, double a, double b, double c)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
const float R
static double PATH_TIME_STEP
Duration of each PTG "step" (default: 10e-3=10 ms)
double eval() const
Evaluates the current value of the precompiled formula.
#define MRPT_END
Definition: exceptions.h:266
double internal_get_T_ramp(const double dir) const
Evals expr_T_ramp.
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
GLuint in
Definition: glext.h:7274
Lightweight 2D pose.
mrpt::expr::CRuntimeCompiledExpression m_expr_T_ramp
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void internal_processNewRobotShape() override
Will be called whenever the robot shape is set / updated.
#define PERFORMANCE_BENCHMARK
GLenum GLint GLint y
Definition: glext.h:3538
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void loadShapeFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
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.
static double eps
Mathematical "epsilon", to detect ill-conditioned situations (e.g.
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
GLenum GLint x
Definition: glext.h:3538
std::shared_ptr< CVehicleVelCmd > Ptr
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
unsigned __int32 uint32_t
Definition: rptypes.h:47
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
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. ...
GLfloat GLfloat p
Definition: glext.h:6305
double rot_speed
: (rad/s) rotational speed for rotating such as the robot slowly faces forward.
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
virtual mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const override
Converts a discretized "alpha" value into a feasible motion command or action.
virtual mrpt::kinematics::CVehicleVelCmd::Ptr getSupportedKinematicVelocityCommand() const override
Returns an empty kinematic velocity command object of the type supported by this PTG.
#define COMMON_PTG_DESIGN_PARAMS
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
void internal_shape_loadFromStream(mrpt::serialization::CArchive &in)
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. ...
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
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
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::expr::CRuntimeCompiledExpression m_expr_w
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019