hpp-rbprm  4.12.0
Implementation of RB-PRM planner using hpp.
sample.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_SAMPLE_HH
20 #define HPP_RBPRM_SAMPLE_HH
21 
22 #include <Eigen/StdVector>
23 
24 #include <hpp/rbprm/config.hh>
25 #include <hpp/pinocchio/device.hh>
26 
27 #include <deque>
28 namespace hpp {
29 
30 namespace rbprm {
31 namespace sampling {
32 HPP_PREDEF_CLASS(Sample);
33 
37 class Sample;
38 typedef std::shared_ptr<Sample> SamplePtr_t;
39 
41  public:
43  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 
52  Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector,
53  const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0), const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0),
54  const std::size_t id = 0);
55 
56  Sample(const std::size_t id, const std::size_t length, const std::size_t startRank, const double staticValue,
57  const fcl::Vec3f& effectorPosition, const fcl::Vec3f& effectorPositionInLimbFrame,
58  const pinocchio::ConfigurationIn_t configuration, const Eigen::MatrixXd& jacobian,
59  const Eigen::Matrix<pinocchio::value_type, 6, 6>& jacobianProduct);
60 
67  Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector, pinocchio::ConfigurationIn_t configuration,
68  const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0), const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0),
69  const std::size_t id = 0);
70  Sample(const Sample& clone);
71  ~Sample() {}
72 
73  public:
74  std::size_t startRank_;
75  std::size_t length_;
76  pinocchio::Configuration_t configuration_;
78  fcl::Vec3f effectorPosition_;
80  Eigen::MatrixXd jacobian_;
82  Eigen::Matrix<pinocchio::value_type, 6, 6> jacobianProduct_;
84  std::size_t id_;
85  double staticValue_;
86  // const fcl::Transform3f rotation_; TODO
87 }; // class Sample
88 
90  bool operator()(const Sample& lhs, const Sample& rhs) const { return lhs.staticValue_ > rhs.staticValue_; }
91 };
92 
93 typedef std::vector<Sample, Eigen::aligned_allocator<Sample> > SampleVector_t;
101 SampleVector_t GenerateSamples(const pinocchio::JointPtr_t limb, const std::string& effector,
102  const std::size_t nbSamples, const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
103  const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0));
104 
108 void Load(const Sample& sample, pinocchio::ConfigurationOut_t robot);
109 
110 } // namespace sampling
111 } // namespace rbprm
112 } // namespace hpp
113 #endif // HPP_RBPRM_SAMPLE_HH
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
Definition: sample.hh:40
std::size_t id_
id in sample container
Definition: sample.hh:84
fcl::Vec3f effectorPositionInLimbFrame_
Definition: sample.hh:79
Definition: algorithm.hh:27
HPP_PREDEF_CLASS(OctreeNode)
Definition: sample.hh:89
pinocchio::Configuration_t configuration_
Definition: sample.hh:76
void Load(const Sample &sample, pinocchio::ConfigurationOut_t robot)
double staticValue_
Definition: sample.hh:85
std::size_t startRank_
Definition: sample.hh:74
fcl::Vec3f effectorPosition_
Position relative to robot root (ie, robot base at 0 everywhere)
Definition: sample.hh:78
std::shared_ptr< Sample > SamplePtr_t
Definition: sample.hh:37
Eigen::Matrix< pinocchio::value_type, 6, 6 > jacobianProduct_
Product of the jacobian by its transpose.
Definition: sample.hh:82
bool operator()(const Sample &lhs, const Sample &rhs) const
Definition: sample.hh:90
Eigen::MatrixXd jacobian_
Definition: sample.hh:80
std::size_t length_
Definition: sample.hh:75
SampleVector_t GenerateSamples(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))
~Sample()
Definition: sample.hh:71
std::vector< Sample, Eigen::aligned_allocator< Sample > > SampleVector_t
Definition: sample.hh:93