hpp-rbprm  4.10.1
Implementation of RB-PRM planner using hpp.
rbprm-fullbody.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_FULLBODY_HH
20 #define HPP_RBPRM_FULLBODY_HH
21 
22 #include <hpp/rbprm/config.hh>
23 #include <hpp/rbprm/rbprm-state.hh>
24 #include <hpp/pinocchio/device.hh>
25 #include <hpp/rbprm/rbprm-limb.hh>
26 #include <hpp/core/collision-validation.hh>
27 #include <hpp/core/problem-solver.hh>
29 #include <hpp/rbprm/reports.hh>
31 #include <vector>
32 
33 namespace hpp {
34 namespace rbprm {
35 
36 using core::size_type;
37 
38 HPP_PREDEF_CLASS(RbPrmFullBody);
39 
45 class RbPrmFullBody;
46 typedef boost::shared_ptr<RbPrmFullBody> RbPrmFullBodyPtr_t;
47 typedef hpp::core::Container<hpp::core::AffordanceObjects_t> affMap_t;
48 typedef std::map<std::string, std::vector<bezier_Ptr> > EffectorTrajectoriesMap_t;
49 
51  public:
52  static RbPrmFullBodyPtr_t create(const pinocchio::DevicePtr_t& device);
53 
54  public:
55  virtual ~RbPrmFullBody();
56 
57  public:
78  void AddLimb(const std::string& id, const std::string& name, const std::string& effectorName,
79  const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset, const fcl::Vec3f& normal, const double x,
80  const double y, const core::ObjectStdVector_t& collisionObjects, const std::size_t nbSamples,
81  const std::string& heuristic = "static", const double resolution = 0.03,
82  ContactType contactType = _6_DOF, const bool disableEffectorCollision = false, const bool grasp = false,
83  const std::string& kinematicConstraintsPath = std::string(), const double kinematicConstraintsMin = 0.);
84 
97  void AddLimb(const std::string& database, const std::string& id, const core::ObjectStdVector_t& collisionObjects,
98  const std::string& heuristicName, const bool loadValues, const bool disableEffectorCollision = false,
99  const bool grasp = false);
100 
109  void AddNonContactingLimb(const std::string& id, const std::string& name, const std::string& effectorName,
110  const hpp::core::ObjectStdVector_t& collisionObjects, const std::size_t nbSamples);
111 
117  bool AddHeuristic(const std::string& name, const sampling::heuristic func);
118 
119  public:
120  typedef std::map<std::string, std::vector<std::string> > T_LimbGroup;
121 
122  public:
123  const rbprm::T_Limb& GetLimbs() { return limbs_; }
124  const rbprm::RbPrmLimbPtr_t GetLimb(std::string name, bool onlyWithContact = false);
125  const rbprm::T_Limb& GetNonContactingLimbs() { return nonContactingLimbs_; }
126  const T_LimbGroup& GetGroups() { return limbGroups_; }
127  const core::CollisionValidationPtr_t& GetCollisionValidation() { return collisionValidation_; }
128  const std::map<std::string, core::CollisionValidationPtr_t>& GetLimbCollisionValidation() {
129  return limbcollisionValidations_;
130  }
131  const pinocchio::DevicePtr_t device_;
132  void staticStability(bool staticStability) { staticStability_ = staticStability; }
133  bool staticStability() const { return staticStability_; }
134  double getFriction() const { return mu_; }
135  void setFriction(double mu) { mu_ = mu; }
136  pinocchio::Configuration_t referenceConfig() { return reference_; }
137  void referenceConfig(pinocchio::Configuration_t referenceConfig);
138  pinocchio::Configuration_t postureWeights() { return postureWeights_; }
139  void postureWeights(pinocchio::Configuration_t postureWeights);
140  bool usePosturalTaskContactCreation() { return usePosturalTaskContactCreation_; }
141  void usePosturalTaskContactCreation(bool usePosturalTaskContactCreation) {
142  usePosturalTaskContactCreation_ = usePosturalTaskContactCreation;
143  }
144  bool addEffectorTrajectory(const size_t pathId, const std::string& effectorName, const bezier_Ptr& trajectory);
145  bool addEffectorTrajectory(const size_t pathId, const std::string& effectorName,
146  const std::vector<bezier_Ptr>& trajectories);
147  bool getEffectorsTrajectories(const size_t pathId, EffectorTrajectoriesMap_t& result);
148  bool getEffectorTrajectory(const size_t pathId, const std::string& effectorName, std::vector<bezier_Ptr>& result);
149  bool toggleNonContactingLimb(std::string name);
150 
151  private:
152  core::CollisionValidationPtr_t collisionValidation_;
153  std::map<std::string, core::CollisionValidationPtr_t> limbcollisionValidations_;
154  rbprm::T_Limb limbs_;
155  rbprm::T_Limb nonContactingLimbs_; // this is the list of limbs that are not used during contact generation.
156  T_LimbGroup limbGroups_;
158  bool staticStability_;
159  double mu_;
160  pinocchio::Configuration_t reference_;
161  pinocchio::Configuration_t postureWeights_; // weight used to compute the distance in the postural tasks
162  bool usePosturalTaskContactCreation_; // if true, during the contact creation the orientation of the feet along the
163  // contact normal is optimized for a postural task
164  std::map<size_t, EffectorTrajectoriesMap_t>
165  effectorsTrajectoriesMaps_; // the map link the pathIndex (the same as in the wholeBody paths in problem solver)
166  // to a map of trajectories for each effectors.
167  private:
168  void AddLimbPrivate(rbprm::RbPrmLimbPtr_t limb, const std::string& id, const std::string& name,
169  const hpp::core::ObjectStdVector_t& collisionObjects, const bool disableEffectorCollision,
170  const bool nonContactingLimb = false);
171 
172  protected:
173  RbPrmFullBody(const pinocchio::DevicePtr_t& device);
174 
178  void init(const RbPrmFullBodyWkPtr_t& weakPtr);
179 
180  private:
181  RbPrmFullBodyWkPtr_t weakPtr_;
182 }; // class RbPrmDevice
183 } // namespace rbprm
184 
185 } // namespace hpp
186 
187 #endif // HPP_RBPRM_DEVICE_HH
hpp::rbprm::RbPrmFullBody::device_
const pinocchio::DevicePtr_t device_
Definition: rbprm-fullbody.hh:131
rbprm-limb.hh
hpp::rbprm::RbPrmLimbPtr_t
boost::shared_ptr< RbPrmLimb > RbPrmLimbPtr_t
Definition: rbprm-limb.hh:41
hpp::rbprm::HPP_PREDEF_CLASS
HPP_PREDEF_CLASS(RbPrmFullBody)
hpp::rbprm::RbPrmFullBody::GetNonContactingLimbs
const rbprm::T_Limb & GetNonContactingLimbs()
Definition: rbprm-fullbody.hh:125
hpp::rbprm::RbPrmFullBody::usePosturalTaskContactCreation
bool usePosturalTaskContactCreation()
Definition: rbprm-fullbody.hh:140
rbprm-state.hh
hpp::rbprm::RbPrmFullBody::postureWeights
pinocchio::Configuration_t postureWeights()
Definition: rbprm-fullbody.hh:138
hpp::rbprm::sampling::HeuristicFactory
Definition: heuristic.hh:45
hpp::rbprm::RbPrmFullBody::usePosturalTaskContactCreation
void usePosturalTaskContactCreation(bool usePosturalTaskContactCreation)
Definition: rbprm-fullbody.hh:141
hpp::rbprm::RbPrmFullBody::getFriction
double getFriction() const
Definition: rbprm-fullbody.hh:134
hpp::rbprm::RbPrmFullBody::GetLimbs
const rbprm::T_Limb & GetLimbs()
Definition: rbprm-fullbody.hh:123
hpp::rbprm::RbPrmFullBody::GetGroups
const T_LimbGroup & GetGroups()
Definition: rbprm-fullbody.hh:126
hpp::rbprm::_6_DOF
@ _6_DOF
Definition: rbprm-limb.hh:31
hpp::rbprm::T_Limb
std::map< std::string, const rbprm::RbPrmLimbPtr_t > T_Limb
Definition: rbprm-limb.hh:43
hpp
Definition: algorithm.hh:27
hpp::rbprm::RbPrmFullBody::GetLimbCollisionValidation
const std::map< std::string, core::CollisionValidationPtr_t > & GetLimbCollisionValidation()
Definition: rbprm-fullbody.hh:128
hpp::rbprm::RbPrmFullBody
Definition: rbprm-fullbody.hh:50
bezier-path.hh
hpp::rbprm::ContactType
ContactType
Definition: rbprm-limb.hh:30
hpp::rbprm::sampling::heuristic
double(* heuristic)(const sampling::Sample &sample, const Eigen::Vector3d &direction, const Eigen::Vector3d &normal, const HeuristicParam &params)
Definition: heuristic.hh:39
hpp::rbprm::EffectorTrajectoriesMap_t
std::map< std::string, std::vector< bezier_Ptr > > EffectorTrajectoriesMap_t
Definition: rbprm-fullbody.hh:48
hpp::rbprm::RbPrmFullBody::staticStability
void staticStability(bool staticStability)
Definition: rbprm-fullbody.hh:132
hpp::rbprm::RbPrmFullBodyPtr_t
boost::shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
hpp::rbprm::RbPrmFullBody::setFriction
void setFriction(double mu)
Definition: rbprm-fullbody.hh:135
hpp::rbprm::RbPrmFullBody::referenceConfig
pinocchio::Configuration_t referenceConfig()
Definition: rbprm-fullbody.hh:136
hpp::rbprm::affMap_t
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
reports.hh
hpp::rbprm::RbPrmFullBody::T_LimbGroup
std::map< std::string, std::vector< std::string > > T_LimbGroup
Definition: rbprm-fullbody.hh:120
heuristic.hh
hpp::rbprm::RbPrmFullBody::GetCollisionValidation
const core::CollisionValidationPtr_t & GetCollisionValidation()
Definition: rbprm-fullbody.hh:127
config.hh
hpp::rbprm::bezier_Ptr
boost::shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:31
hpp::rbprm::RbPrmFullBody::staticStability
bool staticStability() const
Definition: rbprm-fullbody.hh:133
HPP_RBPRM_DLLAPI
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64