hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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/fcl/octree.h>
23
24#include <boost/function.hpp>
25#include <hpp/rbprm/config.hh>
28#include <map>
29#include <vector>
30
31namespace hpp {
32
33namespace rbprm {
34namespace sampling {
35
39 OctreeReport(const Sample*, const fcl::Contact, const double,
40 const fcl::Vec3f& normal, const fcl::Vec3f& v1,
41 const fcl::Vec3f& v2,
42 const fcl::Vec3f& v3); // vertices of triangle collided
46 fcl::Contact contact_;
48 double value_;
50 fcl::Vec3f normal_, v1_, v2_, v3_;
51};
52
57 bool operator()(const OctreeReport& lhs, const OctreeReport& rhs) const {
58 return lhs.value_ > rhs.value_;
59 }
60};
61
62typedef std::multiset<OctreeReport, sample_compare> T_OctreeReport;
63
65
66typedef std::vector<double> T_Double;
67typedef std::map<std::string, T_Double> T_Values;
69
74// typedef double (*evaluate) (const SampleDB& sampleDB, const sampling::Sample&
75// sample);
76typedef boost::function<double(const SampleDB& sampleDB,
77 const sampling::Sample& sample)>
79typedef std::map<std::string, evaluate> T_evaluate;
80// first sample index, number of samples
81typedef std::pair<std::size_t, std::size_t> VoxelSampleId;
82typedef std::map<long int, VoxelSampleId> T_VoxelSampleId;
83typedef std::pair<double, double> ValueBound;
84typedef std::map<std::string, ValueBound> T_ValueBound;
85
91 public:
92 SampleDB(std::ifstream& databaseStream, bool loadValues = true);
93 SampleDB(const pinocchio::JointPtr_t limb, const std::string& effector,
94 const std::size_t nbSamples,
95 const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
96 const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0),
97 const double resolution = 0.1, const T_evaluate& data = T_evaluate(),
98 const std::string& staticValue = "");
100 /*
101 private:
102 SampleDB(const SampleDB&);
103 const SampleDB& operator=(const SampleDB&);
104 */
105 public:
108 hpp::fcl::shared_ptr<const octomap::OcTree> octomapTree_;
109 fcl::OcTree* octree_; // deleted with geometry_
110 hpp::fcl::shared_ptr<fcl::CollisionGeometry> geometry_;
115 fcl::CollisionObject treeObject_;
117 std::map<std::size_t, fcl::CollisionObject*> boxes_;
118
119}; // class SampleDB
120
122 const std::string& valueName,
123 const evaluate eval,
124 bool isStaticValue = true,
125 bool sortSamples = true);
127 std::ofstream& dbFile);
128
140 const SampleDB& sc, const fcl::Transform3f& treeTrf,
141 const hpp::pinocchio::CollisionObjectPtr_t& o2, const fcl::Vec3f& direction,
142 const HeuristicParam& params, const heuristic evaluate = 0);
143
156 const SampleDB& sc, const fcl::Transform3f& treeTrf,
157 const hpp::pinocchio::CollisionObjectPtr_t& o2, const fcl::Vec3f& direction,
158 T_OctreeReport& report, const HeuristicParam& params,
159 const heuristic evaluate = 0);
160
161} // namespace sampling
162} // namespace rbprm
163} // namespace hpp
164#endif // HPP_RBPRM_SAMPLEDB_HH
Definition: sample-db.hh:90
fcl::CollisionObject treeObject_
fcl collision object used for collisions with environment
Definition: sample-db.hh:115
T_VoxelSampleId samplesInVoxels_
Definition: sample-db.hh:113
SampleDB(std::ifstream &databaseStream, bool loadValues=true)
SampleDB(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), const double resolution=0.1, const T_evaluate &data=T_evaluate(), const std::string &staticValue="")
std::map< std::size_t, fcl::CollisionObject * > boxes_
Bounding boxes of areas of interest of the octree.
Definition: sample-db.hh:117
hpp::fcl::shared_ptr< const octomap::OcTree > octomapTree_
Definition: sample-db.hh:108
T_ValueBound valueBounds_
Definition: sample-db.hh:112
double resolution_
Definition: sample-db.hh:106
hpp::fcl::shared_ptr< fcl::CollisionGeometry > geometry_
Definition: sample-db.hh:110
fcl::OcTree * octree_
Definition: sample-db.hh:109
T_Values values_
Definition: sample-db.hh:111
T_Sample samples_
Definition: sample-db.hh:107
Definition: sample.hh:39
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
double(* heuristic)(const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam &params)
Definition: heuristic.hh:38
std::map< std::string, ValueBound > T_ValueBound
Definition: sample-db.hh:84
HPP_RBPRM_DLLAPI SampleDB & addValue(SampleDB &database, const std::string &valueName, const evaluate eval, bool isStaticValue=true, bool sortSamples=true)
std::map< std::string, evaluate > T_evaluate
Definition: sample-db.hh:79
std::vector< double > T_Double
Definition: sample-db.hh:66
std::vector< Sample, Eigen::aligned_allocator< Sample > > SampleVector_t
Definition: sample.hh:100
std::multiset< OctreeReport, sample_compare > T_OctreeReport
Definition: sample-db.hh:62
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)
std::pair< std::size_t, std::size_t > VoxelSampleId
Definition: sample-db.hh:81
std::pair< double, double > ValueBound
Definition: sample-db.hh:83
boost::function< double(const SampleDB &sampleDB, const sampling::Sample &sample)> evaluate
Definition: sample-db.hh:78
HPP_RBPRM_DLLAPI bool saveLimbDatabase(const SampleDB &database, std::ofstream &dbFile)
std::map< std::string, T_Double > T_Values
Definition: sample-db.hh:67
sampling::SampleVector_t T_Sample
Definition: sample-db.hh:68
HPP_PREDEF_CLASS(OctreeNode)
std::map< long int, VoxelSampleId > T_VoxelSampleId
Definition: sample-db.hh:82
Definition: algorithm.hh:26
Defines a parameters set for the ZMP-based heuristic.
Definition: heuristic-tools.hh:13
Definition: sample-db.hh:38
double value_
heuristic evaluation of the sample
Definition: sample-db.hh:48
fcl::Vec3f v2_
Definition: sample-db.hh:50
fcl::Vec3f normal_
normal vector of the surface in contact
Definition: sample-db.hh:50
fcl::Contact contact_
Contact information returned from fcl.
Definition: sample-db.hh:46
OctreeReport(const Sample *, const fcl::Contact, const double, const fcl::Vec3f &normal, const fcl::Vec3f &v1, const fcl::Vec3f &v2, const fcl::Vec3f &v3)
const Sample * sample_
Sample considered for contact generation.
Definition: sample-db.hh:44
fcl::Vec3f v3_
Definition: sample-db.hh:50
fcl::Vec3f v1_
Definition: sample-db.hh:50
Definition: sample-db.hh:56
bool operator()(const OctreeReport &lhs, const OctreeReport &rhs) const
Definition: sample-db.hh:57