A class that implements the Global Body Planner for Legged Robots (GBP-L) More...

#include <gbpl.h>

Inheritance diagram for GBPL:

Collaboration diagram for GBPL:

Public Member Functions
	GBPL ()
	Constructor for GBPL.

int	connect (PlannerClass &T, State s, const PlannerConfig &planner_config, int direction, ros::Publisher &tree_pub)

std::vector< Action >	getActionSequenceReverse (PlannerClass &T, std::vector< int > path)
	Get the actions along the specified path of vertex indices (assumes that actions are synched with the state at which they are executed)

void	postProcessPath (std::vector< State > &state_sequence, std::vector< Action > &action_sequence, const PlannerConfig &planner_config)
	Post process the path by removing extraneous states that can be bypassed.

void	extractPath (PlannerClass &Ta, PlannerClass &Tb, std::vector< State > &state_sequence, std::vector< Action > &action_sequence, const PlannerConfig &planner_config)
	Post process the path by removing extraneous states that can be bypassed.

void	extractClosestPath (PlannerClass &Ta, const State &s_goal, std::vector< State > &state_sequence, std::vector< Action > &action_sequence, const PlannerConfig &planner_config)
	Post process the path by removing extraneous states that can be bypassed.

int	findPlan (const PlannerConfig &planner_config, State s_start, State s_goal, std::vector< State > &state_sequence, std::vector< Action > &action_sequence, ros::Publisher &tree_pub)
	Run the full RRT-Connect planner until the goal is found or time has expired, then post process and update statistics.

Public Member Functions inherited from RRT
	RRT ()
	Constructor for RRT.

	~RRT ()
	Destructor for RRT.

int	attemptConnect (const State &s_existing, const State &s, double t_s, StateActionResult &result, const PlannerConfig &planner_config, int direction)

int	attemptConnect (const State &s_existing, const State &s, StateActionResult &result, const PlannerConfig &planner_config, int direction)

virtual int	extend (PlannerClass &T, const State &s, const PlannerConfig &planner_config, int direction, ros::Publisher &tree_pub)

std::vector< int >	pathFromStart (PlannerClass &T, int idx)
	Get the path from the root vertex to the specified one.

void	printPath (PlannerClass &T, std::vector< int > path)
	Print the states in the specified path via stdout.

void	getStatistics (double &plan_time, int &vertices_generated, double &plan_length, double &path_duration, double &dist_to_goal)
	Get the statistics for the planner solve.

bool	newConfig (State s, State s_near, StateActionResult &result, const PlannerConfig &planner_config, int direction, ros::Publisher &tree_pub)
	Generate a new state that can be connected to the tree and is as close as possible to the specified state.

std::vector< State >	getStateSequence (PlannerClass &T, std::vector< int > path)
	Get the states along the specified path of vertex indices.

std::vector< Action >	getActionSequence (PlannerClass &T, std::vector< int > path)
	Get the actions along the specified path of vertex indices (assumes that actions are synched with the states to which they lead)

Protected Attributes
double	anytime_horizon
	Time horizon (in seconds) the planner is allowed to search until restarted.

const double	planning_rate_estimate
	Initial guess at how quickly the planner executes (in m/s)

double	anytime_horizon_init
	Initial anytime horizon (in seconds)

const double	horizon_expansion_factor = 1.2
	Factor by which horizon is increased if replanning is required.

Protected Attributes inherited from RRT
const double	prob_goal_thresh = 0.05
	Probability with which the goal is sampled (if running vanilla RRT)

bool	goal_found = false
	Boolean for if the goal has been reached.

std::chrono::duration< double >	elapsed_total_
	Total time elapsed in this planner call.

std::chrono::duration< double >	elapsed_to_first_

int	success_ = 0
	Integer checking if a solution was computed in 5 seconds.

int	num_vertices_
	Number of vertices in the tree.

double	path_length_
	Path quality in meters.

double	path_duration_
	The duration of the path in seconds.

double	dist_to_goal_
	Distance from the final state to the goal (m)

visualization_msgs::MarkerArray	tree_viz_msg_
	Message for tree visualization.

Detailed Description

