LocalPlanner Class Reference

Local Body Planner library. More...

#include <local_planner.h>

Collaboration diagram for LocalPlanner:

Public Member Functions
	LocalPlanner (ros::NodeHandle nh)
	Constructor for LocalPlanner.
void	spin ()
	Primary work function in class, called in node file for this component.

Private Member Functions
	FRIEND_TEST (LocalPlannerTest, noInputCase)
void	initLocalBodyPlanner ()
	Initialize the local body planner.
void	initLocalFootstepPlanner ()
	Initialize the local footstep planner.
void	terrainMapCallback (const grid_map_msgs::GridMap::ConstPtr &msg)
	Callback function to handle new terrain map data.
void	robotPlanCallback (const quad_msgs::RobotPlan::ConstPtr &msg)
	Callback function to handle new plans.
void	robotStateCallback (const quad_msgs::RobotState::ConstPtr &msg)
	Callback function to handle new state estimates.
void	cmdVelCallback (const geometry_msgs::Twist::ConstPtr &msg)
	Callback function to handle new desired twist data when using twist input.
void	getReference ()
	Function to compute reference trajectory from twist command or global plan.
bool	computeLocalPlan ()
	Function to compute the local plan.
void	publishLocalPlan ()
	Function to publish the local plan.

Private Attributes
std::string	robot_name_
	Robot type: A1 or Spirit.
ros::Subscriber	terrain_map_sub_
	ROS subscriber for incoming terrain_map.
ros::Subscriber	body_plan_sub_
	ROS subscriber for incoming body plans.
ros::Subscriber	robot_state_sub_
	ROS Subscriber for incoming states.
ros::Subscriber	cmd_vel_sub_
	Subscriber for twist input messages.
ros::Publisher	local_plan_pub_
	ROS publisher for local plan output.
ros::Publisher	foot_plan_discrete_pub_
	ROS publisher for discrete foot plan.
ros::Publisher	foot_plan_continuous_pub_
	ROS publisher for continuous foot plan.
std::string	map_frame_
	Define map frame.
ros::NodeHandle	nh_
	Nodehandle to pub to and sub from.
FastTerrainMap	terrain_
	Struct for terrain map data.
grid_map::GridMap	terrain_grid_
	GridMap for terrain map data.
double	update_rate_
	Update rate for sending and receiving data;.
std::shared_ptr< NMPCController >	local_body_planner_nonlinear_
	Local Body Planner object.
std::shared_ptr< LocalFootstepPlanner >	local_footstep_planner_
	Local Footstep Planner object.
quad_msgs::RobotPlan::ConstPtr	body_plan_msg_
	Most recent robot plan.
quad_msgs::RobotState::ConstPtr	robot_state_msg_
	Most recent robot state.
quad_msgs::MultiFootState	past_footholds_msg_
	Past foothold locations.
ros::Time	current_state_timestamp_
	Timestamp of the state estimate.
Eigen::VectorXd	current_state_
	Current state (ground truth or estimate)
Eigen::VectorXd	current_foot_positions_world_
Eigen::VectorXd	current_foot_velocities_world_
Eigen::VectorXd	current_foot_positions_body_
int	current_plan_index_
	Current index in the global plan.
double	dt_
	local planner timestep (seconds)
double	compute_time_
	Computation time in computeLocalPlan.
double	mean_compute_time_
	Average computation time in computeLocalPlan.
const double	filter_smoothing_constant_ = 0.5
	Exponential filter smoothing constant (higher updates slower)
int	N_
	Standard MPC horizon length.
int	N_current_
	Current MPC horizon length.
const int	Nx_ = 12
	Number of states.
const int	Nu_ = 13
	Number of controls.
const int	num_feet_ = 4
	Number of legs.
const int	num_joints_per_leg_ = 3
	Number of joints per leg.
Eigen::MatrixXd	body_plan_
Eigen::MatrixXd	ref_body_plan_
Eigen::VectorXd	ref_ground_height_
	Vector of ground height along reference trajectory.
Eigen::VectorXi	ref_primitive_plan_
	Vector of primitive along reference trajectory.
