hpp-rbprm  4.11.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  std::map<std::string, fcl::Vec3f> contactPositions_; // to get the others contacts (without the considered sample)
15  fcl::Vec3f comPosition_; // The CoM position
16  fcl::Vec3f comSpeed_; // The CoM speed
17  fcl::Vec3f comAcceleration_; // The CoM acceleration
18  std::string sampleLimbName_; // The name of the considered sample
19  fcl::Transform3f tfWorldRoot_; // The transform between the world coordinate system and the root of the robot
20  core::PathConstPtr_t comPath_; // path followed by the CoM (found by planning)
21  double currentPathId_; // current id inside comPath (comPath(currentPathId) == comPosition
22  fcl::Vec3f limbReferenceOffset_; // offset between position of the root and position of the end effector in the
23  // reference config
24 
26  HeuristicParam(const std::map<std::string, fcl::Vec3f>& cp, const fcl::Vec3f& comPos, const fcl::Vec3f& comSp,
27  const fcl::Vec3f& comAcc, 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  double x;
44  double y;
45  Vec2D() : x(0), y(0) {}
46  Vec2D(double xx, double yy) : x(xx), y(yy) {}
47  Vec2D(const Vec2D& c2D) : x(c2D.x), y(c2D.y) {}
48  Vec2D& operator=(const Vec2D& c);
49  double operator[](int idx) const;
50  double& operator[](int idx);
51  static double euclideanDist(const Vec2D& v1, const Vec2D& v2);
52 };
53 bool operator==(const Vec2D& v1, const Vec2D& v2);
54 bool operator!=(const Vec2D& v1, const Vec2D& v2);
55 std::ostream& operator<<(std::ostream& out, const Vec2D& v);
56 
58 template <typename T>
59 bool contains(const std::vector<T>& vect, const T& val) {
60  bool found(false);
61  for (unsigned int i = 0; !found && (i < vect.size()); ++i) {
62  if (vect[i] == val) found = true;
63  }
64  return found;
65 }
66 
68 struct Plane {
69  double a;
70  double b;
71  double c;
72  double d;
73  Plane() : a(0), b(0), c(1), d(0) {}
74  Plane(double aa, double bb, double cc, double dd) : a(aa), b(bb), c(cc), d(dd) {}
75  Plane(const Plane& pe) : a(pe.a), b(pe.b), c(pe.c), d(pe.d) {}
76  Plane& operator=(const Plane& pe);
77 };
78 
85 double computeAngle(const Vec2D& center, const Vec2D& end1, const Vec2D& end2);
86 
91 std::vector<Vec2D> computeSupportPolygon(const std::map<std::string, fcl::Vec3f>& contactPositions);
92 
97 std::vector<Vec2D> convexHull(std::vector<Vec2D> set);
98 
104 Vec2D weightedCentroidConvex2D(const std::vector<Vec2D>& convexPolygon);
105 
109 void removeNonGroundContacts(std::map<std::string, fcl::Vec3f>& contacts, double groundThreshold);
110 
111 } // namespace sampling
112 } // namespace rbprm
113 } // namespace hpp
114 
115 #endif
HeuristicParam & operator=(const HeuristicParam &zhp)
double a
Definition: heuristic-tools.hh:69
void removeNonGroundContacts(std::map< std::string, fcl::Vec3f > &contacts, double groundThreshold)
double b
Definition: heuristic-tools.hh:70
double computeAngle(const Vec2D &center, const Vec2D &end1, const Vec2D &end2)
std::map< std::string, fcl::Vec3f > contactPositions_
Definition: heuristic-tools.hh:14
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:59
Definition: algorithm.hh:27
Plane()
Definition: heuristic-tools.hh:73
Defines a parameters set for the ZMP-based heuristic.
Definition: heuristic-tools.hh:13
fcl::Vec3f comPosition_
Definition: heuristic-tools.hh:15
Vec2D(const Vec2D &c2D)
Definition: heuristic-tools.hh:47
fcl::Vec3f transform(const fcl::Vec3f &p, const fcl::Vec3f &tr, const fcl::Matrix3f &ro)
std::ostream & operator<<(std::ostream &out, const Vec2D &v)
Vec2D weightedCentroidConvex2D(const std::vector< Vec2D > &convexPolygon)
core::PathConstPtr_t comPath_
Definition: heuristic-tools.hh:20
fcl::Vec3f comSpeed_
Definition: heuristic-tools.hh:16
std::vector< Vec2D > convexHull(std::vector< Vec2D > set)
HeuristicParam()
Definition: heuristic-tools.hh:25
Plane(const Plane &pe)
Definition: heuristic-tools.hh:75
Plane(double aa, double bb, double cc, double dd)
Definition: heuristic-tools.hh:74
fcl::Transform3f tfWorldRoot_
Definition: heuristic-tools.hh:19
Vec2D(double xx, double yy)
Definition: heuristic-tools.hh:46
bool operator==(const Vec2D &v1, const Vec2D &v2)
fcl::Vec3f comAcceleration_
Definition: heuristic-tools.hh:17
double x
Definition: heuristic-tools.hh:43
double c
Definition: heuristic-tools.hh:71
Point center(CIT_Point pointsBegin, CIT_Point pointsEnd)
center compute the center using average method
std::string sampleLimbName_
Definition: heuristic-tools.hh:18
double currentPathId_
Definition: heuristic-tools.hh:21
Data structure to define a plane corresponding to the following equation : ax + by + cz + d = 0...
Definition: heuristic-tools.hh:68
bool operator!=(const Vec2D &v1, const Vec2D &v2)
double y
Definition: heuristic-tools.hh:44
Data structure to store 2-dimensional informations (2D vectors)
Definition: heuristic-tools.hh:42
double d
Definition: heuristic-tools.hh:72
fcl::Vec3f limbReferenceOffset_
Definition: heuristic-tools.hh:22
Vec2D()
Definition: heuristic-tools.hh:45
std::vector< Vec2D > computeSupportPolygon(const std::map< std::string, fcl::Vec3f > &contactPositions)