hpp-rbprm
4.10.1
Implementation of RB-PRM planner using hpp.
|
Go to the documentation of this file.
19 #ifndef HPP_RBPRM_LIMB_HH
20 #define HPP_RBPRM_LIMB_HH
25 #include <hpp/pinocchio/device.hh>
43 typedef std::map<std::string, const rbprm::RbPrmLimbPtr_t>
T_Limb;
45 typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic>
MatrixXX;
46 typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, 3>
MatrixX3;
47 typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, 1>
VectorX;
48 typedef Eigen::Matrix<pinocchio::value_type, 3, 1>
Vector3;
91 static RbPrmLimbPtr_t create(
const pinocchio::JointPtr_t limb,
const std::string& effectorName,
92 const fcl::Vec3f& offset,
const fcl::Vec3f& limbOffset,
const fcl::Vec3f& normal,
93 const double x,
const double y,
const std::size_t nbSamples,
96 bool grasps =
false,
const std::string& kinematicsConstraintsPath = std::string(),
97 const double kinematicConstraintsMinDistance = 0.);
99 static RbPrmLimbPtr_t create(
const pinocchio::DevicePtr_t device, std::ifstream& fileStream,
101 bool disableEndEffectorCollision =
false,
bool grasps =
false);
107 pinocchio::Transform3f octreeRoot()
const;
127 RbPrmLimb(
const pinocchio::JointPtr_t& limb,
const std::string& effectorName,
const fcl::Vec3f& offset,
128 const fcl::Vec3f& limbOffset,
const fcl::Vec3f& normal,
const double x,
const double y,
130 ContactType contactType,
bool disableEndEffectorCollision =
false,
bool grasps =
false,
131 const std::string& kinematicsConstraintsPath = std::string(),
132 const double kinematicConstraintsMinDistance = 0.);
134 RbPrmLimb(
const pinocchio::DevicePtr_t device, std::ifstream& fileStream,
const bool loadValues,
136 bool grasps =
false);
140 void init(
const RbPrmLimbWkPtr_t& weakPtr);
143 RbPrmLimbWkPtr_t weakPtr_;
151 #endif // HPP_RBPRM_LIMB_HH
const double y_
Definition: rbprm-limb.hh:117
boost::shared_ptr< RbPrmLimb > RbPrmLimbPtr_t
Definition: rbprm-limb.hh:41
const pinocchio::JointPtr_t limb_
Definition: rbprm-limb.hh:110
HPP_PREDEF_CLASS(RbPrmFullBody)
Eigen::Matrix< pinocchio::value_type, 3, 1 > Vector3
Definition: rbprm-limb.hh:48
sampling::SampleDB sampleContainer_
Definition: rbprm-limb.hh:120
const ContactType contactType_
Definition: rbprm-limb.hh:118
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 3 > MatrixX3
Definition: rbprm-limb.hh:46
T_Limb::const_iterator CIT_Limb
Definition: rbprm-limb.hh:44
sampling::heuristic evaluate_
Definition: rbprm-limb.hh:119
boost::function< double(const SampleDB &sampleDB, const sampling::Sample &sample)> evaluate
Definition: sample-db.hh:70
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: rbprm-limb.hh:47
@ _6_DOF
Definition: rbprm-limb.hh:31
const bool grasps_
Definition: rbprm-limb.hh:122
std::map< std::string, const rbprm::RbPrmLimbPtr_t > T_Limb
Definition: rbprm-limb.hh:43
Definition: algorithm.hh:27
const fcl::Vec3f offset_
Definition: rbprm-limb.hh:113
fcl::Vec3f effectorReferencePosition_
Definition: rbprm-limb.hh:123
ContactType
Definition: rbprm-limb.hh:30
const fcl::Vec3f normal_
Definition: rbprm-limb.hh:115
double(* heuristic)(const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam ¶ms)
Definition: heuristic.hh:39
HPP_RBPRM_DLLAPI bool saveLimbInfoAndDatabase(const RbPrmLimbPtr_t limb, std::ofstream &dbFile)
const double x_
Definition: rbprm-limb.hh:116
Definition: sample-db.hh:81
std::pair< MatrixXX, MatrixXX > kinematicConstraints_
Definition: rbprm-limb.hh:124
const pinocchio::Frame effector_
Definition: rbprm-limb.hh:111
const bool disableEndEffectorCollision_
Definition: rbprm-limb.hh:121
const fcl::Matrix3f effectorDefaultRotation_
Definition: rbprm-limb.hh:112
@ _3_DOF
Definition: rbprm-limb.hh:32
const fcl::Vec3f limbOffset_
Definition: rbprm-limb.hh:114
Definition: rbprm-limb.hh:50
@ _UNDEFINED
Definition: rbprm-limb.hh:33
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic > MatrixXX
Definition: rbprm-limb.hh:45
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64