A robot model for robot with a differential drive (2 wheels on an axis, fixed orientation). More...

#include <robot/DifferentialRobotModel.h>

Inheritance diagram for DifferentialRobotModel:

[legend]

Public Types
typedef std::list< LinkPtr >	LinkList

typedef std::map< std::string, JointPtr >	JointMap

typedef std::map< std::string, MaterialPtr >	MaterialMap

typedef std::list< std::pair< GeometryPtr, RigidTransform3f >, Eigen::aligned_allocator< std::pair< GeometryPtr, RigidTransform3f > > >	CollisionGeometries

Public Member Functions
template<typename Reflector >
void	reflectDegrees (Reflector &r, const char name, float &member, const char comment)

void	reflectDegrees (PropertySerializer &r, const char name, float &member, const char comment)

void	reflect (XMLDeserializer &r)

float	getMaxTransVelocity () const

virtual PoseVelocityTrajectory	generateTrajectory (Velocity2 v, const Velocity2 &targetV, float lookAheadTime, int samples)
	Generates a trajectory by accelerating/decelerating starting at velocity v for lookAheadTime time up to the velocity targetV. More...

Pose2	globalKinematics (const Velocity2 &v, float dt, const Pose2 &p0) const
	Computes the movement of the robot if it moves with the specified velocity v for the specified time dt. More...

void	clear ()

std::string	getRootLink () const

CollisionGeometries	getCollisionGeometries (std::string targetFrame="", const Time &timestamp=Time::now(), const std::string &filter="") const

std::list< Box3f >	getCollisionBoundingBoxes (std::string targetFrame="", const Time &timestamp=Time::now(), const std::string &filter="") const

Footprint	getFootprint (std::string targetFrame="", const Time &timestamp=Time::now(), const std::string &filter="") const

void	resolveAndPublishLinks (const std::string &ns, const Time &timestamp=Time::now())

void	publishJoint (const std::string &name, float value, const Time &timestamp=Time::now()) const

Class const &	getClass () const

MotionParameters	calculateMotionParameters (const Pose2 &p0, const Pose2 &p1) const
	Calculate the motion model parameters. More...

Pose2	sampleMotion (const Pose2 &startPose, const MotionParameters &parameters) const
	Samples motion using parameters for odometric drift and the odometry motion model from Thrun, Burgard, Fox in Probabilistic Robotics. More...

PoseCov2	gaussianMotion (const PoseCov2 &startPose, const MotionParameters &parameters) const
	Applies a gaussian motion model to the initial pose at startPose. More...

Constructors and reflect
	DifferentialRobotModel ()

template<typename Reflector >
void	reflect (Reflector &r)

Implementation of RobotModel
virtual Pose2	localKinematics (const Velocity2 &v, float dt) const
	Computes the local movement of the robot if it moves with the specified velocity v for the specified time dt. More...

virtual float	predictStandstillDistance (const Velocity2 &v) const
	Calculate the distance that is needed for braking to stand still, if the robot moves with the specified velocity v. More...

virtual float	predictStandstillRotation (const Velocity2 &v) const
	Calculate the rotation that is needed for braking to stand still, if the robot moves with the specified velocity v. More...

Specific operations for a differential drive
SignedAnglef	angleFromTotalDistance (float totalLeft, float totalRight) const
	Calculates the view direction of the robot from given traveled distance of left and right wheel and the wheel distance of the model. More...

Velocity2	convert2Velocity (float vLeft, float vRight) const
	Convert wheel speeds of left and right wheel to velocity (trans, rot) More...

std::pair< float, float >	convert2LRWheelSpeed (const Velocity2 &v) const
	Converts velocity into left and right wheel speeds. More...

float	getMaxVelocity () const
	Returns the absolute max. More...

Implementation of ProbabilisticMotionModel
virtual Pose2	sampleMotion (const Pose2 &startPose, const Pose2 &p0, const Pose2 &p1) const

virtual void	sampleMotion (const std::vector< Pose2 > &startPoses, const Pose2 &p0, const Pose2 &p1, std::vector< Pose2 > &sampledPoses) const
	Like the above sampleMotion but samples multiple poses at once. More...

Static Public Member Functions
static PseudoClass const &	CLASS ()

