Node for Model Predictive Control of a robot. More...

#include <mpc_node.hpp>

Collaboration diagram for mfcpp::MPCNode:

Classes
struct	MPCBounds
	Bounds on the MPC problem. More...

struct	MPCTuningParameters
	MPC tuning parameters. More...

Public Member Functions
	MPCNode ()

	~MPCNode ()

void	run_node ()
	Runs the node. More...

Private Member Functions
void	init_node ()
	Initialises the node and its parameters. More...

std::vector< geometry_msgs::Pose >	adapt_path (const std::vector< geometry_msgs::Pose > &orig_path, const geometry_msgs::Point &current_position, int nbr_steps, float &spatial_resolution, float &time_resolution, float &desired_speed)
	Generates a path of the right size for MPC prediction. More...

template<class VectorT >
bool	fill_ref_pts (int N, int n, int m, const std::vector< geometry_msgs::Pose > &path, float desired_speed, float last_desired_speed, const RobotModel &robot_model, VectorT &X_ref, VectorT &U_ref)
	Fills reference points for MPC optimisation. More...

template<class VectorT >
void	modulo_ref_state (VectorT &X_ref, const VectorT &x0, int n)
	Changes reference state orientation to prevent modulo discontinuity. More...

template<class VectorT , class MatrixT >
void	fill_ltv_G_H_D (const VectorT &X_ref, const VectorT &U_ref, int N, float dt, float ds, MatrixT &G, MatrixT &H, VectorT &D)
	Fills the G and H matrices used to express X with respect to U and X0. More...

template<class MatrixT >
void	fill_lti_G (const MatrixT &Ad, const MatrixT &Bd, int N, MatrixT &G)
	Fills the G matrix used to express X with respect to U and X0. More...

template<class MatrixT >
void	fill_lti_H (const MatrixT &Ad, int N, MatrixT &H)
	Fills the H matrix used to express X with respect to U and X0. More...

template<class MatrixT >
void	fill_L (const MatrixT &P, const MatrixT &Q_x, int N, MatrixT &L)
	Fills the L matrix used for quadratic cost wrt state error. More...

template<class MatrixT >
void	fill_M (const MatrixT &R_u, const MatrixT &R_delta, int N, MatrixT &M)
	Fills the M matrix used for quadratic cost wrt control input error. More...

template<class VectorT , class MatrixT >
void	fill_V (VectorT control_ref, VectorT last_control, const MatrixT &R_delta, int N, VectorT &V)
	Fills the V matrix used for the linear cost wrt control input error. More...

template<class MatrixT >
void	fill_lti_LG (const MatrixT &G, const MatrixT &P, const MatrixT &Q_x, int n, int m, int N, MatrixT &LG)
	Fills the product of L and G matrices. More...

template<class VectorT , class MatrixT >
void	fill_bounds_objs (const MPCBounds &bounds, int n, int N, const VectorT &X0, const VectorT &X_ref, const VectorT &U_ref, const MatrixT &G, const MatrixT &H, const VectorT &D, VectorT &lb, VectorT &ub, MatrixT &Ab)
	Fills the different bounds objects. More...

template<class VectorT , class T >
bool	solve_qp (const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > &P, const VectorT &q, const VectorT &lb, const VectorT &ub, const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > &Ab, VectorT &solution)
	Solves a Quadratic Program. More...

bool	compute_control (float &desired_speed, float last_desired_speed, std::vector< float > &command, geometry_msgs::PoseArray &expected_traj)
	Computes the control signal to send to the robot. More...

void	path_cb (const nav_msgs::Path msg)
	Callback for the desired path. More...

void	state_cb (const mf_common::Float32Array msg)
	Callback for the current robot state. More...

Private Attributes
ros::NodeHandle	nh_
	Node handler (for topics and services) More...

ros::Subscriber	path_sub_
	Subscriber for the desired path. More...

ros::Subscriber	state_sub_
	Subscriber for the current robot state. More...

ros::Publisher	command_pub_
	Publisher for the computed command. More...

