problem-solver.hh

Go to the documentation of this file.

//
// Copyright (c) 2005, 2006, 2007, 2008, 2009, 2010, 2011 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core 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-core 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-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_PROBLEM_SOLVER_HH
# define HPP_CORE_PROBLEM_SOLVER_HH

# include <stdexcept>
# include <boost/function.hpp>

# include <hpp/pinocchio/fwd.hh>

# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/deprecated.hh>
# include <hpp/core/container.hh>

namespace hpp {
  namespace core {
    class Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead
    {
    };
    typedef boost::function < DevicePtr_t (const std::string&) > RobotBuilder_t;
    typedef boost::function < PathOptimizerPtr_t (const Problem&) >
      PathOptimizerBuilder_t;
    typedef boost::function < PathPlannerPtr_t (const Problem&,
        const RoadmapPtr_t&) >
      PathPlannerBuilder_t;
    typedef boost::function < PathValidationPtr_t (const DevicePtr_t&,
        const value_type&) >
      PathValidationBuilder_t;
    typedef boost::function < ConfigValidationPtr_t (const DevicePtr_t&) >
      ConfigValidationBuilder_t;
    typedef boost::function <PathProjectorPtr_t (const Problem&,
        const value_type&) >
      PathProjectorBuilder_t;
    typedef boost::function <ConfigurationShooterPtr_t (const Problem&) >
      ConfigurationShooterBuilder_t;
  typedef boost::function <DistancePtr_t (const Problem&) >
    DistanceBuilder_t;
    typedef boost::function <SteeringMethodPtr_t (const Problem&) >
      SteeringMethodBuilder_t;
    typedef std::vector<std::pair < std::string, CollisionObjectPtr_t > > AffordanceObjects_t;
    typedef vector3_t AffordanceConfig_t;

    class HPP_CORE_DLLAPI ProblemSolver
    {
    public:

      typedef std::vector <PathOptimizerPtr_t> PathOptimizers_t;
      typedef std::vector <std::string> PathOptimizerTypes_t;
      typedef std::vector <std::string> ConfigValidationTypes_t;

      static ProblemSolverPtr_t create ();

      virtual ~ProblemSolver ();

      void robotType (const std::string& type);

      const std::string& robotType () const;

      DevicePtr_t createRobot (const std::string& name);

      virtual void robot (const DevicePtr_t& robot);

      const DevicePtr_t& robot () const;

      ProblemPtr_t problem ()
      {
    return problem_;
      }
      const ConfigurationPtr_t& initConfig () const
      {
    return initConf_;
      }
      virtual void initConfig (const ConfigurationPtr_t& config);
      const Configurations_t& goalConfigs () const;
      virtual void addGoalConfig (const ConfigurationPtr_t& config);
      void resetGoalConfigs ();
      /*
      void addGoalConstraint (const ConstraintPtr_t& constraint);
      void addGoalConstraint (const LockedJointPtr_t& lj);
      void addGoalConstraint (const std::string& constraintName,
          const std::string& functionName, const std::size_t priority);
      void resetGoalConstraint ();
      */
      virtual void pathPlannerType (const std::string& type);
      const std::string& pathPlannerType () const {
        return pathPlannerType_;
      }
      void distanceType (const std::string& type);
      const std::string& distanceType () const {
        return distanceType_;
      }
      void steeringMethodType (const std::string& type);
      const std::string& steeringMethodType () const {
        return steeringMethodType_;
      }
      void configurationShooterType (const std::string& type);
      const std::string& configurationShooterType () const {
        return configurationShooterType_;
      }
      const PathPlannerPtr_t& pathPlanner () const
      {
    return pathPlanner_;
      }

      void addPathOptimizer (const std::string& type);
      const PathOptimizerTypes_t& pathOptimizerTypes () const {
        return pathOptimizerTypes_;
      }
      void clearPathOptimizers ();
      const PathOptimizerPtr_t& pathOptimizer (std::size_t rank) const
      {
    return pathOptimizers_ [rank];
      }

      void optimizePath (PathVectorPtr_t path);

