hpp-rbprm  4.14.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 #include <deque>
24 #include <hpp/pinocchio/device.hh>
25 #include <hpp/rbprm/config.hh>
26 namespace hpp {
27 
28 namespace rbprm {
29 namespace sampling {
31 
36 class Sample;
37 typedef shared_ptr<Sample> SamplePtr_t;
38 
40  public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44 
51  Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector,
52  const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
53  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,
57  const std::size_t startRank, const double staticValue,
58  const fcl::Vec3f& effectorPosition,
59  const fcl::Vec3f& effectorPositionInLimbFrame,
60  const pinocchio::ConfigurationIn_t configuration,
61  const Eigen::MatrixXd& jacobian,
62  const Eigen::Matrix<pinocchio::value_type, 6, 6>& jacobianProduct);
63 
70  Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector,
71  pinocchio::ConfigurationIn_t configuration,
72  const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
73  const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0),
74  const std::size_t id = 0);
75  Sample(const Sample& clone);
76  ~Sample() {}
77 
78  public:
79  std::size_t startRank_;
80  std::size_t length_;
81  pinocchio::Configuration_t configuration_;
83  fcl::Vec3f effectorPosition_;
85  Eigen::MatrixXd jacobian_;
87  Eigen::Matrix<pinocchio::value_type, 6, 6> jacobianProduct_;
89  std::size_t id_;
90  double staticValue_;
91  // const fcl::Transform3f rotation_; TODO
92 }; // class Sample
93 
95  bool operator()(const Sample& lhs, const Sample& rhs) const {
96  return lhs.staticValue_ > rhs.staticValue_;
97  }
98 };
99 
100 typedef std::vector<Sample, Eigen::aligned_allocator<Sample> > SampleVector_t;
109  const pinocchio::JointPtr_t limb, const std::string& effector,
110  const std::size_t nbSamples, const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
111  const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0));
112 
116 void Load(const Sample& sample, pinocchio::ConfigurationOut_t robot);
117 
118 } // namespace sampling
119 } // namespace rbprm
120 } // namespace hpp
121 #endif // HPP_RBPRM_SAMPLE_HH
Definition: sample.hh:39
double staticValue_
Definition: sample.hh:90
std::size_t length_
Definition: sample.hh:80
fcl::Vec3f effectorPosition_
Position relative to robot root (ie, robot base at 0 everywhere)
Definition: sample.hh:83
~Sample()
Definition: sample.hh:76
Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector, const fcl::Vec3f &offset=fcl::Vec3f(0, 0, 0), const fcl::Vec3f &limbOffset=fcl::Vec3f(0, 0, 0), const std::size_t id=0)
std::size_t id_
id in sample container
Definition: sample.hh:89
std::size_t startRank_
Definition: sample.hh:79
Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector, pinocchio::ConfigurationIn_t configuration, const fcl::Vec3f &offset=fcl::Vec3f(0, 0, 0), const fcl::Vec3f &limbOffset=fcl::Vec3f(0, 0, 0), const std::size_t id=0)
Sample(const std::size_t id, const std::size_t length, const std::size_t startRank, const double staticValue, const fcl::Vec3f &effectorPosition, const fcl::Vec3f &effectorPositionInLimbFrame, const pinocchio::ConfigurationIn_t configuration, const Eigen::MatrixXd &jacobian, const Eigen::Matrix< pinocchio::value_type, 6, 6 > &jacobianProduct)
Eigen::MatrixXd jacobian_
Definition: sample.hh:85
fcl::Vec3f effectorPositionInLimbFrame_
Definition: sample.hh:84
Eigen::Matrix< pinocchio::value_type, 6, 6 > jacobianProduct_
Product of the jacobian by its transpose.
Definition: sample.hh:87
Sample(const Sample &clone)
pinocchio::Configuration_t configuration_
Definition: sample.hh:81
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
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))
std::vector< Sample, Eigen::aligned_allocator< Sample > > SampleVector_t
Definition: sample.hh:100
shared_ptr< Sample > SamplePtr_t
Definition: sample.hh:36
void Load(const Sample &sample, pinocchio::ConfigurationOut_t robot)
HPP_PREDEF_CLASS(OctreeNode)
Definition: algorithm.hh:26
Definition: sample.hh:94
bool operator()(const Sample &lhs, const Sample &rhs) const
Definition: sample.hh:95