config-projector.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 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_CONFIG_PROJECTOR_HH
#define HPP_CORE_CONFIG_PROJECTOR_HH
 
#include <Eigen/SVD>
#include <hpp/core/config.hh>
#include <hpp/core/constraint.hh>
#include <hpp/core/deprecated.hh>
#include <hpp/statistics/success-bin.hh>
 
namespace hpp {
namespace core {
typedef constraints::solver::BySubstitution BySubstitution;
 
 
class HPP_CORE_DLLAPI ConfigProjector : public Constraint {
 public:
  enum LineSearchType { Backtracking, ErrorNormBased, FixedSequence, Constant };
 
  static ConfigProjectorPtr_t create(const DevicePtr_t& robot,
                                     const std::string& name,
                                     value_type errorThreshold,
                                     size_type maxIterations);
 
  static ConfigProjectorPtr_t createCopy(const ConfigProjectorPtr_t cp);
 
  virtual ConstraintPtr_t copy() const;
 
  virtual ~ConfigProjector();
 
  bool contains(const constraints::ImplicitPtr_t& numericalConstraint) const;
 
  bool add(const constraints::ImplicitPtr_t& numericalConstraint,
           const std::size_t priority = 0);
 
  void lastIsOptional(bool optional);
 
  bool lastIsOptional() const;
 
  bool optimize(ConfigurationOut_t config, std::size_t maxIter = 0);
 
  const DevicePtr_t& robot() const { return robot_; }
 
  void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity,
                             vectorOut_t result);
 
  virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to,
                               ConfigurationOut_t result);
 
  void computeValueAndJacobian(ConfigurationIn_t configuration,
                               vectorOut_t value, matrixOut_t reducedJacobian);
 
 
  size_type numberFreeVariables() const;
 
  size_type dimension() const;
 
  void compressVector(vectorIn_t normal, vectorOut_t small) const;
 
  void uncompressVector(vectorIn_t small, vectorOut_t normal) const;
 
  void compressMatrix(matrixIn_t normal, matrixOut_t small,
                      bool rows = true) const;
 
  void uncompressMatrix(matrixIn_t small, matrixOut_t normal,
                        bool rows = true) const;
 
 
  void maxIterations(size_type iterations);
  size_type maxIterations() const;
 
  void errorThreshold(const value_type& threshold);
  value_type errorThreshold() const;
 
  value_type residualError() const;
 
  const value_type& sigma() const;
 
 
  vector_t rightHandSideFromConfig(ConfigurationIn_t config);
 
  void rightHandSideFromConfig(const constraints::ImplicitPtr_t& nm,
                               ConfigurationIn_t config);
 
  void rightHandSide(const vector_t& param);
 
  void rightHandSide(const constraints::ImplicitPtr_t& nm, vectorIn_t rhs);
 
  vector_t rightHandSide() const;
 
  void rightHandSideAt(const value_type& s);
 
 
  virtual bool isSatisfied(ConfigurationIn_t config);
 
  virtual bool isSatisfied(ConfigurationIn_t config, value_type errorThreshold);
 
  virtual bool isSatisfied(ConfigurationIn_t config, vector_t& error);
 
  ::hpp::statistics::SuccessStatistics& statistics() { return statistics_; }
 
  const NumericalConstraints_t& numericalConstraints() const;
 
  const BySubstitution& solver() const { return *solver_; }
 
  BySubstitution& solver() { return *solver_; }
 
  void lineSearchType(LineSearchType ls) { lineSearchType_ = ls; }
 
  LineSearchType lineSearchType() const { return lineSearchType_; }
 
  static void defaultLineSearch(LineSearchType ls);
 
 protected:
  ConfigProjector(const DevicePtr_t& robot, const std::string& name,
                  value_type errorThreshold, size_type maxIterations);
  ConfigProjector(const ConfigProjector& cp);
 
  void init(const ConfigProjectorPtr_t& self) {
    Constraint::init(self);
    weak_ = self;
  }
  virtual bool impl_compute(ConfigurationOut_t configuration);
 
 private:
  virtual std::ostream& print(std::ostream& os) const;
 
  DevicePtr_t robot_;
  static LineSearchType defaultLineSearch_;
  LineSearchType lineSearchType_;
  BySubstitution* solver_;
 
  bool solverOneStep(ConfigurationOut_t config) const;
  int solverSolve(ConfigurationOut_t config) const;
 
  ConfigProjectorWkPtr_t weak_;
  ::hpp::statistics::SuccessStatistics statistics_;
 
  ConfigProjector() {}
  HPP_SERIALIZABLE();
};  // class ConfigProjector
}  // namespace core
}  // namespace hpp
#endif  // HPP_CORE_CONFIG_PROJECTOR_HH