      virtual void pathValidationType (const std::string& type,
                                       const value_type& tolerance);
      const std::string& pathValidationType (value_type& tolerance) const {
        tolerance = pathValidationTolerance_;
        return pathValidationType_;
      }

      void pathProjectorType (const std::string& type,
                  const value_type& step);

      const std::string& pathProjectorType (value_type& tolerance) const {
        tolerance = pathProjectorTolerance_;
        return pathProjectorType_;
      }

      virtual void addConfigValidation (const std::string& type);

      const ConfigValidationTypes_t configValidationTypes () {
        return configValidationTypes_;
      }

      // Clear the vector of config validations
      void clearConfigValidations ();

      void addConfigValidationBuilder (const std::string& type,
            const ConfigValidationBuilder_t& builder);

      const RoadmapPtr_t& roadmap () const
      {
    return roadmap_;
      }


      void addConstraint (const ConstraintPtr_t& constraint);

      const ConstraintSetPtr_t& constraints () const
      {
    return constraints_;
      }

      virtual void resetConstraints ();

      virtual void addNumericalConstraintToConfigProjector
    (const std::string& configProjName, const std::string& constraintName,
         const std::size_t priority = 0);

      virtual void addLockedJointToConfigProjector
    (const std::string& configProjName, const std::string& lockedJointName)
        HPP_CORE_DEPRECATED;

      void addNumericalConstraint (const std::string& name,
                   const constraints::ImplicitPtr_t&
                   constraint)
      {
        numericalConstraints.add (name, constraint);
      }

      void comparisonType (const std::string& name,
               const ComparisonTypes_t types);

      void comparisonType (const std::string& name,
                           const ComparisonType &type);

      ComparisonTypes_t comparisonType (const std::string& name) const;

      constraints::ImplicitPtr_t numericalConstraint (const std::string& name)
      {
        return numericalConstraints.get(name, constraints::ImplicitPtr_t());
      }

      void computeValueAndJacobian (const Configuration_t& configuration,
                    vector_t& value, matrix_t& jacobian) const;

      void maxIterProjection (size_type iterations);
      size_type maxIterProjection () const
      {
    return maxIterProjection_;
      }

      void maxIterPathPlanning (size_type iterations);
      size_type maxIterPathPlanning () const
      {
    return maxIterPathPlanning_;
      }

      void setTimeOutPathPlanning(double timeOut){
        timeOutPathPlanning_ = timeOut;
      }

      double getTimeOutPathPlanning(){
        return timeOutPathPlanning_;
      }


      void errorThreshold (const value_type& threshold);
      value_type errorThreshold () const
      {
    return errorThreshold_;
      }

      virtual void resetProblem ();

      virtual void resetRoadmap ();


      void createPathOptimizers ();

      virtual bool prepareSolveStepByStep ();

      virtual bool executeOneStep ();

      virtual void finishSolveStepByStep ();

      virtual void solve ();

      bool directPath (ConfigurationIn_t start, ConfigurationIn_t end,
               bool validate, std::size_t& pathId, std::string& report);

      void addConfigToRoadmap (const ConfigurationPtr_t& config);

      void addEdgeToRoadmap (const ConfigurationPtr_t& config1,
                 const ConfigurationPtr_t& config2,
                 const PathPtr_t& path);

      void interrupt ();

      std::size_t addPath (const PathVectorPtr_t& path)
      {
        std::size_t s = paths_.size();
    paths_.push_back (path);
        return s;
      }

      void erasePath (std::size_t pathId)
      {
    PathVectors_t::iterator it = paths_.begin();
    std::advance(it, pathId);

    paths_.erase(it);
      }

      const PathVectors_t& paths () const
      {
    return paths_;
      }


      virtual void addObstacle (const DevicePtr_t& device, bool collision,
            bool distance);

      virtual void addObstacle (const CollisionObjectPtr_t &inObject, bool collision,
            bool distance);

      virtual void removeObstacle (const std::string& name);

      virtual void addObstacle (const std::string& name,
          /*const*/ FclCollisionObject &inObject,
          bool collision,
          bool distance);

      void removeObstacleFromJoint (const std::string& jointName,
                    const std::string& obstacleName);

      void cutObstacle (const std::string& name, const fcl::AABB& aabb);

