MRPT  1.9.9
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, 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  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 
425  auto* cmd = new mrpt::kinematics::CVehicleVelCmd_Holo();
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 }
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...
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 loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
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:241
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. ...
double x
X,Y coordinates.
Definition: TPose2D.h:30
#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.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
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.
T norm() const
Compute the L2 norm of a vector/array/matrix (the Euclidean distance to the origin, taking all the elements as a single vector).
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
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 < ...
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
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...
double maxTimeInVelCmdNOP(int path_k) const override
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:120
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.
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.
#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
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".
mrpt::expr::CRuntimeCompiledExpression m_expr_v
constexpr double DEG2RAD(const double x)
Degrees to radians.
std::vector< int > m_pathStepCountCache
double internal_get_w(const double dir) const
Evals expr_w.
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:247
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...
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.
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).
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.
return_t square(const num_t x)
Inline function for the square of a number.
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:54
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
mrpt::vision::TStereoCalibResults out
static double PATH_TIME_STEP
Duration of each PTG "step" (default: 10e-3=10 ms)
constexpr double RAD2DEG(const double x)
Radians to degrees.
double eval() const
Evaluates the current value of the precompiled formula.
#define MRPT_END
Definition: exceptions.h:245
double internal_get_T_ramp(const double dir) const
Evals expr_T_ramp.
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)
Lightweight 2D pose.
Definition: TPose2D.h:22
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
#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.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
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:69
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. ...
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
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::kinematics::CVehicleVelCmd::Ptr directionToMotionCommand(uint16_t k) const override
Converts a discretized "alpha" value into a feasible motion command or action.
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:394
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
#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:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at miƩ mar 25 23:05:41 CET 2020