#include <ZMPDistributor.h>

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

◆ leg_type

enum SimpleZMPDistributor::leg_type

Enumerator
RLEG
LLEG
RARM
LARM
BOTH
ALL

Definition at line 64 of file ZMPDistributor.h.

◆ projected_point_region

enum SimpleZMPDistributor::projected_point_region

private

Enumerator
LEFT
MIDDLE
RIGHT

Definition at line 62 of file ZMPDistributor.h.

Constructor & Destructor Documentation

◆ SimpleZMPDistributor()

SimpleZMPDistributor::SimpleZMPDistributor ( const double _dt )

inline

Definition at line 65 of file ZMPDistributor.h.

Member Function Documentation

◆ calc_closest_boundary_point()

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.

◆ calc_convex_hull()

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.

◆ calcAlpha()

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.

◆ calcAlphaFromCOP()

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.

◆ calcAlphaVector()

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.

◆ calcAlphaVectorFromCOP()

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.

◆ calcAlphaVectorFromCOPDistance()

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.

◆ calcAlphaVectorFromCOPDistanceCommon()

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.

◆ calcCrossProduct()

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

inline

Definition at line 1123 of file ZMPDistributor.h.

◆ calcProjectedPoint()

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.

◆ calcWeightedLinearEquation()

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.

◆ compare_eigen2d()

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

inlinestatic

Definition at line 122 of file ZMPDistributor.h.

◆ distributeZMPToForceMoments()

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.

◆ distributeZMPToForceMomentsPseudoInverse()

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.

◆ distributeZMPToForceMomentsPseudoInverse2()

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.

◆ distributeZMPToForceMomentsQP()

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.

◆ get_alpha_cutoff_freq()

double SimpleZMPDistributor::get_alpha_cutoff_freq ( )

inline

Definition at line 262 of file ZMPDistributor.h.

◆ get_leg_front_margin()

double SimpleZMPDistributor::get_leg_front_margin ( )

inline

Definition at line 258 of file ZMPDistributor.h.

◆ get_leg_inside_margin()

double SimpleZMPDistributor::get_leg_inside_margin ( )

inline

Definition at line 260 of file ZMPDistributor.h.

◆ get_leg_outside_margin()

double SimpleZMPDistributor::get_leg_outside_margin ( )

inline

Definition at line 261 of file ZMPDistributor.h.

◆ get_leg_rear_margin()

double SimpleZMPDistributor::get_leg_rear_margin ( )

inline

Definition at line 259 of file ZMPDistributor.h.

◆ get_margined_vertices()

void SimpleZMPDistributor::get_margined_vertices ( std::vector< std::vector< Eigen::Vector2d > > & vs )

inline

Definition at line 264 of file ZMPDistributor.h.

◆ get_vertices()

void SimpleZMPDistributor::get_vertices ( std::vector< std::vector< Eigen::Vector2d > > & vs )

inline

Definition at line 263 of file ZMPDistributor.h.

◆ get_wrench_alpha_blending()

double SimpleZMPDistributor::get_wrench_alpha_blending ( )

inline

Definition at line 257 of file ZMPDistributor.h.

◆ is_front_of_foot()

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.

◆ is_inside_foot()

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.

◆ is_inside_support_polygon()

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.

◆ is_rear_of_foot()

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.

◆ print_params()

void SimpleZMPDistributor::print_params ( const std::string & str )

inline

Definition at line 112 of file ZMPDistributor.h.

◆ print_vertices()

void SimpleZMPDistributor::print_vertices ( const std::string & str )

inline

Definition at line 117 of file ZMPDistributor.h.

◆ set_alpha_cutoff_freq()

void SimpleZMPDistributor::set_alpha_cutoff_freq ( const double a )

inline

Definition at line 185 of file ZMPDistributor.h.

◆ set_leg_front_margin()

void SimpleZMPDistributor::set_leg_front_margin ( const double a )

inline

Definition at line 181 of file ZMPDistributor.h.

◆ set_leg_inside_margin()

void SimpleZMPDistributor::set_leg_inside_margin ( const double a )

inline

Definition at line 183 of file ZMPDistributor.h.

◆ set_leg_outside_margin()

void SimpleZMPDistributor::set_leg_outside_margin ( const double a )

inline

Definition at line 184 of file ZMPDistributor.h.

