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