SimpleZMPDistributor Class Reference

#include <ZMPDistributor.h>

List of all members.

Public Types
enum	leg_type {   RLEG, LLEG, RARM, LARM,   BOTH, ALL }
Public Member Functions
bool	calc_closest_boundary_point (Eigen::Vector2d &p, size_t &right_idx, size_t &left_idx)
void	calc_convex_hull (const std::vector< std::vector< Eigen::Vector2d > > &vs, const std::vector< bool > &cs, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Matrix33 > &ee_rot)
double	calcAlpha (const hrp::Vector3 &tmprefzmp, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name)
double	calcAlphaFromCOP (const hrp::Vector3 &tmprefzmp, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name)
void	calcAlphaVector (std::vector< double > &alpha_vector, std::vector< double > &fz_alpha_vector, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp)
void	calcAlphaVectorFromCOP (std::vector< double > &alpha_vector, std::vector< double > &fz_alpha_vector, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp)
void	calcAlphaVectorFromCOPDistance (std::vector< double > &alpha_vector, std::vector< double > &fz_alpha_vector, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp)
void	calcAlphaVectorFromCOPDistanceCommon (std::vector< double > &alpha_vector, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< std::string > &ee_name, const hrp::Vector3 &ref_zmp)
double	calcCrossProduct (const Eigen::Vector2d &a, const Eigen::Vector2d &b, const Eigen::Vector2d &o)
projected_point_region	calcProjectedPoint (Eigen::Vector2d &ret, const Eigen::Vector2d &target, const Eigen::Vector2d &a, const Eigen::Vector2d &b)
void	calcWeightedLinearEquation (hrp::dvector &ret, const hrp::dmatrix &A, const hrp::dmatrix &W, const hrp::dvector &b)
void	distributeZMPToForceMoments (std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="")
void	distributeZMPToForceMomentsPseudoInverse (std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="", const bool use_cop_distribution=true, const std::vector< bool > is_contact_list=std::vector< bool >())
void	distributeZMPToForceMomentsPseudoInverse2 (std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const hrp::Vector3 &total_force, const hrp::Vector3 &total_moment, const std::vector< hrp::dvector6 > &ee_forcemoment_distribution_weight, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="")
void	distributeZMPToForceMomentsQP (std::vector< hrp::Vector3 > &ref_foot_force, std::vector< hrp::Vector3 > &ref_foot_moment, const std::vector< hrp::Vector3 > &ee_pos, const std::vector< hrp::Vector3 > &cop_pos, const std::vector< hrp::Matrix33 > &ee_rot, const std::vector< std::string > &ee_name, const std::vector< double > &limb_gains, const std::vector< double > &toeheel_ratio, const hrp::Vector3 &new_refzmp, const hrp::Vector3 &ref_zmp, const double total_fz, const double dt, const bool printp=true, const std::string &print_str="", const bool use_cop_distribution=false)
double	get_alpha_cutoff_freq ()
double	get_leg_front_margin ()
double	get_leg_inside_margin ()
double	get_leg_outside_margin ()
double	get_leg_rear_margin ()
void	get_margined_vertices (std::vector< std::vector< Eigen::Vector2d > > &vs)
void	get_vertices (std::vector< std::vector< Eigen::Vector2d > > &vs)
double	get_wrench_alpha_blending ()
bool	is_front_of_foot (const hrp::Vector3 &leg_pos, const double margin=0.0)
bool	is_inside_foot (const hrp::Vector3 &leg_pos, const bool is_lleg, const double margin=0.0)
bool	is_inside_support_polygon (Eigen::Vector2d &p, const hrp::Vector3 &offset=hrp::Vector3::Zero(), const bool &truncate_p=false, const std::string &str="")
bool	is_rear_of_foot (const hrp::Vector3 &leg_pos, const double margin=0.0)
void	print_params (const std::string &str)
void	print_vertices (const std::string &str)
void	set_alpha_cutoff_freq (const double a)
void	set_leg_front_margin (const double a)
void	set_leg_inside_margin (const double a)
void	set_leg_outside_margin (const double a)
void	set_leg_rear_margin (const double a)
void	set_vertices (const std::vector< std::vector< Eigen::Vector2d > > &vs)
void	set_vertices_from_margin_params ()
void	set_vertices_from_margin_params (const std::vector< double > &margin)
void	set_wrench_alpha_blending (const double a)
	SimpleZMPDistributor (const double _dt)
