transition-planner.hh

Go to the documentation of this file.

// Copyright (c) 2023 CNRS
// Authors: Florent Lamiraux
//
 
// 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_PATH_PLANNER_TRANSITION_PLANNER_HH
#define HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH
 
#include <hpp/core/path-planner.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/graph/fwd.hh>
 
namespace hpp {
namespace manipulation {
namespace pathPlanner {
 
 
class HPP_MANIPULATION_DLLAPI TransitionPlanner : public core::PathPlanner {
 public:
  typedef core::PathPlannerPtr_t PathPlannerPtr_t;
  typedef core::PathProjectorPtr_t PathProjectorPtr_t;
  typedef core::PathPtr_t PathPtr_t;
  typedef core::PathOptimizerPtr_t PathOptimizerPtr_t;
  typedef core::PathVector PathVector;
  typedef core::PathVectorPtr_t PathVectorPtr_t;
  typedef core::Parameter Parameter;
 
  static TransitionPlannerPtr_t createWithRoadmap(
      const core::ProblemConstPtr_t& problem,
      const core::RoadmapPtr_t& roadmap);
  PathPlannerPtr_t innerPlanner() const { return innerPlanner_; }
  void innerPlanner(const PathPlannerPtr_t& planner) {
    innerPlanner_ = planner;
  }
  core::ProblemPtr_t innerProblem() const { return innerProblem_; }
  virtual void startSolve();
 
  virtual void oneStep();
 
  PathVectorPtr_t planPath(const Configuration_t qInit, matrixIn_t qGoals,
                           bool resetRoadmap);
  PathPtr_t directPath(ConfigurationIn_t q1, ConfigurationIn_t q2,
                       bool validate, bool& success, std::string& status);
  bool validateConfiguration(ConfigurationIn_t q, std::size_t id,
                             core::ValidationReportPtr_t& report) const;
  PathVectorPtr_t optimizePath(const PathPtr_t& path);
 
  PathVectorPtr_t timeParameterization(const PathVectorPtr_t& path);
 
  void setEdge(std::size_t id);
 
  void setReedsAndSheppSteeringMethod(double turningRadius);
 
  void pathProjector(const PathProjectorPtr_t pathProjector);
 
  void clearPathOptimizers();
 
  void addPathOptimizer(const PathOptimizerPtr_t& pathOptimizer);
 
  void setParameter(const std::string& key, const Parameter& value);
 
 protected:
  TransitionPlanner(const core::ProblemConstPtr_t& problem,
                    const core::RoadmapPtr_t& roadmap);
  void init(TransitionPlannerWkPtr_t weak);
 
 private:
  void checkProblemAndForwardParameters();
  graph::EdgePtr_t getEdgeOrThrow(std::size_t id) const;
  core::ProblemPtr_t innerProblem_;
  PathPlannerPtr_t innerPlanner_;
  std::vector<PathOptimizerPtr_t> pathOptimizers_;
  core::PathOptimizerPtr_t timeParameterization_;
  TransitionPlannerWkPtr_t weakPtr_;
};  // class TransitionPlanner
}  // namespace pathPlanner
}  // namespace manipulation
}  // namespace hpp
#endif  // HPP_MANIPULATION_PATH_PLANNER_TRANSITION_PLANNER_HH