hpp-rbprm  4.14.0
Implementation of RB-PRM planner using hpp.
rbprm-shooter.hh
Go to the documentation of this file.
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Steve Tonneau (steve.tonneau@laas.fr)
3 //
4 // This file is part of hpp-rbprm.
5 // hpp-rbprm is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-rbprm is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-rbprm. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_RBPRM_SHOOTER_HH
18 #define HPP_RBPRM_SHOOTER_HH
19 
20 #include <hpp/core/problem-solver.hh>
21 #include <hpp/pinocchio/joint.hh>
22 #include <hpp/rbprm/config.hh>
25 //# include <hpp/pinocchio/joint-configuration.hh>
26 #include <hpp/core/configuration-shooter.hh>
27 #include <vector>
28 
29 namespace hpp {
30 namespace rbprm {
31 
33  fcl::Vec3f p1, p2, p3;
34 };
36 typedef shared_ptr<RbPrmShooter> RbPrmShooterPtr_t;
37 typedef hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_t;
38 
42 class HPP_RBPRM_DLLAPI RbPrmShooter : public core::ConfigurationShooter {
44  public:
68  const core::ObjectStdVector_t& geometries, const affMap_t& affordances,
69  const std::vector<std::string>& filter = std::vector<std::string>(),
70  const std::map<std::string, std::vector<std::string> >& affFilters =
71  std::map<std::string, std::vector<std::string> >(),
72  const std::size_t shootLimit = 10000,
73  const std::size_t displacementLimit = 100);
74 
75  public:
82  void BoundSO3(const std::vector<double>& limitszyx);
83 
84  void sampleExtraDOF(bool sampleExtraDOF);
85 
86  void ratioWeighted(double ratio) { ratioWeighted_ = ratio; }
87 
88  public:
89  typedef std::pair<fcl::Vec3f, TrianglePoints> T_TriangleNormal;
90 
91  public:
92  const std::size_t shootLimit_;
93  const std::size_t displacementLimit_;
94  const std::vector<std::string> filter_;
95 
96  protected:
99  const pinocchio::RbPrmDevicePtr_t& robot,
100  const core::ObjectStdVector_t& geometries, const affMap_t& affordances,
101  const std::vector<std::string>& filter,
102  const std::map<std::string, std::vector<std::string> >& affFilters,
103  const std::size_t shootLimit = 10000,
104  const std::size_t displacementLimit = 100);
105 
106  void init(const RbPrmShooterPtr_t& self);
107 
108  virtual void impl_shoot(core::Configuration_t& q) const;
109 
110  private:
111  void InitWeightedTriangles(const core::ObjectStdVector_t& geometries);
112  const T_TriangleNormal& RandomPointIntriangle() const;
113  const T_TriangleNormal& WeightedTriangle() const;
114  void randConfigAtPos(const pinocchio::RbPrmDevicePtr_t robot,
115  const std::vector<double>& eulerSo3,
116  core::Configuration_t& config, const fcl::Vec3f p) const;
117 
118  private:
119  std::vector<double> weights_;
120  std::vector<T_TriangleNormal> triangles_;
121  const pinocchio::RbPrmDevicePtr_t robot_;
122  rbprm::RbPrmValidationPtr_t validator_;
123  core::configurationShooter::UniformPtr_t uniformShooter_;
124  double ratioWeighted_;
125  RbPrmShooterWkPtr_t weak_;
126  // pinocchio::DevicePtr_t eulerSo3_;
127  std::vector<double> eulerSo3_;
128 }; // class RbprmShooter
130 } // namespace rbprm
131 } // namespace hpp
132 #endif // HPP_RBPRM_SHOOTER_HH
Definition: rbprm-shooter.hh:42
const std::vector< std::string > filter_
Definition: rbprm-shooter.hh:94
virtual void impl_shoot(core::Configuration_t &q) const
std::pair< fcl::Vec3f, TrianglePoints > T_TriangleNormal
Definition: rbprm-shooter.hh:89
RbPrmShooter(const pinocchio::RbPrmDevicePtr_t &robot, const core::ObjectStdVector_t &geometries, const affMap_t &affordances, const std::vector< std::string > &filter, const std::map< std::string, std::vector< std::string > > &affFilters, const std::size_t shootLimit=10000, const std::size_t displacementLimit=100)
Note that translation joints have to be bounded.
void BoundSO3(const std::vector< double > &limitszyx)
const std::size_t displacementLimit_
Definition: rbprm-shooter.hh:93
void sampleExtraDOF(bool sampleExtraDOF)
const std::size_t shootLimit_
Definition: rbprm-shooter.hh:92
static HPP_RBPRM_DLLAPI RbPrmShooterPtr_t create(const pinocchio::RbPrmDevicePtr_t &robot, const core::ObjectStdVector_t &geometries, const affMap_t &affordances, const std::vector< std::string > &filter=std::vector< std::string >(), const std::map< std::string, std::vector< std::string > > &affFilters=std::map< std::string, std::vector< std::string > >(), const std::size_t shootLimit=10000, const std::size_t displacementLimit=100)
void ratioWeighted(double ratio)
Definition: rbprm-shooter.hh:86
void init(const RbPrmShooterPtr_t &self)
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
shared_ptr< RbPrmDevice > RbPrmDevicePtr_t
Definition: dynamic-validation.hh:27
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
HPP_PREDEF_CLASS(RbPrmFullBody)
shared_ptr< RbPrmValidation > RbPrmValidationPtr_t
Definition: rbprm-validation.hh:34
shared_ptr< RbPrmShooter > RbPrmShooterPtr_t
Definition: rbprm-shooter.hh:36
Definition: algorithm.hh:26
Definition: rbprm-shooter.hh:32
fcl::Vec3f p1
Definition: rbprm-shooter.hh:33
fcl::Vec3f p2
Definition: rbprm-shooter.hh:33
fcl::Vec3f p3
Definition: rbprm-shooter.hh:33