42 #ifdef DO_PERFORMANCE_BENCHMARK
44 #define PERFORMANCE_BENCHMARK \
45 CTimeLoggerEntry tle(tl_holo, __CURRENT_FUNCTION_NAME__);
47 #define PERFORMANCE_BENCHMARK
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);
63 static double calc_trans_distance_t_below_Tramp_abc_analytic(
double t,
double a,
double b,
double c)
68 if (t == 0.0)
return .0;
72 const double discr = b*b - 4 * a*c;
73 if (std::abs(discr)<1e-6)
75 const double r = -b / (2 * a);
77 dist = r*std::abs(r)*0.5 - (r - t)*std::abs(r - t)*0.5;
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);
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;
99 double T,
double a,
double b,
double c)
104 const unsigned int NUM_STEPS = 15;
108 double feval_t = std::sqrt(c);
111 const double At = T / (NUM_STEPS);
113 for (
unsigned int i = 0; i < NUM_STEPS; i++)
117 double dd = a * t * t + b * t + c;
123 feval_tp1 = sqrt(dd);
126 d += At * (feval_t + feval_tp1) * 0.5;
137 double t,
double a,
double b,
double c)
142 double ret = calc_trans_distance_t_below_Tramp_abc_analytic(t, a, b, c);
153 double k2,
double k4,
double vxi,
double vyi,
double t)
160 const double c = (vxi * vxi + vyi * vyi);
161 if (std::abs(k2) >
eps || std::abs(k4) >
eps)
163 const double a = ((k2 * k2) * 4.0 + (k4 * k4) * 4.0);
164 const double b = (k2 * vxi * 4.0 + k4 * vyi * 4.0);
167 if (std::abs(b) <
eps && std::abs(c) <
eps)
170 const double int_t = sqrt(a) * (t * t) * 0.5;
175 return calc_trans_distance_t_below_Tramp_abc(t, a, b, c);
180 return std::sqrt(c) * t;
186 m_pathStepCountCache.assign(m_alphaValuesCount, -1);
194 m_alphaValuesCount = 31;
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);
222 const int WN = 25, WV = 30;
227 sSection,
"T_ramp_max", T_ramp_max, WN, WV,
228 "Max duration of the velocity interpolation since a vel_cmd is issued "
231 sSection,
"v_max_mps", V_MAX, WN, WV,
232 "Maximum linear velocity for trajectories [m/s].");
235 "Maximum angular velocity for trajectories [deg/s].");
237 sSection,
"turningRadiusReference", turningRadiusReference, WN, WV,
238 "An approximate dimension of the robot (not a critical parameter) "
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`.");
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`.");
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`.");
262 "PTG_Holo_Blend_Tramp=%.03f_Vmax=%.03f_Wmax=%.03f", T_ramp_max, V_MAX,
283 in >> T_ramp_max >> V_MAX >> W_MAX >> turningRadiusReference;
286 double dummy_maxAllowedDirAngle;
287 in >> dummy_maxAllowedDirAngle;
291 in >> expr_V >> expr_W >> expr_T_ramp;
305 out << T_ramp_max << V_MAX << W_MAX << turningRadiusReference;
306 out << expr_V << expr_W << expr_T_ramp;
310 double x,
double y,
int& out_k,
double& out_d,
311 [[maybe_unused]]
double tolerance_dist)
const
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;
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);
333 double err_mod = 1e7;
334 bool sol_found =
false;
335 for (
int iters = 0; !sol_found && iters < 25; iters++)
337 const double TR_ = 1.0 / (T_ramp);
338 const double TR2_ = 1.0 / (2 * T_ramp);
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;
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;
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;
364 J(0, 1) = 0.5 * T_ramp + q[0];
368 J(1, 2) = 0.5 * T_ramp + q[0];
372 J(0, 0) = vxi + q[0] * TR_ * (q[1] - vxi);
373 J(0, 1) = TR2_ * q[0] * q[0];
375 J(1, 0) = vyi + q[0] * TR_ * (q[2] - vyi);
377 J(1, 2) = TR2_ * q[0] * q[0];
387 sol_found = (err_mod < err_threshold);
390 if (sol_found && q[0] >= .0)
392 const double alpha = atan2(q[2], q[1]);
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);
399 out_d = found_dist / this->refDistance;
412 return inverseMap_WS2TP(x, y, k, d);
426 cmd->vel = internal_get_v(dir_local);
427 cmd->dir_local = dir_local;
428 cmd->ramp_time = internal_get_T_ramp(dir_local);
436 if (m_pathStepCountCache.size() > k && m_pathStepCountCache[k] > 0)
437 return m_pathStepCountCache[k];
440 if (!getPathStepForDist(k, this->refDistance, step))
443 "Could not solve closed-form distance for k=%u",
444 static_cast<unsigned>(k));
447 if (m_pathStepCountCache.size() != m_alphaValuesCount)
449 m_pathStepCountCache.assign(m_alphaValuesCount, -1);
451 m_pathStepCountCache[k] = step;
458 const double t = PATH_TIME_STEP * step;
462 const double TR2_ = 1.0 / (2 * T_ramp);
467 p.
x = vxi * t + t * t * TR2_ * (vxf - vxi);
468 p.
y = vyi * t + t * t * TR2_ * (vyf - vyi);
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;
477 const double wi = m_nav_dyn_state.curVelLocal.omega;
482 const double a = TR2_ * (wf - wi), b = (wi), c = -
dir;
493 const double t_solve = std::max(r1, r2);
497 p.
phi = wi * t + t * t * TR2_ * (wf - wi);
503 const double t_solve = (
dir - T_ramp * 0.5 * (wi + wf)) / wf + T_ramp;
507 p.
phi = T_ramp * 0.5 * (wi + wf) + (t - T_ramp) * wf;
513 const double t = PATH_TIME_STEP * step;
517 const double TR2_ = 1.0 / (2 * T_ramp);
519 const double k2 = (vxf - vxi) * TR2_;
520 const double k4 = (vyf - vyi) * TR2_;
524 return calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, t);
528 const double dist_trans =
529 (t - T_ramp) * V_MAX +
530 calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
536 uint16_t k,
double dist, uint32_t& out_step)
const
543 const double TR2_ = 1.0 / (2 * T_ramp);
545 const double k2 = (vxf - vxi) * TR2_;
546 const double k4 = (vyf - vyi) * TR2_;
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;
555 if (dist >= dist_trans_T_ramp)
558 t_solved = T_ramp + (dist - dist_trans_T_ramp) / V_MAX;
570 if (std::abs(k2) <
eps && std::abs(k4) <
eps)
573 t_solved = (dist) / V_MAX;
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);
582 if (std::abs(b) <
eps && std::abs(c) <
eps)
585 t_solved = sqrt(2.0) * 1.0 / pow(a, 1.0 / 4.0) * sqrt(dist);
603 t_solved = T_ramp * 0.6;
606 for (
int iters = 0; iters < 10; iters++)
608 double err = calc_trans_distance_t_below_Tramp_abc(
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;
631 double ox,
double oy, uint16_t k,
double& tp_obstacle_k)
const
633 const double R = m_robotRadius;
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;
652 const double k2 = (vxf - vxi) * TR2_;
653 const double k4 = (vyf - vyi) * TR2_;
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;
663 int num_real_sols = 0;
664 if (std::abs(a) >
eps)
671 else if (std::abs(b) >
eps)
681 const double discr = d * d - 4 * c * e;
685 roots[0] = (-d + sqrt(discr)) / (2 * c);
686 roots[1] = (-d - sqrt(discr)) / (2 * c);
694 for (
int i = 0; i < num_real_sols; i++)
696 if (roots[i] == roots[i] &&
697 std::isfinite(roots[i]) && roots[i] >= .0 &&
698 roots[i] <= T_ramp * 1.01)
708 if (sol_t < 0 || sol_t > T_ramp_thres101)
713 const double c1 = TR_2 * (vxi - vxf) - ox;
714 const double c2 = TR_2 * (vyi - vyf) - oy;
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;
720 const double discr = xb * xb - 4 * xa * xc;
723 const double sol_t0 = (-xb + sqrt(discr)) / (2 * xa);
724 const double sol_t1 = (-xb - sqrt(discr)) / (2 * xa);
727 if (sol_t0 < T_ramp && sol_t1 < T_ramp)
729 else if (sol_t0 < T_ramp && sol_t1 >= T_ramp_thres099)
731 else if (sol_t1 < T_ramp && sol_t0 >= T_ramp_thres099)
733 else if (sol_t1 >= T_ramp_thres099 && sol_t0 >= T_ramp_thres099)
734 sol_t = std::min(sol_t0, sol_t1);
739 if (sol_t < 0)
return;
744 dist = calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, sol_t);
746 dist = (sol_t - T_ramp) * V_MAX +
747 calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
750 internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
754 double ox,
double oy, std::vector<double>& tp_obstacles)
const
758 for (
unsigned int k = 0; k < m_alphaValuesCount; k++)
760 updateTPObstacleSingle(ox, oy, k, tp_obstacles[k]);
782 const size_t nSteps = getPathStepCount(path_k);
794 : turningRadiusReference(0.30)
803 std::map<std::string, double*> symbols;
805 symbols[
"V_MAX"] = &
V_MAX;
806 symbols[
"W_MAX"] = &
W_MAX;
836 const std::string& cacheFilename,
const bool verbose)
849 expr_T_ramp, std::map<std::string, double>(),
"expr_T_ramp");
851 #ifdef DO_PERFORMANCE_BENCHMARK