hpp-rbprm  4.10.0
Implementation of RB-PRM planner using hpp.
rbprm-limb.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_LIMB_HH
20 # define HPP_RBPRM_LIMB_HH
21 
22 # include <hpp/rbprm/config.hh>
25 # include <hpp/pinocchio/device.hh>
26 
27 namespace hpp {
28  namespace rbprm {
29 
31  {
32  _6_DOF = 0, // Position and orientation are considered (surface contact)
33  _3_DOF = 1, // Only 3D position is considered (punctual contact)
35  };
36 
37 
38  HPP_PREDEF_CLASS(RbPrmLimb);
39 
43  class RbPrmLimb;
44  typedef boost::shared_ptr <RbPrmLimb> RbPrmLimbPtr_t;
45  typedef std::map<std::string, const rbprm::RbPrmLimbPtr_t > T_Limb;
46  typedef T_Limb::const_iterator CIT_Limb;
47  typedef Eigen::Matrix <pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic> MatrixXX;
48  typedef Eigen::Matrix <pinocchio::value_type, Eigen::Dynamic, 3> MatrixX3;
49  typedef Eigen::Matrix <pinocchio::value_type, Eigen::Dynamic, 1> VectorX;
50  typedef Eigen::Matrix <pinocchio::value_type, 3, 1> Vector3;
51 
52 
54  {
55  public:
73  /*static RbPrmLimbPtr_t create (const pinocchio::JointPtr_t limb, const fcl::Vec3f &offset,
74  const fcl::Vec3f &normal,const double x, const double y,
75  const std::size_t nbSamples, const sampling::heuristic evaluate = 0,
76  const double resolution = 0.1, ContactType contactType = _6_DOF);*/
77 
78 
96  static RbPrmLimbPtr_t create (const pinocchio::JointPtr_t limb, const std::string& effectorName, const fcl::Vec3f &offset,
97  const fcl::Vec3f &limbOffset, const fcl::Vec3f &normal,const double x, const double y,
98  const std::size_t nbSamples, const sampling::heuristic evaluate = 0,
99  const double resolution = 0.1, ContactType contactType = _6_DOF,
100  bool disableEndEffectorCollision = false,
101  bool grasps = false,
102  const std::string& kinematicsConstraintsPath = std::string(),
103  const double kinematicConstraintsMinDistance = 0.);
104 
105  static RbPrmLimbPtr_t create (const pinocchio::DevicePtr_t device, std::ifstream& fileStream, const bool loadValues = true,
107  bool disableEndEffectorCollision = false,
108  bool grasps = false);
109 
110  public:
111  ~RbPrmLimb();
112 
113  public:
114  pinocchio::Transform3f octreeRoot() const;
115 
116  public:
117  const pinocchio::JointPtr_t limb_;
118  const pinocchio::Frame effector_;
119  const fcl::Matrix3f effectorDefaultRotation_; // effector transform in rest pose
120  const fcl::Vec3f offset_; // effector location
121  const fcl::Vec3f limbOffset_; // offset of between the limb_ joint position and it's link
122  const fcl::Vec3f normal_; // effector normal for surface
123  const double x_; // half width
124  const double y_; // half length of contact surface
129  const bool grasps_;
131  std::pair<MatrixXX, MatrixXX> kinematicConstraints_;
132 
133  protected:
134 
135  RbPrmLimb (const pinocchio::JointPtr_t& limb, const std::string& effectorName, const fcl::Vec3f &offset,
136  const fcl::Vec3f &limbOffset, const fcl::Vec3f &normal,const double x, const double y,
137  const std::size_t nbSamples, const sampling::heuristic evaluate,
138  const double resolution, ContactType contactType,
139  bool disableEndEffectorCollision = false,
140  bool grasps = false,
141  const std::string& kinematicsConstraintsPath = std::string(),
142  const double kinematicConstraintsMinDistance = 0.);
143 
144  RbPrmLimb (const pinocchio::DevicePtr_t device, std::ifstream& fileStream, const bool loadValues,
146  bool disableEndEffectorCollision = false,
147  bool grasps = false);
151  void init (const RbPrmLimbWkPtr_t& weakPtr);
152 
153  private:
154  RbPrmLimbWkPtr_t weakPtr_;
155  }; // class RbPrmLimb
156 
157 
158  HPP_RBPRM_DLLAPI bool saveLimbInfoAndDatabase(const RbPrmLimbPtr_t limb, std::ofstream& dbFile);
159 
160  } // namespace rbprm
161 } // namespace hpp
162 
163 #endif // HPP_RBPRM_LIMB_HH
hpp::rbprm::RbPrmLimb::y_
const double y_
Definition: rbprm-limb.hh:124
hpp::rbprm::RbPrmLimbPtr_t
boost::shared_ptr< RbPrmLimb > RbPrmLimbPtr_t
Definition: rbprm-limb.hh:43
hpp::rbprm::RbPrmLimb::limb_
const pinocchio::JointPtr_t limb_
Definition: rbprm-limb.hh:117
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::Vector3
Eigen::Matrix< pinocchio::value_type, 3, 1 > Vector3
Definition: rbprm-limb.hh:50
hpp::rbprm::RbPrmLimb::sampleContainer_
sampling::SampleDB sampleContainer_
Definition: rbprm-limb.hh:127
hpp::rbprm::RbPrmLimb::contactType_
const ContactType contactType_
Definition: rbprm-limb.hh:125
hpp::rbprm::sampling::evaluate
boost::function< double(const SampleDB &sampleDB, const sampling::Sample &sample) > evaluate
Definition: sample-db.hh:75
hpp::rbprm::MatrixX3
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 3 > MatrixX3
Definition: rbprm-limb.hh:48
hpp::rbprm::CIT_Limb
T_Limb::const_iterator CIT_Limb
Definition: rbprm-limb.hh:46
hpp::rbprm::RbPrmLimb::evaluate_
sampling::heuristic evaluate_
Definition: rbprm-limb.hh:126
hpp::rbprm::VectorX
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: rbprm-limb.hh:49
hpp::rbprm::T_Limb
std::map< std::string, const rbprm::RbPrmLimbPtr_t > T_Limb
Definition: rbprm-limb.hh:45
hpp::rbprm::_6_DOF
@ _6_DOF
Definition: rbprm-limb.hh:32
hpp::rbprm::RbPrmLimb::grasps_
const bool grasps_
Definition: rbprm-limb.hh:129
hpp
Definition: algorithm.hh:27
hpp::rbprm::RbPrmLimb::offset_
const fcl::Vec3f offset_
Definition: rbprm-limb.hh:120
hpp::rbprm::RbPrmLimb::effectorReferencePosition_
fcl::Vec3f effectorReferencePosition_
Definition: rbprm-limb.hh:130
hpp::rbprm::ContactType
ContactType
Definition: rbprm-limb.hh:30
hpp::rbprm::RbPrmLimb::normal_
const fcl::Vec3f normal_
Definition: rbprm-limb.hh:122
hpp::rbprm::sampling::heuristic
double(* heuristic)(const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam &params)
Definition: heuristic.hh:41
hpp::rbprm::saveLimbInfoAndDatabase
HPP_RBPRM_DLLAPI bool saveLimbInfoAndDatabase(const RbPrmLimbPtr_t limb, std::ofstream &dbFile)
hpp::rbprm::RbPrmLimb::x_
const double x_
Definition: rbprm-limb.hh:123
sample-db.hh
hpp::rbprm::sampling::SampleDB
Definition: sample-db.hh:86
hpp::rbprm::RbPrmLimb::kinematicConstraints_
std::pair< MatrixXX, MatrixXX > kinematicConstraints_
Definition: rbprm-limb.hh:131
hpp::rbprm::RbPrmLimb::effector_
const pinocchio::Frame effector_
Definition: rbprm-limb.hh:118
hpp::rbprm::RbPrmLimb::disableEndEffectorCollision_
const bool disableEndEffectorCollision_
Definition: rbprm-limb.hh:128
hpp::rbprm::RbPrmLimb::effectorDefaultRotation_
const fcl::Matrix3f effectorDefaultRotation_
Definition: rbprm-limb.hh:119
hpp::rbprm::_3_DOF
@ _3_DOF
Definition: rbprm-limb.hh:33
hpp::rbprm::RbPrmLimb::limbOffset_
const fcl::Vec3f limbOffset_
Definition: rbprm-limb.hh:121
heuristic.hh
hpp::rbprm::RbPrmLimb
Definition: rbprm-limb.hh:53
config.hh
hpp::rbprm::_UNDEFINED
@ _UNDEFINED
Definition: rbprm-limb.hh:34
hpp::rbprm::MatrixXX
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: rbprm-limb.hh:47
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64