vigir_footstep_planning Namespace Reference

Namespaces
namespace	threading
namespace	vis
Classes
class	Footstep
	A class representing a footstep (i.e. a translation and rotation of a specific foot with respect to the supporting leg) that can be performed by a humanoid robot. More...
class	FootstepPlanningVisNode
class	PlanningState
	A class representing the robot's pose (i.e. position and orientation) in the underlying SBPL. More precisely a planning state is a discrete representation of the robot's supporting leg. More...
class	SimpleActionClient
class	SimpleActionServer
class	Singleton
class	State
	A class representing the robot's pose (i.e. position and orientation) in the (continuous) world view. More precisely a state points to the robot's supporting leg. More...
Enumerations
enum	Leg { RIGHT = 0, LEFT = 1, NOLEG = 2 }
Functions
double	angle_cell_2_state (int angle, double angle_bin_size)
	Calculate the respective (continuous) angle for a bin.
int	angle_state_2_cell (double angle, double angle_bin_size)
	Discretize a (continuous) angle into a bin.
unsigned int	calc_hash_tag (int x, int y, int theta, int leg, unsigned int hash_pred, unsigned int hash_succ, int max_hash_size)
double	cell_2_state (int value, double cell_size)
	Calculate the respective (continuous) state value for a cell. (Should be used to get a State from a discretized PlanningState.)
double	cont_val (int length, double cell_size)
	Calculates the continuous value for a length discretized in cell size.
msgs::ErrorStatus	determineGoalFeetPose (msgs::Feet &goal_feet, ros::ServiceClient &generate_feet_pose_client, const geometry_msgs::PoseStamped &goal_pose)
msgs::ErrorStatus	determineStartFeetPose (msgs::Feet &start_feet, ros::ServiceClient &generate_feet_pose_client, const std_msgs::Header &header)
int	disc_val (double length, double cell_size)
	Discretize a (continuous) value into cell size.
double	euclidean_distance (int x1, int y1, int x2, int y2)
double	euclidean_distance (double x1, double y1, double x2, double y2)
double	euclidean_distance (int x1, int y1, int z1, int x2, int y2, int z2)
double	euclidean_distance (double x1, double y1, double z1, double x2, double y2, double z2)
double	euclidean_distance_sq (int x1, int y1, int x2, int y2)
double	euclidean_distance_sq (double x1, double y1, double x2, double y2)
double	euclidean_distance_sq (int x1, int y1, int z1, int x2, int y2, int z2)
double	euclidean_distance_sq (double x1, double y1, double z1, double x2, double y2, double z2)
void	getDeltaStep (const msgs::Foot &current, const msgs::Foot &next, geometry_msgs::Pose &dstep)
void	getDeltaStep (const tf::Pose &current, const tf::Pose &next, tf::Pose &dstep)
bool	getFootSize (ros::NodeHandle &nh, geometry_msgs::Vector3 &foot_size)
bool	getGridMapCoords (const nav_msgs::OccupancyGrid &map, double x, double y, int &map_x, int &map_y)
bool	getGridMapIndex (const nav_msgs::OccupancyGrid &map, double x, double y, int &idx)
bool	getRPY (ros::NodeHandle &nh, const std::string name, geometry_msgs::Vector3 &val)
bool	getUpperBodyOriginShift (ros::NodeHandle &nh, geometry_msgs::Vector3 &upper_body_origin_shift)
bool	getUpperBodySize (ros::NodeHandle &nh, geometry_msgs::Vector3 &upper_body_size)
bool	getXYZ (ros::NodeHandle &nh, const std::string name, geometry_msgs::Vector3 &val)
double	grid_cost (int x1, int y1, int x2, int y2, float cell_size)
unsigned int	int_hash (int key)
void	normalToQuaternion (const geometry_msgs::Vector3 &n, double yaw, geometry_msgs::Quaternion &q)
void	normalToRP (const geometry_msgs::Vector3 &n, double yaw, double &r, double &p)
double	parabol (double x, double y, double a_inv, double b_inv)
double	pceil (double x, double prec)
double	pfloor (double x, double prec)
bool	pointWithinPolygon (int x, int y, const std::vector< std::pair< int, int > > &edges)
	Crossing number method to determine whether a point lies within a polygon or not.
