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



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Tue Dec 8 09:49:21 UTC 2015