A local footstep planning class for quad. More...

#include <local_footstep_planner.h>

Collaboration diagram for LocalFootstepPlanner:

Public Member Functions
	LocalFootstepPlanner ()
	Constructor for LocalFootstepPlanner Class.

void	setTemporalParams (double dt, int period, int horizon_length, const std::vector< double > &duty_cycles, const std::vector< double > &phase_offsets)
	Set the temporal parameters of this object.

void	setSpatialParams (double ground_clearance, double hip_clearance, double grf_weight, double standing_error_threshold, std::shared_ptr< quad_utils::QuadKD > kinematics, double foothold_search_radius, double foothold_obj_threshold, std::string obj_fun_layer, double toe_radius)
	Set the spatial parameters of this object.

void	getFootPositionsBodyFrame (const Eigen::VectorXd &body_plan, const Eigen::VectorXd &foot_positions_world, Eigen::VectorXd &foot_positions_body)
	Transform a vector of foot positions from the world to the body frame.

void	getFootPositionsBodyFrame (const Eigen::MatrixXd &body_plan, const Eigen::MatrixXd &foot_positions_world, Eigen::MatrixXd &foot_positions_body)
	Transform the entire foot plan from the world to the body frame.

void	updateMap (const FastTerrainMap &terrain)
	Update the map of this object.

void	updateMap (const grid_map::GridMap &terrain)
	Update the map of this object.

void	computeContactSchedule (int current_plan_index, const Eigen::MatrixXd &body_plan, const Eigen::VectorXi &ref_primitive_plan_, int control_mode, std::vector< std::vector< bool > > &contact_schedule)
	Compute the contact schedule based on the current phase.

void	computeFootPlan (int current_plan_index, const std::vector< std::vector< bool > > &contact_schedule, const Eigen::MatrixXd &body_plan, const Eigen::MatrixXd &grf_plan, const Eigen::MatrixXd &ref_body_plan, const Eigen::VectorXd &foot_positions_current, const Eigen::VectorXd &foot_velocities_current, double first_element_duration, quad_msgs::MultiFootState &past_footholds_msg, Eigen::MatrixXd &foot_positions, Eigen::MatrixXd &foot_velocities, Eigen::MatrixXd &foot_accelerations)
	Update the discrete footstep plan with the current plan.

void	loadFootPlanMsgs (const std::vector< std::vector< bool > > &contact_schedule, int current_plan_index, double first_element_duration, const Eigen::MatrixXd &foot_positions, const Eigen::MatrixXd &foot_velocities, const Eigen::MatrixXd &foot_accelerations, quad_msgs::MultiFootPlanDiscrete &future_footholds_msg, quad_msgs::MultiFootPlanContinuous &foot_plan_continuous_msg)
	Convert the foot positions and contact schedule into ros messages for the foot plan.

void	printContactSchedule (const std::vector< std::vector< bool > > &contact_schedule)

double	getTerrainHeight (double x, double y)

double	getTerrainSlope (double x, double y, double dx, double dy)

void	getTerrainSlope (double x, double y, double yaw, double &roll, double &pitch)

void	setTerrainMap (const grid_map::GridMap &grid_map)

Eigen::VectorXd	computeFutureBodyPlan (double step, const Eigen::VectorXd &body_plan)

Private Member Functions
void	updateContinuousPlan ()
	Update the continuous foot plan to match the discrete.

void	cubicHermiteSpline (double pos_prev, double vel_prev, double pos_next, double vel_next, double phase, double duration, double &pos, double &vel, double &acc)
	Compute the cubic hermite spline.

Eigen::Vector3d	getNearestValidFoothold (const Eigen::Vector3d &foot_position, const Eigen::Vector3d &foot_position_prev_solve) const
	Search locally around foothold for optimal location.

Eigen::Vector3d	welzlMinimumCircle (std::vector< Eigen::Vector2d > P, std::vector< Eigen::Vector2d > R)
	Compute minimum covering circle problem using Welzl's algorithm.

