hpp-rbprm 4.14.0
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
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
29namespace hpp {
30namespace rbprm {
31
33 fcl::Vec3f p1, p2, p3;
34};
36typedef shared_ptr<RbPrmShooter> RbPrmShooterPtr_t;
37typedef hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_t;
38
42class 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_;
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:28
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:35
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