ros::Publisher	expected_traj_pub_
	Publisher for the expected controlled trajectory. More...

tf2_ros::Buffer	tf_buffer_
	Buffer for tf2. More...

tf2_ros::TransformListener	tf_listener_
	Transform listener for tf2. More...

nav_msgs::Path	path_
	Path to follow. More...

bool	path_received_
	Whether a new path has been received. More...

bool	state_received_
	Whether the robot state has ever been received. More...

RobotModel	robot_model_
	Robot model. More...

std::vector< float >	state_
	Current robot state. More...

std::vector< float >	last_control_
	Last control applied to the robot. More...

MPCTuningParameters	tuning_params_
	MPC tuning parameters. More...

MPCBounds	bounds_
	Bounds for the MPC. More...

Eigen::VectorXf	qp_last_primal_
	Last primal solution of the Quadratic Problem. More...

Eigen::VectorXf	qp_last_dual_
	Last dual solution of the Quadratic Problem. More...

bool	qp_warm_start_
	Whether the QP can start warm. More...

ROS parameters
float	main_freq_
	Frequency of the main loop. More...

std::string	fixed_frame_
	Fixed frame. More...

float	desired_speed_
	Desired speed (m/s) of the robot. More...

float	last_desired_speed_
	Last desired speed (m/s) of the robot. More...

float	time_horizon_
	Time horizon (s) for the MPC prediction. More...

int	nbr_steps_
	Number of steps for the MPC prediction. More...

bool	disable_vbs_
	Whether to disable Variable Buoyancy System (VBS) More...

bool	ltv_mpc_

Detailed Description

Node for Model Predictive Control of a robot.

MatrixT template can either be Eigen::MatrixXf or Eigen::MatrixXd. VectorT template can either be Eigen::VectorXf or Eigen::VectorXd.

Definition at line 33 of file mpc_node.hpp.

Constructor & Destructor Documentation

mfcpp::MPCNode::MPCNode ( )

Definition at line 31 of file mpc_node.cpp.

mfcpp::MPCNode::~MPCNode ( )

Definition at line 39 of file mpc_node.cpp.

Member Function Documentation

std::vector< geometry_msgs::Pose > mfcpp::MPCNode::adapt_path	(	const std::vector< geometry_msgs::Pose > &	orig_path,
		const geometry_msgs::Point &	current_position,
		int	nbr_steps,
		float &	spatial_resolution,
		float &	time_resolution,
		float &	desired_speed
	)

private

Generates a path of the right size for MPC prediction.

The path will have as many poses as steps for MPC, with a desired spatial resolution. The desired speed and size of the path are maintained, so if the desired spatial resolution leads to a too long path, the resolution is decreased to fit. If so, the time resolution is also shrinked (ie the path duration is smaller) and the desired speed is decreased.

Parameters

[in]	orig_path	Path provided by the path planner
[in]	current_position	Current position of the robot
[in]	nbr_steps	Number of MPC prediction steps
[in,out]	spatial_resolution	Distance between two MPC steps
[in,out]	time_resolution	Time between two MPC steps
[in,out]	desired_speed	Desired speed

Returns: Path of size nbr_steps+1

Definition at line 31 of file mpc.cpp.

bool mfcpp::MPCNode::compute_control	(	float &	desired_speed,
		float	last_desired_speed,
		std::vector< float > &	command,
		geometry_msgs::PoseArray &	expected_traj
	)

private

Computes the control signal to send to the robot.

The desired speed might be changed if the robot is close to the end of the path.

Parameters

[in,out]	desired_speed	Desired speed (m/s) of the robot
[in]	last_desired_speed	Last desired speed
[out]	command	Computed control input to apply
[out]	expected_traj	Expected controlled trajectory

Returns: Whether the control could be computed

Definition at line 531 of file mpc.cpp.

template<class VectorT , class MatrixT >

