edge.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_EDGE_HH
# define HPP_MANIPULATION_GRAPH_EDGE_HH

#include <hpp/core/constraint-set.hh>
#include <hpp/core/steering-method.hh>
#include <hpp/core/relative-motion.hh>
#include <hpp/core/path.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
      {
        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 applyConstraints (core::NodePtr_t nStart,
                                         ConfigurationOut_t q) const
            HPP_MANIPULATION_DEPRECATED;

          virtual bool applyConstraints (ConfigurationIn_t qStart,
                                         ConfigurationOut_t q) const
            HPP_MANIPULATION_DEPRECATED;

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

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

          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 applyConstraints (ConfigurationIn_t qStart,
                                         ConfigurationOut_t q) const
            HPP_MANIPULATION_DEPRECATED;

          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 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 applyConstraints (core::NodePtr_t nStart,
                                         ConfigurationOut_t q) const
            HPP_MANIPULATION_DEPRECATED;

          virtual bool applyConstraints (ConfigurationIn_t qStart,
                                         ConfigurationOut_t q) const
            HPP_MANIPULATION_DEPRECATED;

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

          virtual ConstraintSetPtr_t buildTargetConstraint();

          void buildHistogram ();

          LeafHistogramPtr_t histogram () const;


          void insertParamConstraint (const ImplicitPtr_t& nm,
              const segments_t& passiveDofs = segments_t ());

          const NumericalConstraints_t& paramConstraints() const;

          void insertConditionConstraint (const ImplicitPtr_t& nm,
              const segments_t& passiveDofs = segments_t ());

          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_;
          IntervalsContainer_t paramPassiveDofs_;

          // Condition
          // NumericalConstraints_t
          NumericalConstraints_t condNumericalConstraints_;
          IntervalsContainer_t condPassiveDofs_;

          LeafHistogramPtr_t hist_;

          LevelSetEdgeWkPtr_t wkPtr_;
      }; // class LevelSetEdge

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

#endif // HPP_MANIPULATION_GRAPH_EDGE_HH