hpp-manipulation  5.0.0
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 
92  virtual bool generateTargetConfig(core::NodePtr_t nStart,
93  ConfigurationOut_t q) const;
94 
103  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
104  ConfigurationOut_t q) const;
105 
106  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
107 
108  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
109  ConfigurationIn_t q2) const;
110 
112  StatePtr_t stateTo() const;
113 
115  StatePtr_t stateFrom() const;
116 
118  StatePtr_t state() const { return state_.lock(); }
119 
120  void state(StatePtr_t state) { state_ = state; }
121 
124  return steeringMethod_;
125  }
126 
129  return pathValidation_;
130  }
131 
132  const RelativeMotion::matrix_type& relativeMotion() const {
133  return relMotion_;
134  }
135 
137  void relativeMotion(const RelativeMotion::matrix_type& m);
138 
147  void securityMarginForPair(const size_type& row, const size_type& col,
148  const value_type& margin);
149 
151  const matrix_t& securityMargins() const { return securityMargins_; }
152 
155  virtual bool direction(const core::PathPtr_t& path) const;
156 
159  virtual bool intersectionConstraint(const EdgePtr_t& other,
160  ConfigProjectorPtr_t projector) const;
161 
163  virtual std::ostream& dotPrint(
164  std::ostream& os,
166 
168  ConstraintSetPtr_t targetConstraint() const;
169 
170  void setShort(bool isShort) { isShort_ = isShort; }
171 
172  bool isShort() const { return isShort_; }
175  ConstraintSetPtr_t pathConstraint() const;
176 
177  protected:
179  void init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
180  const StateWkPtr_t& from, const StateWkPtr_t& to);
181 
183  Edge(const std::string& name);
184 
186  virtual ConstraintSetPtr_t buildTargetConstraint();
187 
189  virtual ConstraintSetPtr_t buildPathConstraint();
190 
191  virtual void initialize();
192 
194  virtual std::ostream& print(std::ostream& os) const;
195 
196  bool isShort_;
197 
198  private:
200  ConstraintSetPtr_t pathConstraints_;
201 
204  ConstraintSetPtr_t targetConstraints_;
205 
207  StateWkPtr_t from_, to_;
208 
210  StateWkPtr_t state_;
211 
213  core::SteeringMethodPtr_t steeringMethod_;
214 
216  mutable RelativeMotion::matrix_type relMotion_;
218  matrix_t securityMargins_;
219 
220  core::PathValidationPtr_t pathValidation_;
221 
223  EdgeWkPtr_t wkPtr_;
224 
225  friend class Graph;
226 }; // class Edge
227 
263  public:
265  static WaypointEdgePtr_t create(const std::string& name,
266  const GraphWkPtr_t& graph,
267  const StateWkPtr_t& from,
268  const StateWkPtr_t& to);
269 
270  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
271 
272  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
273  ConfigurationIn_t q2) const;
274 
283  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
284  ConfigurationOut_t q) const;
285 
287  const EdgePtr_t& waypoint(const std::size_t index) const;
288 
290  virtual std::ostream& dotPrint(
291  std::ostream& os,
293 
295  void nbWaypoints(const size_type number);
296 
297  std::size_t nbWaypoints() const { return edges_.size() - 1; }
298 
301  void setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
302  const StatePtr_t wTo);
303 
304  protected:
305  WaypointEdge(const std::string& name) : Edge(name), lastSucceeded_(false) {}
307  void init(const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
308  const StateWkPtr_t& from, const StateWkPtr_t& to);
309 
311  virtual void initialize();
313  virtual std::ostream& print(std::ostream& os) const;
314 
315  private:
316  Edges_t edges_;
317  States_t states_;
318 
319  mutable matrix_t configs_;
320  mutable bool lastSucceeded_;
321 
322  WaypointEdgeWkPtr_t wkPtr_;
323 }; // class WaypointEdge
324 
399  public:
400  virtual ~LevelSetEdge();
401 
403  static LevelSetEdgePtr_t create(const std::string& name,
404  const GraphWkPtr_t& graph,
405  const StateWkPtr_t& from,
406  const StateWkPtr_t& to);
407 
416  virtual bool generateTargetConfig(core::NodePtr_t nStart,
417  ConfigurationOut_t q) const;
418 
427  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
428  ConfigurationOut_t q) const;
429 
437  bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
438  ConfigurationIn_t qLeaf,
439  ConfigurationOut_t q) const;
440 
442  virtual ConstraintSetPtr_t buildTargetConstraint();
443 
446  void buildHistogram();
447 
464  LeafHistogramPtr_t histogram() const;
465 
468 
471  void insertParamConstraint(const ImplicitPtr_t& nm);
472 
474  const NumericalConstraints_t& paramConstraints() const;
475 
478  void insertConditionConstraint(const ImplicitPtr_t& nm);
479 
482  const NumericalConstraints_t& conditionConstraints() const;
484 
486  virtual std::ostream& dotPrint(
487  std::ostream& os,
489 
490  protected:
492  void init(const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
493  const StateWkPtr_t& from, const StateWkPtr_t& to);
494 
495  LevelSetEdge(const std::string& name);
496 
498  virtual std::ostream& print(std::ostream& os) const;
499 
501  virtual void populateTooltip(dot::Tooltip& tp) const;
502 
503  virtual void initialize();
504 
505  private:
506  // Parametrizer
507  // NumericalConstraints_t
508  NumericalConstraints_t paramNumericalConstraints_;
509 
510  // Condition
511  // NumericalConstraints_t
512  NumericalConstraints_t condNumericalConstraints_;
513 
515  LeafHistogramPtr_t hist_;
516 
517  LevelSetEdgeWkPtr_t wkPtr_;
518 }; // class LevelSetEdge
519 
521 } // namespace graph
522 } // namespace manipulation
523 } // namespace hpp
524 
525 #endif // HPP_MANIPULATION_GRAPH_EDGE_HH
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:57
const RelativeMotion::matrix_type & relativeMotion() const
Definition: edge.hh:132
const matrix_t & securityMargins() const
Accessor to the security margin.
Definition: edge.hh:151
shared_ptr< WaypointEdge > WaypointEdgePtr_t
Definition: fwd.hh:50
bool isShort() const
Definition: edge.hh:172
core::RelativeMotion RelativeMotion
Definition: edge.hh:75
Definition: main.hh:1
void state(StatePtr_t state)
Definition: edge.hh:120
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:49
bool isShort_
Definition: edge.hh:196
core::value_type value_type
Definition: fwd.hh:89
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:88
shared_ptr< LevelSetEdge > LevelSetEdgePtr_t
Definition: fwd.hh:51
void setShort(bool isShort)
Definition: edge.hh:170
const core::PathValidationPtr_t & pathValidation() const
Get path validation associated to the edge.
Definition: edge.hh:128
std::size_t nbWaypoints() const
Definition: edge.hh:297
core::ConfigProjectorPtr_t ConfigProjectorPtr_t
Definition: fwd.hh:136
std::vector< StatePtr_t > States_t
Definition: fwd.hh:56
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:138
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:131
hpp::core::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:64
core::matrix_t matrix_t
Definition: fwd.hh:146
core::PathValidationPtr_t PathValidationPtr_t
Definition: fwd.hh:144
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:90
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:123
Definition: graph.hh:61
StatePtr_t state() const
Get the state in which path is.
Definition: edge.hh:118
WaypointEdge(const std::string &name)
Definition: edge.hh:305
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:112