Public Attributes
float	wheelDist
	The distance of both wheels. Must be greater than zero. More...

float	maxBackwardVelocity
	The maximum velocity for driving backward (both wheels) [m/s]. More...

float	maxForwardVelocity
	The maximum velocity for driving forward (both wheels) [m/s]. More...

float	maxRotVelocity
	The maximum velocity for rotating [deg/s]. More...

float	transEpsilonVelocity
	the min. More...

float	rotEpsilonVelocity

float	maxTransAcceleration
	Max. acceleration. More...

float	maxTransDeceleration
	Max. deceleration. Must be greater than zero. More...

float	maxEmergencyDeceleration
	Max. deceleration in case of an emergency. Must be greater than zero. More...

float	maxRotAcceleration
	Max. acceleration. More...

float	maxRotDeceleration
	Max. deceleration. Must be greater than zero. More...

std::string	name

LinkList	links

JointMap	joints

MaterialMap	materials

float	alphaRotRot
	Odometry motion parameters for modeling drift. More...

float	alphaRotTrans

float	alphaTransTrans

float	alphaTransRot

Protected Member Functions
virtual Class const &	internalGetClass () const=0

Detailed Description

A robot model for robot with a differential drive (2 wheels on an axis, fixed orientation).

For generic robots with more wheels or non fixed castor wheels please use the GenericRobotModel.

Note: This class only uses the x-component of Velocity2. The y-component is totally ignored.

Constructor & Destructor Documentation

◆ DifferentialRobotModel()

DifferentialRobotModel ( )

Member Function Documentation

◆ reflect()

void reflect ( Reflector & r )

inline

◆ localKinematics()

Pose2 localKinematics	(	const Velocity2 &	v,
		float	dt
	)		const

inlinevirtual

Computes the local movement of the robot if it moves with the specified velocity v for the specified time dt.

The movement is returned as relative pose in relation to the robots movement center, which is assumed to be (0,0,0).

Parameters

[in]	v	The measured velocity.
[in]	dt	The time interval in [s]. It must be >= 0.0f.

Returns: The delta pose.

Implements RobotModel.

◆ predictStandstillDistance()

float predictStandstillDistance ( const Velocity2 & v ) const

inlinevirtual

Calculate the distance that is needed for braking to stand still, if the robot moves with the specified velocity v.

Parameters

[in] v The current velocity.

Returns: The estimated stopping distance.

Implements RobotModel.

◆ predictStandstillRotation()

float predictStandstillRotation ( const Velocity2 & v ) const

inlinevirtual

Calculate the rotation that is needed for braking to stand still, if the robot moves with the specified velocity v.

Parameters

[in] v The current velocity.

Returns: The estimated stopping rotation.

Implements RobotModel.

◆ angleFromTotalDistance()

SignedAnglef angleFromTotalDistance	(	float	totalLeft,
		float	totalRight
	)		const

inline

Calculates the view direction of the robot from given traveled distance of left and right wheel and the wheel distance of the model.

Parameters

[in]	totalLeft	The traveled distance of the left wheel in [m].
[in]	totalRight	The traveled distance of the right wheel in [m].

Returns: The delta of the view direction as a signed angle.

◆ convert2Velocity()

Velocity2 convert2Velocity	(	float	vLeft,
		float	vRight
	)		const

inline

Convert wheel speeds of left and right wheel to velocity (trans, rot)

Parameters

[in]	vLeft	The velocity of the left wheel in [m/s].
[in]	vRight	The velocity of the right wheel in [m/s].

Returns: The translational and rotational velocity.

◆ convert2LRWheelSpeed()

std::pair< float, float > convert2LRWheelSpeed ( const Velocity2 & v ) const

inline

Converts velocity into left and right wheel speeds.

Parameters

[in] v The velocity.

Returns: A pair for left (first value) and right (second value) wheel speeds.

◆ getMaxVelocity()

float getMaxVelocity ( ) const

inline

Returns the absolute max.

speed of the robot, i.e. the max of the forward or backward velocity.

◆ reflectDegrees() [1/2]

void reflectDegrees	(	Reflector &	r,
		const char *	name,
		float &	member,
		const char *	comment
	)

inlineinherited

◆ reflectDegrees() [2/2]