void mfcpp::MPCNode::fill_bounds_objs	(	const MPCBounds &	bounds,
		int	n,
		int	N,
		const VectorT &	X0,
		const VectorT &	X_ref,
		const VectorT &	U_ref,
		const MatrixT &	G,
		const MatrixT &	H,
		const VectorT &	D,
		VectorT &	lb,
		VectorT &	ub,
		MatrixT &	Ab
	)

private

Fills the different bounds objects.

Parameters

[in]	bounds	Bounds of the MPC problem
[in]	n	Size of the state
[in]	N	Number of steps for the MPC prediction
[in]	X0	Initial offset
[in]	X_ref	Reference state (x_0_ref, ..., x_N_ref)
[in]	U_ref	Reference input (u_0_ref, ..., u_N_ref)
[in]	G	Used to express X with respect to U and X0
[in]	H	Used to express X with respect to U and X0
[in]	D	Used to express X with respect to U and X0
[out]	lb	Lower bound on Ab*U
[out]	ub	Upper bound on Ab*U
[out]	Ab	Multiplicative factor in front of U in the bound inequalities

Definition at line 355 of file mpc.cpp.

template<class MatrixT >

void mfcpp::MPCNode::fill_L	(	const MatrixT &	P,
		const MatrixT &	Q_x,
		int	N,
		MatrixT &	L
	)

private

Fills the L matrix used for quadratic cost wrt state error.

Parameters

[in]	P	Penalty on the last state error
[in]	Q_x	Penalty on the intermediary states errors
[in]	N	Number of steps for the MPC prediction
[out]	L	L matrix

Definition at line 287 of file mpc.cpp.

template<class MatrixT >

void mfcpp::MPCNode::fill_lti_G	(	const MatrixT &	Ad,
		const MatrixT &	Bd,
		int	N,
		MatrixT &	G
	)

private

Fills the G matrix used to express X with respect to U and X0.

Linear Time Invariant MPC version.

Parameters

[in]	Ad	Discretised model matrix
[in]	Bd	Discretised model matrix
[in]	N	Number of steps for the MPC prediction
[out]	G	G matrix

Definition at line 254 of file mpc.cpp.

template<class MatrixT >

void mfcpp::MPCNode::fill_lti_H	(	const MatrixT &	Ad,
		int	N,
		MatrixT &	H
	)

private

Fills the H matrix used to express X with respect to U and X0.

Linear Time Invariant MPC version

Parameters

[in]	Ad	Discretised model matrix
[in]	N	Number of steps for the MPC prediction
[out]	H	H matrix

Definition at line 273 of file mpc.cpp.

template<class MatrixT >

void mfcpp::MPCNode::fill_lti_LG	(	const MatrixT &	G,
		const MatrixT &	P,
		const MatrixT &	Q_x,
		int	n,
		int	m,
		int	N,
		MatrixT &	LG
	)

private

Fills the product of L and G matrices.

Optimisation in the Linear Time Invariant MPC case

Parameters

[in]	G	G matrix
[in]	P	Penalty on the last state error
[in]	Q_x	Penalty on the intermediary states errors
[in]	n	Size of the state
[in]	m	Size of the input
[in]	N	Number of steps for the MPC prediction
[out]	LG	LG matrix

Definition at line 336 of file mpc.cpp.

template<class VectorT , class MatrixT >

void mfcpp::MPCNode::fill_ltv_G_H_D	(	const VectorT &	X_ref,
		const VectorT &	U_ref,
		int	N,
		float	dt,
		float	ds,
		MatrixT &	G,
		MatrixT &	H,
		VectorT &	D
	)

private

Fills the G and H matrices used to express X with respect to U and X0.

Linear Time Varying MPC version.

Parameters

[in]	X_ref	State reference
[in]	U_ref	Control reference
[in]	N	Number of steps for the MPC prediction
[in]	dt	Sampling time
[in]	ds	Spatial resolution of reference path
[out]	G	G matrix
[out]	H	H matrix
[out]	D	D vector

Definition at line 176 of file mpc.cpp.

template<class MatrixT >

