Public Types | Public Member Functions | Static Public Member Functions | Private Types | Private Attributes | List of all members
SimpleZMPDistributor Class Reference

#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

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 
)
inlinestatic

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:

hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:53