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
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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 getOdometry(mrpt::math::TPose2D &pose) const
Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates)...
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.
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.
void getRealPose(mrpt::poses::CPose2D &pose) const
Reads the real robot pose.
double getV()
Read the instantaneous, error-free status of the simulated robot.
void getOdometry(mrpt::poses::CPose2D &pose) const
Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates)...
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.
void getRealPose(mrpt::math::TPose2D &pose) const
Reads the real robot pose.
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:36
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:84
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.
This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile...



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Mon Oct 3 19:22:36 UTC 2016