#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 ()
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
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
double	scale_convex_hull_
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 54 of file TestStability.h.

Constructor & Destructor Documentation

hrl_kinematics::TestStability::TestStability ( )

Definition at line 44 of file TestStability.cpp.

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

Definition at line 51 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 153 of file TestStability.cpp.

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

Returns:: the center of mass

Definition at line 87 of file TestStability.h.

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

Returns:: the center of mass (for visualization)

Definition at line 133 of file TestStability.cpp.

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

Returns:: the support polygon (for visualization)

Definition at line 117 of file TestStability.cpp.

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

Definition at line 221 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 55 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