MRPT  2.0.4
CPTG_DiffDrive_C.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/math/wrap2pi.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::nav;
18 using namespace mrpt::system;
19 
22 
24  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
25 {
27 
28  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K, double, cfg, sSection);
29 }
31  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
32 {
34  const int WN = 25, WV = 30;
36 
37  cfg.write(
38  sSection, "K", K, WN, WV,
39  "K=+1 forward paths; K=-1 for backwards paths.");
40 
41  MRPT_END
42 }
43 
45  mrpt::serialization::CArchive& in, uint8_t version)
46 {
48 
49  switch (version)
50  {
51  case 0:
52  in >> K;
53  break;
54  default:
56  };
57 }
58 
59 uint8_t CPTG_DiffDrive_C::serializeGetVersion() const { return 0; }
61 {
63  out << K;
64 }
65 
67 {
68  return mrpt::format("CPTG_DiffDrive_C,K=%i", (int)K);
69 }
70 
72  [[maybe_unused]] float alpha, [[maybe_unused]] float t,
73  [[maybe_unused]] float x, [[maybe_unused]] float y,
74  [[maybe_unused]] float phi, [[maybe_unused]] float& v,
75  [[maybe_unused]] float& w) const
76 {
77  // (v,w)
78  v = V_MAX * sign(K);
79  // Use a linear mapping: (Old was: w = tan( alpha/2 ) * W_MAX * sign(K))
80  w = (alpha / M_PI) * W_MAX * sign(K);
81 }
82 
84  [[maybe_unused]] double x, [[maybe_unused]] double y) const
85 {
86  return true;
87 }
88 
90  double x, double y, int& k_out, double& d_out,
91  [[maybe_unused]] double tolerance_dist) const
92 {
93  bool is_exact = true;
94  if (y != 0)
95  {
96  double R = (x * x + y * y) / (2 * y);
97  const double Rmin = std::abs(V_MAX / W_MAX);
98 
99  double theta;
100 
101  if (K > 0)
102  {
103  if (y > 0)
104  theta = atan2((double)x, fabs(R) - y);
105  else
106  theta = atan2((double)x, y + fabs(R));
107  }
108  else
109  {
110  if (y > 0)
111  theta = atan2(-(double)x, fabs(R) - y);
112  else
113  theta = atan2(-(double)x, y + fabs(R));
114  }
115 
116  // Arc length must be possitive [0,2*pi]
118 
119  // Distance thru arc:
120  d_out = (float)(theta * (fabs(R) + turningRadiusReference));
121 
122  if (std::abs(R) < Rmin)
123  {
124  is_exact = false;
125  R = Rmin * mrpt::sign(R);
126  }
127 
128  // Was: a = 2*atan( V_MAX / (W_MAX*R) );
129  const double a = M_PI * V_MAX / (W_MAX * R);
130  k_out = alpha2index((float)a);
131  }
132  else
133  {
134  if (sign(x) == sign(K))
135  {
136  k_out = alpha2index(0);
137  d_out = x;
138  is_exact = true;
139  }
140  else
141  {
142  k_out = m_alphaValuesCount - 1;
143  d_out = 1e+3;
144  is_exact = false;
145  }
146  }
147 
148  // Normalize:
149  d_out = d_out / refDistance;
150 
151  ASSERT_ABOVEEQ_(k_out, 0);
152  ASSERT_BELOW_(k_out, m_alphaValuesCount);
153 
154  return is_exact;
155 }
156 
158 {
160  K = +1.0;
161 }
#define MRPT_START
Definition: exceptions.h:241
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
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. ...
void internal_readFromStream(mrpt::serialization::CArchive &in) override
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void ptgDiffDriveSteeringFunction(float alpha, float t, float x, float y, float phi, float &v, float &w) const override
The main method to be implemented in derived classes: it defines the differential-driven differential...
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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...
int sign(T x)
Returns the sign of X as "1" or "-1".
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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 wrapTo2PiInPlace(T &a)
Modifies the given angle to translate it into the [0,2pi[ range.
Definition: wrap2pi.h:25
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
const float R
mrpt::vision::TStereoCalibResults out
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define MRPT_END
Definition: exceptions.h:245
IMPLEMENTS_SERIALIZABLE(CPTG_DiffDrive_C, CParameterizedTrajectoryGenerator, mrpt::nav) void CPTG_DiffDrive_C
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Parameters accepted by this base class:
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. ...
A PTG for circular paths ("C" type PTG in papers).
void internal_writeToStream(mrpt::serialization::CArchive &out) const override



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020