Specialization of SystemDynamicsInterface for mobile robots. More...

#include <robot_dynamics_interface.h>

Inheritance diagram for mpc_local_planner::RobotDynamicsInterface:

[legend]

Public Types
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
virtual void	getPoseSE2FromState (const Eigen::Ref< const Eigen::VectorXd > &x, double &pos_x, double &pos_y, double &theta) const =0
	Convert state vector to pose (x,y,theta) More...

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	getPositionFromState (const Eigen::Ref< const Eigen::VectorXd > &x, double &pos_x, double &pos_y) const =0
	Convert state vector to position (x,y) More...

virtual void	getSteadyStateFromPoseSE2 (double pos_x, double pos_y, double theta, Eigen::Ref< Eigen::VectorXd > x) const =0
	Convert pose (x,y,theta) to steady state. 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 bool	mergeStateFeedbackAndOdomFeedback (const teb_local_planner::PoseSE2 &odom_pose, const geometry_msgs::Twist &odom_twist, Eigen::Ref< Eigen::VectorXd > x) const =0
	Merge custom state feedback with pose and twist feedback. More...

virtual void	reset ()

Public Member Functions inherited from corbo::SystemDynamicsInterface
virtual void	dynamics (const Eigen::Ref< const StateVector > &x, const Eigen::Ref< const ControlVector > &u, Eigen::Ref< StateVector > f) const=0

virtual double	getDeadTime () const

virtual int	getInputDimension () const=0

virtual Ptr	getInstance () const=0

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

virtual int	getStateDimension () const=0

virtual bool	isContinuousTime () const=0

virtual bool	isLinear () const=0

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

	SystemDynamicsInterface ()

virtual	~SystemDynamicsInterface ()=default

Detailed Description

Specialization of SystemDynamicsInterface for mobile robots.

This abstract class extends cobro::SystemDynamicsInterface by (abstract) conversion methods between states, poses embedded in SE2, controls and twist.

See also: BaseRobotSE2 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 46 of file robot_dynamics_interface.h.

Member Typedef Documentation

◆ Ptr

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

Definition at line 49 of file robot_dynamics_interface.h.

Member Function Documentation

◆ getPoseSE2FromState() [1/2]

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

pure virtual

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)

Implemented in mpc_local_planner::BaseRobotSE2.

◆ getPoseSE2FromState() [2/2]

virtual void mpc_local_planner::RobotDynamicsInterface::getPoseSE2FromState	(	const Eigen::Ref< const Eigen::VectorXd > &	x,
		teb_local_planner::PoseSE2 &	pose
	)		const

inlinevirtual

Convert state vector to PoseSE2 object.

Parameters

[in]	x	Reference to the state vector [getStateDimension() x 1]
[out]	pose	PoseSE2 object

Definition at line 76 of file robot_dynamics_interface.h.

◆ getPositionFromState()

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

pure virtual

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

Implemented in mpc_local_planner::BaseRobotSE2.

◆ getSteadyStateFromPoseSE2() [1/2]

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

pure virtual

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]

Implemented in mpc_local_planner::BaseRobotSE2.

◆ getSteadyStateFromPoseSE2() [2/2]

virtual void mpc_local_planner::RobotDynamicsInterface::getSteadyStateFromPoseSE2	(	const teb_local_planner::PoseSE2 &	pose,
		Eigen::Ref< Eigen::VectorXd >	x
	)		const

inlinevirtual

Convert PoseSE2 to steady state.

See description of getSteadyStateFromPoseSE2(double pos_x, double pos_y, double theta, Eigen::Ref<Eigen::VectorXd> x)

Parameters

[in]	pose	PoseSE2 object
[out]	x	Reference to the state vector [getStateDimension() x 1]

Definition at line 108 of file robot_dynamics_interface.h.

◆ getTwistFromControl()

virtual bool mpc_local_planner::RobotDynamicsInterface::getTwistFromControl	(	const Eigen::Ref< const Eigen::VectorXd > &	u,
		geometry_msgs::Twist &	twist
	)		const

pure virtual

Convert control vector to twist message.

Convert the control vector to a twist message (containing velocity information) if possible.

Todo:: Maybe add current state x as optional input to allow for computing a twist out of the first state and control

Parameters

[in]	u	Reference to the control vector [getInputDimension() x 1]
[out]	twist	Reference to the twist message

Returns: true, if conversion was successful, false otherwise

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

◆ mergeStateFeedbackAndOdomFeedback()

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

pure virtual

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]