void mfcpp::MPCNode::fill_M	(	const MatrixT &	R_u,
		const MatrixT &	R_delta,
		int	N,
		MatrixT &	M
	)

private

Fills the M matrix used for quadratic cost wrt control input error.

Parameters

[in]	R_u	Penalty on the control input error
[in]	R_delta	Penalty on the control change rate
[in]	N	Number of steps for the MPC prediction
[out]	M	M matrix

Definition at line 300 of file mpc.cpp.

template<class VectorT >

bool mfcpp::MPCNode::fill_ref_pts	(	int	N,
		int	n,
		int	m,
		const std::vector< geometry_msgs::Pose > &	path,
		float	desired_speed,
		float	last_desired_speed,
		const RobotModel &	robot_model,
		VectorT &	X_ref,
		VectorT &	U_ref
	)

private

Fills reference points for MPC optimisation.

Parameters

[in]	N	Number of steps for the MPC prediction
[in]	n	Dimension of the state vector
[in]	m	Dimension of the control vector
[in]	path	Reference path to follow (of size N+1)
[in]	desired_speed	Desired speed (m/s) of the robot
[in]	last_desired_speed	Last desired speed (m/s) of the robot
[in]	robot_model	Robot model
[out]	X_ref	Reference state to fill (x0_ref, ..., xN_ref)
[out]	U_ref	Reference output to fill (u0_ref, ..., uN_ref)

Returns: Whether the points could be filled

Definition at line 105 of file mpc.cpp.

template<class VectorT , class MatrixT >

void mfcpp::MPCNode::fill_V	(	VectorT	control_ref,
		VectorT	last_control,
		const MatrixT &	R_delta,
		int	N,
		VectorT &	V
	)

private

Fills the V matrix used for the linear cost wrt control input error.

Parameters

[in]	control_ref	Reference control
[in]	last_control	Last control applied to the robot
[in]	R_delta	Penalty on the control change rate
[in]	N	Number of steps for the MPC prediction
[out]	V	V matrix

Definition at line 326 of file mpc.cpp.

void mfcpp::MPCNode::init_node ( )

private

Initialises the node and its parameters.

Definition at line 42 of file mpc_node.cpp.

template<class VectorT >

void mfcpp::MPCNode::modulo_ref_state	(	VectorT &	X_ref,
		const VectorT &	x0,
		int	n
	)

private

Changes reference state orientation to prevent modulo discontinuity.

Adds multiple of 2pi to each orientation so that error in orientation is continuous over the reference points.

Parameters

[in,out]	X_ref	Reference state
[in]	x0	Current robot state
[in]	n	Dimension of the state

Definition at line 148 of file mpc.cpp.

void mfcpp::MPCNode::path_cb ( const nav_msgs::Path msg )

private

Callback for the desired path.

Definition at line 138 of file mpc_node.cpp.

void mfcpp::MPCNode::run_node ( )

Runs the node.

Definition at line 92 of file mpc_node.cpp.

template<class VectorT , class T >

bool mfcpp::MPCNode::solve_qp	(	const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > &	P,
		const VectorT &	q,
		const VectorT &	lb,
		const VectorT &	ub,
		const Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > &	Ab,
		VectorT &	solution
	)

private

Solves a Quadratic Program.

Definition at line 413 of file mpc.cpp.

void mfcpp::MPCNode::state_cb ( const mf_common::Float32Array msg )

private

Callback for the current robot state.

Definition at line 145 of file mpc_node.cpp.

Member Data Documentation

MPCBounds mfcpp::MPCNode::bounds_

private

Bounds for the MPC.

Definition at line 80 of file mpc_node.hpp.

ros::Publisher mfcpp::MPCNode::command_pub_

private

Publisher for the computed command.

Definition at line 68 of file mpc_node.hpp.

float mfcpp::MPCNode::desired_speed_

private

Desired speed (m/s) of the robot.

Definition at line 91 of file mpc_node.hpp.

bool mfcpp::MPCNode::disable_vbs_

private

Whether to disable Variable Buoyancy System (VBS)

