hpp-rbprm  4.10.0
Implementation of RB-PRM planner using hpp.
heuristic-tools.hh
Go to the documentation of this file.
1 #ifndef HPP_HEURISTIC_TOOLS_HH
2 #define HPP_HEURISTIC_TOOLS_HH
3 
4 #include <hpp/pinocchio/device.hh> // way to get the includes of fcl, ...
5 #include <hpp/core/path.hh>
6 #include <map>
7 
8 namespace hpp{
9 namespace rbprm{
10 namespace sampling{
11 
14  {
15  std::map<std::string, fcl::Vec3f> contactPositions_; // to get the others contacts (without the considered sample)
16  fcl::Vec3f comPosition_; // The CoM position
17  fcl::Vec3f comSpeed_; // The CoM speed
18  fcl::Vec3f comAcceleration_; // The CoM acceleration
19  std::string sampleLimbName_; // The name of the considered sample
20  fcl::Transform3f tfWorldRoot_; // The transform between the world coordinate system and the root of the robot
21  core::PathConstPtr_t comPath_; // path followed by the CoM (found by planning)
22  double currentPathId_; // current id inside comPath (comPath(currentPathId) == comPosition
23  fcl::Vec3f limbReferenceOffset_; // offset between position of the root and position of the end effector in the reference config
24 
26  HeuristicParam(const std::map<std::string, fcl::Vec3f> & cp, const fcl::Vec3f & comPos, const fcl::Vec3f & comSp, const fcl::Vec3f & comAcc,
27  const std::string & sln, const fcl::Transform3f & tf);
28  HeuristicParam(const HeuristicParam & zhp);
29 
31  };
32 
39  fcl::Vec3f transform(const fcl::Vec3f & p, const fcl::Vec3f & tr, const fcl::Matrix3f & ro);
40 
42  struct Vec2D
43  {
44  double x;
45  double y;
46  Vec2D() : x(0), y(0) {}
47  Vec2D(double xx, double yy) : x(xx), y(yy) {}
48  Vec2D(const Vec2D & c2D) : x(c2D.x), y(c2D.y) {}
49  Vec2D & operator=(const Vec2D & c);
50  double operator[](int idx) const;
51  double & operator[](int idx);
52  static double euclideanDist(const Vec2D & v1, const Vec2D & v2);
53  };
54  bool operator==(const Vec2D & v1, const Vec2D & v2);
55  bool operator!=(const Vec2D & v1, const Vec2D & v2);
56  std::ostream & operator<<(std::ostream & out, const Vec2D & v);
57 
59  template <typename T>
60  bool contains(const std::vector <T> & vect, const T & val)
61  {
62  bool found(false);
63  for(unsigned int i = 0; !found && (i < vect.size()); ++i)
64  {
65  if(vect[i] == val)
66  found = true;
67  }
68  return found;
69  }
70 
72  struct Plane
73  {
74  double a;
75  double b;
76  double c;
77  double d;
78  Plane() : a(0), b(0), c(1), d(0) {}
79  Plane(double aa, double bb, double cc, double dd) : a(aa), b(bb), c(cc), d(dd) {}
80  Plane(const Plane & pe) : a(pe.a), b(pe.b), c(pe.c), d(pe.d) {}
81  Plane & operator=(const Plane & pe);
82  };
83 
90  double computeAngle(const Vec2D & center, const Vec2D & end1, const Vec2D & end2);
91 
96  std::vector <Vec2D> computeSupportPolygon(const std::map <std::string, fcl::Vec3f> & contactPositions);
97 
102  std::vector <Vec2D> convexHull(std::vector <Vec2D> set);
103 
109  Vec2D weightedCentroidConvex2D(const std::vector <Vec2D> & convexPolygon);
110 
114  void removeNonGroundContacts(std::map<std::string, fcl::Vec3f> & contacts, double groundThreshold);
115 
116 } // namespace sampling
117 } // namespace rbprm
118 } // namespace hpp
119 
120 #endif
hpp::rbprm::sampling::Vec2D::y
double y
Definition: heuristic-tools.hh:45
hpp::rbprm::sampling::weightedCentroidConvex2D
Vec2D weightedCentroidConvex2D(const std::vector< Vec2D > &convexPolygon)
hpp::rbprm::sampling::Plane::c
double c
Definition: heuristic-tools.hh:76
hpp::rbprm::sampling::Plane::operator=
Plane & operator=(const Plane &pe)
geom::center
Point center(CIT_Point pointsBegin, CIT_Point pointsEnd)
center compute the center using average method
hpp::rbprm::sampling::computeSupportPolygon
std::vector< Vec2D > computeSupportPolygon(const std::map< std::string, fcl::Vec3f > &contactPositions)
hpp::rbprm::sampling::HeuristicParam::comPath_
core::PathConstPtr_t comPath_
Definition: heuristic-tools.hh:21
hpp::rbprm::sampling::Plane::d
double d
Definition: heuristic-tools.hh:77
hpp::rbprm::sampling::HeuristicParam
Defines a parameters set for the ZMP-based heuristic.
Definition: heuristic-tools.hh:13
hpp::rbprm::sampling::HeuristicParam::limbReferenceOffset_
fcl::Vec3f limbReferenceOffset_
Definition: heuristic-tools.hh:23
hpp::rbprm::sampling::Vec2D::Vec2D
Vec2D(const Vec2D &c2D)
Definition: heuristic-tools.hh:48
hpp::rbprm::sampling::HeuristicParam::sampleLimbName_
std::string sampleLimbName_
Definition: heuristic-tools.hh:19
hpp::rbprm::sampling::Vec2D::Vec2D
Vec2D(double xx, double yy)
Definition: heuristic-tools.hh:47
hpp::rbprm::sampling::HeuristicParam::comSpeed_
fcl::Vec3f comSpeed_
Definition: heuristic-tools.hh:17
hpp::rbprm::sampling::Plane::b
double b
Definition: heuristic-tools.hh:75
hpp::rbprm::sampling::HeuristicParam::tfWorldRoot_
fcl::Transform3f tfWorldRoot_
Definition: heuristic-tools.hh:20
hpp::rbprm::sampling::convexHull
std::vector< Vec2D > convexHull(std::vector< Vec2D > set)
hpp::rbprm::sampling::HeuristicParam::contactPositions_
std::map< std::string, fcl::Vec3f > contactPositions_
Definition: heuristic-tools.hh:15
hpp::rbprm::sampling::Vec2D::operator[]
double operator[](int idx) const
hpp::rbprm::sampling::HeuristicParam::comAcceleration_
fcl::Vec3f comAcceleration_
Definition: heuristic-tools.hh:18
hpp
Definition: algorithm.hh:27
hpp::rbprm::sampling::Vec2D::euclideanDist
static double euclideanDist(const Vec2D &v1, const Vec2D &v2)
hpp::rbprm::sampling::Plane
Data structure to define a plane corresponding to the following equation : ax + by + cz + d = 0.
Definition: heuristic-tools.hh:72
hpp::rbprm::sampling::contains
bool contains(const std::vector< T > &vect, const T &val)
Function to verify the existence of an element in a std::vector.
Definition: heuristic-tools.hh:60
hpp::rbprm::sampling::Vec2D::x
double x
Definition: heuristic-tools.hh:44
hpp::rbprm::sampling::HeuristicParam::operator=
HeuristicParam & operator=(const HeuristicParam &zhp)
hpp::rbprm::sampling::HeuristicParam::comPosition_
fcl::Vec3f comPosition_
Definition: heuristic-tools.hh:16
hpp::rbprm::sampling::Plane::a
double a
Definition: heuristic-tools.hh:74
hpp::rbprm::sampling::Plane::Plane
Plane()
Definition: heuristic-tools.hh:78
hpp::rbprm::sampling::HeuristicParam::HeuristicParam
HeuristicParam()
Definition: heuristic-tools.hh:25
hpp::rbprm::sampling::Vec2D
Data structure to store 2-dimensional informations (2D vectors)
Definition: heuristic-tools.hh:42
hpp::rbprm::sampling::Vec2D::operator=
Vec2D & operator=(const Vec2D &c)
hpp::rbprm::sampling::removeNonGroundContacts
void removeNonGroundContacts(std::map< std::string, fcl::Vec3f > &contacts, double groundThreshold)
hpp::rbprm::sampling::HeuristicParam::currentPathId_
double currentPathId_
Definition: heuristic-tools.hh:22
hpp::rbprm::sampling::operator<<
std::ostream & operator<<(std::ostream &out, const Vec2D &v)
hpp::rbprm::sampling::Plane::Plane
Plane(const Plane &pe)
Definition: heuristic-tools.hh:80
hpp::rbprm::sampling::transform
fcl::Vec3f transform(const fcl::Vec3f &p, const fcl::Vec3f &tr, const fcl::Matrix3f &ro)
hpp::rbprm::sampling::computeAngle
double computeAngle(const Vec2D &center, const Vec2D &end1, const Vec2D &end2)
hpp::rbprm::sampling::operator==
bool operator==(const Vec2D &v1, const Vec2D &v2)
hpp::rbprm::sampling::Vec2D::Vec2D
Vec2D()
Definition: heuristic-tools.hh:46
hpp::rbprm::sampling::operator!=
bool operator!=(const Vec2D &v1, const Vec2D &v2)
hpp::rbprm::sampling::Plane::Plane
Plane(double aa, double bb, double cc, double dd)
Definition: heuristic-tools.hh:79