Simulates the kinematics of a differential-driven planar mobile robot/vehicle, including odometry errors and dynamics limitations.
Definition at line 23 of file CVehicleSimul_DiffDriven.h.
#include <mrpt/kinematics/CVehicleSimul_DiffDriven.h>
Public Types | |
using | kinematic_cmd_t = CVehicleVelCmd_DiffDriven |
Public Member Functions | |
CVehicleSimul_DiffDriven () | |
virtual | ~CVehicleSimul_DiffDriven () |
void | setDelayModelParams (double TAU_delay_sec=1.8, double CMD_delay_sec=0.) |
Change the model of delays used for the orders sent to the robot. More... | |
void | setV (double v) |
void | setW (double w) |
double | getV () |
double | getW () |
void | movementCommand (double lin_vel, double ang_vel) |
Used to command the robot a desired movement: More... | |
void | sendVelCmd (const CVehicleVelCmd &cmd_vel) override |
Sends a velocity command to the robot. More... | |
CVehicleVelCmd::Ptr | getVelCmdType () const override |
Gets an empty velocity command object that can be queried to find out the number of velcmd components,... More... | |
Protected Attributes | |
double | m_firmware_control_period |
The period at which the low-level controller updates velocities (Default: 0.5 ms) More... | |
bool | m_use_odo_error |
Whether to corrupt odometry with noise. More... | |
double | m_Ax_err_bias |
double | m_Ax_err_std |
double | m_Ay_err_bias |
double | m_Ay_err_std |
double | m_Aphi_err_bias |
double | m_Aphi_err_std |
Private Member Functions | |
void | internal_simulControlStep (const double dt) override |
void | internal_clear () override |
Resets all pending cmds. More... | |
Private Attributes | |
double | m_v |
lin & angular velocity in the robot local frame. More... | |
double | m_w |
double | Command_Time |
Dynamic limitations of the robot. More... | |
double | Command_v |
double | Command_w |
double | Command_v0 |
double | Command_w0 |
double | cTAU |
The time-constants for the first order low-pass filter for the velocities changes. More... | |
double | cDELAY |
The delay constant for the velocities changes. More... | |
Definition at line 26 of file CVehicleSimul_DiffDriven.h.
CVehicleSimul_DiffDriven::CVehicleSimul_DiffDriven | ( | ) |
Definition at line 16 of file CVehicleSimul_DiffDriven.cpp.
References mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus(), and mrpt::kinematics::CVehicleSimulVirtualBase::resetTime().
|
virtual |
Definition at line 21 of file CVehicleSimul_DiffDriven.cpp.
|
inlineinherited |
Returns the instantaneous, ground truth pose in world coordinates.
Definition at line 46 of file CVehicleSimulVirtualBase.h.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose.
Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds(), and run_rnav_test().
|
inlineinherited |
Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates.
Definition at line 65 of file CVehicleSimulVirtualBase.h.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel.
Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds().
|
inherited |
Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in the robot local frame.
Definition at line 89 of file CVehicleSimulVirtualBase.cpp.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose, mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel, mrpt::math::TPose2D::phi, and mrpt::math::TTwist2D::rotate().
|
inlineinherited |
Returns the current pose according to (noisy) odometry.
Definition at line 52 of file CVehicleSimulVirtualBase.h.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.
Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds().
|
inlineinherited |
Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates.
Definition at line 72 of file CVehicleSimulVirtualBase.h.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel.
|
inherited |
Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame.
Definition at line 96 of file CVehicleSimulVirtualBase.cpp.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry, mrpt::math::TPose2D::phi, and mrpt::math::TTwist2D::rotate().
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
inlineinherited |
Get the current simulation time.
Definition at line 81 of file CVehicleSimulVirtualBase.h.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_time.
Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getNavigationTime(), mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getNavigationTime(), mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::resetNavigationTimer(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::resetNavigationTimer().
|
inline |
Definition at line 42 of file CVehicleSimul_DiffDriven.h.
References m_v.
|
inlineoverridevirtual |
Gets an empty velocity command object that can be queried to find out the number of velcmd components,...
Implements mrpt::kinematics::CVehicleSimulVirtualBase.
Definition at line 60 of file CVehicleSimul_DiffDriven.h.
|
inline |
Definition at line 43 of file CVehicleSimul_DiffDriven.h.
References m_w.
|
overrideprivatevirtual |
Resets all pending cmds.
Implements mrpt::kinematics::CVehicleSimulVirtualBase.
Definition at line 22 of file CVehicleSimul_DiffDriven.cpp.
References Command_Time, Command_v, Command_w, m_v, and m_w.
|
overrideprivatevirtual |
Implements mrpt::kinematics::CVehicleSimulVirtualBase.
Definition at line 30 of file CVehicleSimul_DiffDriven.cpp.
References cDELAY, Command_Time, Command_v, Command_v0, Command_w, Command_w0, cTAU, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry, mrpt::kinematics::CVehicleSimulVirtualBase::m_time, m_v, m_w, mrpt::math::TTwist2D::omega, mrpt::math::TPose2D::phi, mrpt::math::TTwist2D::vx, and mrpt::math::TTwist2D::vy.
void CVehicleSimul_DiffDriven::movementCommand | ( | double | lin_vel, |
double | ang_vel | ||
) |
Used to command the robot a desired movement:
lin_vel | Linar velocity (m/s) |
ang_vel | Angular velocity (rad/s) |
Definition at line 62 of file CVehicleSimul_DiffDriven.cpp.
References Command_Time, Command_v, Command_v0, Command_w, Command_w0, mrpt::kinematics::CVehicleSimulVirtualBase::m_time, m_v, and m_w.
Referenced by sendVelCmd().
|
inherited |
Definition at line 79 of file CVehicleSimulVirtualBase.cpp.
References mrpt::kinematics::CVehicleSimulVirtualBase::internal_clear(), mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose, mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel, and mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.
Referenced by CVehicleSimul_DiffDriven(), and mrpt::kinematics::CVehicleSimul_Holo::CVehicleSimul_Holo().
|
inherited |
Reset all simulator variables to 0 (except the.
simulation time).
Definition at line 88 of file CVehicleSimulVirtualBase.cpp.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_time.
Referenced by CVehicleSimul_DiffDriven(), and mrpt::kinematics::CVehicleSimul_Holo::CVehicleSimul_Holo().
|
inlineoverridevirtual |
Sends a velocity command to the robot.
The number of components and their meaning depends on the vehicle-kinematics derived class
Implements mrpt::kinematics::CVehicleSimulVirtualBase.
Definition at line 50 of file CVehicleSimul_DiffDriven.h.
References mrpt::kinematics::CVehicleVelCmd_DiffDriven::ang_vel, ASSERTMSG_, mrpt::kinematics::CVehicleVelCmd_DiffDriven::lin_vel, and movementCommand().
Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::changeSpeeds(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::stop().
|
inherited |
Brute-force move robot to target coordinates ("teleport")
Definition at line 24 of file CVehicleSimulVirtualBase.cpp.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose.
|
inlineinherited |
Brute-force overwrite robot odometry
Definition at line 58 of file CVehicleSimulVirtualBase.h.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.
|
inline |
Change the model of delays used for the orders sent to the robot.
Definition at line 33 of file CVehicleSimul_DiffDriven.h.
|
inlineinherited |
Enable/Disable odometry errors.
Errors in odometry are 1 sigma Gaussian values per second
Definition at line 92 of file CVehicleSimulVirtualBase.h.
References mrpt::kinematics::CVehicleSimulVirtualBase::m_Aphi_err_bias, mrpt::kinematics::CVehicleSimulVirtualBase::m_Aphi_err_std, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ax_err_bias, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ax_err_std, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ay_err_bias, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ay_err_std, and mrpt::kinematics::CVehicleSimulVirtualBase::m_use_odo_error.
|
inline |
Definition at line 40 of file CVehicleSimul_DiffDriven.h.
References m_v.
|
inline |
Definition at line 41 of file CVehicleSimul_DiffDriven.h.
References m_w.
|
inherited |
Runs the simulator during "dt" seconds.
It will be split into periods of "m_firmware_control_period".
Definition at line 29 of file CVehicleSimulVirtualBase.cpp.
References mrpt::random::CRandomGenerator::drawGaussian1D_normalized(), mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricVelLocal(), mrpt::random::getRandomGenerator(), mrpt::kinematics::CVehicleSimulVirtualBase::internal_simulControlStep(), mrpt::kinematics::CVehicleSimulVirtualBase::m_Aphi_err_bias, mrpt::kinematics::CVehicleSimulVirtualBase::m_Aphi_err_std, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ax_err_bias, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ax_err_std, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ay_err_bias, mrpt::kinematics::CVehicleSimulVirtualBase::m_Ay_err_std, mrpt::kinematics::CVehicleSimulVirtualBase::m_firmware_control_period, mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose, mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel, mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry, mrpt::kinematics::CVehicleSimulVirtualBase::m_time, mrpt::kinematics::CVehicleSimulVirtualBase::m_use_odo_error, mrpt::math::TTwist2D::omega, mrpt::math::TPose2D::phi, mrpt::math::TTwist2D::rotate(), mrpt::math::TTwist2D::vx, mrpt::math::TTwist2D::vy, mrpt::math::wrapToPiInPlace(), and mrpt::math::TPose2D::x.
Referenced by run_rnav_test().
|
private |
The delay constant for the velocities changes.
Definition at line 83 of file CVehicleSimul_DiffDriven.h.
Referenced by internal_simulControlStep(), and setDelayModelParams().
|
private |
Dynamic limitations of the robot.
Approximation to non-infinity motor forces: A first order low-pass filter, using: Command_Time: Time "t" when the last order was received. Command_v, Command_w: The user-desired velocities. Command_v0, Command_w0: Actual robot velocities at the moment of user request.
Definition at line 77 of file CVehicleSimul_DiffDriven.h.
Referenced by internal_clear(), internal_simulControlStep(), and movementCommand().
|
private |
Definition at line 77 of file CVehicleSimul_DiffDriven.h.
Referenced by internal_clear(), internal_simulControlStep(), and movementCommand().
|
private |
Definition at line 77 of file CVehicleSimul_DiffDriven.h.
Referenced by internal_simulControlStep(), and movementCommand().
|
private |
Definition at line 77 of file CVehicleSimul_DiffDriven.h.
Referenced by internal_clear(), internal_simulControlStep(), and movementCommand().
|
private |
Definition at line 77 of file CVehicleSimul_DiffDriven.h.
Referenced by internal_simulControlStep(), and movementCommand().
|
private |
The time-constants for the first order low-pass filter for the velocities changes.
Definition at line 81 of file CVehicleSimul_DiffDriven.h.
Referenced by internal_simulControlStep(), and setDelayModelParams().
|
protectedinherited |
Definition at line 135 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Definition at line 135 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Definition at line 133 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Definition at line 133 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Definition at line 134 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Definition at line 134 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
The period at which the low-level controller updates velocities (Default: 0.5 ms)
Definition at line 129 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
ground truth pose in world coordinates.
Definition at line 120 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTPose(), mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTVelLocal(), mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus(), mrpt::kinematics::CVehicleSimulVirtualBase::setCurrentGTPose(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Velocity in (x,y,omega)
Definition at line 122 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTVel(), mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTVelLocal(), mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Velocity in (x,y,omega)
Definition at line 124 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricVel(), mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricVelLocal(), mrpt::kinematics::CVehicleSimul_Holo::internal_simulControlStep(), internal_simulControlStep(), mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus(), mrpt::kinematics::CVehicleSimul_Holo::sendVelRampCmd(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Definition at line 125 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricPose(), mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricVelLocal(), mrpt::kinematics::CVehicleSimul_Holo::internal_simulControlStep(), internal_simulControlStep(), mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus(), mrpt::kinematics::CVehicleSimul_Holo::sendVelCmd(), mrpt::kinematics::CVehicleSimulVirtualBase::setCurrentOdometricPose(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
simulation running time
Definition at line 118 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::getTime(), mrpt::kinematics::CVehicleSimul_Holo::internal_simulControlStep(), internal_simulControlStep(), movementCommand(), mrpt::kinematics::CVehicleSimulVirtualBase::resetTime(), mrpt::kinematics::CVehicleSimul_Holo::sendVelRampCmd(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
protectedinherited |
Whether to corrupt odometry with noise.
Definition at line 132 of file CVehicleSimulVirtualBase.h.
Referenced by mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors(), and mrpt::kinematics::CVehicleSimulVirtualBase::simulateOneTimeStep().
|
private |
lin & angular velocity in the robot local frame.
Definition at line 67 of file CVehicleSimul_DiffDriven.h.
Referenced by getV(), internal_clear(), internal_simulControlStep(), movementCommand(), and setV().
|
private |
Definition at line 67 of file CVehicleSimul_DiffDriven.h.
Referenced by getW(), internal_clear(), internal_simulControlStep(), movementCommand(), and setW().
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST |