Detailed Description

Simulates the kinematics of a differential-driven planar mobile robot/vehicle, including odometry errors and dynamics limitations.

Definition at line 21 of file CVehicleSimul_DiffDriven.h.

#include <mrpt/kinematics/CVehicleSimul_DiffDriven.h>

Inheritance diagram for mrpt::kinematics::CVehicleSimul_DiffDriven:

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...

Kinematic simulation and control interface
void	simulateOneTimeStep (const double dt)
	Runs the simulator during "dt" seconds. More...

const mrpt::math::TPose2D &	getCurrentGTPose () const
	Returns the instantaneous, ground truth pose in world coordinates. More...

void	setCurrentGTPose (const mrpt::math::TPose2D &pose)
	Brute-force move robot to target coordinates ("teleport") More...

const mrpt::math::TPose2D &	getCurrentOdometricPose () const
	Returns the current pose according to (noisy) odometry. More...

template<typename T >
void	setCurrentOdometricPose (const T &pose)
	Brute-force overwrite robot odometry. More...

const mrpt::math::TTwist2D &	getCurrentGTVel () const
	Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates. More...

mrpt::math::TTwist2D	getCurrentGTVelLocal () const
	Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in the robot local frame. More...

const mrpt::math::TTwist2D &	getCurrentOdometricVel () const
	Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates. More...

mrpt::math::TTwist2D	getCurrentOdometricVelLocal () const
	Returns the instantaneous, odometric velocity vector (vx,vy,omega) in the robot local frame. More...

double	getTime () const
	Get the current simulation time. More...

void	setOdometryErrors (bool enabled, double Ax_err_bias=1e-3, double Ax_err_std=10e-3, double Ay_err_bias=1e-3, double Ay_err_std=10e-3, double Aphi_err_bias=mrpt::DEG2RAD(1e-3), double Aphi_err_std=mrpt::DEG2RAD(10e-3))
	Enable/Disable odometry errors. More...

void	resetStatus ()