◆ set_leg_rear_margin()

void SimpleZMPDistributor::set_leg_rear_margin ( const double a )

inline

Definition at line 182 of file ZMPDistributor.h.

◆ set_vertices()

void SimpleZMPDistributor::set_vertices ( const std::vector< std::vector< Eigen::Vector2d > > & vs )

inline

Definition at line 186 of file ZMPDistributor.h.

◆ set_vertices_from_margin_params() [1/2]

void SimpleZMPDistributor::set_vertices_from_margin_params ( )

inline

Definition at line 193 of file ZMPDistributor.h.

◆ set_vertices_from_margin_params() [2/2]

void SimpleZMPDistributor::set_vertices_from_margin_params ( const std::vector< double > & margin )

inline

Definition at line 233 of file ZMPDistributor.h.

◆ set_wrench_alpha_blending()

void SimpleZMPDistributor::set_wrench_alpha_blending ( const double a )

inline

Definition at line 180 of file ZMPDistributor.h.

Member Data Documentation

◆ alpha_filter

boost::shared_ptr<FirstOrderLowPassFilter<double> > SimpleZMPDistributor::alpha_filter

private

Definition at line 60 of file ZMPDistributor.h.

◆ convex_hull

std::vector<Eigen::Vector2d> SimpleZMPDistributor::convex_hull

private

Definition at line 61 of file ZMPDistributor.h.

◆ fs

FootSupportPolygon SimpleZMPDistributor::fs

private

Definition at line 58 of file ZMPDistributor.h.

◆ fs_mgn

FootSupportPolygon SimpleZMPDistributor::fs_mgn

private

Definition at line 58 of file ZMPDistributor.h.

◆ leg_front_margin

double SimpleZMPDistributor::leg_front_margin

private

Definition at line 59 of file ZMPDistributor.h.

◆ leg_inside_margin

double SimpleZMPDistributor::leg_inside_margin

private

Definition at line 59 of file ZMPDistributor.h.

◆ leg_outside_margin

double SimpleZMPDistributor::leg_outside_margin

private

Definition at line 59 of file ZMPDistributor.h.

◆ leg_rear_margin

double SimpleZMPDistributor::leg_rear_margin

private

Definition at line 59 of file ZMPDistributor.h.

◆ wrench_alpha_blending

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

Public Types

Public Member Functions

Static Public Member Functions

Private Types

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ leg_type

◆ projected_point_region

Constructor & Destructor Documentation

◆ SimpleZMPDistributor()

Member Function Documentation

◆ calc_closest_boundary_point()

◆ calc_convex_hull()

◆ calcAlpha()

◆ calcAlphaFromCOP()

◆ calcAlphaVector()

◆ calcAlphaVectorFromCOP()

◆ calcAlphaVectorFromCOPDistance()

◆ calcAlphaVectorFromCOPDistanceCommon()

◆ calcCrossProduct()

◆ calcProjectedPoint()

◆ calcWeightedLinearEquation()

◆ compare_eigen2d()

◆ distributeZMPToForceMoments()

◆ distributeZMPToForceMomentsPseudoInverse()

◆ distributeZMPToForceMomentsPseudoInverse2()

◆ distributeZMPToForceMomentsQP()

◆ get_alpha_cutoff_freq()

◆ get_leg_front_margin()

◆ get_leg_inside_margin()

◆ get_leg_outside_margin()

◆ get_leg_rear_margin()

◆ get_margined_vertices()

◆ get_vertices()

◆ get_wrench_alpha_blending()

◆ is_front_of_foot()

◆ is_inside_foot()

◆ is_inside_support_polygon()

◆ is_rear_of_foot()

◆ print_params()

◆ print_vertices()

◆ set_alpha_cutoff_freq()

◆ set_leg_front_margin()

◆ set_leg_inside_margin()

◆ set_leg_outside_margin()

◆ set_leg_rear_margin()

◆ set_vertices()

◆ set_vertices_from_margin_params() [1/2]

◆ set_vertices_from_margin_params() [2/2]

◆ set_wrench_alpha_blending()

Member Data Documentation

◆ alpha_filter

◆ convex_hull

◆ fs

◆ fs_mgn

◆ leg_front_margin

◆ leg_inside_margin

◆ leg_outside_margin

◆ leg_rear_margin

◆ wrench_alpha_blending