Static Public Member Functions
static bool	compare_eigen2d (const Eigen::Vector2d &lv, const Eigen::Vector2d &rv)
Private Types
enum	projected_point_region { LEFT, MIDDLE, RIGHT }
Private Attributes
boost::shared_ptr < FirstOrderLowPassFilter < double > >	alpha_filter
std::vector< Eigen::Vector2d >	convex_hull
FootSupportPolygon	fs
FootSupportPolygon	fs_mgn
double	leg_front_margin
double	leg_inside_margin
double	leg_outside_margin
double	leg_rear_margin
double	wrench_alpha_blending

---

Detailed Description

Definition at line 56 of file ZMPDistributor.h.

---

Member Enumeration Documentation

enum SimpleZMPDistributor::leg_type

Enumerator:

RLEG
LLEG
RARM
LARM
BOTH
ALL

Definition at line 64 of file ZMPDistributor.h.

enum SimpleZMPDistributor::projected_point_region [private]

Enumerator:

LEFT
MIDDLE
RIGHT

Definition at line 62 of file ZMPDistributor.h.

---

Constructor & Destructor Documentation

SimpleZMPDistributor::SimpleZMPDistributor

(

const double

_dt

)

[inline]

Definition at line 65 of file ZMPDistributor.h.

---

Member Function Documentation

bool SimpleZMPDistributor::calc_closest_boundary_point	(	Eigen::Vector2d &	p,
		size_t &	right_idx,
		size_t &	left_idx
	)		[inline]

Definition at line 153 of file ZMPDistributor.h.

void SimpleZMPDistributor::calc_convex_hull	(	const std::vector< std::vector< Eigen::Vector2d > > &	vs,
		const std::vector< bool > &	cs,
		const std::vector< hrp::Vector3 > &	ee_pos,
		const std::vector< hrp::Matrix33 > &	ee_rot
	)		[inline]

Definition at line 128 of file ZMPDistributor.h.

double SimpleZMPDistributor::calcAlpha	(	const hrp::Vector3 &	tmprefzmp,
		const std::vector< hrp::Vector3 > &	ee_pos,
		const std::vector< hrp::Matrix33 > &	ee_rot,
		const std::vector< std::string > &	ee_name
	)		[inline]

Definition at line 266 of file ZMPDistributor.h.

double SimpleZMPDistributor::calcAlphaFromCOP	(	const hrp::Vector3 &	tmprefzmp,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< std::string > &	ee_name
	)		[inline]

Definition at line 325 of file ZMPDistributor.h.

void SimpleZMPDistributor::calcAlphaVector	(	std::vector< double > &	alpha_vector,
		std::vector< double > &	fz_alpha_vector,
		const std::vector< hrp::Vector3 > &	ee_pos,
		const std::vector< hrp::Matrix33 > &	ee_rot,
		const std::vector< std::string > &	ee_name,
		const hrp::Vector3 &	new_refzmp,
		const hrp::Vector3 &	ref_zmp
	)		[inline]

Definition at line 343 of file ZMPDistributor.h.

void SimpleZMPDistributor::calcAlphaVectorFromCOP	(	std::vector< double > &	alpha_vector,
		std::vector< double > &	fz_alpha_vector,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< std::string > &	ee_name,
		const hrp::Vector3 &	new_refzmp,
		const hrp::Vector3 &	ref_zmp
	)		[inline]

Definition at line 362 of file ZMPDistributor.h.

void SimpleZMPDistributor::calcAlphaVectorFromCOPDistance	(	std::vector< double > &	alpha_vector,
		std::vector< double > &	fz_alpha_vector,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< std::string > &	ee_name,
		const hrp::Vector3 &	new_refzmp,
		const hrp::Vector3 &	ref_zmp
	)		[inline]

Definition at line 396 of file ZMPDistributor.h.

void SimpleZMPDistributor::calcAlphaVectorFromCOPDistanceCommon	(	std::vector< double > &	alpha_vector,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< std::string > &	ee_name,
		const hrp::Vector3 &	ref_zmp
	)		[inline]

Definition at line 380 of file ZMPDistributor.h.

double SimpleZMPDistributor::calcCrossProduct	(	const Eigen::Vector2d &	a,
		const Eigen::Vector2d &	b,
		const Eigen::Vector2d &	o
	)		[inline]

Definition at line 1123 of file ZMPDistributor.h.

