problem-solver.hh

Go to the documentation of this file.

//
// Copyright (c) 2005, 2006, 2007, 2008, 2009, 2010, 2011 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_CORE_PROBLEM_SOLVER_HH
#define HPP_CORE_PROBLEM_SOLVER_HH
 
#include <functional>
#include <hpp/core/config.hh>
#include <hpp/core/container.hh>
#include <hpp/core/deprecated.hh>
#include <hpp/core/fwd.hh>
#include <hpp/pinocchio/fwd.hh>
#include <stdexcept>
 
namespace hpp {
namespace core {
class
    Member_lockedJoints_in_class_ProblemSolver_has_been_removed_use_member_numericalConstraints_instead {
};
typedef std::function<DevicePtr_t(const std::string&)> RobotBuilder_t;
typedef std::function<PathOptimizerPtr_t(const ProblemConstPtr_t&)>
    PathOptimizerBuilder_t;
typedef std::function<PathPlannerPtr_t(const ProblemConstPtr_t&,
                                       const RoadmapPtr_t&)>
    PathPlannerBuilder_t;
typedef std::function<PathValidationPtr_t(const DevicePtr_t&,
                                          const value_type&)>
    PathValidationBuilder_t;
typedef std::function<ConfigValidationPtr_t(const DevicePtr_t&)>
    ConfigValidationBuilder_t;
typedef std::function<PathProjectorPtr_t(const ProblemConstPtr_t&,
                                         const value_type&)>
    PathProjectorBuilder_t;
typedef std::function<ConfigurationShooterPtr_t(const ProblemConstPtr_t&)>
    ConfigurationShooterBuilder_t;
typedef std::function<DistancePtr_t(const ProblemConstPtr_t&)>
    DistanceBuilder_t;
typedef std::function<SteeringMethodPtr_t(const ProblemConstPtr_t&)>
    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 Configuration_t& initConfig() const { return initConf_; }
  virtual void initConfig(ConfigurationIn_t config);
  const Configurations_t& goalConfigs() const;
  virtual void addGoalConfig(ConfigurationIn_t config);
  void resetGoalConfigs();
  void setGoalConstraints(const NumericalConstraints_t& constraints);
  void resetGoalConstraints();
  /*
  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);
 
  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(ConfigurationIn_t config);
 
  void addEdgeToRoadmap(ConfigurationIn_t config1, ConfigurationIn_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 CollisionGeometryPtr_t& inObject,
                           const Transform3s& pose, bool collision,
                           bool distance);
 
  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 coal::AABB& aabb);
 
  void filterCollisionPairs();
 
  CollisionObjectPtr_t obstacle(const std::string& name) const;
 
  const Transform3s& 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:
  Configuration_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