hpp-core  5.0.0
Implement basic classes for canonical path planning for kinematic chains.
problem.hh
Go to the documentation of this file.
1 
2 //
3 // Copyright (c) 2005, 2006, 2007, 2008, 2009, 2010, 2011 CNRS
4 // Authors: Florent Lamiraux
5 //
6 
7 // Redistribution and use in source and binary forms, with or without
8 // modification, are permitted provided that the following conditions are
9 // met:
10 //
11 // 1. Redistributions of source code must retain the above copyright
12 // notice, this list of conditions and the following disclaimer.
13 //
14 // 2. Redistributions in binary form must reproduce the above copyright
15 // notice, this list of conditions and the following disclaimer in the
16 // documentation and/or other materials provided with the distribution.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
22 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
23 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
24 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
26 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
27 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
28 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
29 // DAMAGE.
30 
31 #ifndef HPP_CORE_PROBLEM_HH
32 #define HPP_CORE_PROBLEM_HH
33 
34 #include <hpp/core/config.hh>
35 #include <hpp/core/container.hh>
36 #include <hpp/core/parameter.hh>
38 #include <hpp/pinocchio/device.hh>
39 #include <hpp/util/pointer.hh>
40 #include <stdexcept>
41 
42 namespace hpp {
43 namespace core {
46 
58  public:
61  static ProblemPtr_t create(DevicePtr_t robot);
62 
65  static ProblemPtr_t createCopy(const ProblemConstPtr_t& other);
66 
70  Problem();
71 
72  virtual ~Problem();
73 
76 
78  const DevicePtr_t& robot() const { return robot_; }
79 
81  const Configuration_t& initConfig() const { return initConf_; }
83  void initConfig(ConfigurationIn_t inConfig);
85  void target(const ProblemTargetPtr_t& target) { target_ = target; }
87  const ProblemTargetPtr_t& target() const { return target_; }
92  const Configurations_t goalConfigs() const;
97  void addGoalConfig(ConfigurationIn_t config);
102  void resetGoalConfigs();
103 
105 
108 
114  steeringMethod_ = sm;
115  if (constraints_) steeringMethod_->constraints(constraints_);
116  }
117 
119  SteeringMethodPtr_t steeringMethod() const { return steeringMethod_; }
120 
122  void distance(const DistancePtr_t& distance) { distance_ = distance; }
124  const DistancePtr_t& distance() const { return distance_; }
126 
129 
133  void configValidation(const ConfigValidationsPtr_t& configValidations) {
134  configValidations_ = configValidations;
135  }
138  return configValidations_;
139  }
140 
143  void clearConfigValidations();
144 
146  void addConfigValidation(const ConfigValidationPtr_t& configValidation);
147 
151  virtual void pathValidation(const PathValidationPtr_t& pathValidation);
152 
154  PathValidationPtr_t pathValidation() const { return pathValidation_; }
156 
160  void configurationShooter(
161  const ConfigurationShooterPtr_t& configurationShooter);
162 
165  return configurationShooter_;
166  }
168 
172  void pathProjector(const PathProjectorPtr_t& pathProjector) {
173  pathProjector_ = pathProjector;
174  }
175 
177  PathProjectorPtr_t pathProjector() const { return pathProjector_; }
179 
182 
187  void constraints(const ConstraintSetPtr_t& constraints) {
188  constraints_ = constraints;
189  if (steeringMethod_) steeringMethod_->constraints(constraints);
190  }
191 
193  const ConstraintSetPtr_t& constraints() const { return constraints_; }
195 
197  virtual void checkProblem() const;
198 
201 
204  void addObstacle(const CollisionObjectPtr_t& object);
208  void removeObstacleFromJoint(const JointPtr_t& joint,
209  const CollisionObjectConstPtr_t& obstacle);
210 
228  void filterCollisionPairs();
229 
231  void setSecurityMargins(const matrix_t& securityMatrix);
232 
234  const ObjectStdVector_t& collisionObstacles() const;
236  void collisionObstacles(const ObjectStdVector_t& collisionObstacles);
238 
242  const Parameter& getParameter(const std::string& name) const {
243  if (parameters.has(name))
244  return parameters.get(name);
245  else
246  return parameterDescription(name).defaultValue();
247  }
248 
255  void setParameter(const std::string& name, const Parameter& value);
256 
268  static void declareParameter(const ParameterDescription& desc);
269 
271  static const Container<ParameterDescription>& parameterDescriptions();
272 
274  static const ParameterDescription& parameterDescription(
275  const std::string& name);
276 
278 
279  protected:
281  Problem(DevicePtr_t robot);
283  Problem(const Problem& other) = default;
284 
285  void init(ProblemWkPtr_t wkPtr);
286 
287  private:
288  ProblemWkPtr_t wkPtr_;
290  DevicePtr_t robot_;
292  DistancePtr_t distance_;
294  Configuration_t initConf_;
296  ProblemTargetPtr_t target_;
298  SteeringMethodPtr_t steeringMethod_;
300  ConfigValidationsPtr_t configValidations_;
302  PathValidationPtr_t pathValidation_;
304  PathProjectorPtr_t pathProjector_;
306  ObjectStdVector_t collisionObstacles_;
308  ConstraintSetPtr_t constraints_;
310  ConfigurationShooterPtr_t configurationShooter_;
311 }; // class Problem
313 } // namespace core
314 } // namespace hpp
315 
316 #define HPP_START_PARAMETER_DECLARATION(name) \
317  struct HPP_CORE_DLLAPI __InitializerClass_##name { \
318  __InitializerClass_##name() {
319 #define HPP_END_PARAMETER_DECLARATION(name) \
320  } \
321  } \
322  ; \
323  extern "C" { \
324  __InitializerClass_##name __instance_##name; \
325  }
326 
327 #endif // HPP_CORE_PROBLEM_HH
const DistancePtr_t & distance() const
Get distance between configuration.
Definition: problem.hh:124
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition: fwd.hh:113
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:134
void steeringMethod(const SteeringMethodPtr_t &sm)
Definition: problem.hh:113
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:108
Definition: problem.hh:57
Definition: bi-rrt-planner.hh:35
ConfigurationShooterPtr_t configurationShooter() const
Get path validation method.
Definition: problem.hh:164
const ConfigValidationsPtr_t & configValidations() const
Get configuration validation methods.
Definition: problem.hh:137
Container< Parameter > parameters
Definition: problem.hh:277
PathValidationPtr_t pathValidation() const
Get path validation method.
Definition: problem.hh:154
Definition: parameter.hh:128
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:197
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:99
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:151
Definition: container.hh:76
void configValidation(const ConfigValidationsPtr_t &configValidations)
Definition: problem.hh:133
shared_ptr< ConfigValidation > ConfigValidationPtr_t
Definition: fwd.hh:115
PathProjectorPtr_t pathProjector() const
Get path projector method.
Definition: problem.hh:177
const Configuration_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem.hh:81
const ProblemTargetPtr_t & target() const
Get the target.
Definition: problem.hh:87
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:323
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:141
Definition: parameter.hh:63
SteeringMethodPtr_t steeringMethod() const
Get steering method.
Definition: problem.hh:119
void constraints(const ConstraintSetPtr_t &constraints)
Definition: problem.hh:187
pinocchio::matrix_t matrix_t
Definition: fwd.hh:162
shared_ptr< ConfigValidations > ConfigValidationsPtr_t
Definition: fwd.hh:116
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:130
void target(const ProblemTargetPtr_t &target)
Set the target.
Definition: problem.hh:85
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:196
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:213
void distance(const DistancePtr_t &distance)
Set distance between configurations.
Definition: problem.hh:122
std::vector< Configuration_t > Configurations_t
Definition: fwd.hh:110
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:192
void pathProjector(const PathProjectorPtr_t &pathProjector)
Definition: problem.hh:172
const Parameter & getParameter(const std::string &name) const
Definition: problem.hh:242
#define HPP_CORE_DLLAPI
Definition: config.hh:88
pinocchio::Configuration_t Configuration_t
Definition: fwd.hh:107
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition: problem.hh:193
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:100
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:307
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:184
const DevicePtr_t & robot() const
return shared pointer to robot.
Definition: problem.hh:78