| 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/CVehicleVelCmd_DiffDriven.h> |
| 13 |
#include <mrpt/serialization/CArchive.h> |
| 14 |
|
| 15 |
using namespace mrpt::kinematics; |
| 16 |
|
| 17 |
IMPLEMENTS_SERIALIZABLE(CVehicleVelCmd_DiffDriven, CVehicleVelCmd, mrpt::kinematics) |
| 18 |
|
| 19 |
CVehicleVelCmd_DiffDriven::~CVehicleVelCmd_DiffDriven() = default; |
| 20 |
size_t CVehicleVelCmd_DiffDriven::getVelCmdLength() const { return 2; } |
| 21 |
std::string CVehicleVelCmd_DiffDriven::getVelCmdDescription(const int index) const |
| 22 |
{ |
| 23 |
switch (index) |
| 24 |
{ |
| 25 |
case 0: |
| 26 |
return "lin_vel"; |
| 27 |
break; |
| 28 |
case 1: |
| 29 |
return "ang_vel"; |
| 30 |
break; |
| 31 |
default: |
| 32 |
THROW_EXCEPTION_FMT("index out of bounds: %i", index); |
| 33 |
}; |
| 34 |
} |
| 35 |
|
| 36 |
double CVehicleVelCmd_DiffDriven::getVelCmdElement(const int index) const |
| 37 |
{ |
| 38 |
switch (index) |
| 39 |
{ |
| 40 |
case 0: |
| 41 |
return lin_vel; |
| 42 |
break; |
| 43 |
case 1: |
| 44 |
return ang_vel; |
| 45 |
break; |
| 46 |
default: |
| 47 |
THROW_EXCEPTION_FMT("index out of bounds: %i", index); |
| 48 |
}; |
| 49 |
} |
| 50 |
|
| 51 |
void CVehicleVelCmd_DiffDriven::setVelCmdElement(const int index, const double val) |
| 52 |
{ |
| 53 |
switch (index) |
| 54 |
{ |
| 55 |
case 0: |
| 56 |
lin_vel = val; |
| 57 |
break; |
| 58 |
case 1: |
| 59 |
ang_vel = val; |
| 60 |
break; |
| 61 |
default: |
| 62 |
THROW_EXCEPTION_FMT("index out of bounds: %i", index); |
| 63 |
}; |
| 64 |
} |
| 65 |
|
| 66 |
bool CVehicleVelCmd_DiffDriven::isStopCmd() const { return lin_vel == .0 && ang_vel == .0; } |
| 67 |
|
| 68 |
void CVehicleVelCmd_DiffDriven::setToStop() { lin_vel = ang_vel = .0; } |
| 69 |
void CVehicleVelCmd_DiffDriven::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) |
| 70 |
{ |
| 71 |
switch (version) |
| 72 |
{ |
| 73 |
case 0: |
| 74 |
in >> lin_vel >> ang_vel; |
| 75 |
break; |
| 76 |
default: |
| 77 |
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); |
| 78 |
}; |
| 79 |
} |
| 80 |
|
| 81 |
uint8_t CVehicleVelCmd_DiffDriven::serializeGetVersion() const { return 0; } |
| 82 |
void CVehicleVelCmd_DiffDriven::serializeTo(mrpt::serialization::CArchive& out) const |
| 83 |
{ |
| 84 |
out << lin_vel << ang_vel; |
| 85 |
} |
| 86 |
|
| 87 |
void CVehicleVelCmd_DiffDriven::cmdVel_scale(double vel_scale) |
| 88 |
{ |
| 89 |
lin_vel *= vel_scale; |
| 90 |
ang_vel *= vel_scale; |
| 91 |
} |
| 92 |
|
| 93 |
double CVehicleVelCmd_DiffDriven::cmdVel_limits( |
| 94 |
const mrpt::kinematics::CVehicleVelCmd& prev_vel_cmd, |
| 95 |
const double beta, |
| 96 |
const TVelCmdParams& params) |
| 97 |
{ |
| 98 |
ASSERT_(params.robotMax_V_mps > 0); |
| 99 |
ASSERT_(params.robotMax_W_radps > 0); |
| 100 |
const auto* prevcmd = |
| 101 |
dynamic_cast<const mrpt::kinematics::CVehicleVelCmd_DiffDriven*>(&prev_vel_cmd); |
| 102 |
ASSERTMSG_(prevcmd, "Expected prevcmd of type `CVehicleVelCmd_DiffDriven`"); |
| 103 |
|
| 104 |
double speed_scale = filter_max_vw(lin_vel, ang_vel, params); |
| 105 |
|
| 106 |
// i.e. new behavior is nearly a pure rotation |
| 107 |
if (std::abs(lin_vel) < 0.01) |
| 108 |
{ // thus, it's OK to blend the rotational component |
| 109 |
ang_vel = beta * ang_vel + (1 - beta) * prevcmd->ang_vel; |
| 110 |
} |
| 111 |
else // there is a non-zero translational component |
| 112 |
{ |
| 113 |
// must maintain the ratio of w to v (while filtering v) |
| 114 |
double ratio = ang_vel / lin_vel; |
| 115 |
// blend new v value |
| 116 |
lin_vel = beta * lin_vel + (1 - beta) * prevcmd->lin_vel; |
| 117 |
// ensure new w implements expected path curvature |
| 118 |
ang_vel = ratio * lin_vel; |
| 119 |
|
| 120 |
speed_scale *= filter_max_vw(lin_vel, ang_vel, params); |
| 121 |
} |
| 122 |
|
| 123 |
return speed_scale; |
| 124 |
} |
| 125 |
|
| 126 |
double CVehicleVelCmd_DiffDriven::filter_max_vw(double& v, double& w, const TVelCmdParams& p) |
| 127 |
{ |
| 128 |
double speed_scale = 1.0; |
| 129 |
// Ensure maximum speeds: |
| 130 |
if (std::abs(v) > p.robotMax_V_mps) |
| 131 |
{ |
| 132 |
// Scale: |
| 133 |
const double F = std::abs(p.robotMax_V_mps / v); |
| 134 |
v *= F; |
| 135 |
w *= F; |
| 136 |
speed_scale *= F; |
| 137 |
} |
| 138 |
|
| 139 |
if (std::abs(w) > p.robotMax_W_radps) |
| 140 |
{ |
| 141 |
// Scale: |
| 142 |
const double F = std::abs(p.robotMax_W_radps / w); |
| 143 |
v *= F; |
| 144 |
w *= F; |
| 145 |
speed_scale *= F; |
| 146 |
} |
| 147 |
return speed_scale; |
| 148 |
} |