explicit-constraint-set.hh

Go to the documentation of this file.

// Copyright (c) 2017, Joseph Mirabel
// 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_EXPLICIT_CONSTRAINT_SET_HH
#define HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH
 
#include <hpp/constraints/config.hh>
#include <hpp/constraints/differentiable-function-set.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/matrix-view.hh>
#include <vector>
 
namespace hpp {
namespace constraints {
 
class HPP_CONSTRAINTS_DLLAPI ExplicitConstraintSet {
 public:
  typedef Eigen::RowBlockIndices RowBlockIndices;
  typedef Eigen::ColBlockIndices ColBlockIndices;
  typedef Eigen::MatrixBlockView<matrix_t, Eigen::Dynamic, Eigen::Dynamic,
                                 false, false>
      MatrixBlockView;
 
 
  bool solve(vectorOut_t arg) const;
 
  size_type errorSize() const { return errorSize_; }
 
  bool isSatisfied(vectorIn_t arg, value_type errorThreshold = -1) const;
 
  bool isSatisfied(vectorIn_t arg, vectorOut_t error,
                   value_type errorThreshold = -1) const;
 
  bool isConstraintSatisfied(const ImplicitPtr_t& constraint, vectorIn_t arg,
                             vectorOut_t error, bool& constraintFound) const;
 
 
  size_type add(const ExplicitPtr_t& constraint);
 
  bool contains(const ExplicitPtr_t& numericalConstraint) const;
 
  ExplicitConstraintSet(const LiegroupSpacePtr_t& space)
      : configSpace_(space),
        inArgs_(),
        notOutArgs_(),
        inDers_(),
        notOutDers_(),
        outArgs_(),
        outDers_(),
        argFunction_(Eigen::VectorXi::Constant(space->nq(), -1)),
        derFunction_(Eigen::VectorXi::Constant(space->nv(), -1)),
        errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
        errorSize_(0)
        // , Jg (nv, nv)
        ,
        arg_(space->nq()),
        diff_(space->nv()),
        diffSmall_() {
    notOutArgs_.addRow(0, space->nq());
    notOutDers_.addCol(0, space->nv());
  }
 
  ExplicitConstraintSet(const ExplicitConstraintSet& other)
      : configSpace_(other.configSpace_),
        inArgs_(other.inArgs_),
        notOutArgs_(other.notOutArgs_),
        inDers_(other.inDers_),
        notOutDers_(other.notOutDers_),
        outArgs_(other.outArgs_),
        outDers_(other.outDers_),
        inOutDependencies_(other.inOutDependencies_),
        data_(other.data_),
        computationOrder_(other.computationOrder_),
        argFunction_(other.argFunction_),
        derFunction_(other.derFunction_),
        errorThreshold_(other.errorThreshold_),
        errorSize_(other.errorSize_),
        arg_(other.arg_),
        diff_(other.diff_),
        diffSmall_(other.diffSmall_) {}
 
 
  void errorThreshold(const value_type& threshold) {
    errorThreshold_ = threshold;
  }
  value_type errorThreshold() const { return errorThreshold_; }
  value_type squaredErrorThreshold() const {
    return errorThreshold_ * errorThreshold_;
  }
 
 
 
  const RowBlockIndices& inArgs() const { return inArgs_; }
 
  const ColBlockIndices& inDers() const { return inDers_; }
 
  const RowBlockIndices& notOutArgs() const { return notOutArgs_; }
 
  const ColBlockIndices& notOutDers() const { return notOutDers_; }
 
  ColBlockIndices activeParameters() const;
 
  const ColBlockIndices& activeDerivativeParameters() const;
 
  const Eigen::MatrixXi& inOutDependencies() const {
    return inOutDependencies_;
  }
 
  Eigen::MatrixXi inOutDofDependencies() const;
 
  const Eigen::VectorXi& derFunction() const { return derFunction_; }
 
  const RowBlockIndices& outArgs() const { return outArgs_; }
 
  const RowBlockIndices& outDers() const { return outDers_; }
 
  LiegroupSpacePtr_t configSpace() const { return configSpace_; }
 
  std::size_t nq() const { return configSpace_->nq(); }
 
  std::size_t nv() const { return configSpace_->nv(); }
 
 
  inline MatrixBlockView jacobianNotOutToOut(matrix_t& jacobian) const {
    return MatrixBlockView(jacobian, outDers_.nbIndices(), outDers_.indices(),
                           notOutDers_.nbIndices(), notOutDers_.indices());
  }
 
  void jacobian(matrixOut_t jacobian, vectorIn_t q) const;
 
 
  vector_t rightHandSideFromInput(vectorIn_t p);
 
  bool rightHandSideFromInput(const ExplicitPtr_t& constraint, vectorIn_t p);
 
  void rightHandSideFromInput(const size_type& i, vectorIn_t p);
 
  void rightHandSide(vectorIn_t rhs);
 
  bool rightHandSide(const ExplicitPtr_t& constraint, vectorIn_t rhs);
 
  bool getRightHandSide(const ExplicitPtr_t& constraint, vectorOut_t rhs) const;
 
  void rightHandSide(const size_type& i, vectorIn_t rhs);
 
  vector_t rightHandSide() const;
 
  size_type rightHandSideSize() const;
 
 
  std::ostream& print(std::ostream& os) const;
 
 private:
  typedef std::vector<bool> Computed_t;
 
  void solveExplicitConstraint(const std::size_t& i, vectorOut_t arg) const;
  void computeJacobian(const std::size_t& i, matrixOut_t J) const;
  void computeOrder(const std::size_t& iF, std::size_t& iOrder,
                    Computed_t& computed);
 
  void replace(const ConstraintReplacement_t& oldToNew);
 
  LiegroupSpacePtr_t configSpace_;
 
  struct Data {
    Data(const ExplicitPtr_t& constraint);
    ExplicitPtr_t constraint;
    RowBlockIndices equalityIndices;
    LiegroupElement rhs_implicit;
    // implicit formulation
    mutable LiegroupElement h_value;
    // explicit formulation
    mutable vector_t qin, qout;
    mutable LiegroupElement f_value, res_qout;
    // jacobian of f
    mutable matrix_t jacobian;
  };  // struct Data
 
  RowBlockIndices inArgs_, notOutArgs_;
  ColBlockIndices inDers_, notOutDers_;
  RowBlockIndices outArgs_, outDers_;
 
  Eigen::MatrixXi inOutDependencies_;
 
  std::vector<Data> data_;
  std::vector<std::size_t> computationOrder_;
  Eigen::VectorXi argFunction_, derFunction_;
  value_type errorThreshold_;
  size_type errorSize_;
  // mutable matrix_t Jg;
  mutable vector_t arg_, diff_, diffSmall_;
 
  ExplicitConstraintSet()
      : inArgs_(),
        notOutArgs_(),
        inDers_(),
        notOutDers_(),
        outArgs_(),
        outDers_(),
        errorThreshold_(Eigen::NumTraits<value_type>::epsilon()),
        errorSize_(0) {}
  void init(const LiegroupSpacePtr_t& space) {
    configSpace_ = space;
    argFunction_ = Eigen::VectorXi::Constant(space->nq(), -1);
    derFunction_ = Eigen::VectorXi::Constant(space->nv(), -1);
    arg_.resize(space->nq());
    diff_.resize(space->nv());
 
    notOutArgs_.addRow(0, space->nq());
    notOutDers_.addCol(0, space->nv());
  }
 
  friend class solver::BySubstitution;
};  // class ExplicitConstraintSet
}  // namespace constraints
}  // namespace hpp
 
#endif  // HPP_CONSTRAINTS_EXPLICIT_CONSTRAINT_SET_HH