void reflectDegrees	(	PropertySerializer &	r,
		const char *	name,
		float &	member,
		const char *	comment
	)

inlineinherited

◆ getMaxTransVelocity()

float getMaxTransVelocity ( ) const

inlineinherited

◆ generateTrajectory()

virtual PoseVelocityTrajectory generateTrajectory	(	Velocity2	v,
		const Velocity2 &	targetV,
		float	lookAheadTime,
		int	samples
	)

virtualinherited

Generates a trajectory by accelerating/decelerating starting at velocity v for lookAheadTime time up to the velocity targetV.

The resulting trajectory will contain samples+1 trajectory samples starting at position 0,0 with velocity v. It uses the acceleration limits given in the robot model.

Implements RobotModel.

◆ globalKinematics()

Pose2 globalKinematics	(	const Velocity2 &	v,
		float	dt,
		const Pose2 &	p0
	)		const

inlineinherited

Computes the movement of the robot if it moves with the specified velocity v for the specified time dt.

The movement is returned as pose in relation to the specified pose p0.

Parameters

[in]	v	The velocity of the robot.
[in]	dt	The delta time.
[in]	p0	The original pose.

Returns: The movement as a pose in relative to pose p0.

◆ calculateMotionParameters()

MotionParameters calculateMotionParameters	(	const Pose2 &	p0,
		const Pose2 &	p1
	)		const

inherited

Calculate the motion model parameters.

This method computes the motion parameters for odometric drift based on the odometry motion model from Thrun, Burgard, Fox in "Probabilistic Robotics", 2005.

See also: http://www.mrpt.org/Probabilistic_Motion_Models#References

  Y                        /         _
  ^                       /\ rot2_ -
  |                      /  |_ -
  |                     /_ -
  |                  _ -P1
  |              _ -
  |    trans _ - \
  |      _ -      | rot1
  |  _ -          |
P0............... /...........................> X

x,y,phi denote the last estimated pose P0 at t-1 and x',y',phi' the new estimated pose P1 at t. Compute the relative motion parameters:

trans = sqrt((x-x')^2+(y-y')^2) rot1 = atan2(y'-y, x'-x) - phi rot2 = phi' - phi - d_rot1

If (x,y) and (x',y') are very nearby (< 0.005m), rot1 is set to zero.

transStddev = alphaTransTrans * trans + alphaTransRot * (rot1 + rot2) rot1Stddev = alphaRotRot * rot1 + alphaRotTrans * trans rot2Stddev = alphaRotRot * rot2 + alphaRotTrans * trans

Parameters

[in]	p0	The last observed pose
[in]	p1	The current observed pose

Returns: The computed motion parameters.

◆ sampleMotion() [1/3]

virtual Pose2 sampleMotion	(	const Pose2 &	startPose,
		const Pose2 &	p0,
		const Pose2 &	p1
	)		const

virtualinherited

Parameters

[in]	startPose	The starting position for applying the sampled motion
[in]	p0	The last observed pose
[in]	p1	The current observed pose

Returns: Starting position updated by the sampled motion

Implements ProbabilisticMotionModel.

◆ sampleMotion() [2/3]

virtual void sampleMotion	(	const std::vector< Pose2 > &	startPoses,
		const Pose2 &	p0,
		const Pose2 &	p1,
		std::vector< Pose2 > &	sampledPoses
	)		const

virtualinherited

Like the above sampleMotion but samples multiple poses at once.

Default implementation uses a naive approach by calling the above sampleMotion for each start pose. However you should re-implement this method in a derived class to speed up the sampling process.

Parameters

[in]	startPoses	The starting positions for applying the sampled motion
[in]	p0	The last observed pose
[in]	p1	The current observed pose
[out]	sampledPoses	Starting positions updated by the sampled motion

Reimplemented from ProbabilisticMotionModel.

◆ sampleMotion() [3/3]

Pose2 sampleMotion	(	const Pose2 &	startPose,
		const MotionParameters &	parameters
	)		const

inlineinherited

Samples motion using parameters for odometric drift and the odometry motion model from Thrun, Burgard, Fox in Probabilistic Robotics.

This method takes motion model parameters that can be calculate by the method calculateMotionParameters(). A new pose is sampled based on the starting pose startPose.