      void filterCollisionPairs ();

      CollisionObjectPtr_t obstacle (const std::string& name) const;

      const Transform3f& obstacleFramePosition (const std::string& name) const;

      std::list <std::string> obstacleNames (bool collision, bool distance)
    const;

      const DistanceBetweenObjectsPtr_t& distanceBetweenObjects () const
      {
    return distanceBetweenObjects_;
      }

      pinocchio::GeomModelPtr_t obstacleGeomModel () const
      { return obstacleModel_; }
      pinocchio::GeomDataPtr_t  obstacleGeomData () const
      { return obstacleData_; }

      const ObjectStdVector_t& collisionObstacles () const;
      const ObjectStdVector_t& distanceObstacles () const;

      void roadmap (const RoadmapPtr_t& roadmap)
      {
    roadmap_ = roadmap;
      }

      void initDistance ();

      void initSteeringMethod ();

      void initPathProjector ();

      void initPathValidation ();

      void initConfigValidation ();

      void initValidations ();

      virtual void initProblemTarget ();

      Container <RobotBuilder_t>                robots;
      Container <ConfigurationShooterBuilder_t> configurationShooters;
      Container <SteeringMethodBuilder_t>       steeringMethods;
      Container <DistanceBuilder_t>             distances;
      Container <PathValidationBuilder_t>       pathValidations;
      Container <ConfigValidationBuilder_t>     configValidations;
      Container <PathProjectorBuilder_t>        pathProjectors;
      Container <PathPlannerBuilder_t>          pathPlanners;
      Container <PathOptimizerBuilder_t>        pathOptimizers;

      Container <constraints::ImplicitPtr_t>      numericalConstraints;
      Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead                    lockedJoints;
      Container <CenterOfMassComputationPtr_t>  centerOfMassComputations;
      Container <segments_t>                    passiveDofs;
      Container <JointAndShapes_t>              jointAndShapes;
      Container <AffordanceObjects_t>           affordanceObjects;
      Container <AffordanceConfig_t>            affordanceConfigs;

    protected:
      ProblemSolver ();

      ConstraintSetPtr_t constraints_;

      void problem (ProblemPtr_t problem);

      virtual void initializeProblem (ProblemPtr_t problem);

      DevicePtr_t robot_;
      ProblemPtr_t problem_;

      PathPlannerPtr_t pathPlanner_;
      RoadmapPtr_t roadmap_;
      PathVectors_t paths_;
      std::string pathProjectorType_;
      value_type pathProjectorTolerance_;

      std::string pathPlannerType_;

      ProblemTargetPtr_t target_;
    private:
      ConfigurationPtr_t initConf_;
      Configurations_t goalConfigurations_;
      std::string robotType_;
      std::string configurationShooterType_;
      std::string distanceType_;
      std::string steeringMethodType_;
      PathOptimizerTypes_t pathOptimizerTypes_;
      PathOptimizers_t pathOptimizers_;
      std::string pathValidationType_;
      value_type pathValidationTolerance_;
      // Config validation methods
      ConfigValidationTypes_t configValidationTypes_;
      ObjectStdVector_t collisionObstacles_; // FIXME should be removed?
      ObjectStdVector_t distanceObstacles_;  // FIXME should be removed?
      pinocchio::ModelPtr_t obstacleRModel_; // Contains the frames
      pinocchio::DataPtr_t  obstacleRData_;  // Contains the frames
      pinocchio::GeomModelPtr_t obstacleModel_;
      pinocchio::GeomDataPtr_t  obstacleData_;
      // Tolerance for numerical constraint resolution
      value_type errorThreshold_;
      // Maximal number of iterations for numerical constraint resolution
      size_type maxIterProjection_;
      unsigned long int maxIterPathPlanning_;
      double timeOutPathPlanning_;
      segmentsMap_t passiveDofsMap_;
      CenterOfMassComputationMap_t comcMap_;
      DistanceBetweenObjectsPtr_t distanceBetweenObjects_;

      void initProblem ();
    }; // class ProblemSolver
  } // namespace core
} // namespace hpp

#endif // HPP_CORE_PROBLEM_SOLVER_HH