MRPT
2.0.4
CVehicleSimul_DiffDriven.cpp
Go to the documentation of this file.
1
/* +------------------------------------------------------------------------+
2
| Mobile Robot Programming Toolkit (MRPT) |
3
| https://www.mrpt.org/ |
4
| |
5
| Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6
| See: https://www.mrpt.org/Authors - All rights reserved. |
7
| Released under BSD License. See: https://www.mrpt.org/License |
8
+------------------------------------------------------------------------+ */
9
10
#include "
kinematics-precomp.h
"
// Precompiled header
11
12
#include <
mrpt/kinematics/CVehicleSimul_DiffDriven.h
>
13
14
using namespace
mrpt::kinematics
;
15
16
CVehicleSimul_DiffDriven::CVehicleSimul_DiffDriven
()
17
{
18
resetStatus
();
19
resetTime
();
20
}
21
CVehicleSimul_DiffDriven::~CVehicleSimul_DiffDriven
() =
default
;
22
void
CVehicleSimul_DiffDriven::internal_clear
()
23
{
24
Command_Time
= .0;
25
Command_v
= .0;
26
Command_w
= .0;
27
m_v
=
m_w
= 0;
28
}
29
30
void
CVehicleSimul_DiffDriven::internal_simulControlStep
(
const
double
AAt)
31
{
32
// Change velocities:
33
// ----------------------------------------------------------------
34
double
elapsed_time = this->
m_time
-
Command_Time
;
35
elapsed_time -=
cDELAY
;
36
elapsed_time = std::max(0.0, elapsed_time);
37
38
if
(
cTAU
== 0 &&
cDELAY
== 0)
39
{
40
m_v
=
Command_v
;
41
m_w
=
Command_w
;
42
}
43
else
44
{
45
m_v
=
Command_v0
+
46
(
Command_v
-
Command_v0
) * (1 - exp(-elapsed_time /
cTAU
));
47
m_w
=
Command_w0
+
48
(
Command_w
-
Command_w0
) * (1 - exp(-elapsed_time /
cTAU
));
49
}
50
51
// Local to global frame:
52
m_odometric_vel
.
vx
= cos(
m_odometry
.
phi
) *
m_v
;
53
m_odometric_vel
.
vy
= sin(
m_odometry
.
phi
) *
m_v
;
54
m_odometric_vel
.
omega
=
m_w
;
55
}
56
57
/*************************************************************************
58
Gives a movement command to the robot:
59
This actually saves the command for execution later on, if we have
60
to take into account the robot low-pass behavior in velocities.
61
*************************************************************************/
62
void
CVehicleSimul_DiffDriven::movementCommand
(
double
lin_vel,
double
ang_vel)
63
{
64
// Just save the command:
65
Command_Time
=
m_time
;
66
Command_v
= lin_vel;
67
Command_w
= ang_vel;
68
69
// Current values:
70
Command_v0
=
m_v
;
71
Command_w0
=
m_w
;
72
}
mrpt::kinematics::CVehicleSimul_DiffDriven::cDELAY
double cDELAY
The delay constant for the velocities changes.
Definition:
CVehicleSimul_DiffDriven.h:81
mrpt::kinematics::CVehicleSimul_DiffDriven::Command_v0
double Command_v0
Definition:
CVehicleSimul_DiffDriven.h:74
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition:
TPose2D.h:32
mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus
void resetStatus()
Definition:
CVehicleSimulVirtualBase.cpp:73
mrpt::kinematics::CVehicleSimul_DiffDriven::Command_v
double Command_v
Definition:
CVehicleSimul_DiffDriven.h:74
mrpt::kinematics::CVehicleSimul_DiffDriven::m_w
double m_w
Definition:
CVehicleSimul_DiffDriven.h:64
mrpt::math::TTwist2D::vx
double vx
Velocity components: X,Y (m/s)
Definition:
TTwist2D.h:26
mrpt::math::TTwist2D::vy
double vy
Definition:
TTwist2D.h:26
mrpt::kinematics::CVehicleSimul_DiffDriven::cTAU
double cTAU
The time-constants for the first order low-pass filter for the velocities changes.
Definition:
CVehicleSimul_DiffDriven.h:79
mrpt::kinematics::CVehicleSimul_DiffDriven::Command_Time
double Command_Time
Dynamic limitations of the robot.
Definition:
CVehicleSimul_DiffDriven.h:74
mrpt::kinematics::CVehicleSimul_DiffDriven::internal_clear
void internal_clear() override
Resets all pending cmds.
Definition:
CVehicleSimul_DiffDriven.cpp:22
mrpt::kinematics::CVehicleSimul_DiffDriven::CVehicleSimul_DiffDriven
CVehicleSimul_DiffDriven()
Definition:
CVehicleSimul_DiffDriven.cpp:16
kinematics-precomp.h
mrpt::kinematics::CVehicleSimulVirtualBase::resetTime
void resetTime()
Reset all simulator variables to 0 (except the.
Definition:
CVehicleSimulVirtualBase.cpp:82
mrpt::kinematics::CVehicleSimul_DiffDriven::Command_w0
double Command_w0
Definition:
CVehicleSimul_DiffDriven.h:75
mrpt::kinematics::CVehicleSimul_DiffDriven::Command_w
double Command_w
Definition:
CVehicleSimul_DiffDriven.h:74
mrpt::kinematics::CVehicleSimul_DiffDriven::internal_simulControlStep
void internal_simulControlStep(const double dt) override
Definition:
CVehicleSimul_DiffDriven.cpp:30
mrpt::kinematics::CVehicleSimul_DiffDriven::movementCommand
void movementCommand(double lin_vel, double ang_vel)
Used to command the robot a desired movement:
Definition:
CVehicleSimul_DiffDriven.cpp:62
mrpt::kinematics::CVehicleSimul_DiffDriven::~CVehicleSimul_DiffDriven
~CVehicleSimul_DiffDriven() override
CVehicleSimul_DiffDriven.h
mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel
mrpt::math::TTwist2D m_odometric_vel
Velocity in (x,y,omega)
Definition:
CVehicleSimulVirtualBase.h:123
mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry
mrpt::math::TPose2D m_odometry
Definition:
CVehicleSimulVirtualBase.h:124
mrpt::kinematics
Definition:
CKinematicChain.h:18
mrpt::math::TTwist2D::omega
double omega
Angular velocity (rad/s)
Definition:
TTwist2D.h:28
mrpt::kinematics::CVehicleSimul_DiffDriven::m_v
double m_v
lin & angular velocity in the robot local frame.
Definition:
CVehicleSimul_DiffDriven.h:64
mrpt::kinematics::CVehicleSimulVirtualBase::m_time
double m_time
simulation running time
Definition:
CVehicleSimulVirtualBase.h:117
Page generated by
Doxygen 1.8.17
for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020