Definition at line 95 of file mpc_node.hpp.

ros::Publisher mfcpp::MPCNode::expected_traj_pub_

private

Publisher for the expected controlled trajectory.

Definition at line 69 of file mpc_node.hpp.

std::string mfcpp::MPCNode::fixed_frame_

private

Fixed frame.

Definition at line 89 of file mpc_node.hpp.

std::vector<float> mfcpp::MPCNode::last_control_

private

Last control applied to the robot.

Definition at line 78 of file mpc_node.hpp.

float mfcpp::MPCNode::last_desired_speed_

private

Last desired speed (m/s) of the robot.

Definition at line 92 of file mpc_node.hpp.

bool mfcpp::MPCNode::ltv_mpc_

private

Whether to use the Linear Time Varying (LTV) version of MPC

Definition at line 96 of file mpc_node.hpp.

float mfcpp::MPCNode::main_freq_

private

Frequency of the main loop.

Definition at line 88 of file mpc_node.hpp.

int mfcpp::MPCNode::nbr_steps_

private

Number of steps for the MPC prediction.

Definition at line 94 of file mpc_node.hpp.

ros::NodeHandle mfcpp::MPCNode::nh_

private

Node handler (for topics and services)

Definition at line 65 of file mpc_node.hpp.

nav_msgs::Path mfcpp::MPCNode::path_

private

Path to follow.

Definition at line 73 of file mpc_node.hpp.

bool mfcpp::MPCNode::path_received_

private

Whether a new path has been received.

Definition at line 74 of file mpc_node.hpp.

ros::Subscriber mfcpp::MPCNode::path_sub_

private

Subscriber for the desired path.

Definition at line 66 of file mpc_node.hpp.

Eigen::VectorXf mfcpp::MPCNode::qp_last_dual_

private

Last dual solution of the Quadratic Problem.

Definition at line 83 of file mpc_node.hpp.

Eigen::VectorXf mfcpp::MPCNode::qp_last_primal_

private

Last primal solution of the Quadratic Problem.

Definition at line 82 of file mpc_node.hpp.

bool mfcpp::MPCNode::qp_warm_start_

private

Whether the QP can start warm.

Definition at line 84 of file mpc_node.hpp.

RobotModel mfcpp::MPCNode::robot_model_

private

Robot model.

Definition at line 76 of file mpc_node.hpp.

std::vector<float> mfcpp::MPCNode::state_

private

Current robot state.

Definition at line 77 of file mpc_node.hpp.

bool mfcpp::MPCNode::state_received_

private

Whether the robot state has ever been received.

Definition at line 75 of file mpc_node.hpp.

ros::Subscriber mfcpp::MPCNode::state_sub_

private

Subscriber for the current robot state.

Definition at line 67 of file mpc_node.hpp.

tf2_ros::Buffer mfcpp::MPCNode::tf_buffer_

private

Buffer for tf2.

Definition at line 70 of file mpc_node.hpp.

tf2_ros::TransformListener mfcpp::MPCNode::tf_listener_

private

Transform listener for tf2.

Definition at line 71 of file mpc_node.hpp.

float mfcpp::MPCNode::time_horizon_

private

Time horizon (s) for the MPC prediction.

Definition at line 93 of file mpc_node.hpp.

MPCTuningParameters mfcpp::MPCNode::tuning_params_

private

MPC tuning parameters.

Definition at line 79 of file mpc_node.hpp.

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

/home/corentin/thesis_ws/src/marine-farm-cpp/mf_navigation/include/mf_navigation/mpc_node.hpp
/home/corentin/thesis_ws/src/marine-farm-cpp/mf_navigation/src/mf_navigation/mpc.cpp
/home/corentin/thesis_ws/src/marine-farm-cpp/mf_navigation/src/mf_navigation/mpc_node.cpp

MFCPP 1.0	Corentin Chauvin-Hameau – 2019-2020
Coverage Path Planning for an underwater robot surveying a marine farm

Classes

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation