by-substitution.hh

Go to the documentation of this file.

// Copyright (c) 2017, 2018 CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
 
// 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_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
#define HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH
 
#include <hpp/constraints/config.hh>
#include <hpp/constraints/explicit-constraint-set.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/constraints/solver/hierarchical-iterative.hh>
#include <vector>
 
namespace hpp {
namespace constraints {
namespace solver {
 
class HPP_CONSTRAINTS_DLLAPI BySubstitution
    : public solver::HierarchicalIterative {
 public:
  BySubstitution(const LiegroupSpacePtr_t& configSpace);
  BySubstitution(const BySubstitution& other);
 
  virtual ~BySubstitution() {}
 
  bool add(const ImplicitPtr_t& numericalConstraint,
           const std::size_t& priority = 0);
 
  BySubstitution extract(interval_t interval);
 
  const NumericalConstraints_t& numericalConstraints() const {
    return constraints_;
  }
 
  ExplicitConstraintSet& explicitConstraintSet() { return explicit_; }
 
  const ExplicitConstraintSet& explicitConstraintSet() const {
    return explicit_;
  }
 
  size_type numberFreeVariables() const {
    return explicitConstraintSet().notOutDers().nbIndices();
  }
 
  void explicitConstraintSetHasChanged();
 
  template <typename LineSearchType>
  Status solve(vectorOut_t arg, LineSearchType ls = LineSearchType()) const {
    return solve<LineSearchType>(arg, false, ls);
  }
 
  template <typename LineSearchType>
  Status solve(vectorOut_t arg, bool optimize,
               LineSearchType ls = LineSearchType()) const {
    // TODO when there are only locked joint explicit constraints,
    // there is no need for this intricated loop.
    // if (explicit_.isConstant()) {
    // explicit_.solve(arg);
    // iterative_.solve(arg, ls);
    // } else {
    return impl_solve(arg, optimize, ls);
    // }
  }
 
  void projectVectorOnKernel(ConfigurationIn_t from, vectorIn_t velocity,
                             vectorOut_t result) const;
 
  virtual void projectOnKernel(ConfigurationIn_t from, ConfigurationIn_t to,
                               ConfigurationOut_t result);
 
  inline Status solve(vectorOut_t arg) const {
    return solve(arg, DefaultLineSearch());
  }
 
 
  vector_t rightHandSideFromConfig(ConfigurationIn_t config);
 
  bool rightHandSideFromConfig(const ImplicitPtr_t& constraint,
                               ConfigurationIn_t config);
 
  bool rightHandSide(const ImplicitPtr_t& constraint, vectorIn_t rhs);
 
  bool getRightHandSide(const ImplicitPtr_t& constraint, vectorOut_t rhs) const;
 
  void rightHandSide(vectorIn_t rhs);
 
  vector_t rightHandSide() const;
 
  size_type rightHandSideSize() const;
 
 
  size_type errorSize() const {
    return dimension() + explicit_.outDers().nbIndices();
  }
 
  bool isSatisfied(vectorIn_t arg) const {
    return solver::HierarchicalIterative::isSatisfied(arg) &&
           explicit_.isSatisfied(arg);
  }
 
  bool isSatisfied(vectorIn_t arg, value_type errorThreshold) const {
    return solver::HierarchicalIterative::isSatisfied(arg, errorThreshold) &&
           explicit_.isSatisfied(arg, errorThreshold);
  }
  bool isSatisfied(vectorIn_t arg, vectorOut_t error) const {
    assert(error.size() == dimension() + explicit_.errorSize());
    bool iterative = solver::HierarchicalIterative::isSatisfied(arg);
    residualError(error.head(dimension()));
    bool _explicit =
        explicit_.isSatisfied(arg, error.tail(explicit_.errorSize()));
    return iterative && _explicit;
  }
 
  bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
                             vectorOut_t error, bool& constraintFound) const;
 
  template <typename LineSearchType>
  bool oneStep(vectorOut_t arg, LineSearchType& lineSearch) const {
    computeValue<true>(arg);
    updateJacobian(arg);
    computeDescentDirection();
    lineSearch(*this, arg, dq_);
    explicit_.solve(arg);
    return solver::HierarchicalIterative::isSatisfied(arg);
  }
 
  void updateJacobian(vectorIn_t arg) const;
 
  void errorThreshold(const value_type& threshold) {
    solver::HierarchicalIterative::errorThreshold(threshold);
    explicit_.errorThreshold(threshold);
  }
  value_type errorThreshold() const {
    return solver::HierarchicalIterative::errorThreshold();
  }
 
  segments_t implicitDof() const;
 
  virtual std::ostream& print(std::ostream& os) const;
 
  bool integrate(vectorIn_t from, vectorIn_t velocity,
                 vectorOut_t result) const {
    bool res = solver::HierarchicalIterative::integrate(from, velocity, result);
    explicit_.solve(result);
    return res;
  }
 
 protected:
  void computeActiveRowsOfJ(std::size_t iStack);
 
 private:
  typedef solver::HierarchicalIterative parent_t;
 
  template <typename LineSearchType>
  Status impl_solve(vectorOut_t arg, bool optimize, LineSearchType ls) const;
 
  ExplicitConstraintSet explicit_;
  mutable matrix_t Je_, JeExpanded_;
 
  BySubstitution() {}
  HPP_SERIALIZABLE_SPLIT();
};  // class BySubstitution
 
}  // namespace solver
}  // namespace constraints
}  // namespace hpp
 
#endif  // HPP_CONSTRAINTS_SOLVER_BY_SUBSTITUTION_HH