NMPC controller ROS node. More...

#include <nmpc_controller.h>

Collaboration diagram for NMPCController:

[legend]

Public Member Functions
	NMPCController (ros::NodeHandle &nh, int type)
	Constructor for MPCController.

bool	computePlan (const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &ref_traj, const std::vector< std::vector< bool > > &contact_schedule, Eigen::MatrixXd &foot_positions, Eigen::MatrixXd &foot_velocities, Eigen::MatrixXd &state_traj, Eigen::MatrixXd &control_traj)
	Update the contact and dynamic matrices, solve, and return the output.

bool	computeLegPlan (const Eigen::VectorXd &initial_state, const Eigen::MatrixXd &ref_traj, const Eigen::MatrixXd &foot_positions_body, Eigen::MatrixXd &foot_positions_world, Eigen::MatrixXd &foot_velocities, const std::vector< std::vector< bool > > &contact_schedule, const Eigen::VectorXd &ref_ground_height, const double &first_element_duration, int plan_index_diff, const grid_map::GridMap &terrain, Eigen::MatrixXd &state_traj, Eigen::MatrixXd &control_traj)
	Interface to Local Planner to update the contact and dynamic matrices, solve, and return the output.

Eigen::VectorXi	updateAdaptiveComplexitySchedule (const Eigen::MatrixXd &state_traj_heuristic, const Eigen::MatrixXd &control_traj_heuristic, const Eigen::MatrixXd &state_traj_lifted, const Eigen::MatrixXd &control_traj_lifted)

void	updateHorizonLength ()

NLPDiagnostics	getNLPDiagnostics () const
	Return the NLP diagnostics.

Private Attributes
ros::NodeHandle	nh_
	ROS node handler.

int	robot_id_

std::string	robot_ns_
	Robot type: A1 or Spirit.

double	update_rate_
	Update rate for sending and receiving data;.

SmartPtr< quadNLP >	mynlp_
	Pointer to nlp formulation.

SmartPtr< IpoptApplication >	app_
	Pointer to IPOPT solver.

std::shared_ptr< quad_utils::QuadKD >	quadKD_
	Pointer to robot kinematics.

bool	enable_variable_horizon_
	Whether enable variable horizon.

bool	enable_mixed_complexity_ = false
	Whether enable mixed complexity.

bool	enable_adaptive_complexity_ = false
	Whether enable adaptive complexity.

bool	allow_new_interior_complexity_

bool	is_adaptive_complexity_sparse_

int	N_
	Horizon length, maximal horizon length, minimal horizon length.

int	N_max_

int	N_min_

const int	n_body_ = 12
	Number of states in different components.

const int	n_foot_ = 24

const int	n_joints_ = 24

const int	m_body_ = 12

const int	m_foot_ = 24

double	dt_
	Time resolution.

bool	require_init_
	Whether warm start is needed.

Eigen::VectorXi	adaptive_complexity_schedule_
	Adaptive complexity schedule.

NLPDiagnostics	diagnostics_
	Diagnostics struct for gathering metadata.

NLPConfig	config_
	Config struct for storing meta parameters.

Detailed Description

NMPC controller ROS node.

Wrapper around Quadrupedal MPC that interfaces with our ROS architecture

Constructor & Destructor Documentation

◆ NMPCController()

NMPCController::NMPCController	(	ros::NodeHandle &	nh,
		int	type
	)

Constructor for MPCController.

Parameters

[in] nh ROS NodeHandle to publish and subscribe from

Returns: Constructed object of type MPCController

Member Function Documentation

◆ computeLegPlan()

bool NMPCController::computeLegPlan	(	const Eigen::VectorXd &	initial_state,
		const Eigen::MatrixXd &	ref_traj,
		const Eigen::MatrixXd &	foot_positions_body,
		Eigen::MatrixXd &	foot_positions_world,
		Eigen::MatrixXd &	foot_velocities,
		const std::vector< std::vector< bool > > &	contact_schedule,
		const Eigen::VectorXd &	ref_ground_height,
		const double &	first_element_duration,
		int	plan_index_diff,
		const grid_map::GridMap &	terrain,
		Eigen::MatrixXd &	state_traj,
		Eigen::MatrixXd &	control_traj
	)

Interface to Local Planner to update the contact and dynamic matrices, solve, and return the output.

Parameters

initial_state	Vector with initial state
ref_traj	Matrix holding desired reference trajectory
foot_positions_body	Matrix holding foot positions in body frame
foot_positions_world	Matrix holding foot positions in world frame
foot_velocities	Matrix holding foot velocities
contact_schedule	A vector of sequence of contact
ref_ground_height	Matrix holding ground height at reference footholds
first_element_duration	Time duration to the next plan index
plan_index_diff	Plan index difference of the last solve and the current solve
terrain	Gridmap holding terrain height
state_traj	Matrix holding output state trajectory
control_traj	Matrix holding output control trajectory

Returns: whether solve successfully

◆ computePlan()

bool NMPCController::computePlan	(	const Eigen::VectorXd &	initial_state,
		const Eigen::MatrixXd &	ref_traj,
		const std::vector< std::vector< bool > > &	contact_schedule,
		Eigen::MatrixXd &	foot_positions,
		Eigen::MatrixXd &	foot_velocities,
		Eigen::MatrixXd &	state_traj,
		Eigen::MatrixXd &	control_traj
	)

Update the contact and dynamic matrices, solve, and return the output.

Parameters

[in]	initial_state	Vector with initial state
[in]	ref_traj	Matrix holding desired reference trajectory
[in]	contact_schedule	Matrix holding the contact schedule
[in]	foot_positions	Matrix holding foot positions
[in]	foot_velocities	Matrix holding foot velocities
[in]	first_element_duration	Time duration to the next plan index
[in]	plan_index_diff	If the current solving is duplicated in the same index
[out]	state_traj	Optimized state trajectory output
[out]	control_traj	Optimized control trajectory output

Returns: good_solve

◆ getNLPDiagnostics()

NLPDiagnostics NMPCController::getNLPDiagnostics ( ) const

inline

Return the NLP diagnostics.

Returns: NLP diagnostics with most recent meta-data

◆ updateAdaptiveComplexitySchedule()

Eigen::VectorXi NMPCController::updateAdaptiveComplexitySchedule	(	const Eigen::MatrixXd &	state_traj_heuristic,
		const Eigen::MatrixXd &	control_traj_heuristic,
		const Eigen::MatrixXd &	state_traj_lifted,
		const Eigen::MatrixXd &	control_traj_lifted
	)

Method to return the constraint residual for requested data

◆ updateHorizonLength()

void NMPCController::updateHorizonLength ( )

Method to update the prediction horizon length

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

nmpc_controller/include/nmpc_controller/nmpc_controller.h
nmpc_controller/src/adaptive_complexity_utils.cpp
nmpc_controller/src/nmpc_controller.cpp

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ NMPCController()

Member Function Documentation

◆ computeLegPlan()

◆ computePlan()

◆ getNLPDiagnostics()

◆ updateAdaptiveComplexitySchedule()

◆ updateHorizonLength()