hpp-manipulation  4.15.1
Classes for manipulation planning.
edge.hh
Go to the documentation of this file.
1 // Copyright (c) 2014, LAAS-CNRS
2 // Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3 //
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // 1. Redistributions of source code must retain the above copyright
10 // notice, this list of conditions and the following disclaimer.
11 //
12 // 2. Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17 // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18 // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19 // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22 // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27 // DAMAGE.
28 
29 #ifndef HPP_MANIPULATION_GRAPH_EDGE_HH
30 #define HPP_MANIPULATION_GRAPH_EDGE_HH
31 
32 #include <hpp/core/constraint-set.hh>
33 #include <hpp/core/path.hh>
34 #include <hpp/core/relative-motion.hh>
35 #include <hpp/core/steering-method.hh>
36 
38 #include "hpp/manipulation/fwd.hh"
40 
41 namespace hpp {
42 namespace manipulation {
43 namespace graph {
46 
50 
72  friend class WaypointEdge;
73 
74  public:
75  typedef core::RelativeMotion RelativeMotion;
76 
78  virtual ~Edge();
79 
81  static EdgePtr_t create(const std::string& name, const GraphWkPtr_t& graph,
82  const StateWkPtr_t& from, const StateWkPtr_t& to);
83 
93  virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
95 
105  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
107 
116  virtual bool generateTargetConfig(core::NodePtr_t nStart,
117  ConfigurationOut_t q) const;
118 
127  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
128  ConfigurationOut_t q) const;
129 
130  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
131 
132  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
133  ConfigurationIn_t q2) const;
134 
136  StatePtr_t stateTo() const;
137 
139  StatePtr_t stateFrom() const;
140 
142  StatePtr_t state() const { return state_.lock(); }
143 
144  void state(StatePtr_t state) { state_ = state; }
145 
148  return steeringMethod_;
149  }
150 
153  return pathValidation_;
154  }
155 
156  const RelativeMotion::matrix_type& relativeMotion() const {
157  return relMotion_;
158  }
159 
161  void relativeMotion(const RelativeMotion::matrix_type& m);
162 
171  void securityMarginForPair(const size_type& row, const size_type& col,
172  const value_type& margin);
173 
175  const matrix_t& securityMargins() const { return securityMargins_; }
176 
179  virtual bool direction(const core::PathPtr_t& path) const;
180 
183  virtual bool intersectionConstraint(const EdgePtr_t& other,
184  ConfigProjectorPtr_t projector) const;
185 
187  virtual std::ostream& dotPrint(
188  std::ostream& os,
190 
193  ConstraintSetPtr_t configConstraint() const HPP_MANIPULATION_DEPRECATED;
194 
196  ConstraintSetPtr_t targetConstraint() const;
197 
198  void setShort(bool isShort) { isShort_ = isShort; }
199 
200  bool isShort() const { return isShort_; }
203  ConstraintSetPtr_t pathConstraint() const;
204 
205  protected:
207  void init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
208  const StateWkPtr_t& from, const StateWkPtr_t& to);
209 
211  Edge(const std::string& name);
212 
213  virtual ConstraintSetPtr_t buildConfigConstraint()
215 
217  virtual ConstraintSetPtr_t buildTargetConstraint();
218 
220  virtual ConstraintSetPtr_t buildPathConstraint();
221 
222  virtual void initialize();
223 
225  virtual std::ostream& print(std::ostream& os) const;
226 
227  bool isShort_;
228 
229  private:
231  ConstraintSetPtr_t pathConstraints_;
232 
235  ConstraintSetPtr_t targetConstraints_;
236 
238  StateWkPtr_t from_, to_;
239 
241  StateWkPtr_t state_;
242 
244  core::SteeringMethodPtr_t steeringMethod_;
245 
247  mutable RelativeMotion::matrix_type relMotion_;
249  matrix_t securityMargins_;
250 
251  core::PathValidationPtr_t pathValidation_;
252 
254  EdgeWkPtr_t wkPtr_;
255 
256  friend class Graph;
257 }; // class Edge
258 
294  public:
296  static WaypointEdgePtr_t create(const std::string& name,
297  const GraphWkPtr_t& graph,
298  const StateWkPtr_t& from,
299  const StateWkPtr_t& to);
300 
301  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
302 
303  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
304  ConfigurationIn_t q2) const;
305 
315  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
317 
326  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
327  ConfigurationOut_t q) const;
328 
330  const EdgePtr_t& waypoint(const std::size_t index) const;
331 
333  virtual std::ostream& dotPrint(
334  std::ostream& os,
336 
338  void nbWaypoints(const size_type number);
339 
340  std::size_t nbWaypoints() const { return edges_.size() - 1; }
341 
344  void setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
345  const StatePtr_t wTo);
346 
347  protected:
348  WaypointEdge(const std::string& name) : Edge(name), lastSucceeded_(false) {}
350  void init(const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
351  const StateWkPtr_t& from, const StateWkPtr_t& to);
352 
354  virtual void initialize();
356  virtual std::ostream& print(std::ostream& os) const;
357 
358  private:
359  Edges_t edges_;
360  States_t states_;
361 
362  mutable matrix_t configs_;
363  mutable bool lastSucceeded_;
364 
365  WaypointEdgeWkPtr_t wkPtr_;
366 }; // class WaypointEdge
367 
442  public:
443  virtual ~LevelSetEdge();
444 
446  static LevelSetEdgePtr_t create(const std::string& name,
447  const GraphWkPtr_t& graph,
448  const StateWkPtr_t& from,
449  const StateWkPtr_t& to);
450 
460  virtual bool applyConstraints(core::NodePtr_t nStart, ConfigurationOut_t q)
462 
472  virtual bool applyConstraints(ConfigurationIn_t qStart, ConfigurationOut_t q)
474 
483  virtual bool generateTargetConfig(core::NodePtr_t nStart,
484  ConfigurationOut_t q) const;
485 
494  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
495  ConfigurationOut_t q) const;
496 
504  bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
505  ConfigurationIn_t qLeaf,
506  ConfigurationOut_t q) const;
507 
509  virtual ConstraintSetPtr_t buildConfigConstraint()
511 
513  virtual ConstraintSetPtr_t buildTargetConstraint();
514 
517  void buildHistogram();
518 
535  LeafHistogramPtr_t histogram() const;
536 
539 
542  void insertParamConstraint(const ImplicitPtr_t& nm);
543 
545  const NumericalConstraints_t& paramConstraints() const;
546 
549  void insertConditionConstraint(const ImplicitPtr_t& nm);
550 
553  const NumericalConstraints_t& conditionConstraints() const;
555 
557  virtual std::ostream& dotPrint(
558  std::ostream& os,
560 
561  protected:
563  void init(const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
564  const StateWkPtr_t& from, const StateWkPtr_t& to);
565 
566  LevelSetEdge(const std::string& name);
567 
569  virtual std::ostream& print(std::ostream& os) const;
570 
572  virtual void populateTooltip(dot::Tooltip& tp) const;
573 
574  virtual void initialize();
575 
576  private:
577  // Parametrizer
578  // NumericalConstraints_t
579  NumericalConstraints_t paramNumericalConstraints_;
580 
581  // Condition
582  // NumericalConstraints_t
583  NumericalConstraints_t condNumericalConstraints_;
584 
586  LeafHistogramPtr_t hist_;
587 
588  LevelSetEdgeWkPtr_t wkPtr_;
589 }; // class LevelSetEdge
590 
592 } // namespace graph
593 } // namespace manipulation
594 } // namespace hpp
595 
596 #endif // HPP_MANIPULATION_GRAPH_EDGE_HH
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED
Definition: statistics.hh:206
const RelativeMotion::matrix_type & relativeMotion() const
Definition: edge.hh:156
const matrix_t & securityMargins() const
Accessor to the security margin.
Definition: edge.hh:175
shared_ptr< WaypointEdge > WaypointEdgePtr_t
Definition: fwd.hh:50
bool isShort() const
Definition: edge.hh:200
core::RelativeMotion RelativeMotion
Definition: edge.hh:75
Definition: main.hh:1
void state(StatePtr_t state)
Definition: edge.hh:144
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
core::value_type value_type
Definition: fwd.hh:90
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
shared_ptr< LevelSetEdge > LevelSetEdgePtr_t
Definition: fwd.hh:51
const core::PathValidationPtr_t & pathValidation() const
Get path validation associated to the edge.
Definition: edge.hh:152
std::size_t nbWaypoints() const
Definition: edge.hh:340
core::ConfigProjectorPtr_t ConfigProjectorPtr_t
Definition: fwd.hh:121
std::vector< StatePtr_t > States_t
Definition: fwd.hh:56
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:123
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:116
hpp::core::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:64
core::matrix_t matrix_t
Definition: fwd.hh:131
core::PathValidationPtr_t PathValidationPtr_t
Definition: fwd.hh:129
shared_ptr< LeafHistogram > LeafHistogramPtr_t
Definition: fwd.hh:72
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:50
core::size_type size_type
Definition: fwd.hh:91
Define common methods of the graph components.
Definition: graph-component.hh:51
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:49
Definition: edge.hh:71
shared_ptr< State > StatePtr_t
Definition: fwd.hh:48
const core::SteeringMethodPtr_t & steeringMethod() const
Get steering method associated to the edge.
Definition: edge.hh:147
Definition: graph.hh:58
StatePtr_t state() const
Get the state in which path is.
Definition: edge.hh:142
WaypointEdge(const std::string &name)
Definition: edge.hh:348
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:101