manipulation-planner.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_MANIPULATION_PLANNER_HH
# define HPP_MANIPULATION_MANIPULATION_PLANNER_HH

#include <hpp/core/path-planner.hh>

#include <hpp/statistics/success-bin.hh>

#include "hpp/manipulation/graph/statistics.hh"

#include "hpp/manipulation/config.hh"
#include "hpp/manipulation/graph/fwd.hh"
#include "hpp/manipulation/graph/graph.hh"
#include "hpp/manipulation/fwd.hh"

namespace hpp {
  namespace manipulation {

    class HPP_MANIPULATION_DLLAPI ManipulationPlanner :
      public hpp::core::PathPlanner
    {
      public:
        typedef std::list<std::size_t> ErrorFreqs_t;

        static ManipulationPlannerPtr_t create (const core::Problem& problem,
            const core::RoadmapPtr_t& roadmap);

        virtual void oneStep ();

        bool extend (RoadmapNodePtr_t q_near,
            const ConfigurationPtr_t &q_rand, core::PathPtr_t& validPath);

        ErrorFreqs_t getEdgeStat (const graph::EdgePtr_t& edge) const;

        static StringList_t errorList ();

      protected:
        ManipulationPlanner (const Problem& problem,
            const RoadmapPtr_t& roadmap);

        void init (const ManipulationPlannerWkPtr_t& weak);

      private:
        std::size_t tryConnectToRoadmap (const core::Nodes_t nodes);
        std::size_t tryConnectNewNodes (const core::Nodes_t nodes);

        ConfigurationShooterPtr_t shooter_;
        const Problem& problem_;
        RoadmapPtr_t roadmap_;
        ManipulationPlannerWkPtr_t weakPtr_;

        typedef ::hpp::statistics::SuccessStatistics SuccessStatistics;
        typedef ::hpp::statistics::SuccessBin SuccessBin;
        typedef ::hpp::statistics::SuccessBin::Reason Reason;
        SuccessStatistics& edgeStat (const graph::EdgePtr_t& edge);
        std::vector<size_type> indexPerEdgeStatistics_;
        std::vector<SuccessStatistics> perEdgeStatistics_;

        enum TypeOfFailure {
          PATH_PROJECTION_SHORTER = 0,
          PATH_VALIDATION_SHORTER = 1,
      REACHED_DESTINATION_NODE = 2,
      FAILURE = 3,
          PROJECTION = 4,
          STEERING_METHOD = 5,
          PATH_VALIDATION_ZERO = 6,
          PATH_PROJECTION_ZERO = 7
        };
        static const std::vector<Reason> reasons_;

        value_type extendStep_;

        mutable Configuration_t qProj_;
    };
  } // namespace manipulation
} // namespace hpp

#endif // HPP_MANIPULATION_MANIPULATION_PLANNER_HH