graph.hh

Go to the documentation of this file.

// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-manipulation.
// hpp-manipulation is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-manipulation is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_MANIPULATION_GRAPH_GRAPH_HH
# define HPP_MANIPULATION_GRAPH_GRAPH_HH

# include <boost/tuple/tuple.hpp>
# 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);

          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 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 configConstraint (const EdgePtr_t& edge) const
            HPP_MANIPULATION_DEPRECATED;

          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 ();

        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

#endif // HPP_MANIPULATION_GRAPH_GRAPH_HH