MRPT  1.9.9
CPTG_DiffDrive_C.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-2018, 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/math/wrap2pi.h>
14 
15 using namespace mrpt;
16 using namespace mrpt::nav;
17 using namespace mrpt::system;
18 
21 
23  const mrpt::config::CConfigFileBase& cfg, const std::string& sSection)
24 {
26 
27  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(K, double, cfg, sSection);
28 }
30  mrpt::config::CConfigFileBase& cfg, const std::string& sSection) const
31 {
33  const int WN = 25, WV = 30;
35 
36  cfg.write(
37  sSection, "K", K, WN, WV,
38  "K=+1 forward paths; K=-1 for backwards paths.");
39 
40  MRPT_END
41 }
42 
45 {
47 
48  switch (version)
49  {
50  case 0:
51  in >> K;
52  break;
53  default:
55  };
56 }
57 
60 {
62  out << K;
63 }
64 
66 {
67  return mrpt::format("CPTG_DiffDrive_C,K=%i", (int)K);
68 }
69 
71  float alpha, float t, float x, float y, float phi, float& v, float& w) const
72 {
76  MRPT_UNUSED_PARAM(phi);
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 
83 bool CPTG_DiffDrive_C::PTG_IsIntoDomain(double x, double y) const
84 {
87  return true;
88 }
89 
91  double x, double y, int& k_out, double& d_out, double tolerance_dist) const
92 {
93  MRPT_UNUSED_PARAM(tolerance_dist);
94  bool is_exact = true;
95  if (y != 0)
96  {
97  double R = (x * x + y * y) / (2 * y);
98  const double Rmin = std::abs(V_MAX / W_MAX);
99 
100  double theta;
101 
102  if (K > 0)
103  {
104  if (y > 0)
105  theta = atan2((double)x, fabs(R) - y);
106  else
107  theta = atan2((double)x, y + fabs(R));
108  }
109  else
110  {
111  if (y > 0)
112  theta = atan2(-(double)x, fabs(R) - y);
113  else
114  theta = atan2(-(double)x, y + fabs(R));
115  }
116 
117  // Arc length must be possitive [0,2*pi]
119 
120  // Distance thru arc:
121  d_out = (float)(theta * (fabs(R) + turningRadiusReference));
122 
123  if (std::abs(R) < Rmin)
124  {
125  is_exact = false;
126  R = Rmin * mrpt::sign(R);
127  }
128 
129  // Was: a = 2*atan( V_MAX / (W_MAX*R) );
130  const double a = M_PI * V_MAX / (W_MAX * R);
131  k_out = alpha2index((float)a);
132  }
133  else
134  {
135  if (sign(x) == sign(K))
136  {
137  k_out = alpha2index(0);
138  d_out = x;
139  is_exact = true;
140  }
141  else
142  {
143  k_out = m_alphaValuesCount - 1;
144  d_out = 1e+3;
145  is_exact = false;
146  }
147  }
148 
149  // Normalize:
150  d_out = d_out / refDistance;
151 
152  ASSERT_ABOVEEQ_(k_out, 0);
153  ASSERT_BELOW_(k_out, m_alphaValuesCount);
154 
155  return is_exact;
156 }
157 
159 {
161  K = +1.0;
162 }
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble t
Definition: glext.h:3689
bool PTG_IsIntoDomain(double x, double y) const override
Returns the same than inverseMap_WS2TP() but without any additional cost.
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
virtual 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...
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
std::string getDescription() const override
Gets a short textual description of the PTG and its parameters.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
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...
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &sSection) override
Possible values in "params" (those in CParameterizedTrajectoryGenerator, which is called internally...
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
GLsizei const GLchar ** string
Definition: glext.h:4101
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:26
const GLdouble * v
Definition: glext.h:3678
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
The default implementation in this class relies on a look-up-table.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
const float R
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_END
Definition: exceptions.h:266
IMPLEMENTS_SERIALIZABLE(CPTG_DiffDrive_C, CParameterizedTrajectoryGenerator, mrpt::nav) void CPTG_DiffDrive_C
GLuint in
Definition: glext.h:7274
GLenum GLint GLint y
Definition: glext.h:3538
int sign(T x)
Returns the sign of X as "1" or "-1".
virtual void loadDefaultParams() override
Loads a set of default parameters; provided exclusively for the PTG-configurator tool.
GLenum GLint x
Definition: glext.h:3538
virtual 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).
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020