Main MRPT website > C++ reference
MRPT logo
CRobotSimulator.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CRobotSimulator_H
29 #define CRobotSimulator_H
30 
31 #include <mrpt/utils/CSerializable.h>
32 #include <mrpt/poses/CPose2D.h>
33 
34 #include <mrpt/base/link_pragmas.h>
35 
36 namespace mrpt
37 {
38 namespace utils
39 {
40  using namespace mrpt::poses;
41  using namespace mrpt::math;
42 
43  /** This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile robot, including odometry errors and dynamics limitations.
44  * The main methods are:
45  - movementCommand: Call this for send a command to the robot. This comamnd will be
46  delayed and passed throught a first order low-pass filter to simulate
47  robot dynamics.
48  - simulateInterval: Call this for run the simulator for the desired time period.
49  *
50  Versions:
51  - 23/MAR/2009: (JLBC) Changed to reuse MRPT poses and methods renamed to conform to MRPT style.
52  - 29/AUG/2008: (JLBC) Added parameters for odometry noise.
53  - 27/JAN/2008: (JLBC) Translated to English!!! :-)
54  - 17/OCT/2005: (JLBC) Integration into the MRML library.
55  - 1/DIC/2004: (JLBC) Odometry, cumulative errors added.
56  - 18/JUN/2004: (JLBC) First creation.
57  *
58  * \ingroup mrpt_base_grp
59  */
60  class BASE_IMPEXP CRobotSimulator
61  {
62  private:
63  // Internal state variables:
64  // ---------------------------------------
65  mrpt::poses::CPose2D m_pose; //!< Global, absolute and error-free robot coordinates
66  mrpt::poses::CPose2D m_odometry; //!< Used to simulate odometry (with optional error)
67 
68  /** Instantaneous velocity of the robot (linear, m/s)
69  */
70  double v;
71 
72  /** Instantaneous velocity of the robot (angular, rad/s)
73  */
74  double w;
75 
76  /** Simulation time variable
77  */
78  double t;
79 
80  /** Whether to corrupt odometry with noise */
81  bool usar_error_odometrico;
82 
83  /** Dynamic limitations of the robot.
84  * Approximation to non-infinity motor forces: A first order low-pass filter, using:
85  * Command_Time: Time "t" when the last order was received.
86  * Command_v, Command_w: The user-desired velocities.
87  * Command_v0, Command_w0: Actual robot velocities at the moment of user request.
88  */
89  double Command_Time,
90  Command_v, Command_w,
91  Command_v0, Command_w0;
92 
93  /** The time-constants for the first order low-pass filter for the velocities changes. */
94  float cTAU; // 1.8 sec
95 
96  /** The delay constant for the velocities changes. */
97  float cDELAY;
98 
99  double m_Ax_err_bias, m_Ax_err_std;
100  double m_Ay_err_bias, m_Ay_err_std;
101  double m_Aphi_err_bias, m_Aphi_err_std;
102 
103  public:
104  /** Constructor with default dynamic model-parameters
105  */
106  CRobotSimulator( float TAU = 0, float DELAY = 0);
107 
108  /** Destructor
109  */
110  virtual ~CRobotSimulator();
111 
112  /** Change the model of delays used for the orders sent to the robot \sa movementCommand */
113  void setDelayModelParams(float TAU_delay_sec=1.8f, float CMD_delay_sec=0.3f) {
114  cTAU = TAU_delay_sec;
115  cDELAY = CMD_delay_sec;
116  }
117 
118  /** Enable/Disable odometry errors
119  * Errors in odometry are introduced per millisecond.
120  */
121  void setOdometryErrors(
122  bool enabled,
123  double Ax_err_bias = 1e-6,
124  double Ax_err_std = 10e-6,
125  double Ay_err_bias = 1e-6,
126  double Ay_err_std = 10e-6,
127  double Aphi_err_bias = DEG2RAD(1e-6),
128  double Aphi_err_std = DEG2RAD(10e-6)
129  )
130  {
131  usar_error_odometrico=enabled;
132  m_Ax_err_bias=Ax_err_bias;
133  m_Ax_err_std=Ax_err_std;
134  m_Ay_err_bias=Ay_err_bias;
135  m_Ay_err_std=Ay_err_std;
136  m_Aphi_err_bias=Aphi_err_bias;
137  m_Aphi_err_std=Aphi_err_std;
138  }
139 
140  /** Reset actual robot pose (inmediately, without simulating the movement along time)
141  */
142  void setRealPose(mrpt::poses::CPose2D &p ) { this->m_pose = p; }
143 
144  /** Read the instantaneous, error-free status of the simulated robot
145  */
146  double getX() const { return m_pose.x(); }
147 
148  /** Read the instantaneous, error-free status of the simulated robot
149  */
150  double getY() { return m_pose.y(); }
151 
152  /** Read the instantaneous, error-free status of the simulated robot
153  */
154  double getPHI() { return m_pose.phi(); }
155 
156  /** Read the instantaneous, error-free status of the simulated robot
157  */
158  double getT() { return t; }
159 
160  /** Read the instantaneous, error-free status of the simulated robot
161  */
162  double getV() { return v; }
163  /** Read the instantaneous, error-free status of the simulated robot
164  */
165  double getW() { return w; }
166 
167  /** Set actual robot pose (inmediately, without simulating the movement along time) (Not to be called normally!!)
168  * \sa MovementCommand
169  */
170  void setV(double v) { this->v=v; }
171  void setW(double w) { this->w=w; }
172 
173  /** Used to command the robot a desired movement (velocities)
174  */
175  void movementCommand ( double lin_vel, double ang_vel );
176 
177  /** Reset all the simulator variables to 0 (All but current simulator time).
178  */
179  void resetStatus();
180 
181  /** Reset time counter
182  */
183  void resetTime() { t = 0.0; }
184 
185  /** This method must be called periodically to simulate discrete time intervals.
186  */
187  void simulateInterval( double At);
188 
189  /** Forces odometry to be set to a specified values.
190  */
191  void resetOdometry( const mrpt::poses::CPose2D &newOdo = mrpt::poses::CPose2D() ) {
192  m_odometry = newOdo;
193  }
194 
195  /** Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates).
196  * \sa getRealPose
197  */
198  void getOdometry ( CPose2D &pose ) const {
199  pose = m_odometry;
200  }
201 
202  /** Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates).
203  * \sa getRealPose
204  */
205  void getOdometry ( TPose2D &pose ) const {
206  pose = m_odometry;
207  }
208 
209  /** Reads the real robot pose. \sa getOdometry */
210  void getRealPose ( CPose2D &pose ) const {
211  pose = m_pose;
212  }
213 
214  /** Reads the real robot pose. \sa getOdometry */
215  void getRealPose ( TPose2D &pose ) const {
216  pose = m_pose;
217  }
218  };
219 
220  } // End of namespace
221 } // End of namespace
222 
223 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013