Public Types | Public Member Functions | Static Public Member Functions | Private Types | Private Attributes
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

Enumerator:
RLEG 
LLEG 
RARM 
LARM 
BOTH 
ALL 

Definition at line 64 of file ZMPDistributor.h.

Enumerator:
LEFT 
MIDDLE 
RIGHT 

Definition at line 62 of file ZMPDistributor.h.


Constructor & Destructor Documentation

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.

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.

Definition at line 262 of file ZMPDistributor.h.

Definition at line 258 of file ZMPDistributor.h.

Definition at line 260 of file ZMPDistributor.h.

Definition at line 261 of file ZMPDistributor.h.

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.

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.

Definition at line 185 of file ZMPDistributor.h.

Definition at line 181 of file ZMPDistributor.h.

Definition at line 183 of file ZMPDistributor.h.

Definition at line 184 of file ZMPDistributor.h.

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.

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.

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.

Definition at line 58 of file ZMPDistributor.h.

Definition at line 58 of file ZMPDistributor.h.

Definition at line 59 of file ZMPDistributor.h.

Definition at line 59 of file ZMPDistributor.h.

Definition at line 59 of file ZMPDistributor.h.

Definition at line 59 of file ZMPDistributor.h.

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 Wed May 15 2019 05:02:20