Eigen::MatrixXd	grf_plan_
std::vector< std::vector< bool > >	contact_schedule_
	Contact schedule.
Eigen::MatrixXd	foot_positions_world_
	Matrix of continuous foot positions in world frame.
Eigen::MatrixXd	foot_velocities_world_
	Matrix of continuous foot velocities in world frame.
Eigen::MatrixXd	foot_accelerations_world_
	Matrix of continuous foot accelerations in world frame.
Eigen::MatrixXd	foot_positions_body_
	Matrix of continuous foot positions in body frame.
Eigen::MatrixXd	foot_plan_discrete_
	Matrix of foot contact locations (number of contacts x num_legs_)
std::shared_ptr< quad_utils::QuadKD >	quadKD_
	QuadKD class.
Eigen::VectorXd	cmd_vel_
	Twist input.
double	cmd_vel_filter_const_
	Commanded velocity filter constant.
double	cmd_vel_scale_
	Scale for twist cmd_vel.
double	z_des_
	Nominal robot height.
ros::Time	last_cmd_vel_msg_time_
	Time of the most recent cmd_vel data.
double	last_cmd_vel_msg_time_max_
	Threshold for waiting for twist cmd_vel data.
ros::Time	initial_timestamp_
	Initial timestamp for contact cycling.
bool	first_plan_
	Foot initialization flag when using twist input without a global body plan.
bool	use_twist_input_
	Boolean for using twist input instead of a global body plan.
Eigen::Vector3d	stand_pose_
	Vector for stand pose (x, y, yaw)
double	first_element_duration_
	Time duration to the next plan index.
int	plan_index_diff_
	Difference in plan index from last solve.
double	toe_radius_
	Toe radius.
int	control_mode_
	Control mode.
double	stand_vel_threshold_
	Velocity threshold to enter stand mode.
double	stand_cmd_vel_threshold_
	Commanded velocity threshold to enter stand mode.
double	stand_pos_error_threshold_
	Position error threshold (from foot centroid) to enter stand mode.

Detailed Description

Local Body Planner library.

Wrapper around Quadrupedal MPC that interfaces with our ROS architecture

Constructor & Destructor Documentation

LocalPlanner()

LocalPlanner::LocalPlanner

(

ros::NodeHandle

nh

)

Constructor for LocalPlanner.

Parameters

[in]

nh

ROS NodeHandle to publish and subscribe from

Returns

Constructed object of type LocalPlanner

Member Function Documentation

cmdVelCallback()

void LocalPlanner::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr &  msg )

private

Callback function to handle new desired twist data when using twist input.

Parameters

[in]

msg

the message contining twist data

computeLocalPlan()

bool LocalPlanner::computeLocalPlan ( )

private

Function to compute the local plan.

Returns

Boolean if local plan was found successfully

robotPlanCallback()

void LocalPlanner::robotPlanCallback ( const quad_msgs::RobotPlan::ConstPtr &  msg )

private

Callback function to handle new plans.

Parameters

[in]

msg

Robot state trajectory message

robotStateCallback()

void LocalPlanner::robotStateCallback ( const quad_msgs::RobotState::ConstPtr &  msg )

private

Callback function to handle new state estimates.

Parameters

[in]

State

estimate message contining position and velocity for each joint and robot body

terrainMapCallback()

void LocalPlanner::terrainMapCallback ( const grid_map_msgs::GridMap::ConstPtr &  msg )

private

Callback function to handle new terrain map data.

Parameters

[in]

grid_map_msgs::GridMap

contining map data

Member Data Documentation

body_plan_

Eigen::MatrixXd LocalPlanner::body_plan_

private

Matrix of body states (N x Nx: rows correspond to individual states in the horizon)

grf_plan_

Eigen::MatrixXd LocalPlanner::grf_plan_

private

Matrix of grfs (N x Nu: rows correspond to individual arrays of GRFs in the horizon)

ref_body_plan_

Eigen::MatrixXd LocalPlanner::ref_body_plan_

private

Matrix of body states (N x Nx: rows correspond to individual states in the horizon)

---

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

• local_planner/include/local_planner/local_planner.h
• local_planner/src/local_planner.cpp