MRPT  1.9.9
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-2019, 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  float alpha, float t, float x, float y, float phi, float& v, float& w) const
73 {
77  MRPT_UNUSED_PARAM(phi);
78  // (v,w)
79  v = V_MAX * sign(K);
80  // Use a linear mapping: (Old was: w = tan( alpha/2 ) * W_MAX * sign(K))
81  w = (alpha / M_PI) * W_MAX * sign(K);
82 }
83 
84 bool CPTG_DiffDrive_C::PTG_IsIntoDomain(double x, double y) const
85 {
88  return true;
89 }
90 
92  double x, double y, int& k_out, double& d_out, double tolerance_dist) const
93 {
94  MRPT_UNUSED_PARAM(tolerance_dist);
95  bool is_exact = true;
96  if (y != 0)
97  {
98  double R = (x * x + y * y) / (2 * y);
99  const double Rmin = std::abs(V_MAX / W_MAX);
100 
101  double theta;
102 
103  if (K > 0)
104  {
105  if (y > 0)
106  theta = atan2((double)x, fabs(R) - y);
107  else
108  theta = atan2((double)x, y + fabs(R));
109  }
110  else
111  {
112  if (y > 0)
113  theta = atan2(-(double)x, fabs(R) - y);
114  else
115  theta = atan2(-(double)x, y + fabs(R));
116  }
117 
118  // Arc length must be possitive [0,2*pi]
120 
121  // Distance thru arc:
122  d_out = (float)(theta * (fabs(R) + turningRadiusReference));
123 
124  if (std::abs(R) < Rmin)
125  {
126  is_exact = false;
127  R = Rmin * mrpt::sign(R);
128  }
129 
130  // Was: a = 2*atan( V_MAX / (W_MAX*R) );
131  const double a = M_PI * V_MAX / (W_MAX * R);
132  k_out = alpha2index((float)a);
133  }
134  else
135  {
136  if (sign(x) == sign(K))
137  {
138  k_out = alpha2index(0);
139  d_out = x;
140  is_exact = true;
141  }
142  else
143  {
144  k_out = m_alphaValuesCount - 1;
145  d_out = 1e+3;
146  is_exact = false;
147  }
148  }
149 
150  // Normalize:
151  d_out = d_out / refDistance;
152 
153  ASSERT_ABOVEEQ_(k_out, 0);
154  ASSERT_BELOW_(k_out, m_alphaValuesCount);
155 
156  return is_exact;
157 }
158 
160 {
162  K = +1.0;
163 }
#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
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019