hpp-rbprm  4.10.0
Implementation of RB-PRM planner using hpp.
sample-db.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2014 CNRS
3 // Authors: Steve Tonneau (steve.tonneau@laas.fr)
4 //
5 // This file is part of hpp-rbprm.
6 // hpp-rbprm is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-rbprm is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_RBPRM_SAMPLEDB_HH
20 # define HPP_RBPRM_SAMPLEDB_HH
21 
22 #include <hpp/rbprm/config.hh>
25 #include <hpp/fcl/octree.h>
26 #include <boost/function.hpp>
27 #include <vector>
28 #include <map>
29 
30 namespace hpp {
31 
32  namespace rbprm {
33  namespace sampling{
34 
37  struct OctreeReport
38  {
39  OctreeReport(const Sample*, const fcl::Contact, const double, const fcl::Vec3f& normal,
40  const fcl::Vec3f& v1, const fcl::Vec3f& v2, const fcl::Vec3f& v3); // vertices of triangle collided
42  const Sample* sample_;
44  fcl::Contact contact_;
46  double value_;
48  fcl::Vec3f normal_, v1_,v2_,v3_;
49  };
50 
51 
55  struct sample_compare {
56  bool operator() (const OctreeReport& lhs, const OctreeReport& rhs) const{
57  return lhs.value_ > rhs.value_;
58  }
59  };
60 
61  typedef std::multiset<OctreeReport, sample_compare> T_OctreeReport;
62 
64 
65  typedef std::vector<double> T_Double;
66  typedef std::map<std::string, T_Double> T_Values;
68 
69 
74  //typedef double (*evaluate) (const SampleDB& sampleDB, const sampling::Sample& sample);
75  typedef boost::function <double (const SampleDB& sampleDB, const sampling::Sample& sample) > evaluate;
76  typedef std::map<std::string, evaluate> T_evaluate;
77  //first sample index, number of samples
78  typedef std::pair<std::size_t, std::size_t> VoxelSampleId;
79  typedef std::map<long int, VoxelSampleId> T_VoxelSampleId;
80  typedef std::pair<double, double> ValueBound;
81  typedef std::map<std::string, ValueBound> T_ValueBound;
82 
87  {
88  public:
89  SampleDB(std::ifstream& databaseStream, bool loadValues = true);
90  SampleDB(const pinocchio::JointPtr_t limb, const std::string& effector, const std::size_t nbSamples,
91  const fcl::Vec3f& offset= fcl::Vec3f(0,0,0),const fcl::Vec3f& limbOffset= fcl::Vec3f(0,0,0), const double resolution = 0.1, const T_evaluate& data = T_evaluate(), const std::string& staticValue ="");
92  ~SampleDB();
93 /*
94  private:
95  SampleDB(const SampleDB&);
96  const SampleDB& operator=(const SampleDB&);
97 */
98  public:
99  double resolution_;
101  boost::shared_ptr<const octomap::OcTree> octomapTree_;
102  fcl::OcTree* octree_; // deleted with geometry_
103  boost::shared_ptr<fcl::CollisionGeometry> geometry_;
108  fcl::CollisionObject treeObject_;
110  std::map<std::size_t, fcl::CollisionObject*> boxes_;
111 
112 
113  }; // class SampleDB
114 
115  HPP_RBPRM_DLLAPI SampleDB& addValue(SampleDB& database, const std::string& valueName, const evaluate eval, bool isStaticValue=true, bool sortSamples=true);
116  HPP_RBPRM_DLLAPI bool saveLimbDatabase(const SampleDB& database, std::ofstream& dbFile);
117 
129  HPP_RBPRM_DLLAPI T_OctreeReport GetCandidates(const SampleDB& sc, const fcl::Transform3f& treeTrf,
130  const hpp::pinocchio::CollisionObjectPtr_t& o2,
131  const fcl::Vec3f& direction, const HeuristicParam & params, const heuristic evaluate = 0);
132 
145  HPP_RBPRM_DLLAPI bool GetCandidates(const SampleDB& sc, const fcl::Transform3f& treeTrf,
146  const hpp::pinocchio::CollisionObjectPtr_t& o2,
147  const fcl::Vec3f& direction, T_OctreeReport& report, const HeuristicParam & params, const heuristic evaluate = 0);
148 
149  } // namespace sampling
150 } // namespace rbprm
151 } // namespace hpp
152 #endif // HPP_RBPRM_SAMPLEDB_HH
hpp::rbprm::sampling::T_VoxelSampleId
std::map< long int, VoxelSampleId > T_VoxelSampleId
Definition: sample-db.hh:79
hpp::rbprm::sampling::OctreeReport
Definition: sample-db.hh:37
hpp::rbprm::sampling::T_OctreeReport
std::multiset< OctreeReport, sample_compare > T_OctreeReport
Definition: sample-db.hh:61
hpp::rbprm::sampling::OctreeReport::OctreeReport
OctreeReport(const Sample *, const fcl::Contact, const double, const fcl::Vec3f &normal, const fcl::Vec3f &v1, const fcl::Vec3f &v2, const fcl::Vec3f &v3)
hpp::rbprm::sampling::SampleVector_t
std::vector< Sample, Eigen::aligned_allocator< Sample > > SampleVector_t
Definition: sample.hh:92
hpp::rbprm::sampling::SampleDB::boxes_
std::map< std::size_t, fcl::CollisionObject * > boxes_
Bounding boxes of areas of interest of the octree.
Definition: sample-db.hh:110
hpp::rbprm::sampling::OctreeReport::sample_
const Sample * sample_
Sample considered for contact generation.
Definition: sample-db.hh:42
hpp::rbprm::sampling::HeuristicParam
Defines a parameters set for the ZMP-based heuristic.
Definition: heuristic-tools.hh:13
hpp::rbprm::sampling::OctreeReport::v1_
fcl::Vec3f v1_
Definition: sample-db.hh:48
hpp::rbprm::sampling::evaluate
boost::function< double(const SampleDB &sampleDB, const sampling::Sample &sample) > evaluate
Definition: sample-db.hh:75
hpp::rbprm::sampling::OctreeReport::v3_
fcl::Vec3f v3_
Definition: sample-db.hh:48
hpp::rbprm::sampling::GetCandidates
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 &params, const heuristic evaluate=0)
hpp::rbprm::sampling::SampleDB::treeObject_
fcl::CollisionObject treeObject_
fcl collision object used for collisions with environment
Definition: sample-db.hh:108
hpp::rbprm::sampling::Sample
Definition: sample.hh:40
hpp::rbprm::sampling::SampleDB::octomapTree_
boost::shared_ptr< const octomap::OcTree > octomapTree_
Definition: sample-db.hh:101
hpp::rbprm::sampling::OctreeReport::contact_
fcl::Contact contact_
Contact information returned from fcl.
Definition: sample-db.hh:44
hpp::rbprm::sampling::T_Values
std::map< std::string, T_Double > T_Values
Definition: sample-db.hh:66
hpp::rbprm::sampling::SampleDB::resolution_
double resolution_
Definition: sample-db.hh:99
hpp::rbprm::sampling::sample_compare::operator()
bool operator()(const OctreeReport &lhs, const OctreeReport &rhs) const
Definition: sample-db.hh:56
hpp::rbprm::sampling::T_ValueBound
std::map< std::string, ValueBound > T_ValueBound
Definition: sample-db.hh:81
hpp::rbprm::sampling::SampleDB::geometry_
boost::shared_ptr< fcl::CollisionGeometry > geometry_
Definition: sample-db.hh:103
hpp::rbprm::sampling::addValue
HPP_RBPRM_DLLAPI SampleDB & addValue(SampleDB &database, const std::string &valueName, const evaluate eval, bool isStaticValue=true, bool sortSamples=true)
hpp
Definition: algorithm.hh:27
hpp::rbprm::sampling::sample_compare
Definition: sample-db.hh:55
hpp::rbprm::sampling::T_evaluate
std::map< std::string, evaluate > T_evaluate
Definition: sample-db.hh:76
hpp::rbprm::sampling::SampleDB::octree_
fcl::OcTree * octree_
Definition: sample-db.hh:102
sample.hh
hpp::rbprm::sampling::T_Sample
sampling::SampleVector_t T_Sample
Definition: sample-db.hh:67
hpp::rbprm::sampling::SampleDB::samplesInVoxels_
T_VoxelSampleId samplesInVoxels_
Definition: sample-db.hh:106
hpp::rbprm::sampling::heuristic
double(* heuristic)(const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam &params)
Definition: heuristic.hh:41
hpp::rbprm::sampling::T_Double
std::vector< double > T_Double
Definition: sample-db.hh:65
hpp::rbprm::sampling::SampleDB::samples_
T_Sample samples_
Definition: sample-db.hh:100
hpp::rbprm::sampling::SampleDB
Definition: sample-db.hh:86
hpp::rbprm::sampling::SampleDB::valueBounds_
T_ValueBound valueBounds_
Definition: sample-db.hh:105
hpp::rbprm::sampling::ValueBound
std::pair< double, double > ValueBound
Definition: sample-db.hh:80
hpp::rbprm::sampling::OctreeReport::value_
double value_
heuristic evaluation of the sample
Definition: sample-db.hh:46
hpp::rbprm::sampling::OctreeReport::normal_
fcl::Vec3f normal_
normal vector of the surface in contact
Definition: sample-db.hh:48
hpp::rbprm::sampling::OctreeReport::v2_
fcl::Vec3f v2_
Definition: sample-db.hh:48
hpp::rbprm::sampling::SampleDB::values_
T_Values values_
Definition: sample-db.hh:104
hpp::rbprm::sampling::saveLimbDatabase
HPP_RBPRM_DLLAPI bool saveLimbDatabase(const SampleDB &database, std::ofstream &dbFile)
hpp::rbprm::sampling::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(OctreeNode)
heuristic.hh
config.hh
hpp::rbprm::sampling::VoxelSampleId
std::pair< std::size_t, std::size_t > VoxelSampleId
Definition: sample-db.hh:78
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64