state.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_STATE_HH
# define HPP_MANIPULATION_GRAPH_STATE_HH

# include <boost/function.hpp>

#include <hpp/core/constraint-set.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/constraints/implicit.hh>

#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/fwd.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/edge.hh"
#include "hpp/manipulation/graph/graph-component.hh"

namespace hpp {
  namespace manipulation {
    using constraints::Implicit;
    using constraints::ImplicitPtr_t;
    namespace graph {

      class HPP_MANIPULATION_DLLAPI State : public GraphComponent
      {
        public:
    typedef boost::function < EdgePtr_t
                  (const std::string&,
                   const GraphWkPtr_t&,
                   const StateWkPtr_t&, const StateWkPtr_t&) >
    EdgeFactory;
          ~State ();

          static StatePtr_t create (const std::string& name);

          EdgePtr_t linkTo (const std::string& name, const StatePtr_t& to,
                const size_type& w = 1,
                EdgeFactory create = Edge::create);

          virtual bool contains (ConfigurationIn_t config) const;

          inline bool isWaypoint () const
          {
            return isWaypoint_;
          }

          inline void isWaypoint (bool isWaypoint)
          {
            isWaypoint_ = isWaypoint;
          }

          StateSelectorWkPtr_t stateSelector () const
          {
            return selector_;
          }

          void stateSelector (const StateSelectorWkPtr_t& parent)
          {
            selector_ = parent;
          };

          const Neighbors_t& neighbors() const
          {
            return neighbors_;
          }

          Edges_t neighborEdges() const
          {
            return neighbors_.values();
          }

          const Edges_t& hiddenNeighbors() const
          {
            return hiddenNeighbors_;
          }

          void updateWeight (const EdgePtr_t&edge, const Weight_t& w);

          Weight_t getWeight (const EdgePtr_t&edge);

          ConstraintSetPtr_t configConstraint() const
          {
            throwIfNotInitialized ();
            return configConstraints_;
          }

          virtual void addNumericalConstraintForPath (const ImplicitPtr_t& nm,
              const segments_t& passiveDofs = segments_t ())
          {
            isInit_ = false;
            numericalConstraintsForPath_.push_back (nm);
            passiveDofsForPath_.push_back (passiveDofs);
          }

          bool insertNumericalConstraintsForPath (ConfigProjectorPtr_t& proj) const
          {
            assert (numericalConstraintsForPath_.size () == passiveDofsForPath_.size ());
            IntervalsContainer_t::const_iterator itpdofs = passiveDofsForPath_.begin ();
            for (NumericalConstraints_t::const_iterator it = numericalConstraintsForPath_.begin();
                it != numericalConstraintsForPath_.end(); it++) {
              proj->add (*it, *itpdofs);
              itpdofs++;
            }
            return !numericalConstraintsForPath_.empty ();
          }

          const NumericalConstraints_t& numericalConstraintsForPath () const
          {
            return numericalConstraintsForPath_;
          }

          std::ostream& dotPrint (std::ostream& os, dot::DrawingAttributes da = dot::DrawingAttributes ()) const;

        protected:
          void init (const StateWkPtr_t& self);

          State(const std::string& name);

          std::ostream& print (std::ostream& os) const;

          virtual void populateTooltip (dot::Tooltip& tp) const;

          virtual void initialize ();

        private:
          Neighbors_t neighbors_;
          Edges_t hiddenNeighbors_;

          ConstraintSetPtr_t configConstraints_;

          NumericalConstraints_t numericalConstraintsForPath_;
          IntervalsContainer_t passiveDofsForPath_;

          StateSelectorWkPtr_t selector_;

          StateWkPtr_t wkPtr_;

          bool isWaypoint_;
      }; // class State

    } // namespace graph
  } // namespace manipulation
} // namespace hpp

#endif // HPP_MANIPULATION_GRAPH_STATE_HH