double	computeSwingApex (int leg_idx, const Eigen::VectorXd &body_plan, const Eigen::Vector3d &foot_position_prev, const Eigen::Vector3d &foot_position_next)
	Compute swing apex height.

Eigen::Vector3d	getFootData (const Eigen::MatrixXd &foot_state_vars, int horizon_index, int foot_index)
	Extract foot data from the matrix.

bool	isContact (const std::vector< std::vector< bool > > &contact_schedule, int horizon_index, int foot_index)
	Check if a foot is in contact at a given index.

bool	isNewContact (const std::vector< std::vector< bool > > &contact_schedule, int horizon_index, int foot_index)
	Check if a foot is newly in contact at a given index.

bool	isNewLiftoff (const std::vector< std::vector< bool > > &contact_schedule, int horizon_index, int foot_index)
	Check if a foot is newly in swing at a given index.

int	getNextContactIndex (const std::vector< std::vector< bool > > &contact_schedule, int horizon_index, int foot_index)
	Compute the index of the next contact for a foot. If none exist return the last.

int	getNextLiftoffIndex (const std::vector< std::vector< bool > > &contact_schedule, int horizon_index, int foot_index)
	Compute the index of the next liftoff for a foot. If none exist return the last.

Private Attributes
FastTerrainMap	terrain_
	Struct for terrain map data.

grid_map::GridMap	terrain_grid_
	GridMap for terrain map data.

const int	num_feet_ = 4
	Number of feet.

double	dt_
	Timestep for one finite element.

int	period_
	Gait period in timesteps.

int	horizon_length_
	Horizon length in timesteps.

std::vector< double >	phase_offsets_ = {0, 0.5, 0.5, 0.0}
	Phase offsets for the touchdown of each foot.

std::vector< double >	duty_cycles_ = {0.5, 0.5, 0.5, 0.5}
	Duty cycles for the stance duration of each foot.

std::vector< std::vector< bool > >	nominal_contact_schedule_
	Nominal contact schedule.

double	ground_clearance_
	Ground clearance.

double	hip_clearance_
	Hip clearance.

double	grf_weight_
	Weighting on the projection of the grf.

const int	CONNECT_STANCE = 0
	Primitive ids - CONNECT_STANCE TODO(yanhaoy, astutt) make these enums.

const int	LEAP_STANCE = 1
	Primitive ids - LEAP_STANCE.

const int	FLIGHT = 2
	Primitive ids - FLIGHT.

const int	LAND_STANCE = 3
	Primitive ids - LAND_STANCE.

std::shared_ptr< quad_utils::QuadKD >	quadKD_
	QuadKD class.

double	standing_error_threshold_ = 0
	Threshold of body error from desired goal to start stepping.

double	foothold_search_radius_
	Radius to locally search for valid footholds (m)

double	foothold_obj_threshold_
	Minimum objective function value for valid foothold.

std::string	obj_fun_layer_
	Terrain layer for foothold search.

double	toe_radius_
	Toe radius.

Detailed Description

A local footstep planning class for quad.

FootstepPlanner is a container for all of the logic utilized in the local footstep planning node. The implementation must provide a clean and high level interface to the core algorithm

Constructor & Destructor Documentation

◆ LocalFootstepPlanner()

LocalFootstepPlanner::LocalFootstepPlanner ( )

Constructor for LocalFootstepPlanner Class.

Returns: Constructed object of type LocalFootstepPlanner

Member Function Documentation

◆ computeContactSchedule()

void LocalFootstepPlanner::computeContactSchedule	(	int	current_plan_index,
		const Eigen::MatrixXd &	body_plan,
		const Eigen::VectorXi &	ref_primitive_plan_,
		int	control_mode,
		std::vector< std::vector< bool > > &	contact_schedule
	)

Compute the contact schedule based on the current phase.

Parameters

[in]	current_plan_index_	Current index in the plan
[in]	body_plan	Current body plan
[in]	ref_primitive_plan_	Reference primitive plan
[in]	control_mode	Control mode
[out]	contact_schedule	2D array of contact states

◆ computeFootPlan()

