graph.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_GRAPH_HH
#define HPP_MANIPULATION_GRAPH_GRAPH_HH
 
#include <tuple>
 
#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/constraint-set.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/graph-component.hh"
 
namespace hpp {
namespace manipulation {
typedef constraints::ImplicitPtr_t ImplicitPtr_t;
namespace graph {
 
class HPP_MANIPULATION_DLLAPI Graph : public GraphComponent {
 public:
  static GraphPtr_t create(const std::string& name, DevicePtr_t robot,
                           const ProblemPtr_t& problem);
 
  static EdgePtr_t edgeAtParam(const core::PathVectorPtr_t& path,
                               value_type param);
 
  GraphPtr_t self() const { return wkPtr_.lock(); }
 
  StateSelectorPtr_t createStateSelector(const std::string& name);
 
  void stateSelector(StateSelectorPtr_t ns);
 
  StateSelectorPtr_t stateSelector() const { return stateSelector_; }
 
  StatePtr_t getState(ConfigurationIn_t config) const;
 
  StatePtr_t getState(RoadmapNodePtr_t node) const;
 
  Edges_t getEdges(const StatePtr_t& from, const StatePtr_t& to) const;
 
  EdgePtr_t chooseEdge(RoadmapNodePtr_t node) const;
 
  void clearConstraintsAndComplement();
 
  void registerConstraints(const ImplicitPtr_t& constraint,
                           const ImplicitPtr_t& complement,
                           const ImplicitPtr_t& both);
 
  bool isComplement(const ImplicitPtr_t& constraint,
                    const ImplicitPtr_t& complement,
                    ImplicitPtr_t& combinationOfBoth) const;
 
  const ConstraintsAndComplements_t& constraintsAndComplements() const;
 
  ConstraintSetPtr_t configConstraint(const StatePtr_t& state) const;
 
  ConstraintSetPtr_t targetConstraint(const EdgePtr_t& edge) const;
 
  bool getConfigErrorForState(ConfigurationIn_t config, const StatePtr_t& state,
                              vector_t& error) const;
 
  //          of the edge
  bool getConfigErrorForEdge(ConfigurationIn_t config, const EdgePtr_t& edge,
                             vector_t& error) const;
 
  bool getConfigErrorForEdgeLeaf(ConfigurationIn_t leafConfig,
                                 ConfigurationIn_t config,
                                 const EdgePtr_t& edge, vector_t& error) const;
 
  bool getConfigErrorForEdgeTarget(ConfigurationIn_t leafConfig,
                                   ConfigurationIn_t config,
                                   const EdgePtr_t& edge,
                                   vector_t& error) const;
 
  ConstraintSetPtr_t pathConstraint(const EdgePtr_t& edge) const;
 
  void maxIterations(size_type iterations);
 
  size_type maxIterations() const;
 
  void errorThreshold(const value_type& threshold);
 
  value_type errorThreshold() const;
 
  const DevicePtr_t& robot() const;
 
  const ProblemPtr_t& problem() const;
 
  void problem(const ProblemPtr_t& problem);
 
  void insertHistogram(const graph::HistogramPtr_t& hist) {
    hists_.push_back(hist);
  }
 
  const Histograms_t& histograms() const { return hists_; }
 
  GraphComponentWkPtr_t get(std::size_t id) const;
 
  std::size_t nbComponents() const { return components_.size(); }
 
  virtual std::ostream& dotPrint(
      std::ostream& os,
      dot::DrawingAttributes da = dot::DrawingAttributes()) const;
 
  virtual void initialize();
 
  virtual void invalidate();
 
 protected:
  void init(const GraphWkPtr_t& weak, DevicePtr_t robot);
 
  Graph(const std::string& name, const ProblemPtr_t& problem);
 
  std::ostream& print(std::ostream& os) const;
 
 private:
  GraphComponents_t& components();
 
  GraphComponents_t components_;
 
  StateSelectorPtr_t stateSelector_;
 
  ConstraintPtr_t constraints_;
 
  DevicePtr_t robot_;
 
  GraphWkPtr_t wkPtr_;
 
  typedef std::map<StatePtr_t, ConstraintSetPtr_t> MapFromState;
  typedef std::pair<StatePtr_t, ConstraintSetPtr_t> PairStateConstraints;
  MapFromState constraintSetMapFromState_;
 
  Histograms_t hists_;
 
  typedef std::map<EdgePtr_t, ConstraintSetPtr_t> MapFromEdge;
  typedef std::pair<EdgePtr_t, ConstraintSetPtr_t> PairEdgeConstraints;
  MapFromEdge cfgConstraintSetMapFromEdge_, pathConstraintSetMapFromEdge_;
  ProblemPtr_t problem_;
  value_type errorThreshold_;
  size_type maxIterations_;
 
  ConstraintsAndComplements_t constraintsAndComplements_;
  friend class GraphComponent;
};  // Class Graph
 
}  // namespace graph
}  // namespace manipulation
 
}  // namespace hpp
 
BOOST_CLASS_EXPORT_KEY(hpp::manipulation::graph::Graph)
 
#endif  // HPP_MANIPULATION_GRAPH_GRAPH_HH