edge.hh

Go to the documentation of this file.

// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
 
#ifndef HPP_MANIPULATION_GRAPH_EDGE_HH
#define HPP_MANIPULATION_GRAPH_EDGE_HH
 
#include <hpp/core/constraint-set.hh>
#include <hpp/core/path.hh>
#include <hpp/core/relative-motion.hh>
#include <hpp/core/steering-method.hh>
 
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/graph.hh"
 
namespace hpp {
namespace manipulation {
namespace graph {
 
 
class HPP_MANIPULATION_DLLAPI Edge : public GraphComponent {
  friend class WaypointEdge;
 
 public:
  typedef core::RelativeMotion RelativeMotion;
 
  virtual ~Edge();
 
  static EdgePtr_t create(const std::string& name, const GraphWkPtr_t& graph,
                          const StateWkPtr_t& from, const StateWkPtr_t& to);
 
  virtual bool generateTargetConfig(core::NodePtr_t nStart,
                                    ConfigurationOut_t q) const;
 
  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
                                    ConfigurationOut_t q) const;
 
  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
 
  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
                     ConfigurationIn_t q2) const;
 
  StatePtr_t stateTo() const;
 
  StatePtr_t stateFrom() const;
 
  StatePtr_t state() const { return state_.lock(); }
 
  void state(StatePtr_t state) { state_ = state; }
 
  const core::SteeringMethodPtr_t& steeringMethod() const {
    return steeringMethod_;
  }
 
  const core::PathValidationPtr_t& pathValidation() const {
    return pathValidation_;
  }
 
  const RelativeMotion::matrix_type& relativeMotion() const {
    return relMotion_;
  }
 
  void relativeMotion(const RelativeMotion::matrix_type& m);
 
  void securityMarginForPair(const size_type& row, const size_type& col,
                             const value_type& margin);
 
  const matrix_t& securityMargins() const { return securityMargins_; }
 
  virtual bool direction(const core::PathPtr_t& path) const;
 
  virtual bool intersectionConstraint(const EdgePtr_t& other,
                                      ConfigProjectorPtr_t projector) const;
 
  virtual std::ostream& dotPrint(
      std::ostream& os,
      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
 
  ConstraintSetPtr_t targetConstraint() const;
 
  void setShort(bool isShort) { isShort_ = isShort; }
 
  bool isShort() const { return isShort_; }
  ConstraintSetPtr_t pathConstraint() const;
 
 protected:
  void init(const EdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
            const StateWkPtr_t& from, const StateWkPtr_t& to);
 
  Edge(const std::string& name);
 
  virtual ConstraintSetPtr_t buildTargetConstraint();
 
  virtual ConstraintSetPtr_t buildPathConstraint();
 
  virtual void initialize();
 
  virtual std::ostream& print(std::ostream& os) const;
 
  bool isShort_;
 
 private:
  ConstraintSetPtr_t pathConstraints_;
 
  ConstraintSetPtr_t targetConstraints_;
 
  StateWkPtr_t from_, to_;
 
  StateWkPtr_t state_;
 
  core::SteeringMethodPtr_t steeringMethod_;
 
  mutable RelativeMotion::matrix_type relMotion_;
  matrix_t securityMargins_;
 
  core::PathValidationPtr_t pathValidation_;
 
  EdgeWkPtr_t wkPtr_;
 
  friend class Graph;
};  // class Edge
 
class HPP_MANIPULATION_DLLAPI WaypointEdge : public Edge {
 public:
  static WaypointEdgePtr_t create(const std::string& name,
                                  const GraphWkPtr_t& graph,
                                  const StateWkPtr_t& from,
                                  const StateWkPtr_t& to);
 
  virtual bool canConnect(ConfigurationIn_t q1, ConfigurationIn_t q2) const;
 
  virtual bool build(core::PathPtr_t& path, ConfigurationIn_t q1,
                     ConfigurationIn_t q2) const;
 
  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
                                    ConfigurationOut_t q) const;
 
  const EdgePtr_t& waypoint(const std::size_t index) const;
 
  virtual std::ostream& dotPrint(
      std::ostream& os,
      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
 
  void nbWaypoints(const size_type number);
 
  std::size_t nbWaypoints() const { return edges_.size() - 1; }
 
  void setWaypoint(const std::size_t index, const EdgePtr_t wEdge,
                   const StatePtr_t wTo);
 
 protected:
  WaypointEdge(const std::string& name) : Edge(name), lastSucceeded_(false) {}
  void init(const WaypointEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
            const StateWkPtr_t& from, const StateWkPtr_t& to);
 
  virtual void initialize();
  virtual std::ostream& print(std::ostream& os) const;
 
 private:
  Edges_t edges_;
  States_t states_;
 
  mutable matrix_t configs_;
  mutable bool lastSucceeded_;
 
  WaypointEdgeWkPtr_t wkPtr_;
};  // class WaypointEdge
 
class HPP_MANIPULATION_DLLAPI LevelSetEdge : public Edge {
 public:
  virtual ~LevelSetEdge();
 
  static LevelSetEdgePtr_t create(const std::string& name,
                                  const GraphWkPtr_t& graph,
                                  const StateWkPtr_t& from,
                                  const StateWkPtr_t& to);
 
  virtual bool generateTargetConfig(core::NodePtr_t nStart,
                                    ConfigurationOut_t q) const;
 
  virtual bool generateTargetConfig(ConfigurationIn_t qStart,
                                    ConfigurationOut_t q) const;
 
  bool generateTargetConfigOnLeaf(ConfigurationIn_t qStart,
                                  ConfigurationIn_t qLeaf,
                                  ConfigurationOut_t q) const;
 
  virtual ConstraintSetPtr_t buildTargetConstraint();
 
  void buildHistogram();
 
  LeafHistogramPtr_t histogram() const;
 
 
  void insertParamConstraint(const ImplicitPtr_t& nm);
 
  const NumericalConstraints_t& paramConstraints() const;
 
  void insertConditionConstraint(const ImplicitPtr_t& nm);
 
  const NumericalConstraints_t& conditionConstraints() const;
 
  virtual std::ostream& dotPrint(
      std::ostream& os,
      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
 
 protected:
  void init(const LevelSetEdgeWkPtr_t& weak, const GraphWkPtr_t& graph,
            const StateWkPtr_t& from, const StateWkPtr_t& to);
 
  LevelSetEdge(const std::string& name);
 
  virtual std::ostream& print(std::ostream& os) const;
 
  virtual void populateTooltip(dot::Tooltip& tp) const;
 
  virtual void initialize();
 
 private:
  // Parametrizer
  // NumericalConstraints_t
  NumericalConstraints_t paramNumericalConstraints_;
 
  // Condition
  // NumericalConstraints_t
  NumericalConstraints_t condNumericalConstraints_;
 
  LeafHistogramPtr_t hist_;
 
  LevelSetEdgeWkPtr_t wkPtr_;
};  // class LevelSetEdge
 
}  // namespace graph
}  // namespace manipulation
}  // namespace hpp
 
#endif  // HPP_MANIPULATION_GRAPH_EDGE_HH