constraint-set.hh

Go to the documentation of this file.

//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#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)
      {
    constraint->addToConstraintSet (weak_.lock ());
      }

      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;
    private:
      virtual void addToConstraintSet (const ConstraintSetPtr_t& constraintSet)
      {
    for (Constraints_t::iterator itConstraint = constraints_.begin ();
         itConstraint != constraints_.end (); itConstraint ++) {
      (*itConstraint)->addToConstraintSet (constraintSet);
    }
      }

      void removeFirstElement ();

      Constraints_t constraints_;
      Constraints_t::iterator configProjectorIt_;
      Constraints_t::iterator trivialOrNotConfigProjectorIt_;
      ConstraintSetWkPtr_t weak_;

      friend class constraints::LockedJoint;
      friend class Constraint;
      friend class ConfigProjector;
    }; // class ConstraintSet
  } // namespace core
} // namespace hpp

#endif // HPP_CORE_CONSTRAINT_SET_HH