projected_point_region SimpleZMPDistributor::calcProjectedPoint	(	Eigen::Vector2d &	ret,
		const Eigen::Vector2d &	target,
		const Eigen::Vector2d &	a,
		const Eigen::Vector2d &	b
	)		[inline]

Definition at line 1128 of file ZMPDistributor.h.

void SimpleZMPDistributor::calcWeightedLinearEquation	(	hrp::dvector &	ret,
		const hrp::dmatrix &	A,
		const hrp::dmatrix &	W,
		const hrp::dvector &	b
	)		[inline]

Definition at line 762 of file ZMPDistributor.h.

static bool SimpleZMPDistributor::compare_eigen2d	(	const Eigen::Vector2d &	lv,
		const Eigen::Vector2d &	rv
	)		[inline, static]

Definition at line 122 of file ZMPDistributor.h.

void SimpleZMPDistributor::distributeZMPToForceMoments	(	std::vector< hrp::Vector3 > &	ref_foot_force,
		std::vector< hrp::Vector3 > &	ref_foot_moment,
		const std::vector< hrp::Vector3 > &	ee_pos,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< hrp::Matrix33 > &	ee_rot,
		const std::vector< std::string > &	ee_name,
		const std::vector< double > &	limb_gains,
		const std::vector< double > &	toeheel_ratio,
		const hrp::Vector3 &	new_refzmp,
		const hrp::Vector3 &	ref_zmp,
		const double	total_fz,
		const double	dt,
		const bool	printp = true,
		const std::string &	print_str = ""
	)		[inline]

Definition at line 409 of file ZMPDistributor.h.

void SimpleZMPDistributor::distributeZMPToForceMomentsPseudoInverse	(	std::vector< hrp::Vector3 > &	ref_foot_force,
		std::vector< hrp::Vector3 > &	ref_foot_moment,
		const std::vector< hrp::Vector3 > &	ee_pos,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< hrp::Matrix33 > &	ee_rot,
		const std::vector< std::string > &	ee_name,
		const std::vector< double > &	limb_gains,
		const std::vector< double > &	toeheel_ratio,
		const hrp::Vector3 &	new_refzmp,
		const hrp::Vector3 &	ref_zmp,
		const double	total_fz,
		const double	dt,
		const bool	printp = true,
		const std::string &	print_str = "",
		const bool	use_cop_distribution = true,
		const std::vector< bool >	is_contact_list = std::vector<bool>()
	)		[inline]

Definition at line 773 of file ZMPDistributor.h.

void SimpleZMPDistributor::distributeZMPToForceMomentsPseudoInverse2	(	std::vector< hrp::Vector3 > &	ref_foot_force,
		std::vector< hrp::Vector3 > &	ref_foot_moment,
		const std::vector< hrp::Vector3 > &	ee_pos,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< hrp::Matrix33 > &	ee_rot,
		const std::vector< std::string > &	ee_name,
		const std::vector< double > &	limb_gains,
		const std::vector< double > &	toeheel_ratio,
		const hrp::Vector3 &	new_refzmp,
		const hrp::Vector3 &	ref_zmp,
		const hrp::Vector3 &	total_force,
		const hrp::Vector3 &	total_moment,
		const std::vector< hrp::dvector6 > &	ee_forcemoment_distribution_weight,
		const double	total_fz,
		const double	dt,
		const bool	printp = true,
		const std::string &	print_str = ""
	)		[inline]

Definition at line 974 of file ZMPDistributor.h.

void SimpleZMPDistributor::distributeZMPToForceMomentsQP	(	std::vector< hrp::Vector3 > &	ref_foot_force,
		std::vector< hrp::Vector3 > &	ref_foot_moment,
		const std::vector< hrp::Vector3 > &	ee_pos,
		const std::vector< hrp::Vector3 > &	cop_pos,
		const std::vector< hrp::Matrix33 > &	ee_rot,
		const std::vector< std::string > &	ee_name,
		const std::vector< double > &	limb_gains,
		const std::vector< double > &	toeheel_ratio,
		const hrp::Vector3 &	new_refzmp,
		const hrp::Vector3 &	ref_zmp,
		const double	total_fz,
		const double	dt,
		const bool	printp = true,
		const std::string &	print_str = "",
		const bool	use_cop_distribution = false
	)		[inline]

Definition at line 742 of file ZMPDistributor.h.

double SimpleZMPDistributor::get_alpha_cutoff_freq

(

)

[inline]

Definition at line 262 of file ZMPDistributor.h.

double SimpleZMPDistributor::get_leg_front_margin

(

)