A class that implements the Global Body Planner for Legged Robots (GBP-L)

This class implements the Global Body Planner for Legged Robots (GBP-L), an RRT-Connect based planning algorithm that leverages mixed motion primitives to rapidly plan global motions for legged platforms.

Constructor & Destructor Documentation

◆ GBPL()

GBPL::GBPL ( )

Constructor for GBPL.

Returns: Constructed object of type GBPL

Member Function Documentation

◆ connect()

int GBPL::connect	(	PlannerClass &	T,
		State	s,
		const PlannerConfig &	planner_config,
		int	direction,
		ros::Publisher &	tree_pub
	)

Connect the tree to the desired state

Parameters

[in]	T	The PlannerClass instance containing the tree
[in]	s	The state to connect the tree towards
[in]	planner_config	Configuration parameters
[in]	direction	The direction with which to peform the extension (FORWARD to go away from the root vertex, REVERSE to go towards it)
[in]	tree_pub	Publisher for broadcasting the tree visual

◆ extractClosestPath()

void GBPL::extractClosestPath	(	PlannerClass &	Ta,
		const State &	s_goal,
		std::vector< State > &	state_sequence,
		std::vector< Action > &	action_sequence,
		const PlannerConfig &	planner_config
	)

Post process the path by removing extraneous states that can be bypassed.

Parameters

[in]	Ta	The planning tree originating from the start state
[in]	s_start	The start state of the planner
[out]	state_sequence	The sequence of states in the path
[out]	action_sequence	The sequence of actions in th e path
[in]	terrain	Height map of the terrain

◆ extractPath()

void GBPL::extractPath	(	PlannerClass &	Ta,
		PlannerClass &	Tb,
		std::vector< State > &	state_sequence,
		std::vector< Action > &	action_sequence,
		const PlannerConfig &	planner_config
	)

Post process the path by removing extraneous states that can be bypassed.

Parameters

[in]	Ta	The planning tree originating from the start state
[in]	Tb	The planning tree originating from the end state
[out]	state_sequence	The sequence of states in the path
[out]	action_sequence	The sequence of actions in th e path
[in]	planner_config	Configuration parameters

◆ findPlan()

int GBPL::findPlan	(	const PlannerConfig &	planner_config,
		State	s_start,
		State	s_goal,
		std::vector< State > &	state_sequence,
		std::vector< Action > &	action_sequence,
		ros::Publisher &	tree_pub
	)

Run the full RRT-Connect planner until the goal is found or time has expired, then post process and update statistics.

Parameters

[in]	planner_config	Configuration parameters
[in]	s_start	The start state of the planner
[in]	s_goal	The goal state of the planner
[out]	state_sequence	The sequence of states in the final path
[out]	action_sequence	The sequence of actions in the final path

Returns: Boolean for success of the planner

◆ getActionSequenceReverse()

std::vector< Action > GBPL::getActionSequenceReverse	(	PlannerClass &	T,
		std::vector< int >	path
	)

Get the actions along the specified path of vertex indices (assumes that actions are synched with the state at which they are executed)

Parameters

[in]	T	The PlannerClass instance containing the tree
[in]	path	Vector of vertices in the path

Returns: The sequence of actions in the path

◆ postProcessPath()

void GBPL::postProcessPath	(	std::vector< State > &	state_sequence,
		std::vector< Action > &	action_sequence,
		const PlannerConfig &	planner_config
	)

Post process the path by removing extraneous states that can be bypassed.

Parameters

[in]	state_sequence	The sequence of states in the path
[in]	action_sequence	The sequence of actions in the path
[in]	planner_config	Configuration parameters

Member Data Documentation

◆ planning_rate_estimate

const double GBPL::planning_rate_estimate

protected

Initial value:

=

16.0

Initial guess at how quickly the planner executes (in m/s)

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

global_body_planner/include/global_body_planner/gbpl.h
global_body_planner/src/gbpl.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ GBPL()

Member Function Documentation

◆ connect()

◆ extractClosestPath()

◆ extractPath()

◆ findPlan()

◆ getActionSequenceReverse()

◆ postProcessPath()

Member Data Documentation

◆ planning_rate_estimate