double	pround (double x, double prec)
void	quaternionToNormal (const geometry_msgs::Quaternion &q, geometry_msgs::Vector3 &n)
void	quaternionToNormalYaw (const geometry_msgs::Quaternion &q, geometry_msgs::Vector3 &n, double &yaw)
void	RPYToNormal (double r, double p, double y, geometry_msgs::Vector3 &n)
int	state_2_cell (float value, float cell_size)
	Discretize a (continuous) state value into a cell. (Should be used to discretize a State to a PlanningState.)
std::string &	strip (std::string &s, const char c)
std::string	strip_const (const std::string &s, const char c)
msgs::ErrorStatus	transform (msgs::Foot &foot, ros::ServiceClient &transform_foot_pose_client, const std::string &target_frame)
msgs::ErrorStatus	transform (msgs::Feet &feet, ros::ServiceClient &transform_feet_poses_client, const std::string &target_frame)
msgs::ErrorStatus	transform (msgs::StepPlan &step_plan, ros::ServiceClient &transform_step_plan_client, const std::string &target_frame)
template<typename T >
msgs::ErrorStatus	transformToPlannerFrame (T &p, ros::ServiceClient &foot_pose_transformer_client)
template<typename T >
msgs::ErrorStatus	transformToRobotFrame (T &p, ros::ServiceClient &foot_pose_transformer_client)
Variables
static const int	cvMmScale = 1000
	Used to scale continuous values in meter to discrete values in mm.
static const double	FLOAT_CMP_THR = 0.0001
static const double	TWO_PI = 2 * M_PI

---

Enumeration Type Documentation

enum vigir_footstep_planning::Leg

Enumerator:

RIGHT
LEFT
NOLEG

Definition at line 54 of file math.h.

---

Function Documentation

double vigir_footstep_planning::angle_cell_2_state	(	int	angle,
		double	angle_bin_size
	)		[inline]

Calculate the respective (continuous) angle for a bin.

Definition at line 178 of file math.h.

int vigir_footstep_planning::angle_state_2_cell	(	double	angle,
		double	angle_bin_size
	)		[inline]

Discretize a (continuous) angle into a bin.

Definition at line 167 of file math.h.

unsigned int vigir_footstep_planning::calc_hash_tag	(	int	x,
		int	y,
		int	theta,
		int	leg,
		unsigned int	hash_pred,
		unsigned int	hash_succ,
		int	max_hash_size
	)		[inline]

Returns:

The hash tag for a PlanningState (represented by x, y, theta and leg). The component z may be ignored because it depends directly on x and y. Also swing height and step duration will be uniquely selected for each x and y tupel.

Definition at line 229 of file math.h.

double vigir_footstep_planning::cell_2_state	(	int	value,
		double	cell_size
	)		[inline]

Calculate the respective (continuous) state value for a cell. (Should be used to get a State from a discretized PlanningState.)

Definition at line 202 of file math.h.

double vigir_footstep_planning::cont_val	(	int	length,
		double	cell_size
	)		[inline]

Calculates the continuous value for a length discretized in cell size.

Definition at line 159 of file math.h.

msgs::ErrorStatus vigir_footstep_planning::determineGoalFeetPose	(	msgs::Feet &	goal_feet,
		ros::ServiceClient &	generate_feet_pose_client,
		const geometry_msgs::PoseStamped &	goal_pose
	)

Definition at line 43 of file helper.cpp.

msgs::ErrorStatus vigir_footstep_planning::determineStartFeetPose	(	msgs::Feet &	start_feet,
		ros::ServiceClient &	generate_feet_pose_client,
		const std_msgs::Header &	header
	)

Definition at line 5 of file helper.cpp.

int vigir_footstep_planning::disc_val	(	double	length,
		double	cell_size
	)		[inline]

Discretize a (continuous) value into cell size.

Definition at line 147 of file math.h.

double vigir_footstep_planning::euclidean_distance	(	int	x1,
		int	y1,
		int	x2,
		int	y2
	)		[inline]

Returns:

Euclidean distance between two integer coordinates (cells).

Definition at line 88 of file math.h.

double vigir_footstep_planning::euclidean_distance	(	double	x1,
		double	y1,
		double	x2,
		double	y2
	)		[inline]

Returns:

Euclidean distance between two coordinates.

Definition at line 94 of file math.h.

double vigir_footstep_planning::euclidean_distance	(	int	x1,
		int	y1,
		int	z1,
		int	x2,
		int	y2,
		int	z2
	)		[inline]

Returns:

Euclidean distance between two integer coordinates (cells).

Definition at line 100 of file math.h.

double vigir_footstep_planning::euclidean_distance	(	double	x1,
		double	y1,
		double	z1,
		double	x2,
		double	y2,
		double	z2
	)		[inline]

Returns:

Euclidean distance between two coordinates.

Definition at line 106 of file math.h.

double vigir_footstep_planning::euclidean_distance_sq	(	int	x1,
		int	y1,
		int	x2,
		int	y2
	)		[inline]

Returns:

Squared euclidean distance between two integer coordinates (cells).

Definition at line 61 of file math.h.

double vigir_footstep_planning::euclidean_distance_sq	(	double	x1,
		double	y1,
		double	x2,
		double	y2
	)		[inline]

Returns:

Squared euclidean distance between two coordinates.

Definition at line 68 of file math.h.

double vigir_footstep_planning::euclidean_distance_sq	(	int	x1,
		int	y1,
		int	z1,
		int	x2,
		int	y2,
		int	z2
	)		[inline]

Definition at line 74 of file math.h.

double vigir_footstep_planning::euclidean_distance_sq	(	double	x1,
		double	y1,
		double	z1,
		double	x2,
		double	y2,
		double	z2
	)		[inline]

Returns:

Squared euclidean distance between two coordinates.

Definition at line 81 of file math.h.

void vigir_footstep_planning::getDeltaStep	(	const msgs::Foot &	current,
		const msgs::Foot &	next,
		geometry_msgs::Pose &	dstep
	)

Definition at line 27 of file math.cpp.

void vigir_footstep_planning::getDeltaStep	(	const tf::Pose &	current,
		const tf::Pose &	next,
		tf::Pose &	dstep
	)

Definition at line 54 of file math.cpp.

bool vigir_footstep_planning::getFootSize	(	ros::NodeHandle &	nh,
		geometry_msgs::Vector3 &	foot_size
	)

Definition at line 130 of file helper.cpp.

bool vigir_footstep_planning::getGridMapCoords	(	const nav_msgs::OccupancyGrid &	map,
		double	x,
		double	y,
		int &	map_x,
		int &	map_y
	)

Definition at line 147 of file helper.cpp.

bool vigir_footstep_planning::getGridMapIndex	(	const nav_msgs::OccupancyGrid &	map,
		double	x,
		double	y,
		int &	idx
	)

Definition at line 159 of file helper.cpp.

bool vigir_footstep_planning::getRPY	(	ros::NodeHandle &	nh,
		const std::string	name,
		geometry_msgs::Vector3 &	val
	)

Definition at line 116 of file helper.cpp.

bool vigir_footstep_planning::getUpperBodyOriginShift	(	ros::NodeHandle &	nh,
		geometry_msgs::Vector3 &	upper_body_origin_shift
	)

Definition at line 140 of file helper.cpp.

bool vigir_footstep_planning::getUpperBodySize	(	ros::NodeHandle &	nh,
		geometry_msgs::Vector3 &	upper_body_size
	)

Definition at line 135 of file helper.cpp.

bool vigir_footstep_planning::getXYZ	(	ros::NodeHandle &	nh,
		const std::string	name,
		geometry_msgs::Vector3 &	val
	)

Definition at line 102 of file helper.cpp.

double vigir_footstep_planning::grid_cost	(	int	x1,
		int	y1,
		int	x2,
		int	y2,
		float	cell_size
	)		[inline]

Returns:

The distance of two neighbored cell.

Definition at line 117 of file math.h.

unsigned int vigir_footstep_planning::int_hash

(

int

key

)

[inline]

Returns:

The hash value of the key.

Definition at line 210 of file math.h.

void vigir_footstep_planning::normalToQuaternion	(	const geometry_msgs::Vector3 &	n,
		double	yaw,
		geometry_msgs::Quaternion &	q
	)

Definition at line 84 of file math.cpp.

void vigir_footstep_planning::normalToRP	(	const geometry_msgs::Vector3 &	n,
		double	yaw,
		double &	r,
		double &	p
	)

Definition at line 104 of file math.cpp.

double vigir_footstep_planning::parabol	(	double	x,
		double	y,
		double	a_inv,
		double	b_inv
	)		[inline]

Definition at line 111 of file math.h.

double vigir_footstep_planning::pceil	(	double	x,
		double	prec
	)		[inline]

Definition at line 134 of file math.h.

double vigir_footstep_planning::pfloor	(	double	x,
		double	prec
	)		[inline]

Definition at line 139 of file math.h.

bool vigir_footstep_planning::pointWithinPolygon	(	int	x,
		int	y,
		const std::vector< std::pair< int, int > > &	edges
	)

Crossing number method to determine whether a point lies within a polygon or not.

Parameters:

edges

(x,y)-points defining the polygon.

Check http://geomalgorithms.com/a03-_inclusion.html for further details.

Definition at line 5 of file math.cpp.

double vigir_footstep_planning::pround	(	double	x,
		double	prec
	)		[inline]

Definition at line 129 of file math.h.

void vigir_footstep_planning::quaternionToNormal	(	const geometry_msgs::Quaternion &	q,
		geometry_msgs::Vector3 &	n
	)

Definition at line 73 of file math.cpp.

void vigir_footstep_planning::quaternionToNormalYaw	(	const geometry_msgs::Quaternion &	q,
		geometry_msgs::Vector3 &	n,
		double &	yaw
	)

Definition at line 60 of file math.cpp.

void vigir_footstep_planning::RPYToNormal	(	double	r,
		double	p,
		double	y,
		geometry_msgs::Vector3 &	n
	)

Definition at line 91 of file math.cpp.

int vigir_footstep_planning::state_2_cell	(	float	value,
		float	cell_size
	)		[inline]

Discretize a (continuous) state value into a cell. (Should be used to discretize a State to a PlanningState.)

Definition at line 191 of file math.h.

std::string& vigir_footstep_planning::strip	(	std::string &	s,
		const char	c
	)		[inline]

Definition at line 153 of file helper.h.

std::string vigir_footstep_planning::strip_const	(	const std::string &	s,
		const char	c
	)		[inline]

Definition at line 154 of file helper.h.

msgs::ErrorStatus vigir_footstep_planning::transform	(	msgs::Foot &	foot,
		ros::ServiceClient &	transform_foot_pose_client,
		const std::string &	target_frame
	)

Definition at line 58 of file helper.cpp.

msgs::ErrorStatus vigir_footstep_planning::transform	(	msgs::Feet &	feet,
		ros::ServiceClient &	transform_feet_poses_client,
		const std::string &	target_frame
	)

Definition at line 72 of file helper.cpp.

msgs::ErrorStatus vigir_footstep_planning::transform	(	msgs::StepPlan &	step_plan,
		ros::ServiceClient &	transform_step_plan_client,
		const std::string &	target_frame
	)

Definition at line 86 of file helper.cpp.

template<typename T >

msgs::ErrorStatus vigir_footstep_planning::transformToPlannerFrame	(	T &	p,
		ros::ServiceClient &	foot_pose_transformer_client
	)

Definition at line 125 of file helper.h.

template<typename T >

msgs::ErrorStatus vigir_footstep_planning::transformToRobotFrame	(	T &	p,
		ros::ServiceClient &	foot_pose_transformer_client
	)

Definition at line 131 of file helper.h.

---

Variable Documentation

const int vigir_footstep_planning::cvMmScale = 1000 [static]

Used to scale continuous values in meter to discrete values in mm.

Definition at line 52 of file math.h.

const double vigir_footstep_planning::FLOAT_CMP_THR = 0.0001 [static]

Definition at line 49 of file math.h.

const double vigir_footstep_planning::TWO_PI = 2 * M_PI [static]

Definition at line 47 of file math.h.