void LocalFootstepPlanner::computeFootPlan	(	int	current_plan_index,
		const std::vector< std::vector< bool > > &	contact_schedule,
		const Eigen::MatrixXd &	body_plan,
		const Eigen::MatrixXd &	grf_plan,
		const Eigen::MatrixXd &	ref_body_plan,
		const Eigen::VectorXd &	foot_positions_current,
		const Eigen::VectorXd &	foot_velocities_current,
		double	first_element_duration,
		quad_msgs::MultiFootState &	past_footholds_msg,
		Eigen::MatrixXd &	foot_positions,
		Eigen::MatrixXd &	foot_velocities,
		Eigen::MatrixXd &	foot_accelerations
	)

Update the discrete footstep plan with the current plan.

Parameters

[in]	current_plan_index	Current plan index
[in]	contact_schedule	Current contact schedule
[in]	body_plan	Current body plan
[in]	grf_plan	Current grf plan
[in]	ref_body_plan	Reference body plan
[in]	foot_positions_current	Current foot position in the world frame
[in]	foot_velocities_current	Current foot position in the world frame
[in]	first_element_duration	Duration of first element of horizon (may not be dt)
[in]	past_footholds_msg	Message of past footholds, used for interpolation of swing state
[out]	foot_positions	Foot positions over the horizon
[out]	foot_velocities	Foot velocities over the horizon
[out]	foot_accelerations	Foot accelerations over the horizon

◆ computeSwingApex()

double LocalFootstepPlanner::computeSwingApex	(	int	leg_idx,
		const Eigen::VectorXd &	body_plan,
		const Eigen::Vector3d &	foot_position_prev,
		const Eigen::Vector3d &	foot_position_next
	)

private

Compute swing apex height.

Parameters

[in]	leg_idx	Leg index
[in]	body_plan	Body plan in the mid air index
[in]	foot_position_prev	Position of the previous foothold
[in]	foot_position_next	Position of the next foothold

Returns: Apex height

◆ cubicHermiteSpline()

void LocalFootstepPlanner::cubicHermiteSpline	(	double	pos_prev,
		double	vel_prev,
		double	pos_next,
		double	vel_next,
		double	phase,
		double	duration,
		double &	pos,
		double &	vel,
		double &	acc
	)

private

Compute the cubic hermite spline.

Parameters

[in]	pos_prev	Previous position
[in]	vel_prev	Previous velocity
[in]	pos_next	Next position
[in]	vel_next	Next velocity
[in]	phase	Interplation phase
[in]	duration	Interplation duration
[out]	pos	Interplated position
[out]	vel	Interplated velocity
[out]	acc	Interplated accleration

◆ getFootPositionsBodyFrame() [1/2]

void LocalFootstepPlanner::getFootPositionsBodyFrame	(	const Eigen::MatrixXd &	body_plan,
		const Eigen::MatrixXd &	foot_positions_world,
		Eigen::MatrixXd &	foot_positions_body
	)

Transform the entire foot plan from the world to the body frame.

Parameters

[in]	body_plan	Current body plan
[in]	foot_positions_world	Foot positions in the world frame
[out]	foot_positions_body	Foot positions in the body frame

◆ getFootPositionsBodyFrame() [2/2]

void LocalFootstepPlanner::getFootPositionsBodyFrame	(	const Eigen::VectorXd &	body_plan,
		const Eigen::VectorXd &	foot_positions_world,
		Eigen::VectorXd &	foot_positions_body
	)

Transform a vector of foot positions from the world to the body frame.

Parameters

[in]	body_plan	Current body plan
[in]	foot_positions_world	Foot positions in the world frame
[out]	foot_positions_body	Foot positions in the body frame

◆ getNearestValidFoothold()

Eigen::Vector3d LocalFootstepPlanner::getNearestValidFoothold	(	const Eigen::Vector3d &	foot_position,
		const Eigen::Vector3d &	foot_position_prev_solve
	)		const

private

Search locally around foothold for optimal location.

Parameters

[in]	foot_position	Foothold to optimize around
[in]	foot_position_prev_solve	Foothold in prior solve

Returns: Optimized foothold

◆ loadFootPlanMsgs()

