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,
282 in >> T_ramp_max >> V_MAX >> W_MAX >> turningRadiusReference;
285 double dummy_maxAllowedDirAngle;
286 in >> dummy_maxAllowedDirAngle;
290 in >> expr_V >> expr_W >> expr_T_ramp;
310 out << T_ramp_max << V_MAX << W_MAX << turningRadiusReference;
311 out << expr_V << expr_W << expr_T_ramp;
315 double x,
double y,
int& out_k,
double& out_d,
double tolerance_dist)
const 322 const double err_threshold = 1e-3;
323 const double T_ramp = T_ramp_max;
324 const double vxi = m_nav_dyn_state.curVelLocal.vx,
325 vyi = m_nav_dyn_state.curVelLocal.vy;
333 q[0] = T_ramp_max * 1.1;
334 q[1] = V_MAX *
x / sqrt(
x *
x +
y *
y);
335 q[2] = V_MAX *
y / sqrt(
x *
x +
y *
y);
338 double err_mod = 1e7;
339 bool sol_found =
false;
340 for (
int iters = 0; !sol_found && iters < 25; iters++)
342 const double TR_ = 1.0 / (T_ramp);
343 const double TR2_ = 1.0 / (2 * T_ramp);
349 r[0] = 0.5 * T_ramp * (vxi +
q[1]) + (
q[0] - T_ramp) *
q[1] -
x;
350 r[1] = 0.5 * T_ramp * (vyi +
q[2]) + (
q[0] - T_ramp) *
q[2] -
y;
354 r[0] = vxi *
q[0] +
q[0] *
q[0] * TR2_ * (
q[1] - vxi) -
x;
355 r[1] = vyi *
q[0] +
q[0] *
q[0] * TR2_ * (
q[2] - vyi) -
y;
357 const double alpha = atan2(
q[2],
q[1]);
359 r[2] =
q[1] *
q[1] +
q[2] *
q[2] - V_MAXsq;
369 J(0, 1) = 0.5 * T_ramp +
q[0];
373 J(1, 2) = 0.5 * T_ramp +
q[0];
377 J(0, 0) = vxi +
q[0] * TR_ * (
q[1] - vxi);
378 J(0, 1) = TR2_ *
q[0] *
q[0];
380 J(1, 0) = vyi +
q[0] * TR_ * (
q[2] - vyi);
382 J(1, 2) = TR2_ *
q[0] *
q[0];
388 Eigen::Vector3d q_incr = J.householderQr().solve(
r);
392 sol_found = (err_mod < err_threshold);
395 if (sol_found &&
q[0] >= .0)
397 const double alpha = atan2(
q[2],
q[1]);
400 const double solved_t =
q[0];
401 const unsigned int solved_step = solved_t / PATH_TIME_STEP;
402 const double found_dist = this->getPathDist(out_k, solved_step);
404 out_d = found_dist / this->refDistance;
417 return inverseMap_WS2TP(
x,
y, k, d);
432 cmd->
vel = internal_get_v(dir_local);
434 cmd->
ramp_time = internal_get_T_ramp(dir_local);
443 if (m_pathStepCountCache.size() > k && m_pathStepCountCache[k] > 0)
444 return m_pathStepCountCache[k];
447 if (!getPathStepForDist(k, this->refDistance, step))
450 "Could not solve closed-form distance for k=%u",
451 static_cast<unsigned>(k));
454 if (m_pathStepCountCache.size() != m_alphaValuesCount)
456 m_pathStepCountCache.assign(m_alphaValuesCount, -1);
458 m_pathStepCountCache[k] = step;
465 const double t = PATH_TIME_STEP * step;
470 const double TR2_ = 1.0 / (2 * T_ramp);
475 p.x = vxi *
t +
t *
t * TR2_ * (vxf - vxi);
476 p.y = vyi *
t +
t *
t * TR2_ * (vyf - vyi);
480 p.x = T_ramp * 0.5 * (vxi + vxf) + (
t - T_ramp) * vxf;
481 p.y = T_ramp * 0.5 * (vyi + vyf) + (
t - T_ramp) * vyf;
485 const double wi = m_nav_dyn_state.curVelLocal.omega;
490 const double a = TR2_ * (wf - wi),
b = (wi),
c = -dir;
501 const double t_solve = std::max(r1, r2);
505 p.phi = wi *
t +
t *
t * TR2_ * (wf - wi);
511 const double t_solve = (dir - T_ramp * 0.5 * (wi + wf)) / wf + T_ramp;
515 p.phi = T_ramp * 0.5 * (wi + wf) + (
t - T_ramp) * wf;
521 const double t = PATH_TIME_STEP * step;
525 const double TR2_ = 1.0 / (2 * T_ramp);
527 const double k2 = (vxf - vxi) * TR2_;
528 const double k4 = (vyf - vyi) * TR2_;
532 return calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi,
t);
536 const double dist_trans =
537 (
t - T_ramp) * V_MAX +
538 calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
551 const double TR2_ = 1.0 / (2 * T_ramp);
553 const double k2 = (vxf - vxi) * TR2_;
554 const double k4 = (vyf - vyi) * TR2_;
559 const double dist_trans_T_ramp =
560 calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
561 double t_solved = -1;
563 if (dist >= dist_trans_T_ramp)
566 t_solved = T_ramp + (dist - dist_trans_T_ramp) / V_MAX;
578 if (std::abs(k2) <
eps && std::abs(k4) <
eps)
581 t_solved = (dist) / V_MAX;
585 const double a = ((k2 * k2) * 4.0 + (k4 * k4) * 4.0);
586 const double b = (k2 * vxi * 4.0 + k4 * vyi * 4.0);
587 const double c = (vxi * vxi + vyi * vyi);
590 if (std::abs(
b) <
eps && std::abs(
c) <
eps)
593 t_solved = sqrt(2.0) * 1.0 / pow(
a, 1.0 / 4.0) * sqrt(dist);
611 t_solved = T_ramp * 0.6;
614 for (
int iters = 0; iters < 10; iters++)
616 double err = calc_trans_distance_t_below_Tramp_abc(
620 std::sqrt(
a * t_solved * t_solved +
b * t_solved +
c);
621 ASSERT_(std::abs(diff) > 1e-40);
622 t_solved -= (err) / diff;
623 if (t_solved < 0) t_solved = .0;
624 if (std::abs(err) < 1e-3)
break;
639 double ox,
double oy,
uint16_t k,
double& tp_obstacle_k)
const 641 const double R = m_robotRadius;
645 const double TR2_ = 1.0 / (2 * T_ramp);
646 const double TR_2 = T_ramp * 0.5;
647 const double T_ramp_thres099 = T_ramp * 0.99;
648 const double T_ramp_thres101 = T_ramp * 1.01;
660 const double k2 = (vxf - vxi) * TR2_;
661 const double k4 = (vyf - vyi) * TR2_;
664 const double a = (k2 * k2 + k4 * k4);
665 const double b = (k2 * vxi * 2.0 + k4 * vyi * 2.0);
666 const double c = -(k2 * ox * 2.0 + k4 * oy * 2.0 - vxi * vxi - vyi * vyi);
667 const double d = -(ox * vxi * 2.0 + oy * vyi * 2.0);
668 const double e = -
R *
R + ox * ox + oy * oy;
671 int num_real_sols = 0;
672 if (std::abs(
a) >
eps)
679 else if (std::abs(
b) >
eps)
689 const double discr = d * d - 4 *
c * e;
693 roots[0] = (-d + sqrt(discr)) / (2 *
c);
694 roots[1] = (-d - sqrt(discr)) / (2 *
c);
702 for (
int i = 0; i < num_real_sols; i++)
704 if (roots[i] == roots[i] &&
705 std::isfinite(roots[i]) && roots[i] >= .0 &&
706 roots[i] <= T_ramp * 1.01)
716 if (sol_t < 0 || sol_t > T_ramp_thres101)
721 const double c1 = TR_2 * (vxi - vxf) - ox;
722 const double c2 = TR_2 * (vyi - vyf) - oy;
724 const double a = vf_mod * vf_mod;
725 const double b = 2 * (c1 * vxf + c2 * vyf);
726 const double c = c1 * c1 + c2 * c2 -
R *
R;
728 const double discr =
b *
b - 4 *
a *
c;
731 const double sol_t0 = (-
b + sqrt(discr)) / (2 *
a);
732 const double sol_t1 = (-
b - sqrt(discr)) / (2 *
a);
735 if (sol_t0 < T_ramp && sol_t1 < T_ramp)
737 else if (sol_t0 < T_ramp && sol_t1 >= T_ramp_thres099)
739 else if (sol_t1 < T_ramp && sol_t0 >= T_ramp_thres099)
741 else if (sol_t1 >= T_ramp_thres099 && sol_t0 >= T_ramp_thres099)
747 if (sol_t < 0)
return;
752 dist = calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, sol_t);
754 dist = (sol_t - T_ramp) * V_MAX +
755 calc_trans_distance_t_below_Tramp(k2, k4, vxi, vyi, T_ramp);
758 internal_TPObsDistancePostprocess(ox, oy, dist, tp_obstacle_k);
762 double ox,
double oy, std::vector<double>& tp_obstacles)
const 766 for (
unsigned int k = 0; k < m_alphaValuesCount; k++)
768 updateTPObstacleSingle(ox, oy, k, tp_obstacles[k]);
790 const size_t nSteps = getPathStepCount(path_k);
800 : T_ramp_max(-1.0), V_MAX(-1.0), W_MAX(-1.0), turningRadiusReference(0.30)
807 : turningRadiusReference(0.30)
816 std::map<std::string, double*> symbols;
818 symbols[
"V_MAX"] = &
V_MAX;
819 symbols[
"W_MAX"] = &
W_MAX;
823 m_expr_v.register_symbol_table(symbols);
824 m_expr_w.register_symbol_table(symbols);
850 const std::string& cacheFilename,
const bool verbose)
860 m_expr_v.compile(
expr_V, std::map<std::string, double>(),
"expr_V");
861 m_expr_w.compile(
expr_W, std::map<std::string, double>(),
"expr_w");
863 expr_T_ramp, std::map<std::string, double>(),
"expr_T_ramp");
865 #ifdef DO_PERFORMANCE_BENCHMARK 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".
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
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
unsigned __int16 uint16_t
#define MRPT_LOAD_HERE_CONFIG_VAR( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
void loadShapeFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double DEG2RAD(const double x)
Degrees to radians.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
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.
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
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_LOAD_HERE_CONFIG_VAR_DEGREES_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
#define MRPT_CHECK_NORMAL_NUMBER(val)
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
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
mrpt::math::CRuntimeCompiledExpression m_expr_T_ramp
void internal_deinitialize() override
This must be called to de-initialize the PTG if some parameter is to be changed.
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) 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 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_construct_exprs()
virtual double maxTimeInVelCmdNOP(int path_k) const override
Only for PTGs supporting supportVelCmdNOP(): this is the maximum time (in seconds) for which the path...
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...
virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
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.
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...
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_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).
double getPathStepDuration() const override
Returns the duration (in seconds) of each "step".
void internal_shape_saveToStream(mrpt::utils::CStream &out) const
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
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. ...
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 internal_shape_loadFromStream(mrpt::utils::CStream &in)
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
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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...
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.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
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.
#define MRPT_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( variableName, variableType, targetVariable, configFileObject, sectionNameStr)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void write(const std::string §ion, 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())
static double calc_trans_distance_t_below_Tramp_abc_numeric(double T, double a, double b, double c)
GLdouble GLdouble GLdouble r
static double PATH_TIME_STEP
Duration of each PTG "step" (default: 10e-3=10 ms)
T square(const T x)
Inline function for the square of a number.
double internal_get_T_ramp(const double dir) const
Evals expr_T_ramp.
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
void internal_processNewRobotShape() override
Will be called whenever the robot shape is set / updated.
int round(const T value)
Returns the closer integer (int) to x.
#define PERFORMANCE_BENCHMARK
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.
#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...
std::shared_ptr< CVehicleVelCmd > Ptr
unsigned __int32 uint32_t
virtual ~CPTG_Holo_Blend()
GLubyte GLubyte GLubyte a
mrpt::math::CRuntimeCompiledExpression m_expr_v
void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const 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.
int solve_poly3(double *x, double a, double b, double c) noexcept
Solves cubic equation x^3 + a*x^2 + b*x + c = 0.
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.
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.
virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &cfg, const std::string &sSection) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...