Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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/utils/types_math.h>
13 #include <mrpt/utils/CStream.h>
14 #include <mrpt/utils/round.h>
15 #include <mrpt/utils/CTimeLogger.h>
16 #include <mrpt/math/poly_roots.h>
18 
19 using namespace mrpt::nav;
20 using namespace mrpt::utils;
21 using namespace mrpt::system;
22 
24 
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 = V_MAX*sin(alpha)
30 - T_ramp_max max time for velocity interpolation (xip,yip) -> (xfp, yfp)
31 - W_MAX: Rotational velocity for robot heading forwards.
32 
33 Number of steps "d" for each PTG path "k":
34 - Step = time increment PATH_TIME_STEP
35 
36 */
37 
38 // Uncomment only for benchmarking during development
39 //#define DO_PERFORMANCE_BENCHMARK
40 
41 #ifdef DO_PERFORMANCE_BENCHMARK
42  mrpt::utils::CTimeLogger tl_holo("CPTG_Holo_Blend");
43  #define PERFORMANCE_BENCHMARK CTimeLoggerEntry tle(tl_holo, __CURRENT_FUNCTION_NAME__);
44 #else
45  #define PERFORMANCE_BENCHMARK
46 #endif
47 
48 double CPTG_Holo_Blend::PATH_TIME_STEP = 10e-3; // 10 ms
49 double CPTG_Holo_Blend::eps = 1e-4; // epsilon for detecting 1/0 situation
50 
51 // As a macro instead of a function (uglier) to allow for const variables (safer)
52 #define COMMON_PTG_DESIGN_PARAMS \
53  const double vxi = m_nav_dyn_state.curVelLocal.vx, vyi = m_nav_dyn_state.curVelLocal.vy; \
54  const double vf_mod = internal_get_v(dir); \
55  const double vxf = vf_mod*cos(dir), vyf = vf_mod* sin(dir); \
56  const double T_ramp = internal_get_T_ramp(dir);
57 
58 #if 0
59 static double calc_trans_distance_t_below_Tramp_abc_analytic(double t, double a, double b, double c)
60 {
62 
63  ASSERT_(t >= 0);
64  if (t == 0.0) return .0;
65 
66  double dist;
67  // Handle special case: degenerate sqrt(a*t^2+b*t+c) = sqrt((t-r)^2) = |t-r|
68  const double discr = b*b - 4 * a*c;
69  if (std::abs(discr)<1e-6)
70  {
71  const double r = -b / (2 * a);
72  // dist= definite integral [0,t] of: |t-r| dt
73  dist = r*std::abs(r)*0.5 - (r - t)*std::abs(r - t)*0.5;
74  }
75  else
76  {
77  // General case:
78  // Indefinite integral of sqrt(a*t^2+b*t+c):
79  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);
80  // Limit when t->0:
81  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);
82  dist = int_t - int_t0;// Definite integral [0,t]
83  }
84 #ifdef _DEBUG
85  using namespace mrpt;
87  ASSERT_(dist >= .0);
88 #endif
89  return dist;
90 }
91 #endif
92 
93 // Numeric integration of: sqrt(a*t^2+b*t+c) for t=[0,T]
94 static double calc_trans_distance_t_below_Tramp_abc_numeric(double T, double a, double b, double c)
95 {
97 
98  double d = .0;
99  const unsigned int NUM_STEPS = 15;
100 
101  ASSERT_(a >= .0);
102  ASSERT_(c >= .0);
103  double feval_t = std::sqrt(c); // t (initial: t=0)
104  double feval_tp1; // t+1
105 
106  const double At = T / (NUM_STEPS);
107  double t = .0;
108  for (unsigned int i = 0; i < NUM_STEPS; i++)
109  {
110  // Eval function at t+1:
111  t += At;
112  double dd = a*t*t + b*t + c;
113 
114  // handle numerical innacuracies near t=T_ramp:
115  ASSERT_(dd>-1e-5);
116  if (dd < 0) dd = .0;
117 
118  feval_tp1 = sqrt(dd);
119 
120  // Trapezoidal rule:
121  d += At*(feval_t+ feval_tp1)*0.5;
122 
123  // for next step:
124  feval_t = feval_tp1;
125  }
126 
127  return d;
128 }
129 
130 // Axiliary function for calc_trans_distance_t_below_Tramp() and others:
131 double CPTG_Holo_Blend::calc_trans_distance_t_below_Tramp_abc(double t, double a,double b, double c)
132 {
133  // JLB (29 Jan 2017): it turns out that numeric integration is *faster* and more accurate (does not have "special cases")...
134 #if 0
135  double ret = calc_trans_distance_t_below_Tramp_abc_analytic(t, a, b, c);
136 #else
138 #endif
139 
140  return ret;
141 }
142 
143 
144 // Axiliary function for computing the line-integral distance along the trajectory, handling special cases of 1/0:
145 double CPTG_Holo_Blend::calc_trans_distance_t_below_Tramp(double k2, double k4, double vxi,double vyi, double t)
146 {
147 /*
148 dd = sqrt( (4*k2^2 + 4*k4^2)*t^2 + (4*k2*vxi + 4*k4*vyi)*t + vxi^2 + vyi^2 ) dt
149  a t^2 + b t + c
150 */
151  const double c = (vxi*vxi + vyi*vyi);
152  if (std::abs(k2)>eps || std::abs(k4)>eps)
153  {
154  const double a = ((k2*k2)*4.0 + (k4*k4)*4.0);
155  const double b = (k2*vxi*4.0 + k4*vyi*4.0);
156 
157  // Numerically-ill case: b=c=0 (initial vel=0)
158  if (std::abs(b)<eps && std::abs(c)<eps) {
159  // Indefinite integral of simplified case: sqrt(a)*t
160  const double int_t = sqrt(a)*(t*t)*0.5;
161  return int_t; // Definite integral [0,t]
162  }
163  else {
164  return calc_trans_distance_t_below_Tramp_abc(t,a,b,c);
165  }
166  }
167  else {
168  return std::sqrt(c)*t;
169  }
170 }
171 
173 {
174  m_pathStepCountCache.assign(m_alphaValuesCount, -1); // mark as invalid
175 }
176 
178 {
181 
182  m_alphaValuesCount = 31;
183  T_ramp_max = 0.9;
184  V_MAX = 1.0;
185  W_MAX = mrpt::utils::DEG2RAD(40);
186 }
187 
189 {
192 
193  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(T_ramp_max ,double, T_ramp_max, cfg,sSection);
194  MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(v_max_mps ,double, V_MAX, cfg,sSection);
195  MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(w_max_dps ,double, W_MAX, cfg,sSection);
196  MRPT_LOAD_CONFIG_VAR(turningRadiusReference ,double, cfg,sSection);
197 
198  MRPT_LOAD_HERE_CONFIG_VAR(expr_V, string, expr_V, cfg, sSection);
199  MRPT_LOAD_HERE_CONFIG_VAR(expr_W, string, expr_W, cfg, sSection);
200  MRPT_LOAD_HERE_CONFIG_VAR(expr_T_ramp, string, expr_T_ramp, cfg, sSection);
201 }
203 {
204  MRPT_START
205  const int WN = 25, WV = 30;
206 
208 
209  cfg.write(sSection,"T_ramp_max",T_ramp_max, WN,WV, "Max duration of the velocity interpolation since a vel_cmd is issued [s].");
210  cfg.write(sSection,"v_max_mps",V_MAX, WN,WV, "Maximum linear velocity for trajectories [m/s].");
211  cfg.write(sSection,"w_max_dps",mrpt::utils::RAD2DEG(W_MAX), WN,WV, "Maximum angular velocity for trajectories [deg/s].");
212  cfg.write(sSection,"turningRadiusReference",turningRadiusReference, WN,WV, "An approximate dimension of the robot (not a critical parameter) [m].");
213 
214  cfg.write(sSection, "expr_V", expr_V, WN, WV, "Math expr for |V| as a function of `dir`,`V_MAX`,`W_MAX`,`T_ramp_max`.");
215  cfg.write(sSection, "expr_W", expr_W, WN, WV, "Math expr for |omega| (disregarding the sign, only the module) as a function of `dir`,`V_MAX`,`W_MAX`,`T_ramp_max`.");
216  cfg.write(sSection, "expr_T_ramp", expr_T_ramp, WN, WV, "Math expr for `T_ramp` as a function of `dir`,`V_MAX`,`W_MAX`,`T_ramp_max`.");
217 
219 
220  MRPT_END
221 }
222 
223 
225 {
226  return mrpt::format("PTG_Holo_Blend_Tramp=%.03f_Vmax=%.03f_Wmax=%.03f",T_ramp_max,V_MAX,W_MAX);
227 }
228 
229 
231 {
233 
234  switch (version)
235  {
236  case 0:
237  case 1:
238  case 2:
239  case 3:
240  case 4:
241  if (version>=1) {
243  }
244 
245  in >> T_ramp_max >> V_MAX >> W_MAX >> turningRadiusReference;
246  if (version==2) {
247  double dummy_maxAllowedDirAngle; // removed in v3
248  in >> dummy_maxAllowedDirAngle;
249  }
250  if (version >= 4) {
251  in >> expr_V >> expr_W >> expr_T_ramp;
252  }
253  break;
254  default:
256  };
257 }
258 
260 {
261  if (version)
262  {
263  *version = 4;
264  return;
265  }
266 
269 
270  out << T_ramp_max << V_MAX << W_MAX << turningRadiusReference;
271  out << expr_V << expr_W << expr_T_ramp;
272 }
273 
274 bool CPTG_Holo_Blend::inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist) const
275 {
277 
278  MRPT_UNUSED_PARAM(tolerance_dist);
279  ASSERT_(x!=0 || y!=0);
280 
281  const double err_threshold = 1e-3;
282  const double T_ramp = T_ramp_max;
283  const double vxi = m_nav_dyn_state.curVelLocal.vx, vyi = m_nav_dyn_state.curVelLocal.vy;
284 
285  // Use a Newton iterative non-linear optimizer to find the "exact" solution for (t,alpha)
286  // in each case: (1) t<T_ramp and (2) t>T_ramp
287 
288  // Initial value:
289  Eigen::Vector3d q; // [t vxf vyf]
290  q[0]=T_ramp_max*1.1;
291  q[1]=V_MAX*x/sqrt(x*x+y*y);
292  q[2]=V_MAX*y/sqrt(x*x+y*y);
293 
294  // Iterate: case (2) t > T_ramp
295  double err_mod=1e7;
296  bool sol_found = false;
297  for (int iters=0;!sol_found && iters<25;iters++)
298  {
299  const double TR_ = 1.0/(T_ramp);
300  const double TR2_ = 1.0/(2*T_ramp);
301 
302  // Eval residual:
303  Eigen::Vector3d r;
304  if (q[0]>=T_ramp)
305  {
306  r[0] = 0.5*T_ramp *( vxi + q[1] ) + (q[0]-T_ramp)*q[1] - x;
307  r[1] = 0.5*T_ramp *( vyi + q[2] ) + (q[0]-T_ramp)*q[2] - y;
308  }
309  else
310  {
311  r[0] = vxi * q[0] + q[0]*q[0] * TR2_ * (q[1]-vxi) - x;
312  r[1] = vyi * q[0] + q[0]*q[0] * TR2_ * (q[2]-vyi) - y;
313  }
314  const double alpha = atan2(q[2], q[1]);
315  const double V_MAXsq = mrpt::math::square(this->internal_get_v(alpha));
316  r[2] = q[1]*q[1]+q[2]*q[2] - V_MAXsq;
317 
318  // Jacobian: q=[t vxf vyf] q0=t q1=vxf q2=vyf
319  // dx/dt dx/dvxf dx/dvyf
320  // dy/dt dy/dvxf dy/dvyf
321  // dVF/dt dVF/dvxf dVF/dvyf
322  Eigen::Matrix3d J;
323  if (q[0]>=T_ramp)
324  {
325  J(0,0) = q[1]; J(0,1) = 0.5*T_ramp+q[0]; J(0,2) = 0.0;
326  J(1,0) = q[2]; J(1,1) = 0.0; J(1,2) = 0.5*T_ramp+q[0];
327  }
328  else
329  {
330  J(0,0) = vxi + q[0]*TR_*(q[1]-vxi); J(0,1) = TR2_*q[0]*q[0]; J(0,2) = 0.0;
331  J(1,0) = vyi + q[0]*TR_*(q[2]-vyi); J(1,1) = 0.0; J(1,2) = TR2_*q[0]*q[0];
332  }
333  J(2,0) = 0.0; J(2,1) = 2*q[1]; J(2,2) = 2*q[2];
334 
335  Eigen::Vector3d q_incr = J.householderQr().solve(r);
336  q-=q_incr;
337 
338  err_mod = r.norm();
339  sol_found = (err_mod<err_threshold);
340  }
341 
342  if (sol_found && q[0]>=.0)
343  {
344  const double alpha = atan2(q[2],q[1]);
346 
347  const double solved_t = q[0];
348  const unsigned int solved_step = solved_t/PATH_TIME_STEP;
349  const double found_dist = this->getPathDist(out_k, solved_step);
350 
351  out_d = found_dist / this->refDistance;
352  return true;
353  }
354  else {
355  return false;
356  }
357 }
358 
359 bool CPTG_Holo_Blend::PTG_IsIntoDomain(double x, double y ) const
360 {
361  int k;
362  double d;
363  return inverseMap_WS2TP(x,y,k,d);
364 }
365 
367 {
368  // Nothing to do in a closed-form PTG.
369 }
370 
371 mrpt::kinematics::CVehicleVelCmdPtr CPTG_Holo_Blend::directionToMotionCommand( uint16_t k) const
372 {
373  const double dir_local = CParameterizedTrajectoryGenerator::index2alpha(k);
374 
376  cmd->vel = internal_get_v(dir_local);
377  cmd->dir_local = dir_local;
378  cmd->ramp_time = internal_get_T_ramp(dir_local);
379  cmd->rot_speed = mrpt::utils::signWithZero(dir_local) * internal_get_w(dir_local);
380 
381  return mrpt::kinematics::CVehicleVelCmdPtr(cmd);
382 }
383 
385 {
386  if (m_pathStepCountCache.size() > k && m_pathStepCountCache[k] > 0)
387  return m_pathStepCountCache[k];
388 
389  uint32_t step;
390  if (!getPathStepForDist(k,this->refDistance,step)) {
391  THROW_EXCEPTION_FMT("Could not solve closed-form distance for k=%u",static_cast<unsigned>(k));
392  }
393  ASSERT_(step>0);
394  if (m_pathStepCountCache.size() != m_alphaValuesCount) {
395  m_pathStepCountCache.assign(m_alphaValuesCount, -1);
396  }
397  m_pathStepCountCache[k] = step;
398  return step;
399 }
400 
402 {
403  const double t = PATH_TIME_STEP*step;
406  const double wf = mrpt::utils::signWithZero(dir) * this->internal_get_w(dir);
407  const double TR2_ = 1.0/(2*T_ramp);
408 
409  // Translational part:
410  if (t<T_ramp)
411  {
412  p.x = vxi * t + t*t * TR2_ * (vxf - vxi);
413  p.y = vyi * t + t*t * TR2_ * (vyf - vyi);
414  }
415  else
416  {
417  p.x = T_ramp *0.5*(vxi + vxf) + (t - T_ramp) * vxf;
418  p.y = T_ramp *0.5*(vyi + vyf) + (t - T_ramp) * vyf;
419  }
420 
421  // Rotational part:
422  const double wi = m_nav_dyn_state.curVelLocal.omega;
423 
424  if (t<T_ramp)
425  {
426  // Time required to align completed?
427  const double a = TR2_ * (wf - wi), b = (wi), c = -dir;
428 
429  // Solves equation `a*x^2 + b*x + c = 0`.
430  double r1, r2;
431  int nroots = mrpt::math::solve_poly2(a,b,c, r1,r2);
432  if (nroots!=2) {
433  p.phi = .0; // typical case: wi=wf=0
434  } else {
435  const double t_solve = std::max(r1,r2);
436  if (t > t_solve)
437  p.phi = dir;
438  else
439  p.phi = wi*t + t*t* TR2_ * (wf-wi);
440  }
441  }
442  else
443  {
444  // Time required to align completed?
445  const double t_solve = (dir - T_ramp *0.5*(wi + wf))/wf + T_ramp;
446  if (t > t_solve)
447  p.phi = dir;
448  else
449  p.phi = T_ramp *0.5*(wi + wf) + (t - T_ramp) * wf;
450  }
451 }
452 
454 {
455  const double t = PATH_TIME_STEP*step;
457 
459  const double TR2_ = 1.0/(2*T_ramp);
460 
461  const double k2 = (vxf-vxi)*TR2_;
462  const double k4 = (vyf-vyi)*TR2_;
463 
464  if (t<T_ramp)
465  {
466  return calc_trans_distance_t_below_Tramp(k2,k4,vxi,vyi,t);
467  }
468  else
469  {
470  const double dist_trans = (t-T_ramp) * V_MAX + calc_trans_distance_t_below_Tramp(k2,k4,vxi,vyi,T_ramp);
471  return dist_trans;
472  }
473 }
474 
475 bool CPTG_Holo_Blend::getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const
476 {
478 
481 
482  const double TR2_ = 1.0/(2*T_ramp);
483 
484  const double k2 = (vxf-vxi)*TR2_;
485  const double k4 = (vyf-vyi)*TR2_;
486 
487  // --------------------------------------
488  // Solution within t >= T_ramp ??
489  // --------------------------------------
490  const double dist_trans_T_ramp = calc_trans_distance_t_below_Tramp(k2,k4,vxi,vyi,T_ramp);
491  double t_solved = -1;
492 
493  if (dist>=dist_trans_T_ramp)
494  {
495  // Good solution:
496  t_solved = T_ramp + (dist-dist_trans_T_ramp)/V_MAX;
497  }
498  else
499  {
500  // ------------------------------------
501  // Solutions within t < T_ramp
502  //
503  // Cases:
504  // 1) k2=k4=0 --> vi=vf. Path is straight line
505  // 2) b=c=0 -> vi=0
506  // 3) Otherwise, general case
507  // ------------------------------------
508  if (std::abs(k2)<eps && std::abs(k4)<eps)
509  {
510  // Case 1
511  t_solved = (dist)/V_MAX;
512  }
513  else
514  {
515  const double a = ((k2*k2)*4.0+(k4*k4)*4.0);
516  const double b = (k2*vxi*4.0+k4*vyi*4.0);
517  const double c = (vxi*vxi+vyi*vyi);
518 
519  // Numerically-ill case: b=c=0 (initial vel=0)
520  if (std::abs(b)<eps && std::abs(c)<eps)
521  {
522  // Case 2:
523  t_solved = sqrt(2.0)*1.0/pow(a,1.0/4.0)*sqrt(dist);
524  }
525  else
526  {
527  // Case 3: general case with non-linear equation:
528  // dist = (t/2 + b/(4*a))*(a*t^2 + b*t + c)^(1/2) - (b*c^(1/2))/(4*a) + (log((b/2 + a*t)/a^(1/2) + (a*t^2 + b*t + c)^(1/2))*(- b^2/4 + a*c))/(2*a^(3/2)) - (log((b + 2*a^(1/2)*c^(1/2))/(2*a^(1/2)))*(- b^2/4 + a*c))/(2*a^(3/2))
529  // dist = (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);
530 
531  // We must solve this by iterating:
532  // Newton method:
533  // Minimize f(t)-dist = 0
534  // with: f(t)=calc_trans_distance_t_below_Tramp_abc(t)
535  // and: f'(t) = sqrt(a*t^2+b*t+c)
536 
537  t_solved = T_ramp*0.6; // Initial value for starting interation inside the valid domain of the function t=[0,T_ramp]
538  for (int iters=0;iters<10;iters++)
539  {
540  double err = calc_trans_distance_t_below_Tramp_abc(t_solved,a,b,c) - dist;
541  const double diff = std::sqrt(a*t_solved*t_solved+b*t_solved+c);
542  ASSERT_(std::abs(diff)>1e-40);
543  t_solved -= (err) / diff;
544  if (t_solved<0)
545  t_solved=.0;
546  if (std::abs(err)<1e-3)
547  break; // Good enough!
548  }
549  }
550  }
551  }
552  if (t_solved>=0)
553  {
554  out_step = mrpt::utils::round( t_solved / PATH_TIME_STEP );
555  return true;
556  }
557  else return false;
558 }
559 
560 
561 void CPTG_Holo_Blend::updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const
562 {
563  const double R = m_robotRadius;
566 
567  const double TR2_ = 1.0 / (2 * T_ramp);
568  const double TR_2 = T_ramp*0.5;
569  const double T_ramp_thres099 = T_ramp*0.99;
570  const double T_ramp_thres101 = T_ramp*1.01;
571 
572  double sol_t = -1.0; // candidate solution for shortest time to collision
573 
574  // Note: It's tempting to try to solve first for t>T_ramp because it has simpler (faster) equations,
575  // but there are cases in which we will have valid collisions for t>T_ramp but other valid ones
576  // for t<T_ramp as well, so the only SAFE way to detect shortest distances is to check over increasing values of "t".
577 
578  // Try to solve first for t<T_ramp:
579  const double k2 = (vxf - vxi)*TR2_;
580  const double k4 = (vyf - vyi)*TR2_;
581 
582  // equation: a*t^4 + b*t^3 + c*t^2 + d*t + e = 0
583  const double a = (k2*k2 + k4*k4);
584  const double b = (k2*vxi*2.0 + k4*vyi*2.0);
585  const double c = -(k2*ox*2.0 + k4*oy*2.0 - vxi*vxi - vyi*vyi);
586  const double d = -(ox*vxi*2.0 + oy*vyi*2.0);
587  const double e = -R*R + ox*ox + oy*oy;
588 
589  double roots[4];
590  int num_real_sols = 0;
591  if (std::abs(a)>eps)
592  {
593  // General case: 4th order equation
594  // a * x^4 + b * x^3 + c * x^2 + d * x + e
595  num_real_sols = mrpt::math::solve_poly4(roots, b / a, c / a, d / a, e / a);
596  }
597  else if (std::abs(b)>eps) {
598  // Special case: k2=k4=0 (straight line path, no blend)
599  // 3rd order equation:
600  // b * x^3 + c * x^2 + d * x + e
601  num_real_sols = mrpt::math::solve_poly3(roots, c / b, d / b, e / b);
602  }
603  else
604  {
605  // Special case: 2nd order equation (a=b=0)
606  const double discr = d*d - 4 * c*e; // c*t^2 + d*t + e = 0
607  if (discr >= 0)
608  {
609  num_real_sols = 2;
610  roots[0] = (-d + sqrt(discr)) / (2 * c);
611  roots[1] = (-d - sqrt(discr)) / (2 * c);
612  }
613  else {
614  num_real_sols = 0;
615  }
616  }
617 
618  for (int i = 0; i<num_real_sols; i++)
619  {
620  if (roots[i] == roots[i] && // not NaN
621  mrpt::math::isFinite(roots[i]) &&
622  roots[i] >= .0 &&
623  roots[i] <= T_ramp*1.01)
624  {
625  if (sol_t<0) sol_t = roots[i];
626  else mrpt::utils::keep_min(sol_t, roots[i]);
627  }
628  }
629 
630  // Invalid with these equations?
631  if (sol_t<0 || sol_t>T_ramp_thres101)
632  {
633  // Now, attempt to solve with the equations for t>T_ramp:
634  sol_t = -1.0;
635 
636  const double c1 = TR_2*(vxi - vxf) - ox;
637  const double c2 = TR_2*(vyi - vyf) - oy;
638 
639  const double a = vf_mod*vf_mod;
640  const double b = 2 * (c1*vxf + c2*vyf);
641  const double c = c1*c1 + c2*c2 - R*R;
642 
643  const double discr = b*b - 4 * a*c;
644  if (discr >= 0)
645  {
646  const double sol_t0 = (-b + sqrt(discr)) / (2 * a);
647  const double sol_t1 = (-b - sqrt(discr)) / (2 * a);
648 
649  // Identify the shortest valid collision time:
650  if (sol_t0<T_ramp && sol_t1<T_ramp) sol_t = -1.0;
651  else if (sol_t0<T_ramp && sol_t1 >= T_ramp_thres099) sol_t = sol_t1;
652  else if (sol_t1<T_ramp && sol_t0 >= T_ramp_thres099) sol_t = sol_t0;
653  else if (sol_t1 >= T_ramp_thres099 && sol_t0 >= T_ramp_thres099) sol_t = std::min(sol_t0, sol_t1);
654  }
655  }
656 
657  // Valid solution?
658  if (sol_t<0) return;
659  // Compute the transversed distance:
660  double dist;
661 
662  if (sol_t<T_ramp)
663  dist = calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, sol_t);
664  else dist = (sol_t - T_ramp) * V_MAX + calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
665 
666  // Store in the output variable:
667  internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
668 }
669 
670 void CPTG_Holo_Blend::updateTPObstacle(double ox, double oy, std::vector<double> &tp_obstacles) const
671 {
673 
674  for (unsigned int k = 0; k < m_alphaValuesCount; k++)
675  {
676  updateTPObstacleSingle(ox, oy, k, tp_obstacles[k]);
677  } // end for each "k" alpha
678 }
679 
681 {
682  // Nothing to do in a closed-form PTG.
683 }
684 
685 mrpt::kinematics::CVehicleVelCmdPtr CPTG_Holo_Blend::getSupportedKinematicVelocityCommand() const
686 {
687  return mrpt::kinematics::CVehicleVelCmdPtr(new mrpt::kinematics::CVehicleVelCmd_Holo());
688 }
689 
691 {
692  return true;
693 }
694 
695 double CPTG_Holo_Blend::maxTimeInVelCmdNOP(int path_k) const
696 {
697 // const double dir_local = CParameterizedTrajectoryGenerator::index2alpha(path_k);
698 
699  const size_t nSteps = getPathStepCount(path_k);
700  const double max_t = PATH_TIME_STEP * (nSteps * 0.7 /* leave room for obstacle detection ahead when we are far down the predicted PTG path */);
701  return max_t;
702 }
703 
705 {
706  return PATH_TIME_STEP;
707 }
708 
710  T_ramp_max(-1.0),
711  V_MAX(-1.0),
712  W_MAX(-1.0),
713  turningRadiusReference(0.30)
714 {
716 }
717 
719  turningRadiusReference(0.30)
720 {
722  this->loadFromConfigFile(cfg, sSection);
723 }
724 
726 {
727 }
728 
730 {
731  std::map<std::string, double *> symbols;
732  symbols["dir"] = &m_expr_dir;
733  symbols["V_MAX"] = &V_MAX;
734  symbols["W_MAX"] = &W_MAX;
735  symbols["T_ramp_max"] = &T_ramp_max;
736  symbols["T_ramp_max"] = &T_ramp_max;
737 
741 
742  // Default expressions (can be overloaded by values in a config file)
743  expr_V = "V_MAX";
744  expr_W = "W_MAX";
745  expr_T_ramp = "T_ramp_max";
746 }
747 
748 double CPTG_Holo_Blend::internal_get_v(const double dir) const
749 {
750  const_cast<double&>(m_expr_dir) = dir;
751  return std::abs(m_expr_v.eval());
752 }
753 double CPTG_Holo_Blend::internal_get_w(const double dir) const
754 {
755  const_cast<double&>(m_expr_dir) = dir;
756  return std::abs(m_expr_w.eval());
757 }
758 double CPTG_Holo_Blend::internal_get_T_ramp(const double dir) const
759 {
760  const_cast<double&>(m_expr_dir) = dir;
761  return m_expr_T_ramp.eval();
762 }
763 
764 void CPTG_Holo_Blend::internal_initialize(const std::string & cacheFilename, const bool verbose)
765 {
766  // No need to initialize anything, just do some params sanity checks:
767  ASSERT_(T_ramp_max>0);
768  ASSERT_(V_MAX>0);
769  ASSERT_(W_MAX>0);
772 
773  // Compile user-given expressions:
774  m_expr_v.compile(expr_V, std::map<std::string, double>(), "expr_V");
775  m_expr_w.compile(expr_W, std::map<std::string, double>(), "expr_w");
776  m_expr_T_ramp.compile(expr_T_ramp, std::map<std::string, double>(), "expr_T_ramp");
777 
778 #ifdef DO_PERFORMANCE_BENCHMARK
779  tl.dumpAllStats();
780 #endif
781 
782  m_pathStepCountCache.clear();
783 }
double index2alpha(uint16_t k) const
Alpha value for the discrete corresponding value.
int signWithZero(T x)
Returns the sign of X as "0", "1" or "-1".
bool inverseMap_WS2TP(double x, double y, int &out_k, double &out_d, double tolerance_dist=0.10) const MRPT_OVERRIDE
Computes the closest (alpha,d) TP coordinates of the trajectory point closest to the Workspace (WS) C...
int BASE_IMPEXP solve_poly3(double *x, double a, double b, double c) MRPT_NO_THROWS
Solves cubic equation x^3 + a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:29
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
std::string getDescription() const MRPT_OVERRIDE
Gets a short textual description of the PTG and its parameters.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble t
Definition: glext.h:3610
void internal_deinitialize() MRPT_OVERRIDE
This must be called to de-initialize the PTG if some parameter is to be changed.
#define min(a, b)
unsigned __int16 uint16_t
Definition: rptypes.h:46
void loadShapeFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
static double PATH_TIME_STEP
Duration of each PTG "step" (default: 10e-3=10 ms)
double DEG2RAD(const double x)
Degrees to radians.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
virtual void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
mrpt::math::CRuntimeCompiledExpression m_expr_w
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define MRPT_CHECK_NORMAL_NUMBER(val)
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
int BASE_IMPEXP solve_poly4(double *x, double a, double b, double c, double d) MRPT_NO_THROWS
Solves quartic equation x^4 + a*x^3 + b*x^2 + c*x + d = 0 by Dekart-Euler method. ...
Definition: poly_roots.cpp:215
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) MRPT_OVERRIDE
Parameters accepted by this base class:
mrpt::math::CRuntimeCompiledExpression m_expr_T_ramp
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
void updateTPObstacleSingle(double ox, double oy, uint16_t k, double &tp_obstacle_k) const MRPT_OVERRIDE
Like updateTPObstacle() but for one direction only (k) in TP-Space.
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) MRPT_OVERRIDE
Parameters accepted by this base class:
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...
void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
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...
virtual void internal_writeToStream(mrpt::utils::CStream &out) const
This class allows loading and storing values and vectors of different types from a configuration text...
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.
double internal_get_v(const double dir) const
Evals expr_v.
virtual void loadDefaultParams()
Loads a set of default parameters into the PTG.
double ramp_time
: Blending time between current and target time.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This is the base class for any user-defined PTG.
A PTG for circular-shaped robots with holonomic kinematics.
double RAD2DEG(const double x)
Radians to degrees.
virtual void internal_readFromStream(mrpt::utils::CStream &in)
#define MRPT_END
double eval() const
Evaluates the current value of the precompiled formula.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
double dir_local
: direction, relative to the current robot heading (radians). 0 means forward.
void internal_shape_saveToStream(mrpt::utils::CStream &out) const
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
virtual bool supportVelCmdNOP() const MRPT_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).
std::vector< int > m_pathStepCountCache
double internal_get_w(const double dir) const
Evals expr_w.
void internal_processNewRobotShape() MRPT_OVERRIDE
Will be called whenever the robot shape is set / updated.
GLubyte GLubyte b
Definition: glext.h:5575
const double eps
int version
Definition: mrpt_jpeglib.h:898
void internal_shape_loadFromStream(mrpt::utils::CStream &in)
virtual double maxTimeInVelCmdNOP(int path_k) const MRPT_OVERRIDE
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
bool PTG_IsIntoDomain(double x, double y) const MRPT_OVERRIDE
Returns the same than inverseMap_WS2TP() but without any additional cost.
#define MRPT_START
#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...
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 mrpt::kinematics::CVehicleVelCmdPtr directionToMotionCommand(uint16_t k) const MRPT_OVERRIDE
Converts a discretized "alpha" value into a feasible motion command or action.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
#define MRPT_LOAD_HERE_CONFIG_VAR(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void write(const std::string &section, const std::string &name, const data_t &value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
const float R
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
T square(const T x)
Inline function for the square of a number.
void updateTPObstacle(double ox, double oy, std::vector< double > &tp_obstacles) const MRPT_OVERRIDE
Updates the radial map of closest TP-Obstacles given a single obstacle point at (ox,oy)
size_t getPathStepCount(uint16_t k) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): number of discrete "steps" along the trajectory.
double internal_get_T_ramp(const double dir) const
Evals expr_T_ramp.
GLuint in
Definition: glext.h:6301
Lightweight 2D pose.
#define ASSERT_(f)
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
void getPathPose(uint16_t k, uint32_t step, mrpt::math::TPose2D &p) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): pose of the vehicle at discrete step step.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
#define MRPT_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
#define PERFORMANCE_BENCHMARK
GLenum GLint GLint y
Definition: glext.h:3516
double getPathStepDuration() const MRPT_OVERRIDE
Returns the duration (in seconds) of each "step".
int BASE_IMPEXP solve_poly2(double a, double b, double c, double &r1, double &r2) MRPT_NO_THROWS
Solves equation a*x^2 + b*x + c = 0.
Definition: poly_roots.cpp:302
bool BASE_IMPEXP isFinite(float v) MRPT_NO_THROWS
double getPathDist(uint16_t k, uint32_t step) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): traversed distance at discrete step step.
GLenum GLint x
Definition: glext.h:3516
void onNewNavDynamicState() MRPT_OVERRIDE
Invoked when m_nav_dyn_state has changed; gives the PTG the opportunity to react and parameterize pat...
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(variableName, variableType, targetVariable, configFileObject, sectionNameStr)
static double eps
Mathematical "epsilon", to detect ill-conditioned situations (e.g. 1/0) (Default: 1e-4) ...
unsigned __int32 uint32_t
Definition: rptypes.h:49
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...
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::math::CRuntimeCompiledExpression m_expr_v
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const MRPT_OVERRIDE
This method saves the options to a ".ini"-like file or memory-stored string list. ...
double rot_speed
: (rad/s) rotational speed for rotating such as the robot slowly faces forward.
bool getPathStepForDist(uint16_t k, double dist, uint32_t &out_step) const MRPT_OVERRIDE
Access path k ([0,N-1]=>[-pi,pi] in alpha): largest step count for which the traversed distance is < ...
#define COMMON_PTG_DESIGN_PARAMS
uint16_t m_alphaValuesCount
The number of discrete values for "alpha" between -PI and +PI.
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
void loadDefaultParams() MRPT_OVERRIDE
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
virtual mrpt::kinematics::CVehicleVelCmdPtr getSupportedKinematicVelocityCommand() const MRPT_OVERRIDE
Returns an empty kinematic velocity command object of the type supported by this PTG.
void internal_initialize(const std::string &cacheFilename=std::string(), const bool verbose=true) MRPT_OVERRIDE
Must be called after setting all PTG parameters and before requesting converting obstacles to TP-Spac...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019