hpp-rbprm  4.10.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/rbprm/config.hh>
21 # include <hpp/rbprm/rbprm-device.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  {
35  fcl::Vec3f p1, p2, p3;
36  };
38  typedef boost::shared_ptr <RbPrmShooter>
40  typedef hpp::core::Container <hpp::core::AffordanceObjects_t> affMap_t;
41 
45  class HPP_RBPRM_DLLAPI RbPrmShooter : public core::ConfigurationShooter{
47  public:
66  const core::ObjectStdVector_t &geometries,
67  const affMap_t& affordances,
68  const std::vector<std::string>& filter = std::vector<std::string>(),
69  const std::map<std::string, std::vector<std::string> >& affFilters = std::map<std::string, std::vector<std::string> >(),
70  const std::size_t shootLimit = 10000,
71  const std::size_t displacementLimit = 100);
72 
73  public:
79  void BoundSO3(const std::vector<double>& limitszyx);
80 
81  void sampleExtraDOF(bool sampleExtraDOF);
82 
83  void ratioWeighted(double ratio){ratioWeighted_ = ratio;}
84 
85 
86  public:
87  typedef std::pair<fcl::Vec3f, TrianglePoints> T_TriangleNormal;
88 
89  public:
90  const std::size_t shootLimit_;
91  const std::size_t displacementLimit_;
92  const std::vector<std::string> filter_;
93 
94 
95  protected:
98  const core::ObjectStdVector_t &geometries,
99  const affMap_t &affordances,
100  const std::vector<std::string>& filter,
101  const std::map<std::string, std::vector<std::string> >& affFilters,
102  const std::size_t shootLimit = 10000,
103  const std::size_t displacementLimit = 100);
104 
105  void init (const RbPrmShooterPtr_t& self);
106 
107  virtual void impl_shoot (core::Configuration_t& q) const;
108 
109  private:
110  void InitWeightedTriangles (const core::ObjectStdVector_t &geometries);
111  const T_TriangleNormal& RandomPointIntriangle () const;
112  const T_TriangleNormal& WeightedTriangle () const;
113  void randConfigAtPos(const pinocchio::RbPrmDevicePtr_t robot, const std::vector<double>& eulerSo3, core::Configuration_t& config, const fcl::Vec3f p) const;
114 
115 
116  private:
117  std::vector<double> weights_;
118  std::vector<T_TriangleNormal> triangles_;
119  const pinocchio::RbPrmDevicePtr_t robot_;
120  rbprm::RbPrmValidationPtr_t validator_;
121  core::configurationShooter::UniformPtr_t uniformShooter_;
122  double ratioWeighted_;
123  RbPrmShooterWkPtr_t weak_;
124  //pinocchio::DevicePtr_t eulerSo3_;
125  std::vector<double> eulerSo3_;
126  }; // class RbprmShooter
128  } // namespace rbprm
129 } // namespace hpp
130 #endif // HPP_RBPRM_SHOOTER_HH
131 
hpp::rbprm::RbPrmShooter::T_TriangleNormal
std::pair< fcl::Vec3f, TrianglePoints > T_TriangleNormal
Definition: rbprm-shooter.hh:87
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:35
hpp::rbprm::RbPrmShooter::displacementLimit_
const std::size_t displacementLimit_
Definition: rbprm-shooter.hh:91
hpp::rbprm::RbPrmShooterPtr_t
boost::shared_ptr< RbPrmShooter > RbPrmShooterPtr_t
Definition: rbprm-shooter.hh:39
hpp::rbprm::TrianglePoints
Definition: rbprm-shooter.hh:33
hpp
Definition: algorithm.hh:27
hpp::rbprm::TrianglePoints::p3
fcl::Vec3f p3
Definition: rbprm-shooter.hh:35
hpp::rbprm::RbPrmShooter::shootLimit_
const std::size_t shootLimit_
Definition: rbprm-shooter.hh:90
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:83
hpp::rbprm::TrianglePoints::p2
fcl::Vec3f p2
Definition: rbprm-shooter.hh:35
hpp::rbprm::RbPrmShooter
Definition: rbprm-shooter.hh:45
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:92
config.hh
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64