hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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/core/path.hh>
5#include <hpp/pinocchio/device.hh> // way to get the includes of fcl, ...
6#include <map>
7
8namespace hpp {
9namespace rbprm {
10namespace sampling {
11
14 std::map<std::string, fcl::Vec3f>
15 contactPositions_; // to get the others contacts (without the considered
16 // sample)
17 fcl::Vec3f comPosition_; // The CoM position
18 fcl::Vec3f comSpeed_; // The CoM speed
19 fcl::Vec3f comAcceleration_; // The CoM acceleration
20 std::string sampleLimbName_; // The name of the considered sample
21 fcl::Transform3f tfWorldRoot_; // The transform between the world coordinate
22 // system and the root of the robot
23 core::PathConstPtr_t
24 comPath_; // path followed by the CoM (found by planning)
25 double currentPathId_; // current id inside comPath (comPath(currentPathId)
26 // == comPosition
27 fcl::Vec3f
28 limbReferenceOffset_; // offset between position of the root and position
29 // of the end effector in the reference config
30
32 HeuristicParam(const std::map<std::string, fcl::Vec3f>& cp,
33 const fcl::Vec3f& comPos, const fcl::Vec3f& comSp,
34 const fcl::Vec3f& comAcc, const std::string& sln,
35 const fcl::Transform3f& tf);
37
39};
40
47fcl::Vec3f transform(const fcl::Vec3f& p, const fcl::Vec3f& tr,
48 const fcl::Matrix3f& ro);
49
51struct Vec2D {
52 double x;
53 double y;
54 Vec2D() : x(0), y(0) {}
55 Vec2D(double xx, double yy) : x(xx), y(yy) {}
56 Vec2D(const Vec2D& c2D) : x(c2D.x), y(c2D.y) {}
57 Vec2D& operator=(const Vec2D& c);
58 double operator[](int idx) const;
59 double& operator[](int idx);
60 static double euclideanDist(const Vec2D& v1, const Vec2D& v2);
61};
62bool operator==(const Vec2D& v1, const Vec2D& v2);
63bool operator!=(const Vec2D& v1, const Vec2D& v2);
64std::ostream& operator<<(std::ostream& out, const Vec2D& v);
65
67template <typename T>
68bool contains(const std::vector<T>& vect, const T& val) {
69 bool found(false);
70 for (unsigned int i = 0; !found && (i < vect.size()); ++i) {
71 if (vect[i] == val) found = true;
72 }
73 return found;
74}
75
78struct Plane {
79 double a;
80 double b;
81 double c;
82 double d;
83 Plane() : a(0), b(0), c(1), d(0) {}
84 Plane(double aa, double bb, double cc, double dd)
85 : a(aa), b(bb), c(cc), d(dd) {}
86 Plane(const Plane& pe) : a(pe.a), b(pe.b), c(pe.c), d(pe.d) {}
87 Plane& operator=(const Plane& pe);
88};
89
96double computeAngle(const Vec2D& center, const Vec2D& end1, const Vec2D& end2);
97
103std::vector<Vec2D> computeSupportPolygon(
104 const std::map<std::string, fcl::Vec3f>& contactPositions);
105
110std::vector<Vec2D> convexHull(std::vector<Vec2D> set);
111
118Vec2D weightedCentroidConvex2D(const std::vector<Vec2D>& convexPolygon);
119
123void removeNonGroundContacts(std::map<std::string, fcl::Vec3f>& contacts,
124 double groundThreshold);
125
126} // namespace sampling
127} // namespace rbprm
128} // namespace hpp
129
130#endif
bool operator==(const Vec2D &v1, const Vec2D &v2)
std::vector< Vec2D > convexHull(std::vector< Vec2D > set)
double computeAngle(const Vec2D &center, const Vec2D &end1, const Vec2D &end2)
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:68
std::vector< Vec2D > computeSupportPolygon(const std::map< std::string, fcl::Vec3f > &contactPositions)
bool operator!=(const Vec2D &v1, const Vec2D &v2)
Vec2D weightedCentroidConvex2D(const std::vector< Vec2D > &convexPolygon)
fcl::Vec3f transform(const fcl::Vec3f &p, const fcl::Vec3f &tr, const fcl::Matrix3f &ro)
std::ostream & operator<<(std::ostream &out, const Vec2D &v)
void removeNonGroundContacts(std::map< std::string, fcl::Vec3f > &contacts, double groundThreshold)
Definition: algorithm.hh:26
Defines a parameters set for the ZMP-based heuristic.
Definition: heuristic-tools.hh:13
fcl::Transform3f tfWorldRoot_
Definition: heuristic-tools.hh:21
fcl::Vec3f comSpeed_
Definition: heuristic-tools.hh:18
HeuristicParam(const std::map< std::string, fcl::Vec3f > &cp, const fcl::Vec3f &comPos, const fcl::Vec3f &comSp, const fcl::Vec3f &comAcc, const std::string &sln, const fcl::Transform3f &tf)
core::PathConstPtr_t comPath_
Definition: heuristic-tools.hh:24
std::string sampleLimbName_
Definition: heuristic-tools.hh:20
fcl::Vec3f comPosition_
Definition: heuristic-tools.hh:17
std::map< std::string, fcl::Vec3f > contactPositions_
Definition: heuristic-tools.hh:15
double currentPathId_
Definition: heuristic-tools.hh:25
HeuristicParam & operator=(const HeuristicParam &zhp)
fcl::Vec3f limbReferenceOffset_
Definition: heuristic-tools.hh:28
fcl::Vec3f comAcceleration_
Definition: heuristic-tools.hh:19
HeuristicParam()
Definition: heuristic-tools.hh:31
HeuristicParam(const HeuristicParam &zhp)
Definition: heuristic-tools.hh:78
Plane()
Definition: heuristic-tools.hh:83
Plane & operator=(const Plane &pe)
double c
Definition: heuristic-tools.hh:81
double b
Definition: heuristic-tools.hh:80
double d
Definition: heuristic-tools.hh:82
Plane(const Plane &pe)
Definition: heuristic-tools.hh:86
Plane(double aa, double bb, double cc, double dd)
Definition: heuristic-tools.hh:84
double a
Definition: heuristic-tools.hh:79
Data structure to store 2-dimensional informations (2D vectors)
Definition: heuristic-tools.hh:51
double operator[](int idx) const
double y
Definition: heuristic-tools.hh:53
double x
Definition: heuristic-tools.hh:52
static double euclideanDist(const Vec2D &v1, const Vec2D &v2)
Vec2D & operator=(const Vec2D &c)
double & operator[](int idx)
Vec2D(double xx, double yy)
Definition: heuristic-tools.hh:55
Vec2D()
Definition: heuristic-tools.hh:54
Vec2D(const Vec2D &c2D)
Definition: heuristic-tools.hh:56