Main MRPT website > C++ reference
MRPT logo

CRobotSimulator.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CRobotSimulator_H
00029 #define CRobotSimulator_H
00030 
00031 #include <mrpt/utils/CSerializable.h>
00032 #include <mrpt/poses/CPose2D.h>
00033 
00034 #include <mrpt/base/link_pragmas.h>
00035 
00036 namespace mrpt
00037 {
00038 namespace utils
00039 {
00040         using namespace mrpt::poses;
00041         using namespace mrpt::math;
00042 
00043         /** This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile robot, including odometry errors and dynamics limitations.
00044          *  The main methods are:
00045                         - movementCommand: Call this for send a command to the robot. This comamnd will be
00046                                                                 delayed and passed throught a first order low-pass filter to simulate
00047                                                                 robot dynamics.
00048                         - simulateInterval: Call this for run the simulator for the desired time period.
00049          *
00050                 Versions:
00051                         - 23/MAR/2009: (JLBC) Changed to reuse MRPT poses and methods renamed to conform to MRPT style.
00052                         - 29/AUG/2008: (JLBC) Added parameters for odometry noise.
00053                         - 27/JAN/2008: (JLBC) Translated to English!!! :-)
00054                         - 17/OCT/2005: (JLBC) Integration into the MRML library.
00055                         - 1/DIC/2004: (JLBC) Odometry, cumulative errors added.
00056                         - 18/JUN/2004: (JLBC) First creation.
00057          *
00058          */
00059         class BASE_IMPEXP CRobotSimulator
00060         {
00061                 private:
00062                         //      Internal state variables:
00063                         // ---------------------------------------
00064                         mrpt::poses::CPose2D  m_pose;   //!< Global, absolute and error-free robot coordinates
00065                         mrpt::poses::CPose2D m_odometry;        //!< Used to simulate odometry (with optional error)
00066 
00067                         /** Instantaneous velocity of the robot (linear, m/s)
00068                           */
00069                         double          v;
00070 
00071                         /** Instantaneous velocity of the robot (angular, rad/s)
00072                           */
00073                         double          w;
00074 
00075                         /** Simulation time variable
00076                           */
00077                         double          t;
00078 
00079                         /** Whether to corrupt odometry with noise  */
00080                         bool            usar_error_odometrico;
00081 
00082                         /** Dynamic limitations of the robot.
00083                           * Approximation to non-infinity motor forces: A first order low-pass filter, using:
00084                           *   Command_Time: Time "t" when the last order was received.
00085                           *   Command_v, Command_w: The user-desired velocities.
00086                           *   Command_v0, Command_w0: Actual robot velocities at the moment of user request.
00087                           */
00088                         double Command_Time,
00089                                Command_v, Command_w,
00090                                    Command_v0, Command_w0;
00091 
00092                         /** The time-constants for the first order low-pass filter for the velocities changes. */
00093                         float                   cTAU;                   // 1.8 sec
00094 
00095                         /** The delay constant for the velocities changes.  */
00096                         float                   cDELAY;
00097 
00098                         double m_Ax_err_bias, m_Ax_err_std;
00099                         double m_Ay_err_bias, m_Ay_err_std;
00100                         double m_Aphi_err_bias, m_Aphi_err_std;
00101 
00102                 public:
00103                         /** Constructor with default dynamic model-parameters
00104                           */
00105                         CRobotSimulator( float TAU = 0, float DELAY = 0);
00106 
00107                         /** Destructor
00108                           */
00109                         virtual ~CRobotSimulator();
00110 
00111                         /** Change the model of delays used for the orders sent to the robot \sa movementCommand */
00112                         void setDelayModelParams(float TAU_delay_sec=1.8f, float CMD_delay_sec=0.3f) {
00113                                 cTAU = TAU_delay_sec;
00114                                 cDELAY = CMD_delay_sec;
00115                         }
00116 
00117                         /** Enable/Disable odometry errors
00118                           *  Errors in odometry are introduced per millisecond.
00119                           */
00120                         void setOdometryErrors(
00121                                 bool enabled,
00122                                 double Ax_err_bias  =  1e-6,
00123                                 double Ax_err_std   = 10e-6,
00124                                 double Ay_err_bias =  1e-6,
00125                                 double Ay_err_std  = 10e-6,
00126                                 double Aphi_err_bias =  DEG2RAD(1e-6),
00127                                 double Aphi_err_std  = DEG2RAD(10e-6)
00128                                 )
00129                         {
00130                                 usar_error_odometrico=enabled;
00131                                 m_Ax_err_bias=Ax_err_bias;
00132                                 m_Ax_err_std=Ax_err_std;
00133                                 m_Ay_err_bias=Ay_err_bias;
00134                                 m_Ay_err_std=Ay_err_std;
00135                                 m_Aphi_err_bias=Aphi_err_bias;
00136                                 m_Aphi_err_std=Aphi_err_std;
00137                         }
00138 
00139                         /** Reset actual robot pose (inmediately, without simulating the movement along time)
00140                           */
00141                         void  setRealPose(mrpt::poses::CPose2D &p ) { this->m_pose = p; }
00142 
00143                         /** Read the instantaneous, error-free status of the simulated robot
00144                           */
00145                         double  getX() const { return m_pose.x(); }
00146 
00147                         /** Read the instantaneous, error-free status of the simulated robot
00148                           */
00149                         double  getY() { return m_pose.y(); }
00150 
00151                         /** Read the instantaneous, error-free status of the simulated robot
00152                           */
00153                         double  getPHI() { return m_pose.phi(); }
00154 
00155                         /** Read the instantaneous, error-free status of the simulated robot
00156                           */
00157                         double  getT()   { return t; }
00158 
00159                         /** Read the instantaneous, error-free status of the simulated robot
00160                           */
00161                         double  getV() { return v; }
00162                         /** Read the instantaneous, error-free status of the simulated robot
00163                           */
00164                         double  getW() { return w; }
00165 
00166                         /** Set actual robot pose (inmediately, without simulating the movement along time) (Not to be called normally!!)
00167                           * \sa MovementCommand
00168                           */
00169                         void    setV(double v) { this->v=v; }
00170                         void    setW(double w) { this->w=w; }
00171 
00172                         /** Used to command the robot a desired movement (velocities)
00173                           */
00174                         void    movementCommand ( double lin_vel, double ang_vel );
00175 
00176                         /** Reset all the simulator variables to 0 (All but current simulator time).
00177                           */
00178                         void    resetStatus();
00179 
00180                         /** Reset time counter
00181                           */
00182                         void    resetTime()  { t = 0.0; }
00183 
00184                         /** This method must be called periodically to simulate discrete time intervals.
00185                           */
00186                         void    simulateInterval( double At);
00187 
00188                         /** Forces odometry to be set to a specified values.
00189                           */
00190                         void    resetOdometry( const mrpt::poses::CPose2D &newOdo = mrpt::poses::CPose2D() ) {
00191                                 m_odometry = newOdo;
00192                         }
00193 
00194                         /** Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates).
00195                           * \sa getRealPose
00196                           */
00197                         void    getOdometry ( CPose2D &pose ) const {
00198                                 pose = m_odometry;
00199                         }
00200 
00201                         /** Reads the simulated robot odometry (this is NOT equal to the actual error-free robot coordinates).
00202                           * \sa getRealPose
00203                           */
00204                         void    getOdometry ( TPose2D &pose ) const {
00205                                 pose = m_odometry;
00206                         }
00207 
00208                         /** Reads the real robot pose. \sa getOdometry  */
00209                         void    getRealPose ( CPose2D &pose ) const {
00210                                 pose = m_pose;
00211                         }
00212 
00213                         /** Reads the real robot pose. \sa getOdometry  */
00214                         void    getRealPose ( TPose2D &pose ) const {
00215                                 pose = m_pose;
00216                         }
00217         };
00218 
00219         } // End of namespace
00220 } // End of namespace
00221 
00222 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011