hpp-rbprm  4.10.1
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/rbprm/config.hh>
23 #include <hpp/core/problem-solver.hh>
24 #include <hpp/pinocchio/joint.hh>
25 //# include <hpp/pinocchio/joint-configuration.hh>
26 #include <hpp/core/configuration-shooter.hh>
27 
28 #include <vector>
29 
30 namespace hpp {
31 namespace rbprm {
32 
34  fcl::Vec3f p1, p2, p3;
35 };
37 typedef boost::shared_ptr<RbPrmShooter> RbPrmShooterPtr_t;
38 typedef hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_t;
39 
43 class HPP_RBPRM_DLLAPI RbPrmShooter : public core::ConfigurationShooter {
45  public:
64  const core::ObjectStdVector_t& geometries,
65  const affMap_t& affordances,
66  const std::vector<std::string>& filter = std::vector<std::string>(),
67  const std::map<std::string, std::vector<std::string> >& affFilters =
68  std::map<std::string, std::vector<std::string> >(),
69  const std::size_t shootLimit = 10000,
70  const std::size_t displacementLimit = 100);
71 
72  public:
78  void BoundSO3(const std::vector<double>& limitszyx);
79 
80  void sampleExtraDOF(bool sampleExtraDOF);
81 
82  void ratioWeighted(double ratio) { ratioWeighted_ = ratio; }
83 
84  public:
85  typedef std::pair<fcl::Vec3f, TrianglePoints> T_TriangleNormal;
86 
87  public:
88  const std::size_t shootLimit_;
89  const std::size_t displacementLimit_;
90  const std::vector<std::string> filter_;
91 
92  protected:
94  RbPrmShooter(const pinocchio::RbPrmDevicePtr_t& robot, const core::ObjectStdVector_t& geometries,
95  const affMap_t& affordances, const std::vector<std::string>& filter,
96  const std::map<std::string, std::vector<std::string> >& affFilters,
97  const std::size_t shootLimit = 10000, const std::size_t displacementLimit = 100);
98 
99  void init(const RbPrmShooterPtr_t& self);
100 
101  virtual void impl_shoot(core::Configuration_t& q) const;
102 
103  private:
104  void InitWeightedTriangles(const core::ObjectStdVector_t& geometries);
105  const T_TriangleNormal& RandomPointIntriangle() const;
106  const T_TriangleNormal& WeightedTriangle() const;
107  void randConfigAtPos(const pinocchio::RbPrmDevicePtr_t robot, const std::vector<double>& eulerSo3,
108  core::Configuration_t& config, const fcl::Vec3f p) const;
109 
110  private:
111  std::vector<double> weights_;
112  std::vector<T_TriangleNormal> triangles_;
113  const pinocchio::RbPrmDevicePtr_t robot_;
114  rbprm::RbPrmValidationPtr_t validator_;
115  core::configurationShooter::UniformPtr_t uniformShooter_;
116  double ratioWeighted_;
117  RbPrmShooterWkPtr_t weak_;
118  // pinocchio::DevicePtr_t eulerSo3_;
119  std::vector<double> eulerSo3_;
120 }; // class RbprmShooter
122 } // namespace rbprm
123 } // namespace hpp
124 #endif // HPP_RBPRM_SHOOTER_HH
hpp::rbprm::RbPrmShooter::T_TriangleNormal
std::pair< fcl::Vec3f, TrianglePoints > T_TriangleNormal
Definition: rbprm-shooter.hh:85
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::RbPrmValidationPtr_t
boost::shared_ptr< RbPrmValidation > RbPrmValidationPtr_t
Definition: rbprm-validation.hh:34
rbprm-device.hh
hpp::rbprm::TrianglePoints::p1
fcl::Vec3f p1
Definition: rbprm-shooter.hh:34
hpp::rbprm::RbPrmShooter::displacementLimit_
const std::size_t displacementLimit_
Definition: rbprm-shooter.hh:89
hpp::rbprm::RbPrmShooterPtr_t
boost::shared_ptr< RbPrmShooter > RbPrmShooterPtr_t
Definition: rbprm-shooter.hh:37
hpp::rbprm::TrianglePoints
Definition: rbprm-shooter.hh:33
hpp
Definition: algorithm.hh:27
hpp::rbprm::TrianglePoints::p3
fcl::Vec3f p3
Definition: rbprm-shooter.hh:34
hpp::rbprm::RbPrmShooter::shootLimit_
const std::size_t shootLimit_
Definition: rbprm-shooter.hh:88
hpp::pinocchio::RbPrmDevicePtr_t
boost::shared_ptr< RbPrmDevice > RbPrmDevicePtr_t
Definition: dynamic-validation.hh:27
rbprm-validation.hh
hpp::rbprm::RbPrmShooter::ratioWeighted
void ratioWeighted(double ratio)
Definition: rbprm-shooter.hh:82
hpp::rbprm::TrianglePoints::p2
fcl::Vec3f p2
Definition: rbprm-shooter.hh:34
hpp::rbprm::RbPrmShooter
Definition: rbprm-shooter.hh:43
hpp::rbprm::affMap_t
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
hpp::rbprm::RbPrmShooter::filter_
const std::vector< std::string > filter_
Definition: rbprm-shooter.hh:90
config.hh
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64