constraint-set.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_CONSTRAINT_SET_HH
#define HPP_CORE_CONSTRAINT_SET_HH
 
#include <deque>
#include <hpp/core/constraint.hh>
 
namespace hpp {
namespace core {
 
class HPP_CORE_DLLAPI ConstraintSet : public Constraint {
 public:
  static ConstraintSetPtr_t create(const DevicePtr_t& robot,
                                   const std::string& name) {
    ConstraintSet* ptr = new ConstraintSet(robot, name);
    ConstraintSetPtr_t shPtr(ptr);
    ptr->init(shPtr);
    return shPtr;
  }
 
  static ConstraintSetPtr_t createCopy(const ConstraintSetPtr_t& cs) {
    ConstraintSet* ptr = new ConstraintSet(*cs);
    ConstraintSetPtr_t shPtr(ptr);
    ptr->init(shPtr);
    return shPtr;
  }
 
  virtual ConstraintPtr_t copy() const;
 
  void addConstraint(const ConstraintPtr_t& constraint);
 
  ConfigProjectorPtr_t configProjector() const;
 
  Constraints_t::iterator begin() { return constraints_.begin(); }
  Constraints_t::iterator end() { return constraints_.end(); }
 
  virtual bool isSatisfied(ConfigurationIn_t config);
 
  virtual bool isSatisfied(ConfigurationIn_t config, vector_t& error);
 
 
  size_type numberNonLockedDof() 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;
 
 protected:
  ConstraintSet(const DevicePtr_t& robot, const std::string& name);
  ConstraintSet(const ConstraintSet& other);
  void init(const ConstraintSetPtr_t& self) {
    Constraint::init(self);
    weak_ = self;
  }
  virtual bool impl_compute(ConfigurationOut_t configuration);
 
  virtual std::ostream& print(std::ostream& os) const;
 
  ConstraintSet() {}
 
 private:
  ConfigProjectorPtr_t _configProj() const;
 
  Constraints_t constraints_;
  int configProjI_;
  ConstraintSetWkPtr_t weak_;
 
  friend class constraints::LockedJoint;
  friend class Constraint;
  friend class ConfigProjector;
 
  HPP_SERIALIZABLE();
};  // class ConstraintSet
}  // namespace core
}  // namespace hpp
 
BOOST_CLASS_EXPORT_KEY(hpp::core::ConstraintSet)
 
#endif  // HPP_CORE_CONSTRAINT_SET_HH