Specialization of RobotDynamicsInterface for mobile robots operating in SE2. More...

#include <base_robot_se2.h>

Inheritance diagram for mpc_local_planner::BaseRobotSE2:

[legend]

Public Types
using	Ptr = std::shared_ptr< BaseRobotSE2 >

Public Types inherited from mpc_local_planner::RobotDynamicsInterface
using	Ptr = std::shared_ptr< RobotDynamicsInterface >

Public Types inherited from corbo::SystemDynamicsInterface
typedef Eigen::VectorXd	ControlVector

typedef std::shared_ptr< SystemDynamicsInterface >	Ptr

typedef Eigen::VectorXd	StateVector

Public Member Functions
	BaseRobotSE2 ()=default
	Default constructor. More...

void	dynamics (const Eigen::Ref< const StateVector > &x, const Eigen::Ref< const ControlVector > &u, Eigen::Ref< StateVector > f) const override=0

int	getInputDimension () const override=0

corbo::SystemDynamicsInterface::Ptr	getInstance () const override=0

void	getPoseSE2FromState (const Eigen::Ref< const Eigen::VectorXd > &x, double &pos_x, double &pos_y, double &theta) const override
	Convert state vector to pose (x,y,theta) More...

void	getPositionFromState (const Eigen::Ref< const Eigen::VectorXd > &x, double &pos_x, double &pos_y) const override
	Convert state vector to position (x,y) More...

int	getStateDimension () const override

void	getSteadyStateFromPoseSE2 (double pos_x, double pos_y, double theta, Eigen::Ref< Eigen::VectorXd > x) const override
	Convert pose (x,y,theta) to steady state. More...

bool	isContinuousTime () const override

bool	isLinear () const override

virtual bool	mergeStateFeedbackAndOdomFeedback (const teb_local_planner::PoseSE2 &odom_pose, const geometry_msgs::Twist &odom_twist, Eigen::Ref< Eigen::VectorXd > x) const override
	Merge custom state feedback with pose and twist feedback. More...

Public Member Functions inherited from mpc_local_planner::RobotDynamicsInterface
virtual void	getPoseSE2FromState (const Eigen::Ref< const Eigen::VectorXd > &x, teb_local_planner::PoseSE2 &pose) const
	Convert state vector to PoseSE2 object. More...

virtual void	getSteadyStateFromPoseSE2 (const teb_local_planner::PoseSE2 &pose, Eigen::Ref< Eigen::VectorXd > x) const
	Convert PoseSE2 to steady state. More...

virtual bool	getTwistFromControl (const Eigen::Ref< const Eigen::VectorXd > &u, geometry_msgs::Twist &twist) const =0
	Convert control vector to twist message. More...

virtual void	reset ()

Public Member Functions inherited from corbo::SystemDynamicsInterface
virtual double	getDeadTime () const

virtual void	getLinearA (const StateVector &x0, const ControlVector &u0, Eigen::MatrixXd &A) const

virtual void	getLinearB (const StateVector &x0, const ControlVector &u0, Eigen::MatrixXd &B) const

void	setLinearizationMethod (std::shared_ptr< FiniteDifferencesInterface > lin_method)

	SystemDynamicsInterface ()

virtual	~SystemDynamicsInterface ()=default

Detailed Description

Specialization of RobotDynamicsInterface for mobile robots operating in SE2.

This abstract class defines a base class for robots in which the full state vector x is embedded in SE2, i.e. x = [pos_x, pos_y, theta] with pos_x, pos_y as real numbers and theta in [-pi, pi).

Note, these models allow for simple conversion between state vectors and poses.

See also: RobotDynamicsInterface corbo::SystemDynamicsInterface

Author: Christoph Rösmann (chris.nosp@m.toph.nosp@m..roes.nosp@m.mann.nosp@m.@tu-d.nosp@m.ortm.nosp@m.und.d.nosp@m.e)

Definition at line 63 of file base_robot_se2.h.

Member Typedef Documentation

◆ Ptr

using mpc_local_planner::BaseRobotSE2::Ptr = std::shared_ptr<BaseRobotSE2>

Definition at line 86 of file base_robot_se2.h.

Constructor & Destructor Documentation

◆ BaseRobotSE2()

mpc_local_planner::BaseRobotSE2::BaseRobotSE2 ( )

default

Default constructor.

Member Function Documentation

◆ dynamics()

void mpc_local_planner::BaseRobotSE2::dynamics	(	const Eigen::Ref< const StateVector > &	x,
		const Eigen::Ref< const ControlVector > &	u,
		Eigen::Ref< StateVector >	f
	)		const

overridepure virtual

Implements corbo::SystemDynamicsInterface.

Implemented in mpc_local_planner::SimpleCarFrontWheelDrivingModel, mpc_local_planner::SimpleCarModel, mpc_local_planner::KinematicBicycleModelVelocityInput, and mpc_local_planner::UnicycleModel.

◆ getInputDimension()

int mpc_local_planner::BaseRobotSE2::getInputDimension ( ) const

overridepure virtual

Implements corbo::SystemDynamicsInterface.

Implemented in mpc_local_planner::SimpleCarModel, mpc_local_planner::KinematicBicycleModelVelocityInput, and mpc_local_planner::UnicycleModel.

◆ getInstance()

