implicit.hh

Go to the documentation of this file.

// Copyright (c) 2015 - 2018 CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr), 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_CONSTRAINTS_IMPLICIT_HH
#define HPP_CONSTRAINTS_IMPLICIT_HH
 
#include <hpp/constraints/comparison-types.hh>
#include <hpp/constraints/config.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/matrix-view.hh>
#include <hpp/pinocchio/liegroup-element.hh>
#include <hpp/util/serialization-fwd.hh>
 
namespace hpp {
namespace constraints {
class HPP_CONSTRAINTS_DLLAPI Implicit {
 public:
  bool operator==(const Implicit& other) const;
  Implicit& operator=(const Implicit& other) {
    comparison_ = other.comparison_;
    rhs_ = other.rhs_;
    parameterSize_ = other.parameterSize_;
    function_ = other.function_;
    rhsFunction_ = other.rhsFunction_;
    mask_ = other.mask_;
    activeRows_ = other.activeRows_;
    inactiveRows_ = other.inactiveRows_;
    inequalityIndices_ = other.inequalityIndices_;
    equalityIndices_ = other.equalityIndices_;
    return *this;
  }
  virtual ImplicitPtr_t copy() const;
  static ImplicitPtr_t create(const DifferentiableFunctionPtr_t& func,
                              ComparisonTypes_t comp,
                              std::vector<bool> mask = std::vector<bool>());
 
  static ImplicitPtr_t createCopy(const ImplicitPtr_t& other);
 
  virtual ~Implicit() {};
 
 
  void rightHandSideFromConfig(ConfigurationIn_t config,
                               LiegroupElementRef rhs);
 
  bool checkRightHandSide(LiegroupElementConstRef rhs) const;
 
  size_type parameterSize() const;
 
  size_type rightHandSideSize() const;
 
  void rightHandSideFunction(const DifferentiableFunctionPtr_t& rhsF);
 
  const DifferentiableFunctionPtr_t& rightHandSideFunction() const {
    return rhsFunction_;
  }
 
  vectorIn_t rightHandSideAt(const value_type& s);
 
 
  const ComparisonTypes_t& comparisonType() const;
 
  void comparisonType(const ComparisonTypes_t& comp);
 
  const segments_t& activeRows() const { return activeRows_; }
 
  bool checkAllRowsActive() const { return inactiveRows_.nbRows() == 0; }
 
  const Eigen::RowBlockIndices& equalityIndices() const {
    return equalityIndices_;
  }
 
  void setInactiveRowsToZero(vectorOut_t error) const;
 
  DifferentiableFunction& function() const { return *function_; }
 
  const DifferentiableFunctionPtr_t& functionPtr() const { return function_; }
 
  virtual std::pair<JointConstPtr_t, JointConstPtr_t>
  doesConstrainRelPoseBetween(DeviceConstPtr_t robot) const;
 
 protected:
  Implicit(const DifferentiableFunctionPtr_t& function, ComparisonTypes_t comp,
           std::vector<bool> mask);
 
  Implicit(const Implicit& other);
 
  virtual bool isEqual(const Implicit& other, bool swapAndTest) const;
 
  // Store weak pointer to itself
  void init(const ImplicitWkPtr_t& weak) { weak_ = weak; }
 
  friend class ImplicitConstraintSet;
 
 private:
  void computeIndices();
  void computeActiveRows();
  ComparisonTypes_t comparison_;
  vector_t rhs_;
  size_type parameterSize_;
  DifferentiableFunctionPtr_t function_;
  DifferentiableFunctionPtr_t rhsFunction_;
  std::vector<bool> mask_;
  segments_t activeRows_;
  Eigen::RowBlockIndices inactiveRows_;
  std::vector<std::size_t> inequalityIndices_;
  Eigen::RowBlockIndices equalityIndices_;
  ImplicitWkPtr_t weak_;
  // To avoid dynamic memory allocation
  mutable LiegroupElement output_;
  mutable vector_t logOutput_;
 
 protected:
  Implicit() {}
 
 private:
  HPP_SERIALIZABLE();
};  // class Implicit
}  // namespace constraints
}  // namespace hpp
 
#endif  // HPP_CONSTRAINTS_IMPLICIT_HH