[inline]

Definition at line 258 of file ZMPDistributor.h.

double SimpleZMPDistributor::get_leg_inside_margin

(

)

[inline]

Definition at line 260 of file ZMPDistributor.h.

double SimpleZMPDistributor::get_leg_outside_margin

(

)

[inline]

Definition at line 261 of file ZMPDistributor.h.

double SimpleZMPDistributor::get_leg_rear_margin

(

)

[inline]

Definition at line 259 of file ZMPDistributor.h.

void SimpleZMPDistributor::get_margined_vertices

(

std::vector< std::vector< Eigen::Vector2d > > &

vs

)

[inline]

Definition at line 264 of file ZMPDistributor.h.

void SimpleZMPDistributor::get_vertices

(

std::vector< std::vector< Eigen::Vector2d > > &

vs

)

[inline]

Definition at line 263 of file ZMPDistributor.h.

double SimpleZMPDistributor::get_wrench_alpha_blending

(

)

[inline]

Definition at line 257 of file ZMPDistributor.h.

bool SimpleZMPDistributor::is_front_of_foot	(	const hrp::Vector3 &	leg_pos,
		const double	margin = 0.0
	)		[inline]

Definition at line 75 of file ZMPDistributor.h.

bool SimpleZMPDistributor::is_inside_foot	(	const hrp::Vector3 &	leg_pos,
		const bool	is_lleg,
		const double	margin = 0.0
	)		[inline]

Definition at line 70 of file ZMPDistributor.h.

bool SimpleZMPDistributor::is_inside_support_polygon	(	Eigen::Vector2d &	p,
		const hrp::Vector3 &	offset = hrp::Vector3::Zero(),
		const bool &	truncate_p = false,
		const std::string &	str = ""
	)		[inline]

Definition at line 83 of file ZMPDistributor.h.

bool SimpleZMPDistributor::is_rear_of_foot	(	const hrp::Vector3 &	leg_pos,
		const double	margin = 0.0
	)		[inline]

Definition at line 79 of file ZMPDistributor.h.

void SimpleZMPDistributor::print_params

(

const std::string &

str

)

[inline]

Definition at line 112 of file ZMPDistributor.h.

void SimpleZMPDistributor::print_vertices

(

const std::string &

str

)

[inline]

Definition at line 117 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_alpha_cutoff_freq

(

const double

a

)

[inline]

Definition at line 185 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_leg_front_margin

(

const double

a

)

[inline]

Definition at line 181 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_leg_inside_margin

(

const double

a

)

[inline]

Definition at line 183 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_leg_outside_margin

(

const double

a

)

[inline]

Definition at line 184 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_leg_rear_margin

(

const double

a

)

[inline]

Definition at line 182 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_vertices

(

const std::vector< std::vector< Eigen::Vector2d > > &

vs

)

[inline]

Definition at line 186 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_vertices_from_margin_params

(

)

[inline]

Definition at line 193 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_vertices_from_margin_params

(

const std::vector< double > &

margin

)

[inline]

Definition at line 233 of file ZMPDistributor.h.

void SimpleZMPDistributor::set_wrench_alpha_blending

(

const double

a

)

[inline]

Definition at line 180 of file ZMPDistributor.h.

---

Member Data Documentation

boost::shared_ptr<FirstOrderLowPassFilter<double> > SimpleZMPDistributor::alpha_filter [private]

Definition at line 60 of file ZMPDistributor.h.

std::vector<Eigen::Vector2d> SimpleZMPDistributor::convex_hull [private]

Definition at line 61 of file ZMPDistributor.h.

FootSupportPolygon SimpleZMPDistributor::fs [private]

Definition at line 58 of file ZMPDistributor.h.

FootSupportPolygon SimpleZMPDistributor::fs_mgn [private]

Definition at line 58 of file ZMPDistributor.h.

double SimpleZMPDistributor::leg_front_margin [private]

Definition at line 59 of file ZMPDistributor.h.

double SimpleZMPDistributor::leg_inside_margin [private]

Definition at line 59 of file ZMPDistributor.h.

double SimpleZMPDistributor::leg_outside_margin [private]

Definition at line 59 of file ZMPDistributor.h.

double SimpleZMPDistributor::leg_rear_margin [private]

Definition at line 59 of file ZMPDistributor.h.

double SimpleZMPDistributor::wrench_alpha_blending [private]

Definition at line 59 of file ZMPDistributor.h.

---

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

• ZMPDistributor.h