hpp-manipulation  4.11.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 // This file is part of hpp-manipulation.
5 // hpp-manipulation is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // hpp-manipulation is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
16 
17 #ifndef HPP_MANIPULATION_GRAPH_EDGE_HH
18 # define HPP_MANIPULATION_GRAPH_EDGE_HH
19 
20 #include <hpp/core/constraint-set.hh>
21 #include <hpp/core/steering-method.hh>
22 #include <hpp/core/relative-motion.hh>
23 #include <hpp/core/path.hh>
24 
26 #include "hpp/manipulation/fwd.hh"
28 
29 namespace hpp {
30  namespace manipulation {
31  namespace graph {
34 
38 
60  {
61  friend class WaypointEdge;
62  public:
63  typedef core::RelativeMotion RelativeMotion;
64 
66  virtual ~Edge ();
67 
69  static EdgePtr_t create
70  (const std::string& name,
71  const GraphWkPtr_t& graph,
72  const StateWkPtr_t& from,
73  const StateWkPtr_t& to);
74 
84  virtual bool applyConstraints (core::NodePtr_t nStart,
85  ConfigurationOut_t q) const
87 
97  virtual bool applyConstraints (ConfigurationIn_t qStart,
98  ConfigurationOut_t q) const
100 
109  virtual bool generateTargetConfig(core::NodePtr_t nStart,
110  ConfigurationOut_t q) const;
111 
120  virtual bool generateTargetConfig (ConfigurationIn_t qStart,
121  ConfigurationOut_t q) const;
122 
123  virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
124 
125  virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1,
126  ConfigurationIn_t q2) const;
127 
129  StatePtr_t stateTo () const;
130 
132  StatePtr_t stateFrom () const;
133 
135  StatePtr_t state () const
136  {
137  return state_.lock();
138  }
139 
140  void state (StatePtr_t state)
141  {
142  state_ = state;
143  }
144 
147  {
148  return steeringMethod_;
149  }
150 
153  {
154  return pathValidation_;
155  }
156 
157  const RelativeMotion::matrix_type& relativeMotion () const
158  {
159  return relMotion_;
160  }
161 
163  void relativeMotion(const RelativeMotion::matrix_type & m);
164 
173  void securityMarginForPair(const size_type& row, const size_type& col,
174  const value_type& margin);
175 
177  const matrix_t& securityMargins() const
178  {
179  return securityMargins_;
180  }
181 
184  virtual bool direction (const core::PathPtr_t& path) const;
185 
188  virtual bool intersectionConstraint (const EdgePtr_t& other,
189  ConfigProjectorPtr_t projector) const;
190 
192  virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
193 
196  ConstraintSetPtr_t configConstraint() const
198 
200  ConstraintSetPtr_t targetConstraint() const;
201 
202  void setShort (bool isShort) {
203  isShort_ = isShort;
204  }
205 
206  bool isShort () const {
207  return isShort_;
208  }
211  ConstraintSetPtr_t pathConstraint() const;
212 
213  protected:
215  void init (const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
216  const StateWkPtr_t& to);
217 
219  Edge (const std::string& name);
220 
221  virtual ConstraintSetPtr_t buildConfigConstraint()
223 
225  virtual ConstraintSetPtr_t buildTargetConstraint();
226 
228  virtual ConstraintSetPtr_t buildPathConstraint();
229 
230  virtual void initialize ();
231 
233  virtual std::ostream& print (std::ostream& os) const;
234 
235  bool isShort_;
236 
237  private:
239  ConstraintSetPtr_t pathConstraints_;
240 
243  ConstraintSetPtr_t targetConstraints_;
244 
246  StateWkPtr_t from_, to_;
247 
249  StateWkPtr_t state_;
250 
252  core::SteeringMethodPtr_t steeringMethod_;
253 
255  mutable RelativeMotion::matrix_type relMotion_;
257  matrix_t securityMargins_;
258 
259  core::PathValidationPtr_t pathValidation_;
260 
262  EdgeWkPtr_t wkPtr_;
263 
264  friend class Graph;
265  }; // class Edge
266 
302  {
303  public:
305  static WaypointEdgePtr_t create
306  (const std::string& name,
307  const GraphWkPtr_t& graph, const StateWkPtr_t& from,
308  const StateWkPtr_t& to);
309 
310  virtual bool canConnect (ConfigurationIn_t q1, ConfigurationIn_t q2) const;
311 
312  virtual bool build (core::PathPtr_t& path, ConfigurationIn_t q1, ConfigurationIn_t q2) const;
313 
314 
324  virtual bool applyConstraints (ConfigurationIn_t qStart,
325  ConfigurationOut_t q) const
327 
336  virtual bool generateTargetConfig (ConfigurationIn_t qStart,
337  ConfigurationOut_t q) const;
338 
340  const EdgePtr_t& waypoint (const std::size_t index) const;
341 
343  virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
344 
346  void nbWaypoints (const size_type number);
347 
348  std::size_t nbWaypoints () const
349  {
350  return edges_.size () - 1;
351  }
352 
355  void setWaypoint (const std::size_t index, const EdgePtr_t wEdge, const StatePtr_t wTo);
356 
357  protected:
358  WaypointEdge (const std::string& name) :
359  Edge (name),
360  lastSucceeded_ (false)
361  {
362  }
364  void init (const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
365  const StateWkPtr_t& to);
366 
368  virtual void initialize ();
370  virtual std::ostream& print (std::ostream& os) const;
371 
372  private:
373  Edges_t edges_;
374  States_t states_;
375 
376  mutable matrix_t configs_;
377  mutable bool lastSucceeded_;
378 
379  WaypointEdgeWkPtr_t wkPtr_;
380  }; // class WaypointEdge
381 
455  {
456  public:
457  virtual ~LevelSetEdge ();
458 
460  static LevelSetEdgePtr_t create
461  (const std::string& name,
462  const GraphWkPtr_t& graph, const StateWkPtr_t& from,
463  const StateWkPtr_t& to);
464 
474  virtual bool applyConstraints (core::NodePtr_t nStart,
475  ConfigurationOut_t q) const
477 
487  virtual bool applyConstraints (ConfigurationIn_t qStart,
488  ConfigurationOut_t q) const
490 
499  virtual bool generateTargetConfig(core::NodePtr_t nStart,
500  ConfigurationOut_t q) const;
501 
510  virtual bool generateTargetConfig (ConfigurationIn_t qStart,
511  ConfigurationOut_t q) const;
512 
520  bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
521  ConfigurationIn_t qLeaf,
522  ConfigurationOut_t q) const;
523 
525  virtual ConstraintSetPtr_t buildConfigConstraint()
527 
529  virtual ConstraintSetPtr_t buildTargetConstraint();
530 
533  void buildHistogram ();
534 
551  LeafHistogramPtr_t histogram () const;
552 
555 
558  void insertParamConstraint (const ImplicitPtr_t& nm);
559 
561  const NumericalConstraints_t& paramConstraints() const;
562 
565  void insertConditionConstraint (const ImplicitPtr_t& nm);
566 
569  const NumericalConstraints_t& conditionConstraints() const;
571 
573  virtual std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;
574 
575  protected:
577  void init (const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph, const StateWkPtr_t& from,
578  const StateWkPtr_t& to);
579 
580  LevelSetEdge (const std::string& name);
581 
583  virtual std::ostream& print (std::ostream& os) const;
584 
586  virtual void populateTooltip (dot::Tooltip& tp) const;
587 
588  virtual void initialize ();
589 
590  private:
591  // Parametrizer
592  // NumericalConstraints_t
593  NumericalConstraints_t paramNumericalConstraints_;
594 
595  // Condition
596  // NumericalConstraints_t
597  NumericalConstraints_t condNumericalConstraints_;
598 
600  LeafHistogramPtr_t hist_;
601 
602  LevelSetEdgeWkPtr_t wkPtr_;
603  }; // class LevelSetEdge
604 
606  } // namespace graph
607  } // namespace manipulation
608 } // namespace hpp
609 
610 #endif // HPP_MANIPULATION_GRAPH_EDGE_HH
StateHistogram NodeHistogram HPP_MANIPULATION_DEPRECATED
Definition: statistics.hh:200
const RelativeMotion::matrix_type & relativeMotion() const
Definition: edge.hh:157
const matrix_t & securityMargins() const
Accessor to the security margin.
Definition: edge.hh:177
std::vector< EdgePtr_t > Edges_t
Definition: fwd.hh:46
shared_ptr< SteeringMethod > SteeringMethodPtr_t
Definition: fwd.hh:93
bool isShort() const
Definition: edge.hh:206
core::RelativeMotion RelativeMotion
Definition: edge.hh:63
Definition: main.hh:1
void state(StatePtr_t state)
Definition: edge.hh:140
pinocchio::ConfigurationIn_t ConfigurationIn_t
Definition: fwd.hh:38
bool isShort_
Definition: edge.hh:235
core::value_type value_type
Definition: fwd.hh:82
#define HPP_MANIPULATION_DLLAPI
Definition: config.hh:64
shared_ptr< WaypointEdge > WaypointEdgePtr_t
Definition: fwd.hh:38
void setShort(bool isShort)
Definition: edge.hh:202
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:348
core::ConfigProjectorPtr_t ConfigProjectorPtr_t
Definition: fwd.hh:113
shared_ptr< LevelSetEdge > LevelSetEdgePtr_t
Definition: fwd.hh:39
shared_ptr< ConstraintSet > ConstraintSetPtr_t
Definition: fwd.hh:115
constraints::ImplicitPtr_t ImplicitPtr_t
Definition: fwd.hh:108
hpp::core::NumericalConstraints_t NumericalConstraints_t
Definition: fwd.hh:53
shared_ptr< State > StatePtr_t
Definition: fwd.hh:36
core::matrix_t matrix_t
Definition: fwd.hh:123
core::PathValidationPtr_t PathValidationPtr_t
Definition: fwd.hh:121
shared_ptr< LeafHistogram > LeafHistogramPtr_t
Definition: fwd.hh:61
pinocchio::ConfigurationOut_t ConfigurationOut_t
Definition: fwd.hh:39
core::size_type size_type
Definition: fwd.hh:83
shared_ptr< Edge > EdgePtr_t
Definition: fwd.hh:37
Define common methods of the graph components.
Definition: graph-component.hh:39
Definition: edge.hh:59
std::vector< StatePtr_t > States_t
Definition: fwd.hh:45
const core::SteeringMethodPtr_t & steeringMethod() const
Get steering method associated to the edge.
Definition: edge.hh:146
Definition: graph.hh:45
StatePtr_t state() const
Get the state in which path is.
Definition: edge.hh:135
WaypointEdge(const std::string &name)
Definition: edge.hh:358