hpp-rbprm  4.15.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/core/collision-validation.hh>
23 #include <hpp/core/problem-solver.hh>
24 #include <hpp/pinocchio/device.hh>
25 #include <hpp/rbprm/config.hh>
27 #include <hpp/rbprm/rbprm-limb.hh>
28 #include <hpp/rbprm/rbprm-state.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 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> >
50 
52  public:
53  static RbPrmFullBodyPtr_t create(const pinocchio::DevicePtr_t& device);
54 
55  public:
56  virtual ~RbPrmFullBody();
57 
58  public:
83  void AddLimb(const std::string& id, const std::string& name,
84  const std::string& effectorName, const fcl::Vec3f& offset,
85  const fcl::Vec3f& limbOffset, const fcl::Vec3f& normal,
86  const double x, const double y,
87  const core::ObjectStdVector_t& collisionObjects,
88  const std::size_t nbSamples,
89  const std::string& heuristic = "static",
90  const double resolution = 0.03, ContactType contactType = _6_DOF,
91  const bool disableEffectorCollision = false,
92  const bool grasp = false,
93  const std::string& kinematicConstraintsPath = std::string(),
94  const double kinematicConstraintsMin = 0.);
95 
111  void AddLimb(const std::string& database, const std::string& id,
112  const core::ObjectStdVector_t& collisionObjects,
113  const std::string& heuristicName, const bool loadValues,
114  const bool disableEffectorCollision = false,
115  const bool grasp = false);
116 
127  const std::string& id, const std::string& name,
128  const std::string& effectorName,
129  const hpp::core::ObjectStdVector_t& collisionObjects,
130  const std::size_t nbSamples);
131 
138  bool AddHeuristic(const std::string& name, const sampling::heuristic func);
139 
140  public:
141  typedef std::map<std::string, std::vector<std::string> > T_LimbGroup;
142 
143  public:
144  const rbprm::T_Limb& GetLimbs() { return limbs_; }
145  const rbprm::RbPrmLimbPtr_t GetLimb(std::string name,
146  bool onlyWithContact = false);
147  const rbprm::T_Limb& GetNonContactingLimbs() { return nonContactingLimbs_; }
148  const T_LimbGroup& GetGroups() { return limbGroups_; }
149  const core::CollisionValidationPtr_t& GetCollisionValidation() {
150  return collisionValidation_;
151  }
152  const std::map<std::string, core::CollisionValidationPtr_t>&
154  return limbcollisionValidations_;
155  }
156  const pinocchio::DevicePtr_t device_;
157  void staticStability(bool staticStability) {
158  staticStability_ = staticStability;
159  }
160  bool staticStability() const { return staticStability_; }
161  double getFriction() const { return mu_; }
162  void setFriction(double mu) { mu_ = mu; }
163  pinocchio::Configuration_t referenceConfig() { return reference_; }
164  void referenceConfig(pinocchio::Configuration_t referenceConfig);
165  pinocchio::Configuration_t postureWeights() { return postureWeights_; }
166  void postureWeights(pinocchio::Configuration_t postureWeights);
168  return usePosturalTaskContactCreation_;
169  }
170  void usePosturalTaskContactCreation(bool usePosturalTaskContactCreation) {
171  usePosturalTaskContactCreation_ = usePosturalTaskContactCreation;
172  }
173  bool addEffectorTrajectory(const size_t pathId,
174  const std::string& effectorName,
175  const bezier_Ptr& trajectory);
176  bool addEffectorTrajectory(const size_t pathId,
177  const std::string& effectorName,
178  const std::vector<bezier_Ptr>& trajectories);
179  bool getEffectorsTrajectories(const size_t pathId,
180  EffectorTrajectoriesMap_t& result);
181  bool getEffectorTrajectory(const size_t pathId,
182  const std::string& effectorName,
183  std::vector<bezier_Ptr>& result);
184  bool toggleNonContactingLimb(std::string name);
185 
186  private:
187  core::CollisionValidationPtr_t collisionValidation_;
188  std::map<std::string, core::CollisionValidationPtr_t>
189  limbcollisionValidations_;
190  rbprm::T_Limb limbs_;
191  rbprm::T_Limb nonContactingLimbs_; // this is the list of limbs that are not
192  // used during contact generation.
193  T_LimbGroup limbGroups_;
195  bool staticStability_;
196  double mu_;
197  pinocchio::Configuration_t reference_;
198  pinocchio::Configuration_t postureWeights_; // weight used to compute the
199  // distance in the postural tasks
200  bool usePosturalTaskContactCreation_; // if true, during the contact creation
201  // the orientation of the feet along
202  // the contact normal is optimized for
203  // a postural task
204  std::map<size_t, EffectorTrajectoriesMap_t>
205  effectorsTrajectoriesMaps_; // the map link the pathIndex (the same as in
206  // the wholeBody paths in problem solver) to
207  // a map of trajectories for each effectors.
208  private:
209  void AddLimbPrivate(rbprm::RbPrmLimbPtr_t limb, const std::string& id,
210  const std::string& name,
211  const hpp::core::ObjectStdVector_t& collisionObjects,
212  const bool disableEffectorCollision,
213  const bool nonContactingLimb = false);
214 
215  protected:
216  RbPrmFullBody(const pinocchio::DevicePtr_t& device);
217 
221  void init(const RbPrmFullBodyWkPtr_t& weakPtr);
222 
223  private:
224  RbPrmFullBodyWkPtr_t weakPtr_;
225 }; // class RbPrmDevice
226 } // namespace rbprm
227 
228 } // namespace hpp
229 
230 #endif // HPP_RBPRM_DEVICE_HH
Definition: rbprm-fullbody.hh:51
void AddNonContactingLimb(const std::string &id, const std::string &name, const std::string &effectorName, const hpp::core::ObjectStdVector_t &collisionObjects, const std::size_t nbSamples)
AddNonContactingLimb add a limb not used for contact generation.
std::map< std::string, std::vector< std::string > > T_LimbGroup
Definition: rbprm-fullbody.hh:141
pinocchio::Configuration_t postureWeights()
Definition: rbprm-fullbody.hh:165
const std::map< std::string, core::CollisionValidationPtr_t > & GetLimbCollisionValidation()
Definition: rbprm-fullbody.hh:153
bool addEffectorTrajectory(const size_t pathId, const std::string &effectorName, const std::vector< bezier_Ptr > &trajectories)
void staticStability(bool staticStability)
Definition: rbprm-fullbody.hh:157
void init(const RbPrmFullBodyWkPtr_t &weakPtr)
Initialization.
const rbprm::T_Limb & GetNonContactingLimbs()
Definition: rbprm-fullbody.hh:147
bool staticStability() const
Definition: rbprm-fullbody.hh:160
static RbPrmFullBodyPtr_t create(const pinocchio::DevicePtr_t &device)
const rbprm::RbPrmLimbPtr_t GetLimb(std::string name, bool onlyWithContact=false)
bool toggleNonContactingLimb(std::string name)
void postureWeights(pinocchio::Configuration_t postureWeights)
const T_LimbGroup & GetGroups()
Definition: rbprm-fullbody.hh:148
void setFriction(double mu)
Definition: rbprm-fullbody.hh:162
bool AddHeuristic(const std::string &name, const sampling::heuristic func)
void AddLimb(const std::string &database, const std::string &id, const core::ObjectStdVector_t &collisionObjects, const std::string &heuristicName, const bool loadValues, const bool disableEffectorCollision=false, const bool grasp=false)
const core::CollisionValidationPtr_t & GetCollisionValidation()
Definition: rbprm-fullbody.hh:149
double getFriction() const
Definition: rbprm-fullbody.hh:161
bool getEffectorsTrajectories(const size_t pathId, EffectorTrajectoriesMap_t &result)
bool addEffectorTrajectory(const size_t pathId, const std::string &effectorName, const bezier_Ptr &trajectory)
const rbprm::T_Limb & GetLimbs()
Definition: rbprm-fullbody.hh:144
void usePosturalTaskContactCreation(bool usePosturalTaskContactCreation)
Definition: rbprm-fullbody.hh:170
bool usePosturalTaskContactCreation()
Definition: rbprm-fullbody.hh:167
bool getEffectorTrajectory(const size_t pathId, const std::string &effectorName, std::vector< bezier_Ptr > &result)
void AddLimb(const std::string &id, const std::string &name, const std::string &effectorName, const fcl::Vec3f &offset, const fcl::Vec3f &limbOffset, const fcl::Vec3f &normal, const double x, const double y, const core::ObjectStdVector_t &collisionObjects, const std::size_t nbSamples, const std::string &heuristic="static", const double resolution=0.03, ContactType contactType=_6_DOF, const bool disableEffectorCollision=false, const bool grasp=false, const std::string &kinematicConstraintsPath=std::string(), const double kinematicConstraintsMin=0.)
void referenceConfig(pinocchio::Configuration_t referenceConfig)
RbPrmFullBody(const pinocchio::DevicePtr_t &device)
pinocchio::Configuration_t referenceConfig()
Definition: rbprm-fullbody.hh:163
const pinocchio::DevicePtr_t device_
Definition: rbprm-fullbody.hh:156
#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
hpp::core::Container< hpp::core::AffordanceObjects_t > affMap_t
Definition: rbprm-fullbody.hh:47
HPP_PREDEF_CLASS(RbPrmFullBody)
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:11
shared_ptr< bezier_t > bezier_Ptr
Definition: bezier-path.hh:32
std::map< std::string, std::vector< bezier_Ptr > > EffectorTrajectoriesMap_t
Definition: rbprm-fullbody.hh:49
ContactType
Definition: rbprm-limb.hh:30
@ _6_DOF
Definition: rbprm-limb.hh:31
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
Definition: heuristic.hh:47