void LocalFootstepPlanner::loadFootPlanMsgs	(	const std::vector< std::vector< bool > > &	contact_schedule,
		int	current_plan_index,
		double	first_element_duration,
		const Eigen::MatrixXd &	foot_positions,
		const Eigen::MatrixXd &	foot_velocities,
		const Eigen::MatrixXd &	foot_accelerations,
		quad_msgs::MultiFootPlanDiscrete &	future_footholds_msg,
		quad_msgs::MultiFootPlanContinuous &	foot_plan_continuous_msg
	)

Convert the foot positions and contact schedule into ros messages for the foot plan.

Parameters

[in]	contact_schedule	Current contact schedule
[in]	current_plan_index	Current plan index
[in]	first_element_duration	Duration of first element of horizon (may not be dt)
[in]	foot_positions	Foot positions over the horizon
[in]	foot_velocities	Foot velocities over the horizon
[in]	foot_accelerations	Foot accelerations over the horizon
[out]	future_footholds_msg	Message for future (planned) footholds
[out]	foot_plan_continuous_msg	Message for continuous foot trajectories

◆ setSpatialParams()

void LocalFootstepPlanner::setSpatialParams	(	double	ground_clearance,
		double	hip_clearance,
		double	grf_weight,
		double	standing_error_threshold,
		std::shared_ptr< quad_utils::QuadKD >	kinematics,
		double	foothold_search_radius,
		double	foothold_obj_threshold,
		std::string	obj_fun_layer,
		double	toe_radius
	)

Set the spatial parameters of this object.

Parameters

[in]	ground_clearance	The foot clearance over adjacent footholds in m
[in]	hip_clearance	The foot clearance under hip in m
[in]	standing_error_threshold	Threshold of body error from desired goal to start stepping
[in]	grf_weight	Weight on GRF projection (0 to 1)
[in]	kinematics	Kinematics class for computations
[in]	foothold_search_radius	Radius to locally search for valid footholds (m)
[in]	foothold_obj_threshold	Minimum objective function value for valid foothold
[in]	obj_fun_layer	Terrain layer for foothold search
[in]	toe_radius	Toe radius

◆ setTemporalParams()

void LocalFootstepPlanner::setTemporalParams	(	double	dt,
		int	period,
		int	horizon_length,
		const std::vector< double > &	duty_cycles,
		const std::vector< double > &	phase_offsets
	)

Set the temporal parameters of this object.

Parameters

[in]	dt	The duration of one timestep in the plan
[in]	period	The period of a gait cycle in number of timesteps
[in]	horizon_length	The length of the planning horizon in number of timesteps

◆ updateMap() [1/2]

void LocalFootstepPlanner::updateMap ( const FastTerrainMap & terrain )

Update the map of this object.

Parameters

[in] terrain The map of the terrain

◆ updateMap() [2/2]

void LocalFootstepPlanner::updateMap ( const grid_map::GridMap & terrain )

Update the map of this object.

Parameters

[in] terrain The map of the terrain

◆ welzlMinimumCircle()

Eigen::Vector3d LocalFootstepPlanner::welzlMinimumCircle	(	std::vector< Eigen::Vector2d >	P,
		std::vector< Eigen::Vector2d >	R
	)

private

Compute minimum covering circle problem using Welzl's algorithm.

Parameters

[in]	P	Hip position in the plan
[in]	R	Vertex storeage for the circle

Returns: Center and radius of the circle

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

local_planner/include/local_planner/local_footstep_planner.h
local_planner/src/local_footstep_planner.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ LocalFootstepPlanner()

Member Function Documentation

◆ computeContactSchedule()

◆ computeFootPlan()

◆ computeSwingApex()

◆ cubicHermiteSpline()

◆ getFootPositionsBodyFrame() [1/2]

◆ getFootPositionsBodyFrame() [2/2]

◆ getNearestValidFoothold()

◆ loadFootPlanMsgs()

◆ setSpatialParams()

◆ setTemporalParams()

◆ updateMap() [1/2]

◆ updateMap() [2/2]

◆ welzlMinimumCircle()