hpp-rbprm
4.10.0
Implementation of RB-PRM planner using hpp.
|
Go to the documentation of this file. 1 #ifndef HPP_HEURISTIC_TOOLS_HH
2 #define HPP_HEURISTIC_TOOLS_HH
4 #include <hpp/pinocchio/device.hh>
5 #include <hpp/core/path.hh>
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);
39 fcl::Vec3f
transform(
const fcl::Vec3f & p,
const fcl::Vec3f & tr,
const fcl::Matrix3f & ro);
47 Vec2D(
double xx,
double yy) :
x(xx),
y(yy) {}
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);
60 bool contains(
const std::vector <T> & vect,
const T & val)
63 for(
unsigned int i = 0; !found && (i < vect.size()); ++i)
79 Plane(
double aa,
double bb,
double cc,
double dd) :
a(aa),
b(bb),
c(cc),
d(dd) {}
96 std::vector <Vec2D>
computeSupportPolygon(
const std::map <std::string, fcl::Vec3f> & contactPositions);
102 std::vector <Vec2D>
convexHull(std::vector <Vec2D> set);
double y
Definition: heuristic-tools.hh:45
Vec2D weightedCentroidConvex2D(const std::vector< Vec2D > &convexPolygon)
double c
Definition: heuristic-tools.hh:76
Plane & operator=(const Plane &pe)
Point center(CIT_Point pointsBegin, CIT_Point pointsEnd)
center compute the center using average method
std::vector< Vec2D > computeSupportPolygon(const std::map< std::string, fcl::Vec3f > &contactPositions)
core::PathConstPtr_t comPath_
Definition: heuristic-tools.hh:21
double d
Definition: heuristic-tools.hh:77
Defines a parameters set for the ZMP-based heuristic.
Definition: heuristic-tools.hh:13
fcl::Vec3f limbReferenceOffset_
Definition: heuristic-tools.hh:23
Vec2D(const Vec2D &c2D)
Definition: heuristic-tools.hh:48
std::string sampleLimbName_
Definition: heuristic-tools.hh:19
Vec2D(double xx, double yy)
Definition: heuristic-tools.hh:47
fcl::Vec3f comSpeed_
Definition: heuristic-tools.hh:17
double b
Definition: heuristic-tools.hh:75
fcl::Transform3f tfWorldRoot_
Definition: heuristic-tools.hh:20
std::vector< Vec2D > convexHull(std::vector< Vec2D > set)
std::map< std::string, fcl::Vec3f > contactPositions_
Definition: heuristic-tools.hh:15
double operator[](int idx) const
fcl::Vec3f comAcceleration_
Definition: heuristic-tools.hh:18
Definition: algorithm.hh:27
static double euclideanDist(const Vec2D &v1, const Vec2D &v2)
Data structure to define a plane corresponding to the following equation : ax + by + cz + d = 0.
Definition: heuristic-tools.hh:72
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
double x
Definition: heuristic-tools.hh:44
HeuristicParam & operator=(const HeuristicParam &zhp)
fcl::Vec3f comPosition_
Definition: heuristic-tools.hh:16
double a
Definition: heuristic-tools.hh:74
Plane()
Definition: heuristic-tools.hh:78
HeuristicParam()
Definition: heuristic-tools.hh:25
Data structure to store 2-dimensional informations (2D vectors)
Definition: heuristic-tools.hh:42
Vec2D & operator=(const Vec2D &c)
void removeNonGroundContacts(std::map< std::string, fcl::Vec3f > &contacts, double groundThreshold)
double currentPathId_
Definition: heuristic-tools.hh:22
std::ostream & operator<<(std::ostream &out, const Vec2D &v)
Plane(const Plane &pe)
Definition: heuristic-tools.hh:80
fcl::Vec3f transform(const fcl::Vec3f &p, const fcl::Vec3f &tr, const fcl::Matrix3f &ro)
double computeAngle(const Vec2D ¢er, const Vec2D &end1, const Vec2D &end2)
bool operator==(const Vec2D &v1, const Vec2D &v2)
Vec2D()
Definition: heuristic-tools.hh:46
bool operator!=(const Vec2D &v1, const Vec2D &v2)
Plane(double aa, double bb, double cc, double dd)
Definition: heuristic-tools.hh:79