states-path-finder.hh

Go to the documentation of this file.

// Copyright (c) 2017, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr),
//          Florent Lamiraux (florent.lamiraux@laas.fr)
//          Alexandre Thiault (athiault@laas.fr)
//          Le Quang Anh (quang-anh.le@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_PATH_PLANNER_STATES_PATH_FINDER_HH
#define HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH
 
#include <hpp/core/config-projector.hh>
#include <hpp/core/config-validations.hh>
#include <hpp/core/fwd.hh>
#include <hpp/core/path-planner.hh>
#include <hpp/core/path.hh>
#include <hpp/core/projection-error.hh>
#include <hpp/core/validation-report.hh>
#include <hpp/manipulation/config.hh>
#include <hpp/manipulation/fwd.hh>
#include <hpp/manipulation/problem.hh>
#include <unordered_map>
 
namespace hpp {
namespace manipulation {
namespace pathPlanner {
 
 
class HPP_MANIPULATION_DLLAPI StatesPathFinder : public core::PathPlanner {
 public:
  struct OptimizationData;
 
  virtual ~StatesPathFinder() {};
 
  static StatesPathFinderPtr_t create(const core::ProblemConstPtr_t& problem);
 
  static StatesPathFinderPtr_t createWithRoadmap(
      const core::ProblemConstPtr_t& problem,
      const core::RoadmapPtr_t& roadmap);
 
  StatesPathFinderPtr_t copy() const;
 
  core::Configurations_t computeConfigList(ConfigurationIn_t q1,
                                           ConfigurationIn_t q2);
 
  // access functions for Python interface
  std::vector<std::string> constraintNamesFromSolverAtWaypoint(std::size_t wp);
  std::vector<std::string> lastBuiltTransitions() const;
  std::string displayConfigsSolved() const;
  bool buildOptimizationProblemFromNames(std::vector<std::string> names);
 
  // Substeps of method solveOptimizationProblem
  void initWPRandom(std::size_t wp);
  void initWPNear(std::size_t wp);
  void initWP(std::size_t wp, ConfigurationIn_t q);
  enum SolveStepStatus {
    VALID_SOLUTION,
    NO_SOLUTION,
    COLLISION_BEFORE,
    COLLISION_AFTER,
  };
 
  SolveStepStatus solveStep(std::size_t wp);
 
  void reset();
 
  virtual void startSolve();
  virtual void oneStep();
  virtual void tryConnectInitAndGoals();
 
 protected:
  StatesPathFinder(const core::ProblemConstPtr_t& problem,
                   const core::RoadmapPtr_t&);
  StatesPathFinder(const StatesPathFinder& other);
 
  void init(StatesPathFinderWkPtr_t weak) { weak_ = weak; }
 
 private:
  typedef constraints::solver::BySubstitution Solver_t;
  struct GraphSearchData;
 
  void gatherGraphConstraints();
 
  bool findTransitions(GraphSearchData& data) const;
 
  graph::Edges_t getTransitionList(const GraphSearchData& data,
                                   const std::size_t& i) const;
 
 
  // check that the solver either contains exactly same constraint
  // or a constraint with similar parametrizable form
  // constraint/both and constraint/complement
  bool contains(const Solver_t& solver, const ImplicitPtr_t& c) const;
 
  // check that the solver either contains exactly same constraint
  // or a stricter version of the constraint
  // constraint/both stricter than constraint and stricter than
  // constraint/complement
  bool containsStricter(const Solver_t& solver, const ImplicitPtr_t& c) const;
  bool checkConstantRightHandSide(size_type index);
  bool buildOptimizationProblem(const graph::Edges_t& transitions);
 
  void preInitializeRHS(std::size_t j, Configuration_t& q);
  bool analyseOptimizationProblem(const graph::Edges_t& transitions,
                                  core::ProblemConstPtr_t _problem);
 
  void initializeRHS(std::size_t j);
  bool solveOptimizationProblem();
 
  // Data structure used to store a constraint right hand side as value and its
  // name as key, both in form of hash numbers (so that names and rhs of two
  // constraints can be easily merge). Exemple : ConstraintMap_t map =
  // {{nameStringHash, rhsVectorHash}}; With rhsVectorHash the hash of a
  // vector_t of rights hand side constraints with hashRHS, and nameStringHash
  // the hash of a std::string - obtained for instance with std::hash.
  typedef std::unordered_map<size_t, size_t> ConstraintMap_t;  // map (name,
                                                               // rhs)
 
  Configuration_t getConfigStatus(size_type i, size_type j) const;
 
  vector_t getConstraintRHS(ImplicitPtr_t constraint, Configuration_t q) const;
 
  size_t hashRHS(vector_t rhs) const;
 
  bool checkSolvers(ConstraintMap_t const& pairMap,
                    ConstraintMap_t const& constraintMap) const;
 
  bool saveIncompatibleRHS(ConstraintMap_t& pairMap,
                           ConstraintMap_t& constraintMap, size_type const wp);
 
  // For a joint get his most, constrained with it, far parent
  core::JointConstPtr_t maximalJoint(size_t const wp, core::JointConstPtr_t a);
 
  core::Configurations_t getConfigList() const;
 
  bool checkWaypointRightHandSide(std::size_t ictr, std::size_t jslv) const;
  bool checkSolverRightHandSide(std::size_t ictr, std::size_t jslv) const;
  bool checkWaypointRightHandSide(std::size_t jslv) const;
  bool checkSolverRightHandSide(std::size_t jslv) const;
 
  void displayRhsMatrix();
  void displayStatusMatrix(const graph::Edges_t& transitions);
 
  ProblemConstPtr_t problem_;
  core::ProblemPtr_t inStateProblem_;
 
  NumericalConstraints_t constraints_;
  std::map<std::string, std::size_t> index_;
 
  std::map<ImplicitPtr_t, ImplicitPtr_t> sameRightHandSide_;
 
  std::map<ImplicitPtr_t, ImplicitPtr_t> stricterConstraints_;
 
  mutable OptimizationData* optData_;
  mutable std::shared_ptr<GraphSearchData> graphData_;
  graph::Edges_t lastBuiltTransitions_;
 
  NumericalConstraints_t goalConstraints_;
  bool goalDefinedByConstraints_;
  // Variables used across several calls to oneStep
  Configuration_t q1_, q2_;
  core::Configurations_t configList_;
  std::size_t idxConfigList_;
  size_type nTryConfigList_;
  bool solved_, interrupt_;
 
  StatesPathFinderWkPtr_t weak_;
 
};  // class StatesPathFinder
 
}  // namespace pathPlanner
}  // namespace manipulation
}  // namespace hpp
 
#endif  // HPP_MANIPULATION_PATH_PLANNER_STATES_PATH_FINDER_HH