void	resetTime ()
	Reset all simulator variables to 0 (except the. 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

State vector
double	m_time
	simulation running time More...

mrpt::math::TPose2D	m_GT_pose
	ground truth pose in world coordinates. More...

mrpt::math::TTwist2D	m_GT_vel
	Velocity in (x,y,omega) More...

mrpt::math::TTwist2D	m_odometric_vel
	Velocity in (x,y,omega) More...

mrpt::math::TPose2D	m_odometry

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...

Member Typedef Documentation

◆ kinematic_cmd_t

using mrpt::kinematics::CVehicleSimul_DiffDriven::kinematic_cmd_t = CVehicleVelCmd_DiffDriven

Definition at line 24 of file CVehicleSimul_DiffDriven.h.

Constructor & Destructor Documentation

◆ CVehicleSimul_DiffDriven()

CVehicleSimul_DiffDriven::CVehicleSimul_DiffDriven ( )

Definition at line 16 of file CVehicleSimul_DiffDriven.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::resetStatus(), and mrpt::kinematics::CVehicleSimulVirtualBase::resetTime().

◆ ~CVehicleSimul_DiffDriven()

CVehicleSimul_DiffDriven::~CVehicleSimul_DiffDriven ( )

virtual

Definition at line 21 of file CVehicleSimul_DiffDriven.cpp.

Member Function Documentation

◆ getCurrentGTPose()

const mrpt::math::TPose2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTPose ( ) const

inlineinherited

Returns the instantaneous, ground truth pose in world coordinates.

Definition at line 44 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().

◆ getCurrentGTVel()

const mrpt::math::TTwist2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentGTVel ( ) const

inlineinherited

Returns the instantaneous, ground truth velocity vector (vx,vy,omega) in world coordinates.

Definition at line 63 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_vel.

Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds().

◆ getCurrentGTVelLocal()

mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentGTVelLocal ( ) const

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().

◆ getCurrentOdometricPose()

const mrpt::math::TPose2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricPose ( ) const

inlineinherited

Returns the current pose according to (noisy) odometry.

See also: setOdometryErrors

Definition at line 50 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.

Referenced by mrpt::nav::CRobot2NavInterfaceForSimulator_Holo::getCurrentPoseAndSpeeds(), and mrpt::nav::CRobot2NavInterfaceForSimulator_DiffDriven::getCurrentPoseAndSpeeds().

◆ getCurrentOdometricVel()

const mrpt::math::TTwist2D& mrpt::kinematics::CVehicleSimulVirtualBase::getCurrentOdometricVel ( ) const

inlineinherited

Returns the instantaneous, odometric velocity vector (vx,vy,omega) in world coordinates.

Definition at line 70 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometric_vel.

◆ getCurrentOdometricVelLocal()

mrpt::math::TTwist2D CVehicleSimulVirtualBase::getCurrentOdometricVelLocal ( ) const

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().

◆ getTime()

double mrpt::kinematics::CVehicleSimulVirtualBase::getTime ( ) const

inlineinherited

Get the current simulation time.

Definition at line 79 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().

◆ getV()

double mrpt::kinematics::CVehicleSimul_DiffDriven::getV ( )

inline

Definition at line 40 of file CVehicleSimul_DiffDriven.h.

References m_v.

◆ getVelCmdType()

CVehicleVelCmd::Ptr mrpt::kinematics::CVehicleSimul_DiffDriven::getVelCmdType ( ) const

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 58 of file CVehicleSimul_DiffDriven.h.

◆ getW()

double mrpt::kinematics::CVehicleSimul_DiffDriven::getW ( )

inline

Definition at line 41 of file CVehicleSimul_DiffDriven.h.

References m_w.

◆ internal_clear()

void CVehicleSimul_DiffDriven::internal_clear ( )

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.

◆ internal_simulControlStep()

void CVehicleSimul_DiffDriven::internal_simulControlStep ( const double dt )

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.

◆ movementCommand()

void CVehicleSimul_DiffDriven::movementCommand	(	double	lin_vel,
		double	ang_vel
	)

Used to command the robot a desired movement:

Parameters

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().

◆ resetStatus()

void CVehicleSimulVirtualBase::resetStatus ( )

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().

◆ resetTime()

void CVehicleSimulVirtualBase::resetTime ( )

inherited

Reset all simulator variables to 0 (except the.

simulation time).

See also: resetTime Reset time counter; resetStatus

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().

◆ sendVelCmd()

void mrpt::kinematics::CVehicleSimul_DiffDriven::sendVelCmd ( const CVehicleVelCmd & cmd_vel )

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 48 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().

◆ setCurrentGTPose()

void CVehicleSimulVirtualBase::setCurrentGTPose ( const mrpt::math::TPose2D & pose )

inherited

Brute-force move robot to target coordinates ("teleport")

Definition at line 24 of file CVehicleSimulVirtualBase.cpp.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_GT_pose.

◆ setCurrentOdometricPose()

template<typename T >

void mrpt::kinematics::CVehicleSimulVirtualBase::setCurrentOdometricPose ( const T & pose )

inlineinherited

Brute-force overwrite robot odometry.

Definition at line 56 of file CVehicleSimulVirtualBase.h.

References mrpt::kinematics::CVehicleSimulVirtualBase::m_odometry.

◆ setDelayModelParams()

void mrpt::kinematics::CVehicleSimul_DiffDriven::setDelayModelParams	(	double	TAU_delay_sec = `1.8`,
		double	CMD_delay_sec = `0.`
	)

inline

Change the model of delays used for the orders sent to the robot.

See also: movementCommand

Definition at line 31 of file CVehicleSimul_DiffDriven.h.

References cDELAY, and cTAU.

◆ setOdometryErrors()

void mrpt::kinematics::CVehicleSimulVirtualBase::setOdometryErrors	(	bool	enabled,
		double	Ax_err_bias = `1e-3`,
		double	Ax_err_std = `10e-3`,
		double	Ay_err_bias = `1e-3`,
		double	Ay_err_std = `10e-3`,
		double	Aphi_err_bias = `mrpt::DEG2RAD(1e-3)`,
		double	Aphi_err_std = `mrpt::DEG2RAD(10e-3)`
	)

inlineinherited

Enable/Disable odometry errors.

Errors in odometry are 1 sigma Gaussian values per second

Definition at line 90 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.

◆ setV()

void mrpt::kinematics::CVehicleSimul_DiffDriven::setV ( double v )

inline

Definition at line 38 of file CVehicleSimul_DiffDriven.h.

References m_v.

◆ setW()

void mrpt::kinematics::CVehicleSimul_DiffDriven::setW ( double w )

inline

Definition at line 39 of file CVehicleSimul_DiffDriven.h.

References m_w.

◆ simulateOneTimeStep()

void CVehicleSimulVirtualBase::simulateOneTimeStep ( const double dt )

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.

Referenced by run_rnav_test().

Member Data Documentation

◆ cDELAY

double mrpt::kinematics::CVehicleSimul_DiffDriven::cDELAY

private

The delay constant for the velocities changes.

Definition at line 81 of file CVehicleSimul_DiffDriven.h.

Referenced by internal_simulControlStep(), and setDelayModelParams().

◆ Command_Time

double mrpt::kinematics::CVehicleSimul_DiffDriven::Command_Time

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 75 of file CVehicleSimul_DiffDriven.h.

Referenced by internal_clear(), internal_simulControlStep(), and movementCommand().