hpp-core  4.11.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 // This file is part of hpp-core
7 // hpp-core is free software: you can redistribute it
8 // and/or modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation, either version
10 // 3 of the License, or (at your option) any later version.
11 //
12 // hpp-core is distributed in the hope that it will be
13 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Lesser Public License for more details. You should have
16 // received a copy of the GNU Lesser General Public License along with
17 // hpp-core If not, see
18 // <http://www.gnu.org/licenses/>.
19 
20 #ifndef HPP_CORE_PROBLEM_HH
21 # define HPP_CORE_PROBLEM_HH
22 
23 # include <stdexcept>
24 
25 # include <hpp/pinocchio/device.hh>
26 # include <hpp/util/pointer.hh>
27 
28 # include <hpp/core/config.hh>
30 # include <hpp/core/container.hh>
31 # include <hpp/core/parameter.hh>
32 
33 namespace hpp {
34  namespace core {
37 
49  {
50  public:
53  static ProblemPtr_t create (DevicePtr_t robot);
54 
57  static ProblemPtr_t createCopy(const ProblemConstPtr_t& other);
58 
62  Problem ();
63 
64  virtual ~Problem ();
65 
68 
70  const DevicePtr_t& robot () const {
71  return robot_;
72  }
73 
76  {
77  return initConf_;
78  }
80  void initConfig (const ConfigurationPtr_t& inConfig);
82  void target (const ProblemTargetPtr_t& target)
83  {
84  target_ = target;
85  }
87  const ProblemTargetPtr_t& target () const
88  {
89  return target_;
90  }
92  const Configurations_t& goalConfigs () const;
94  void addGoalConfig (const ConfigurationPtr_t& config);
96  void resetGoalConfigs ();
97 
99 
102 
108  steeringMethod_ = sm;
109  if (constraints_) steeringMethod_->constraints (constraints_);
110  }
111 
114  return steeringMethod_;
115  }
116 
118  void distance (const DistancePtr_t& distance)
119  {
120  distance_ = distance;
121  }
123  const DistancePtr_t& distance () const
124  {
125  return distance_;
126  }
128 
131 
135  void configValidation (const ConfigValidationsPtr_t& configValidations)
136  {
137  configValidations_ = configValidations;
138  }
141  {
142  return configValidations_;
143  }
144 
148  void resetConfigValidations () HPP_CORE_DEPRECATED;
149 
152  void clearConfigValidations ();
153 
155  void addConfigValidation (const ConfigValidationPtr_t& configValidation);
156 
160  virtual void pathValidation (const PathValidationPtr_t& pathValidation);
161 
164  {
165  return pathValidation_;
166  }
168 
169 
173  void configurationShooter (const ConfigurationShooterPtr_t& configurationShooter);
174 
177  {
178  return configurationShooter_;
179  }
181 
185  void pathProjector (const PathProjectorPtr_t& pathProjector)
186  {
187  pathProjector_ = pathProjector;
188  }
189 
192  {
193  return pathProjector_;
194  }
196 
199 
204  void constraints (const ConstraintSetPtr_t& constraints)
205  {
206  constraints_ = constraints;
207  if (steeringMethod_) steeringMethod_->constraints (constraints);
208  }
209 
212  {
213  return constraints_;
214  }
216 
218  virtual void checkProblem () const;
219 
222 
225  void addObstacle (const CollisionObjectPtr_t &object);
229  void removeObstacleFromJoint (const JointPtr_t& joint,
230  const CollisionObjectConstPtr_t& obstacle);
231 
249  void filterCollisionPairs ();
250 
252  void setSecurityMargins(const matrix_t& securityMatrix);
253 
255  const ObjectStdVector_t &collisionObstacles() const;
257  void collisionObstacles (const ObjectStdVector_t &collisionObstacles);
259 
263  const Parameter& getParameter (const std::string& name) const
264  {
265  if (parameters.has(name))
266  return parameters.get(name);
267  else
268  return parameterDescription(name).defaultValue();
269  }
270 
277  void setParameter (const std::string& name, const Parameter& value);
278 
290  static void declareParameter (const ParameterDescription& desc);
291 
293  static const Container<ParameterDescription>& parameterDescriptions ();
294 
296  static const ParameterDescription& parameterDescription (const std::string& name);
297 
299 
300  protected:
302  Problem (DevicePtr_t robot);
304  Problem (const Problem& other) = default;
305 
306  void init (ProblemWkPtr_t wkPtr);
307 
308  private :
309  ProblemWkPtr_t wkPtr_;
311  DevicePtr_t robot_;
313  DistancePtr_t distance_;
315  ConfigurationPtr_t initConf_;
317  Configurations_t goalConfigurations_;
319  ProblemTargetPtr_t target_;
321  SteeringMethodPtr_t steeringMethod_;
323  ConfigValidationsPtr_t configValidations_;
325  PathValidationPtr_t pathValidation_;
327  PathProjectorPtr_t pathProjector_;
329  ObjectStdVector_t collisionObstacles_;
331  ConstraintSetPtr_t constraints_;
333  ConfigurationShooterPtr_t configurationShooter_;
334  }; // class Problem
336  } // namespace core
337 } // namespace hpp
338 
339 #define HPP_START_PARAMETER_DECLARATION(name) \
340  struct HPP_CORE_DLLAPI __InitializerClass_##name { \
341  __InitializerClass_##name () {
342 
343 #define HPP_END_PARAMETER_DECLARATION(name) \
344  } \
345  }; \
346  extern "C" { \
347  __InitializerClass_##name __instance_##name; \
348  }
349 
350 #endif // HPP_CORE_PROBLEM_HH
const DistancePtr_t & distance() const
Get distance between configuration.
Definition: problem.hh:123
shared_ptr< ConfigurationShooter > ConfigurationShooterPtr_t
Definition: fwd.hh:103
pinocchio::DevicePtr_t DevicePtr_t
Definition: fwd.hh:114
void steeringMethod(const SteeringMethodPtr_t &sm)
Definition: problem.hh:107
Definition: problem.hh:48
Definition: bi-rrt-planner.hh:24
ConfigurationShooterPtr_t configurationShooter() const
Get path validation method.
Definition: problem.hh:176
const ConfigValidationsPtr_t & configValidations() const
Get configuration validation methods.
Definition: problem.hh:140
PathValidationPtr_t pathValidation() const
Get path validation method.
Definition: problem.hh:163
Definition: parameter.hh:117
shared_ptr< const Problem > ProblemConstPtr_t
Definition: fwd.hh:180
pinocchio::CollisionObjectPtr_t CollisionObjectPtr_t
Definition: fwd.hh:89
pinocchio::JointPtr_t JointPtr_t
Definition: fwd.hh:133
Definition: container.hh:46
void configValidation(const ConfigValidationsPtr_t &configValidations)
Definition: problem.hh:135
shared_ptr< ConfigValidation > ConfigValidationPtr_t
Definition: fwd.hh:105
PathProjectorPtr_t pathProjector() const
Get path projector method.
Definition: problem.hh:191
const ProblemTargetPtr_t & target() const
Get the target.
Definition: problem.hh:87
shared_ptr< PathProjector > PathProjectorPtr_t
Definition: fwd.hh:307
shared_ptr< Distance > DistancePtr_t
Definition: fwd.hh:122
Definition: parameter.hh:53
SteeringMethodPtr_t steeringMethod() const
Get steering method.
Definition: problem.hh:113
const ConfigurationPtr_t & initConfig() const
Get shared pointer to initial configuration.
Definition: problem.hh:75
void constraints(const ConstraintSetPtr_t &constraints)
Definition: problem.hh:204
pinocchio::matrix_t matrix_t
Definition: fwd.hh:145
shared_ptr< ConfigValidations > ConfigValidationsPtr_t
Definition: fwd.hh:106
#define HPP_CORE_DEPRECATED
Definition: deprecated.hh:32
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:110
void target(const ProblemTargetPtr_t &target)
Set the target.
Definition: problem.hh:82
std::vector< ConfigurationPtr_t > Configurations_t
Definition: fwd.hh:100
pinocchio::ConfigurationPtr_t ConfigurationPtr_t
Definition: fwd.hh:99
Container< Parameter > parameters
Definition: problem.hh:298
shared_ptr< Problem > ProblemPtr_t
Definition: fwd.hh:179
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:195
void distance(const DistancePtr_t &distance)
Set distance between configurations.
Definition: problem.hh:118
shared_ptr< ProblemTarget > ProblemTargetPtr_t
Definition: fwd.hh:175
void pathProjector(const PathProjectorPtr_t &pathProjector)
Definition: problem.hh:185
const Parameter & getParameter(const std::string &name) const
Definition: problem.hh:263
#define HPP_CORE_DLLAPI
Definition: config.hh:64
const ConstraintSetPtr_t & constraints() const
Get constraint set.
Definition: problem.hh:211
pinocchio::CollisionObjectConstPtr_t CollisionObjectConstPtr_t
Definition: fwd.hh:90
shared_ptr< PathValidation > PathValidationPtr_t
Definition: fwd.hh:291
std::vector< CollisionObjectPtr_t > ObjectStdVector_t
Definition: fwd.hh:167
const DevicePtr_t & robot() const
return shared pointer to robot.
Definition: problem.hh:70