| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, 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 + (Command_v - Command_v0) * (1 - exp(-elapsed_time / cTAU)); |
| 46 |
m_w = Command_w0 + (Command_w - Command_w0) * (1 - exp(-elapsed_time / cTAU)); |
| 47 |
} |
| 48 |
|
| 49 |
// Local to global frame: |
| 50 |
m_odometric_vel.vx = cos(m_odometry.phi) * m_v; |
| 51 |
m_odometric_vel.vy = sin(m_odometry.phi) * m_v; |
| 52 |
m_odometric_vel.omega = m_w; |
| 53 |
} |
| 54 |
|
| 55 |
/************************************************************************* |
| 56 |
Gives a movement command to the robot: |
| 57 |
This actually saves the command for execution later on, if we have |
| 58 |
to take into account the robot low-pass behavior in velocities. |
| 59 |
*************************************************************************/ |
| 60 |
void CVehicleSimul_DiffDriven::movementCommand(double lin_vel, double ang_vel) |
| 61 |
{ |
| 62 |
// Just save the command: |
| 63 |
Command_Time = m_time; |
| 64 |
Command_v = lin_vel; |
| 65 |
Command_w = ang_vel; |
| 66 |
|
| 67 |
// Current values: |
| 68 |
Command_v0 = m_v; |
| 69 |
Command_w0 = m_w; |
| 70 |
} |