Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
16
17namespace mrpt
18{
19namespace 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 */
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 */
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 */
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
#define DEG2RAD
Definition: bits.h:86
A class used to store a 2D pose.
Definition: CPose2D.h:37
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
This class can be used to simulate the kinematics and dynamics of a differential driven planar mobile...
void movementCommand(double lin_vel, double ang_vel)
Used to command the robot a desired movement (velocities)
double getT()
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).
bool usar_error_odometrico
Whether to corrupt odometry with noise
void resetOdometry(const mrpt::poses::CPose2D &newOdo=mrpt::poses::CPose2D())
Forces odometry to be set to a specified values.
double Command_Time
Dynamic limitations of the robot.
double getV()
Read the instantaneous, error-free status of the simulated robot.
double getW()
Read the instantaneous, error-free status of the simulated robot.
void simulateInterval(double At)
This method must be called periodically to simulate discrete time intervals.
float cTAU
The time-constants for the first order low-pass filter for the velocities changes.
CRobotSimulator(float TAU=0, float DELAY=0)
Constructor with default dynamic model-parameters.
double getY()
Read the instantaneous, error-free status of the simulated robot.
double w
Instantaneous velocity of the robot (angular, rad/s)
virtual ~CRobotSimulator()
Destructor.
double v
Instantaneous velocity of the robot (linear, m/s)
void resetTime()
Reset time counter.
double t
Simulation time variable.
mrpt::poses::CPose2D m_odometry
Used to simulate odometry (with optional error)
void resetStatus()
Reset all the simulator variables to 0 (All but current simulator time).
void setV(double v)
Set actual robot pose (inmediately, without simulating the movement along time) (Not to be called nor...
double getPHI()
Read the instantaneous, error-free status of the simulated robot.
void getRealPose(mrpt::poses::CPose2D &pose) const
Reads the real robot pose.
void getRealPose(mrpt::math::TPose2D &pose) const
Reads the real robot pose.
double getX() const
Read the instantaneous, error-free status of the simulated robot.
void getOdometry(mrpt::math::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 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.
void setRealPose(const mrpt::poses::CPose2D &p)
Reset actual robot pose (inmediately, without simulating the movement along time)
mrpt::poses::CPose2D m_pose
Global, absolute and error-free robot coordinates.
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.
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Mon Dec 26 04:51:47 UTC 2022