Main MRPT website > C++ reference
MRPT logo
CRobotSimulator.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2014, 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 #ifndef CRobotSimulator_H
10 #define CRobotSimulator_H
11 
13 #include <mrpt/poses/CPose2D.h>
14 
15 #include <mrpt/base/link_pragmas.h>
16 
17 namespace mrpt
18 {
19 namespace utils
20 {
21  using namespace mrpt::poses;
22  using namespace mrpt::math;
23 
24  /** This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile robot, including odometry errors and dynamics limitations.
25  * The main methods are:
26  - movementCommand: Call this for send a command to the robot. This comamnd will be
27  delayed and passed throught a first order low-pass filter to simulate
28  robot dynamics.
29  - simulateInterval: Call this for run the simulator for the desired time period.
30  *
31  Versions:
32  - 23/MAR/2009: (JLBC) Changed to reuse MRPT poses and methods renamed to conform to MRPT style.
33  - 29/AUG/2008: (JLBC) Added parameters for odometry noise.
34  - 27/JAN/2008: (JLBC) Translated to English!!! :-)
35  - 17/OCT/2005: (JLBC) Integration into the MRML library.
36  - 1/DIC/2004: (JLBC) Odometry, cumulative errors added.
37  - 18/JUN/2004: (JLBC) First creation.
38  *
39  * \ingroup mrpt_base_grp
40  */
42  {
43  private:
44  // Internal state variables:
45  // ---------------------------------------
46  mrpt::poses::CPose2D m_pose; //!< Global, absolute and error-free robot coordinates
47  mrpt::poses::CPose2D m_odometry; //!< Used to simulate odometry (with optional error)
48 
49  /** Instantaneous velocity of the robot (linear, m/s)
50  */
51  double v;
52 
53  /** Instantaneous velocity of the robot (angular, rad/s)
54  */
55  double w;
56 
57  /** Simulation time variable
58  */
59  double t;
60 
61  /** Whether to corrupt odometry with noise */
63 
64  /** Dynamic limitations of the robot.
65  * Approximation to non-infinity motor forces: A first order low-pass filter, using:
66  * Command_Time: Time "t" when the last order was received.
67  * Command_v, Command_w: The user-desired velocities.
68  * Command_v0, Command_w0: Actual robot velocities at the moment of user request.
69  */
70  double Command_Time,
71  Command_v, Command_w,
72  Command_v0, Command_w0;
73 
74  /** The time-constants for the first order low-pass filter for the velocities changes. */
75  float cTAU; // 1.8 sec
76 
77  /** The delay constant for the velocities changes. */
78  float cDELAY;
79 
80  double m_Ax_err_bias, m_Ax_err_std;
81  double m_Ay_err_bias, m_Ay_err_std;
82  double m_Aphi_err_bias, m_Aphi_err_std;
83 
84  public:
85  /** Constructor with default dynamic model-parameters
86  */
87  CRobotSimulator( float TAU = 0, float DELAY = 0);
88 
89  /** Destructor
90  */
91  virtual ~CRobotSimulator();
92 
93  /** Change the model of delays used for the orders sent to the robot \sa movementCommand */
94  void setDelayModelParams(float TAU_delay_sec=1.8f, float CMD_delay_sec=0.3f) {
95  cTAU = TAU_delay_sec;
96  cDELAY = CMD_delay_sec;
97  }
98 
99  /** Enable/Disable odometry errors
100  * Errors in odometry are introduced per millisecond.
101  */
103  bool enabled,
104  double Ax_err_bias = 1e-6,
105  double Ax_err_std = 10e-6,
106  double Ay_err_bias = 1e-6,
107  double Ay_err_std = 10e-6,
108  double Aphi_err_bias = DEG2RAD(1e-6),
109  double Aphi_err_std = DEG2RAD(10e-6)
110  )
111  {
112  usar_error_odometrico=enabled;
113  m_Ax_err_bias=Ax_err_bias;
114  m_Ax_err_std=Ax_err_std;
115  m_Ay_err_bias=Ay_err_bias;
116  m_Ay_err_std=Ay_err_std;
117  m_Aphi_err_bias=Aphi_err_bias;
118  m_Aphi_err_std=Aphi_err_std;
119  }
120 
121  /** Reset actual robot pose (inmediately, without simulating the movement along time)
122  */
123  void setRealPose(const mrpt::poses::CPose2D &p ) { this->m_pose = p; }
124 
125  /** Read the instantaneous, error-free status of the simulated robot
126  */
127  double getX() const { return m_pose.x(); }
128 
129  /** Read the instantaneous, error-free status of the simulated robot
130  */
131  double getY() { return m_pose.y(); }
132 
133  /** Read the instantaneous, error-free status of the simulated robot
134  */
135  double getPHI() { return m_pose.phi(); }
136 
137  /** Read the instantaneous, error-free status of the simulated robot
138  */
139  double getT() { return t; }
140 
141  /** Read the instantaneous, error-free status of the simulated robot
142  */
143  double getV() { return v; }
144  /** Read the instantaneous, error-free status of the simulated robot
145  */
146  double getW() { return w; }
147 
148  /** Set actual robot pose (inmediately, without simulating the movement along time) (Not to be called normally!!)
149  * \sa MovementCommand
150  */
151  void setV(double v) { this->v=v; }
152  void setW(double w) { this->w=w; }
153 
154  /** Used to command the robot a desired movement (velocities)
155  */
156  void movementCommand ( double lin_vel, double ang_vel );
157 
158  /** Reset all the simulator variables to 0 (All but current simulator time).
159  */
160  void resetStatus();
161 
162  /** Reset time counter
163  */
164  void resetTime() { t = 0.0; }
165 
166  /** This method must be called periodically to simulate discrete time intervals.
167  */
168  void simulateInterval( double At);
169 
170  /** Forces odometry to be set to a specified values.
171  */
173  m_odometry = newOdo;
174  }
175 
176  /** Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates).
177  * \sa getRealPose
178  */
179  void getOdometry ( CPose2D &pose ) const {
180  pose = m_odometry;
181  }
182 
183  /** Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates).
184  * \sa getRealPose
185  */
186  void getOdometry ( TPose2D &pose ) const {
187  pose = m_odometry;
188  }
189 
190  /** Reads the real robot pose. \sa getOdometry */
191  void getRealPose ( CPose2D &pose ) const {
192  pose = m_pose;
193  }
194 
195  /** Reads the real robot pose. \sa getOdometry */
196  void getRealPose ( TPose2D &pose ) const {
197  pose = m_pose;
198  }
199  };
200 
201  } // End of namespace
202 } // End of namespace
203 
204 #endif
void getRealPose(TPose2D &pose) const
Reads the real robot pose.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:116
double v
Instantaneous velocity of the robot (linear, m/s)
double getT()
Read the instantaneous, error-free status of the simulated robot.
double w
Instantaneous velocity of the robot (angular, rad/s)
void setV(double v)
Set actual robot pose (inmediately, without simulating the movement along time) (Not to be called nor...
void resetOdometry(const mrpt::poses::CPose2D &newOdo=mrpt::poses::CPose2D())
Forces odometry to be set to a specified values.
void getOdometry(CPose2D &pose) const
Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates)...
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
mrpt::poses::CPose2D m_pose
Global, absolute and error-free robot coordinates.
void resetTime()
Reset time counter.
double getV()
Read the instantaneous, error-free status of the simulated robot.
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
float cTAU
The time-constants for the first order low-pass filter for the velocities changes.
mrpt::poses::CPose2D m_odometry
Used to simulate odometry (with optional error)
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:73
double getY()
Read the instantaneous, error-free status of the simulated robot.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double t
Simulation time variable.
bool usar_error_odometrico
Whether to corrupt odometry with noise.
double getX() const
Read the instantaneous, error-free status of the simulated robot.
void setOdometryErrors(bool enabled, double Ax_err_bias=1e-6, double Ax_err_std=10e-6, double Ay_err_bias=1e-6, double Ay_err_std=10e-6, double Aphi_err_bias=DEG2RAD(1e-6), double Aphi_err_std=DEG2RAD(10e-6))
Enable/Disable odometry errors Errors in odometry are introduced per millisecond. ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void setDelayModelParams(float TAU_delay_sec=1.8f, float CMD_delay_sec=0.3f)
Change the model of delays used for the orders sent to the robot.
A class used to store a 2D pose.
Definition: CPose2D.h:35
double getW()
Read the instantaneous, error-free status of the simulated robot.
double getPHI()
Read the instantaneous, error-free status of the simulated robot.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:83
Lightweight 2D pose.
void setRealPose(const mrpt::poses::CPose2D &p)
Reset actual robot pose (inmediately, without simulating the movement along time) ...
void getOdometry(TPose2D &pose) const
Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates)...
float cDELAY
The delay constant for the velocities changes.
void getRealPose(CPose2D &pose) const
Reads the real robot pose.
This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile...



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo