#include <TestStability.h>

Inheritance diagram for hrl_kinematics::TestStability:

Public Member Functions
tf::Point	getCOM () const
visualization_msgs::Marker	getCOMMarker () const
geometry_msgs::PolygonStamped	getSupportPolygon () const
bool	isPoseStable (const std::map< std::string, double > &joint_positions, FootSupport support_mode)
bool	isPoseStable (const std::map< std::string, double > &joint_positions, FootSupport support_mode, const tf::Vector3 &normal_vector)
void	scaleConvexHull (double scale)
	TestStability (std::string rfoot_mesh_link_name="RAnkleRoll_link", std::string root_link_name="base_link", std::string rfoot_link_name="r_sole", std::string lfoot_link_name="l_sole", const boost::shared_ptr< const urdf::ModelInterface > &urdf_model=boost::shared_ptr< const urdf::ModelInterface >(), double foot_polygon_scale=1.0)
	Constructor.
virtual	~TestStability ()
Protected Member Functions
std::vector< tf::Point >	convexHull (const std::vector< tf::Point > &points) const
void	initFootPolygon (double scale=1.0)
bool	loadFootPolygon ()
bool	pointInConvexHull (const tf::Point &point, const std::vector< tf::Point > &polygon) const
	tests if point is in polygon in 2D. point and polygon will be projected down to z=0 (z values will be ignored)
Protected Attributes
double	foot_polygon_scale_
std::vector< tf::Point >	foot_support_polygon_left_
	Support polygon (x,y) for the left foot (mirrored from right)
std::vector< tf::Point >	foot_support_polygon_right_
	Support polygon (x,y) for the right foot.
tf::Point	p_com_
std::string	rfoot_mesh_link_name_
std::vector< tf::Point >	support_polygon_
tf::Transform	tf_to_support_

Detailed Description

Class to test whether the kinematic state of a (humanoid) robot is statically stable, given its joint configuration, support mode and the normal vector of the supporting plane.

See also:: isPoseStable()

Definition at line 51 of file TestStability.h.

Constructor & Destructor Documentation

hrl_kinematics::TestStability::TestStability	(	std::string	rfoot_mesh_link_name = `"RAnkleRoll_link"`,
		std::string	root_link_name = `"base_link"`,
		std::string	rfoot_link_name = `"r_sole"`,
		std::string	lfoot_link_name = `"l_sole"`,
		const boost::shared_ptr< const urdf::ModelInterface > &	urdf_model = `boost::shared_ptr<const urdf::ModelInterface>()`,
		double	foot_polygon_scale = `1.0`
	)

Constructor.

Parameters:

rfoot_mesh_link_name	- the model mesh used for displaying the foot polygon for both feet
root_link_name	- name of link that is base of robot
rfoot_link_name	- name of link that is considered the right foot
lfoot_link_name	- name of link that is considered the left foot
urdf_model	- a pointer to a pre-loaded URDF model that can speed up initialization if provided
foot_polygon_scale	- default 1.0 - determines the safety margin of the foot support polygon

Definition at line 41 of file TestStability.cpp.

hrl_kinematics::TestStability::~TestStability ( ) [virtual]

Definition at line 52 of file TestStability.cpp.

Member Function Documentation

std::vector< tf::Point > hrl_kinematics::TestStability::convexHull ( const std::vector< tf::Point > & points ) const [protected]

Definition at line 154 of file TestStability.cpp.

tf::Point hrl_kinematics::TestStability::getCOM ( ) const [inline]

Returns:: the center of mass

Definition at line 98 of file TestStability.h.

visualization_msgs::Marker hrl_kinematics::TestStability::getCOMMarker ( ) const

Returns:: the center of mass (for visualization)

Definition at line 134 of file TestStability.cpp.

geometry_msgs::PolygonStamped hrl_kinematics::TestStability::getSupportPolygon ( ) const

Returns:: the support polygon (for visualization)

Definition at line 118 of file TestStability.cpp.

void hrl_kinematics::TestStability::initFootPolygon ( double scale = 1.0 ) [protected]

Definition at line 217 of file TestStability.cpp.

bool hrl_kinematics::TestStability::isPoseStable	(	const std::map< std::string, double > &	joint_positions,
		FootSupport	support_mode
	)

Test stability based on center of mass and support polygon, assuming the robot is standing on a horizontal supporting plane.

Parameters:

joint_positions	given robot configuration
support_mode	which foot the robot is standing on (SUPPORT_SINGLE_RIGHT, SUPPORT_SINGLE_LEFT, SUPPORT_DOUBLE)

Returns:: true if pose is stable

Definition at line 56 of file TestStability.cpp.

bool hrl_kinematics::TestStability::isPoseStable	(	const std::map< std::string, double > &	joint_positions,
		FootSupport	support_mode,
		const tf::Vector3 &	normal_vector
	)

Test stability based on center of mass and support polygon, assuming the robot is standing on an arbitrary supporting plane (given by normal_vector)

Parameters:

joint_positions	given robot configuration
support_mode	which foot the robot is standing on (SUPPORT_SINGLE_RIGHT, SUPPORT_SINGLE_LEFT, SUPPORT_DOUBLE)
normal_vector	normal vector of the supporting plane