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
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.
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:69
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.



Page generated by Doxygen 1.9.4 for MRPT 1.4.0 SVN: at Sun Aug 14 11:34:44 UTC 2022