corbo::SystemDynamicsInterface::Ptr mpc_local_planner::BaseRobotSE2::getInstance ( ) const

overridepure virtual

Implements corbo::SystemDynamicsInterface.

Implemented in mpc_local_planner::SimpleCarFrontWheelDrivingModel, mpc_local_planner::SimpleCarModel, mpc_local_planner::KinematicBicycleModelVelocityInput, and mpc_local_planner::UnicycleModel.

◆ getPoseSE2FromState()

void mpc_local_planner::BaseRobotSE2::getPoseSE2FromState	(	const Eigen::Ref< const Eigen::VectorXd > &	x,
		double &	pos_x,
		double &	pos_y,
		double &	theta
	)		const

inlineoverridevirtual

Convert state vector to pose (x,y,theta)

Parameters

[in]	x	Reference to the state vector [getStateDimension() x 1]
[out]	pos_x	X-component of the position part
[out]	pos_y	Y-component of the position part
[out]	theta	Orientation (yaw angle) in [-pi, pi)

Implements mpc_local_planner::RobotDynamicsInterface.

Definition at line 114 of file base_robot_se2.h.

◆ getPositionFromState()

void mpc_local_planner::BaseRobotSE2::getPositionFromState	(	const Eigen::Ref< const Eigen::VectorXd > &	x,
		double &	pos_x,
		double &	pos_y
	)		const

inlineoverridevirtual

Convert state vector to position (x,y)

Parameters

[in]	x	Reference to the state vector [getStateDimension() x 1]
[out]	pos_x	X-component of the position
[out]	pos_y	Y-component of the position

Implements mpc_local_planner::RobotDynamicsInterface.

Definition at line 106 of file base_robot_se2.h.

◆ getStateDimension()

int mpc_local_planner::BaseRobotSE2::getStateDimension ( ) const

inlineoverridevirtual

Implements corbo::SystemDynamicsInterface.

Definition at line 97 of file base_robot_se2.h.

◆ getSteadyStateFromPoseSE2()

void mpc_local_planner::BaseRobotSE2::getSteadyStateFromPoseSE2	(	double	pos_x,
		double	pos_y,
		double	theta,
		Eigen::Ref< Eigen::VectorXd >	x
	)		const

inlineoverridevirtual

Convert pose (x,y,theta) to steady state.

Converts a pose to state vector with dimensions [getStateDimension() x 1]. In case getStateDimension() > 3, the state information is incomplete and hence it is assumed that a steady state for this particular pose is avaiable, which means that there exist a control u such that f(x,u) = 0 (continuous time) or f(x,u) = x (discrete time).

For example, in case the remaining state components correspond to just integrators, they can be set to zero.

Parameters

[in]	pos_x	X-component of the position part
[in]	pos_y	Y-component of the position part
[in]	theta	Orientation (yaw angle) in [-pi, pi)
[out]	x	Reference to the state vector [getStateDimension() x 1]

Implements mpc_local_planner::RobotDynamicsInterface.

Definition at line 123 of file base_robot_se2.h.

◆ isContinuousTime()

bool mpc_local_planner::BaseRobotSE2::isContinuousTime ( ) const

inlineoverridevirtual

Implements corbo::SystemDynamicsInterface.

Definition at line 100 of file base_robot_se2.h.

◆ isLinear()

bool mpc_local_planner::BaseRobotSE2::isLinear ( ) const

inlineoverridevirtual

Implements corbo::SystemDynamicsInterface.

Definition at line 103 of file base_robot_se2.h.

◆ mergeStateFeedbackAndOdomFeedback()

virtual bool mpc_local_planner::BaseRobotSE2::mergeStateFeedbackAndOdomFeedback	(	const teb_local_planner::PoseSE2 &	odom_pose,
		const geometry_msgs::Twist &	odom_twist,
		Eigen::Ref< Eigen::VectorXd >	x
	)		const

inlineoverridevirtual

Merge custom state feedback with pose and twist feedback.

Many robots in ROS publish their state information (pose, linear and angular velocity)e.g. via an odometry message (potentially corrected by localization). However, for more complex robot models, it might not be possible to obtain the full state information without any observer or further measurements. For this purpose, custom state feedback can be provided. But as the complete navigation stack relies on the information obtained by odometry, this method allows for merging of both sources. In particular, it overrides only state components related to the pose and velocity.

Parameters

[in]	odom_pose	PoseSE2 object obtained from odometry
[in]	odom_twist	geometry::Twist message obtained from odometry
[in,out]	x	Custom state feedback in which related will be overriden [getStateDimension() x 1]

Returns: true, if merging was successful, false otherwise

Implements mpc_local_planner::RobotDynamicsInterface.

Definition at line 133 of file base_robot_se2.h.

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

base_robot_se2.h

Public Types

Public Member Functions

Detailed Description

Member Typedef Documentation

◆ Ptr

Constructor & Destructor Documentation

◆ BaseRobotSE2()

Member Function Documentation

◆ dynamics()

◆ getInputDimension()

◆ getInstance()

◆ getPoseSE2FromState()

◆ getPositionFromState()

◆ getStateDimension()

◆ getSteadyStateFromPoseSE2()

◆ isContinuousTime()

◆ isLinear()

◆ mergeStateFeedbackAndOdomFeedback()