Calculation of a motion sample:

trans^ = trans - N(0, transStddev * transStddev) rot1^ = rot1 - N(0, rot1Stddev * rot1Stddev) rot2^ = rot2 - N(0, rot2Stddev * rot2Stddev)

x = trans^ * cos(startPhi + rot1^) y = trans^ * sin(startPhi + rot1^) phi = rot1^ + rot2^

The sampled motion can be used to adapt a pose of a particle:

x' = startX + x y' = startY + y phi' = startPhi + phi

If parameters are known and many motion samples need to be computed (e.g for a particle filter) this method can be used to save some performance.

Parameters

[in]	startPose	The starting position for applying the sampled motion
[in]	parameters	The motion model parameters (can be obtained via calculateMotionParameters())

Returns: Starting position updated by the sampled motion

◆ gaussianMotion()

PoseCov2 gaussianMotion	(	const PoseCov2 &	startPose,
		const MotionParameters &	parameters
	)		const

inherited

Applies a gaussian motion model to the initial pose at startPose.

Parameters

[in]	startPose	The starting position for applying the sampled motion
[in]	parameters	The motion model parameters (can be obtained via calculateMotionParameters())

Returns: Returns the new robot pose with the updated covariance.

Member Data Documentation

◆ wheelDist

float wheelDist

The distance of both wheels. Must be greater than zero.

◆ maxBackwardVelocity

float maxBackwardVelocity

inherited

The maximum velocity for driving backward (both wheels) [m/s].

Use negative values, if the robot is allowed to drive backward.

◆ maxForwardVelocity

float maxForwardVelocity

inherited

The maximum velocity for driving forward (both wheels) [m/s].

◆ maxRotVelocity

float maxRotVelocity

inherited

The maximum velocity for rotating [deg/s].

◆ transEpsilonVelocity

float transEpsilonVelocity

inherited

the min.

speed that the robot platform can do, lower velocities result in standing still.

◆ rotEpsilonVelocity

float rotEpsilonVelocity

inherited

◆ maxTransAcceleration

float maxTransAcceleration

inherited

Max. acceleration.

◆ maxTransDeceleration

float maxTransDeceleration

inherited

Max. deceleration. Must be greater than zero.

◆ maxEmergencyDeceleration

float maxEmergencyDeceleration

inherited

Max. deceleration in case of an emergency. Must be greater than zero.

◆ maxRotAcceleration

float maxRotAcceleration

inherited

Max. acceleration.

◆ maxRotDeceleration

float maxRotDeceleration

inherited

Max. deceleration. Must be greater than zero.

◆ alphaRotRot

float alphaRotRot

inherited

Odometry motion parameters for modeling drift.

See also: sampleMotion()

◆ alphaRotTrans

float alphaRotTrans

inherited

◆ alphaTransTrans

float alphaTransTrans

inherited

◆ alphaTransRot

float alphaTransRot

inherited

The documentation for this class was generated from the following file:

include/robot/DifferentialRobotModel.h

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ DifferentialRobotModel()

Member Function Documentation

◆ reflect()

◆ localKinematics()

◆ predictStandstillDistance()

◆ predictStandstillRotation()

◆ angleFromTotalDistance()

◆ convert2Velocity()

◆ convert2LRWheelSpeed()

◆ getMaxVelocity()

◆ reflectDegrees() [1/2]

◆ reflectDegrees() [2/2]

◆ getMaxTransVelocity()

◆ generateTrajectory()

◆ globalKinematics()

◆ calculateMotionParameters()

◆ sampleMotion() [1/3]

◆ sampleMotion() [2/3]

◆ sampleMotion() [3/3]

◆ gaussianMotion()

Member Data Documentation

◆ wheelDist

◆ maxBackwardVelocity

◆ maxForwardVelocity

◆ maxRotVelocity

◆ transEpsilonVelocity

◆ rotEpsilonVelocity

◆ maxTransAcceleration

◆ maxTransDeceleration

◆ maxEmergencyDeceleration

◆ maxRotAcceleration

◆ maxRotDeceleration

◆ alphaRotRot

◆ alphaRotTrans

◆ alphaTransTrans

◆ alphaTransRot