1 #ifndef HPP_HEURISTIC_TOOLS_HH 2 #define HPP_HEURISTIC_TOOLS_HH 4 #include <hpp/core/path.hh> 5 #include <hpp/pinocchio/device.hh> 14 std::map<std::string, fcl::Vec3f>
33 const fcl::Vec3f& comPos,
const fcl::Vec3f& comSp,
34 const fcl::Vec3f& comAcc,
const std::string& sln,
35 const fcl::Transform3f& tf);
47 fcl::Vec3f
transform(
const fcl::Vec3f& p,
const fcl::Vec3f& tr,
48 const fcl::Matrix3f& ro);
55 Vec2D(
double xx,
double yy) : x(xx), y(yy) {}
58 double operator[](
int idx)
const;
59 double& operator[](
int idx);
60 static double euclideanDist(
const Vec2D& v1,
const Vec2D& v2);
68 bool contains(
const std::vector<T>& vect,
const T& val) {
70 for (
unsigned int i = 0; !found && (i < vect.size()); ++i) {
71 if (vect[i] == val) found =
true;
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) {}
104 const std::map<std::string, fcl::Vec3f>& contactPositions);
110 std::vector<Vec2D>
convexHull(std::vector<Vec2D>
set);
124 double groundThreshold);
HeuristicParam & operator=(const HeuristicParam &zhp)
double a
Definition: heuristic-tools.hh:79
void removeNonGroundContacts(std::map< std::string, fcl::Vec3f > &contacts, double groundThreshold)
double b
Definition: heuristic-tools.hh:80
double computeAngle(const Vec2D ¢er, const Vec2D &end1, const Vec2D &end2)
std::map< std::string, fcl::Vec3f > contactPositions_
Definition: heuristic-tools.hh:15
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
Definition: algorithm.hh:26
Plane()
Definition: heuristic-tools.hh:83
Defines a parameters set for the ZMP-based heuristic.
Definition: heuristic-tools.hh:13
fcl::Vec3f comPosition_
Definition: heuristic-tools.hh:17
Vec2D(const Vec2D &c2D)
Definition: heuristic-tools.hh:56
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:24
fcl::Vec3f comSpeed_
Definition: heuristic-tools.hh:18
std::vector< Vec2D > convexHull(std::vector< Vec2D > set)
HeuristicParam()
Definition: heuristic-tools.hh:31
Plane(const Plane &pe)
Definition: heuristic-tools.hh:86
Plane(double aa, double bb, double cc, double dd)
Definition: heuristic-tools.hh:84
fcl::Transform3f tfWorldRoot_
Definition: heuristic-tools.hh:21
Vec2D(double xx, double yy)
Definition: heuristic-tools.hh:55
bool operator==(const Vec2D &v1, const Vec2D &v2)
fcl::Vec3f comAcceleration_
Definition: heuristic-tools.hh:19
double x
Definition: heuristic-tools.hh:52
double c
Definition: heuristic-tools.hh:81
Point center(CIT_Point pointsBegin, CIT_Point pointsEnd)
center compute the center using average method
std::string sampleLimbName_
Definition: heuristic-tools.hh:20
double currentPathId_
Definition: heuristic-tools.hh:25
Definition: heuristic-tools.hh:78
bool operator!=(const Vec2D &v1, const Vec2D &v2)
double y
Definition: heuristic-tools.hh:53
Data structure to store 2-dimensional informations (2D vectors)
Definition: heuristic-tools.hh:51
double d
Definition: heuristic-tools.hh:82
fcl::Vec3f limbReferenceOffset_
Definition: heuristic-tools.hh:28
Vec2D()
Definition: heuristic-tools.hh:54
std::vector< Vec2D > computeSupportPolygon(const std::map< std::string, fcl::Vec3f > &contactPositions)