hpp-rbprm
4.10.0
Implementation of RB-PRM planner using hpp.
|
Classes | |
struct | AnalysisFactory |
struct | HeuristicFactory |
struct | HeuristicParam |
Defines a parameters set for the ZMP-based heuristic. More... | |
class | OctreeNode |
struct | OctreeReport |
struct | Plane |
Data structure to define a plane corresponding to the following equation : ax + by + cz + d = 0. More... | |
class | Sample |
struct | sample_compare |
struct | sample_greater |
class | SampleDB |
struct | Vec2D |
Data structure to store 2-dimensional informations (2D vectors) More... | |
Typedefs | |
typedef double(* | heuristic) (const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam ¶ms) |
typedef boost::shared_ptr< OctreeNode > | OctreeNodePtr_t |
typedef std::multiset< OctreeReport, sample_compare > | T_OctreeReport |
typedef std::vector< double > | T_Double |
typedef std::map< std::string, T_Double > | T_Values |
typedef sampling::SampleVector_t | T_Sample |
typedef boost::function< double(const SampleDB &sampleDB, const sampling::Sample &sample) > | evaluate |
typedef std::map< std::string, evaluate > | T_evaluate |
typedef std::pair< std::size_t, std::size_t > | VoxelSampleId |
typedef std::map< long int, VoxelSampleId > | T_VoxelSampleId |
typedef std::pair< double, double > | ValueBound |
typedef std::map< std::string, ValueBound > | T_ValueBound |
typedef boost::shared_ptr< Sample > | SamplePtr_t |
typedef std::vector< Sample, Eigen::aligned_allocator< Sample > > | SampleVector_t |
Functions | |
fcl::Vec3f | transform (const fcl::Vec3f &p, const fcl::Vec3f &tr, const fcl::Matrix3f &ro) |
bool | operator== (const Vec2D &v1, const Vec2D &v2) |
bool | operator!= (const Vec2D &v1, const Vec2D &v2) |
std::ostream & | operator<< (std::ostream &out, const Vec2D &v) |
template<typename T > | |
bool | contains (const std::vector< T > &vect, const T &val) |
Function to verify the existence of an element in a std::vector. More... | |
double | computeAngle (const Vec2D ¢er, const Vec2D &end1, const Vec2D &end2) |
std::vector< Vec2D > | computeSupportPolygon (const std::map< std::string, fcl::Vec3f > &contactPositions) |
std::vector< Vec2D > | convexHull (std::vector< Vec2D > set) |
Vec2D | weightedCentroidConvex2D (const std::vector< Vec2D > &convexPolygon) |
void | removeNonGroundContacts (std::map< std::string, fcl::Vec3f > &contacts, double groundThreshold) |
HPP_PREDEF_CLASS (OctreeNode) | |
HPP_PREDEF_CLASS (SampleDB) | |
HPP_RBPRM_DLLAPI SampleDB & | addValue (SampleDB &database, const std::string &valueName, const evaluate eval, bool isStaticValue=true, bool sortSamples=true) |
HPP_RBPRM_DLLAPI bool | saveLimbDatabase (const SampleDB &database, std::ofstream &dbFile) |
HPP_RBPRM_DLLAPI T_OctreeReport | GetCandidates (const SampleDB &sc, const fcl::Transform3f &treeTrf, const hpp::pinocchio::CollisionObjectPtr_t &o2, const fcl::Vec3f &direction, const HeuristicParam ¶ms, const heuristic evaluate=0) |
HPP_RBPRM_DLLAPI bool | GetCandidates (const SampleDB &sc, const fcl::Transform3f &treeTrf, const hpp::pinocchio::CollisionObjectPtr_t &o2, const fcl::Vec3f &direction, T_OctreeReport &report, const HeuristicParam ¶ms, const heuristic evaluate=0) |
HPP_PREDEF_CLASS (Sample) | |
SampleVector_t | GenerateSamples (const pinocchio::JointPtr_t limb, const std::string &effector, const std::size_t nbSamples, const fcl::Vec3f &offset=fcl::Vec3f(0, 0, 0), const fcl::Vec3f &limbOffset=fcl::Vec3f(0, 0, 0)) |
void | Load (const Sample &sample, pinocchio::ConfigurationOut_t robot) |
typedef boost::function<double (const SampleDB& sampleDB, const sampling::Sample& sample) > hpp::rbprm::sampling::evaluate |
Defines an evaluation function for a sample.
SampleDB | used database with already computed values |
sample | sample candidate |
normal | contact surface normal relatively to the candidate |
typedef double(* hpp::rbprm::sampling::heuristic) (const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam ¶ms) |
Defines a heuristic method to sort samples the higher the score, the better the sample in presented joint
sample | sample candidate |
direction | overall direction of motion |
normal | contact surface normal relatively to the candidate |
typedef boost::shared_ptr<OctreeNode> hpp::rbprm::sampling::OctreeNodePtr_t |
typedef boost::shared_ptr<Sample> hpp::rbprm::sampling::SamplePtr_t |
typedef std::vector<Sample, Eigen::aligned_allocator<Sample> > hpp::rbprm::sampling::SampleVector_t |
typedef std::vector<double> hpp::rbprm::sampling::T_Double |
typedef std::map<std::string, evaluate> hpp::rbprm::sampling::T_evaluate |
typedef std::multiset<OctreeReport, sample_compare> hpp::rbprm::sampling::T_OctreeReport |
typedef std::map<std::string, ValueBound> hpp::rbprm::sampling::T_ValueBound |
typedef std::map<std::string, T_Double> hpp::rbprm::sampling::T_Values |
typedef std::map<long int, VoxelSampleId> hpp::rbprm::sampling::T_VoxelSampleId |
typedef std::pair<double, double> hpp::rbprm::sampling::ValueBound |
typedef std::pair<std::size_t, std::size_t> hpp::rbprm::sampling::VoxelSampleId |
HPP_RBPRM_DLLAPI SampleDB& hpp::rbprm::sampling::addValue | ( | SampleDB & | database, |
const std::string & | valueName, | ||
const evaluate | eval, | ||
bool | isStaticValue = true , |
||
bool | sortSamples = true |
||
) |
double hpp::rbprm::sampling::computeAngle | ( | const Vec2D & | center, |
const Vec2D & | end1, | ||
const Vec2D & | end2 | ||
) |
Computes the angle between 2 vectors on the same base point
center | The base point of the vectors |
end1 | The end point of the first vector |
end2 | The end point of the second vector |
std::vector<Vec2D> hpp::rbprm::sampling::computeSupportPolygon | ( | const std::map< std::string, fcl::Vec3f > & | contactPositions | ) |
Computes the support polygon
contactPositions | The map of the contact positions |
bool hpp::rbprm::sampling::contains | ( | const std::vector< T > & | vect, |
const T & | val | ||
) |
Function to verify the existence of an element in a std::vector.
Computes the convex hull of a set
set | The set we want to get the convex hull |
SampleVector_t hpp::rbprm::sampling::GenerateSamples | ( | const pinocchio::JointPtr_t | limb, |
const std::string & | effector, | ||
const std::size_t | nbSamples, | ||
const fcl::Vec3f & | offset = fcl::Vec3f(0, 0, 0) , |
||
const fcl::Vec3f & | limbOffset = fcl::Vec3f(0, 0, 0) |
||
) |
Automatically generates a deque of sample configuration for a given limb of a robot
limb | root of the considered limb |
effector | tag identifying the end effector of the limb |
nbSamples | number of samples to be generated |
offset | location of the contact point of the effector relatively to the effector joint origin |
limbOffset | offset betwwen the limb joint position and it's link |
HPP_RBPRM_DLLAPI T_OctreeReport hpp::rbprm::sampling::GetCandidates | ( | const SampleDB & | sc, |
const fcl::Transform3f & | treeTrf, | ||
const hpp::pinocchio::CollisionObjectPtr_t & | o2, | ||
const fcl::Vec3f & | direction, | ||
const HeuristicParam & | params, | ||
const heuristic | evaluate = 0 |
||
) |
Given the current position of a robot, returns a set of candidate sample configurations for contact generation. The set is strictly ordered using a heuristic to determine the most relevant contacts.
sc | the SampleDB containing all the samples for a given limb |
treeTrf | the current transformation of the root of the robot |
direction | the current direction of motion, used to evaluate the sample heuristically |
evaluate | heuristic used to sort candidates |
HPP_RBPRM_DLLAPI bool hpp::rbprm::sampling::GetCandidates | ( | const SampleDB & | sc, |
const fcl::Transform3f & | treeTrf, | ||
const hpp::pinocchio::CollisionObjectPtr_t & | o2, | ||
const fcl::Vec3f & | direction, | ||
T_OctreeReport & | report, | ||
const HeuristicParam & | params, | ||
const heuristic | evaluate = 0 |
||
) |
Given the current position of a robot, returns a set of candidate sample configurations for contact generation. The set is strictly ordered using a heuristic to determine the most relevant contacts.
sc | the SampleDB containing all the samples for a given limb |
treeTrf | the current transformation of the root of the robot |
direction | the current direction of motion, used to evaluate the sample heuristically |
a | set of OctreeReport updated as the samples are explored |
evaluate | heuristic used to sort candidates |
hpp::rbprm::sampling::HPP_PREDEF_CLASS | ( | OctreeNode | ) |
hpp::rbprm::sampling::HPP_PREDEF_CLASS | ( | Sample | ) |
hpp::rbprm::sampling::HPP_PREDEF_CLASS | ( | SampleDB | ) |
void hpp::rbprm::sampling::Load | ( | const Sample & | sample, |
pinocchio::ConfigurationOut_t | robot | ||
) |
Assigns the limb configuration associated with a sample to a robot configuration
sample | The limb configuration to load |
robot | the configuration to be modified |
std::ostream& hpp::rbprm::sampling::operator<< | ( | std::ostream & | out, |
const Vec2D & | v | ||
) |
void hpp::rbprm::sampling::removeNonGroundContacts | ( | std::map< std::string, fcl::Vec3f > & | contacts, |
double | groundThreshold | ||
) |
Remove the contacts that does not belong to the "ground"
contacts | The considered contacts map |
HPP_RBPRM_DLLAPI bool hpp::rbprm::sampling::saveLimbDatabase | ( | const SampleDB & | database, |
std::ofstream & | dbFile | ||
) |
fcl::Vec3f hpp::rbprm::sampling::transform | ( | const fcl::Vec3f & | p, |
const fcl::Vec3f & | tr, | ||
const fcl::Matrix3f & | ro | ||
) |
Computes the transform of a point
p | The considered point |
tr | The translation to apply |
ro | The rotation to apply |
Computes the weighted centroid of a convex polygon This is the "real (visual) center" of a polygon (an approximation of it in the worst case)
convexPolygon | The convex polygon to whom we want to find the weighted centroid |