Constraint.h

Go to the documentation of this file.

#ifndef _OCRABASE_CONSTRAINT_H_
#define _OCRABASE_CONSTRAINT_H_

#ifdef WIN32
# pragma once
#endif


// includes
#include "ocra/optim/Variable.h"
#include "ocra/optim/Function.h"
#include "ocra/optim/FunctionInterfaceMapping.h"
#include <ocra/util/Macros.h>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_base_of.hpp>


//#include <sstream>

namespace ocra
{
  //never change this enum
  enum eConstraintType
  {
    CSTR_EQUAL_ZERO=0,        //> \f$ f(x,t) = 0 \f$
    CSTR_EQUAL_B,             //> \f$ f(x,t) = b \f$
    CSTR_LOWER_ZERO,          //> \f$ f(x,t) \le 0 \f$
    CSTR_LOWER_U,             //> \f$ f(x,t) \le u \f$
    CSTR_GREATER_ZERO,        //> \f$ f(x,t) \ge 0 \f$
    CSTR_GREATER_L,           //> \f$ f(x,t) \ge l \f$
    CSTR_LOWER_AND_GREATER    //> \f$ l \le f(x,t) \le u \f$
  };

  template<class T>
  class Constraint
    : public Constraint<typename T::functionType_t>
    , public FunctionInterfaceMapping<Constraint<T> >
  //TODO [mineur] ? Function could be put to T::functionType_t to verify
  //that functionType_t is defined according to the inheritance tree of Function
  {
      DEFINE_CLASS_POINTER_TYPEDEFS(Constraint<T>);
  private:
    typedef Constraint<typename T::functionType_t>    BaseConstraint;

    Constraint(Constraint&);
    Constraint& operator= (const Constraint&);

  public:
    Constraint(T* function, bool equality, const VectorXd& v=VectorXd());

    Constraint(T* function, const VectorXd& l=VectorXd(), const VectorXd& u=VectorXd());

    inline operator const T& () {return static_cast<T&>(BaseConstraint::getFunction());}

    inline virtual T& getFunction(void) {return static_cast<T&>(BaseConstraint::getFunction());}
    inline virtual const T& getFunction(void) const {return static_cast<const T&>(BaseConstraint::getFunction());}

  private:
    typedef boost::is_base_of<Function, T> T_must_derived_from_Function;
    BOOST_STATIC_ASSERT(T_must_derived_from_Function           ::value);
  };









  template<>
  class Constraint<Function>
    : public FunctionInterfaceMapping<Constraint<Function> >
    , public SubjectBase<EVT_CSTR_CHANGE_BOUNDS_NUMBER>
  {
    // ------------------------ structures --------------------------------------
  public:
    /* The slack variable functionalities are disabled for now
    class UpdatableSlackVariable: public ObserverSubject
    {
    private :
      UpdatableSlackVariable();
      UpdatableSlackVariable(UpdatableSlackVariable&);
    public:
      UpdatableSlackVariable(Function* f)
        :_f(f)
      {
        std::stringstream s;
        s << "_slack_";
        s << cpt++;
        _slack = new BaseVariable(s.str(), f->getDimension());
        f->attach(*this);
      }

      ~UpdatableSlackVariable()
      {
        delete _slack;
        _f->completelyDetach(*this);
      }

      virtual void updateSize(void);
      inline Variable* getSlackVariable(void) {return _slack;}

    protected:
      static int cpt;
      BaseVariable* _slack;
      Function* _f;
    };*/

    // ------------------------ constructors ------------------------------------
  private:
    Constraint(Constraint&);
    Constraint& operator= (const Constraint&);

  public:
    Constraint(Function* function, bool equality, const VectorXd& v=VectorXd());
    Constraint(Function* function, const VectorXd& l=VectorXd(), const VectorXd& u=VectorXd());

    // ------------------------ public interface --------------------------------
  public:
    inline virtual Function& getFunction() {return _function;}
    inline virtual const Function& getFunction() const {return _function;}

    inline bool isRespected(int index=-1) const;

    inline bool isEquality() const;
    inline bool isInequality() const;

    void setB(const VectorXd& b);

    void setL(const VectorXd& l);

    void setU(const VectorXd& u);

    void setLandU(const VectorXd& l, const VectorXd& u);

    eConstraintType getType() const;
    const VectorXd& getB() const;
    const VectorXd& getL() const;
    const VectorXd& getU() const;

    void    setViolationTolerance(double tol);
    double  getViolationTolerance() const;

    /* methods related to slack variable,
    void makeSlacked();
    void unmakeSlacked();
    inline bool isSlacked() const;
    inline Variable* getSlackVariable();
    */

    // ------------------------ protected members -------------------------------
  protected:
    Function&               _function;  //< the function on which the constaint is built

    // ------------------------ private members ---------------------------------
  private:
    eConstraintType         _type;      //< true if this is an equality constraint
    VectorXd                _l;         //< lower bound
    VectorXd                _u;         //< upper bound, or equality value (b)
    double                  _violation; //< accepted violation of the constraint
    // UpdatableSlackVariable* _slack;     //< possible slack variables


    //TODO [mineur] : add a scaling factor : need Constraint to be an observer and have its own memory for results
  };





  inline bool Constraint<Function>::isRespected(int index) const
  {
    if (index < 0)
    {
      switch (_type)
      {
        case CSTR_EQUAL_ZERO:         return getValue().isZero(_violation);                               break;
        case CSTR_EQUAL_B:            return ((getValue().array()-_u.array()).abs() <= _violation).all(); break;
        case CSTR_LOWER_ZERO:         return (getValue().array() <= _violation).all();                    break;
        case CSTR_LOWER_U:            return ((getValue().array()-_u.array()) <= _violation).all();       break;
        case CSTR_GREATER_ZERO:       return (getValue().array() >= -_violation).all();                   break;
        case CSTR_GREATER_L:          return ((getValue().array()-_l.array()) >= -_violation).all();      break;
        case CSTR_LOWER_AND_GREATER:  return ((getValue().array()-_u.array()) <= _violation).all()
                                          && ((getValue().array()-_l.array()) >= -_violation).all();      break;
        default:
          throw std::runtime_error("[Constraint<T>::isValid] invalid constraint type");
      }
    }
    else
    {
      switch (_type)
      {
        case CSTR_EQUAL_ZERO:         return fabs(getValue(index)) <= _violation;           break;
        case CSTR_EQUAL_B:            return fabs(getValue(index)-_u[index]) <= _violation; break;
        case CSTR_LOWER_ZERO:         return getValue(index) <= _violation;                 break;
        case CSTR_LOWER_U:            return (getValue(index)-_u[index]) <= _violation;     break;
        case CSTR_GREATER_ZERO:       return getValue(index) >= -_violation;                break;
        case CSTR_GREATER_L:          return (getValue(index)-_l[index]) >= -_violation;    break;
        case CSTR_LOWER_AND_GREATER:  return (getValue(index)-_u[index]) <= _violation
                                          && (getValue(index)-_l[index]) >= -_violation;    break;
        default:
          throw std::runtime_error("[Constraint<T>::isValid] invalid constraint type");
      }
    }
  }

  inline bool Constraint<Function>::isEquality() const
  {
    return _type < CSTR_LOWER_ZERO;
  }

  inline bool Constraint<Function>::isInequality() const
  {
    return _type >= CSTR_LOWER_ZERO;
  }

  inline void Constraint<Function>::setB(const VectorXd& b)
  {
    ocra_assert(isEquality() && "You can change b only for an equality constraint");
    if (b.size() > 0)
    {
      ocra_assert(b.size() == getDimension()  && "b has not the size of the function");
      _type = CSTR_EQUAL_B;
    }
    else
      _type = CSTR_EQUAL_ZERO;
    _u = b;
  }

  inline void Constraint<Function>::setL(const VectorXd& l)
  {
    ocra_assert(isInequality() && "You can change l only for an inequality constraint");
    if (l.size() > 0)
    {
      ocra_assert(l.size() == getDimension()  && "l has not the size of the function");
      ocra_assert(
        (
        _type == CSTR_GREATER_ZERO
        || _type == CSTR_GREATER_L
        || (_type == CSTR_LOWER_ZERO && (l.array()<0).all() )
        || (_u.size() != 0 && (l.array()-_violation < _u.array()).all())
        )
        && "when there is a upper bound, it must be greater than l");
      switch(_type)
      {
        case CSTR_LOWER_ZERO:         _u = VectorXd::Zero(getDimension());                      //no break here on purpose
        case CSTR_LOWER_U:            _type = CSTR_LOWER_AND_GREATER;
                                      SubjectBase<EVT_CSTR_CHANGE_BOUNDS_NUMBER>::propagate();  break;
        case CSTR_GREATER_ZERO:       _type = CSTR_GREATER_L;                                   break;
        case CSTR_GREATER_L:          /*nothing to do*/                                         break;
        case CSTR_LOWER_AND_GREATER:  /*nothing to do*/                                         break;
      }
    }
    else
    {
      switch(_type)
      {
        case CSTR_LOWER_ZERO:         /*nothing to do*/                                         break;
        case CSTR_LOWER_U:            /*nothing to do*/                                         break;
        case CSTR_GREATER_ZERO:       /*nothing to do*/                                         break;
        case CSTR_GREATER_L:          _type = CSTR_GREATER_ZERO;                                break;
        case CSTR_LOWER_AND_GREATER:  _type = CSTR_LOWER_U;
                                      SubjectBase<EVT_CSTR_CHANGE_BOUNDS_NUMBER>::propagate();  break;
      }
    }
    _l = l;
  }

  inline void Constraint<Function>::setU(const VectorXd& u)
  {
    ocra_assert(isInequality() && "You can change u only for an inequality constraint");
    if (u.size() > 0)
    {
      ocra_assert(u.size() == getDimension()  && "u has not the size of the function");
      ocra_assert(
        (
        _type == CSTR_LOWER_ZERO
        || _type == CSTR_LOWER_U
        || ( _type == CSTR_GREATER_ZERO && (u.array()>0).all() )
        || (_l.size() != 0 && (_l.array()-_violation < u.array()).all())
        )
        && "when there is a lower bound, it must be lower than u");
      switch(_type)
      {
        case CSTR_LOWER_ZERO:         _type =  CSTR_LOWER_U;                                    break;
        case CSTR_LOWER_U:            /*nothing to do*/                                         break;
        case CSTR_GREATER_ZERO:       _l = VectorXd::Zero(getDimension());                      //no break here on purpose
        case CSTR_GREATER_L:          _type = CSTR_LOWER_AND_GREATER;
                                      SubjectBase<EVT_CSTR_CHANGE_BOUNDS_NUMBER>::propagate();  break;
        case CSTR_LOWER_AND_GREATER:  /*nothing to do*/                                         break;
      }
    }
    else
    {
      switch(_type)
      {
        case CSTR_LOWER_ZERO:         /*nothing to do*/                                         break;
        case CSTR_LOWER_U:            _type = CSTR_LOWER_ZERO;                                  break;
        case CSTR_GREATER_ZERO:       /*nothing to do*/                                         break;
        case CSTR_GREATER_L:          /*nothing to do*/                                         break;
        case CSTR_LOWER_AND_GREATER:  _type = CSTR_GREATER_L;
                                      SubjectBase<EVT_CSTR_CHANGE_BOUNDS_NUMBER>::propagate();  break;
      }
    }
    _u = u;
  }

  inline void Constraint<Function>::setLandU(const VectorXd& l, const VectorXd& u)
  {
    ocra_assert(isInequality() && "You can change l and u only for an inequality constraint");
    if (l.size()>0)
    {
      ocra_assert(l.size() == getDimension()  && "l has not the size of the function");
      if (u.size() > 0)
      {
        ocra_assert(u.size() == getDimension()  && "u has not the size of the function");
        ocra_assert((l.array()-_violation < u.array()).all() && "l must be lower than u");
        switch(_type)
        {
          case CSTR_LOWER_ZERO:
          case CSTR_LOWER_U:
          case CSTR_GREATER_ZERO:
          case CSTR_GREATER_L:          _type = CSTR_LOWER_AND_GREATER;
                                        SubjectBase<EVT_CSTR_CHANGE_BOUNDS_NUMBER>::propagate();  break;
          case CSTR_LOWER_AND_GREATER:  /*nothing to do*/                                         break;
        }
        _l = l;
        _u = u;
      }
      else
      {
        // l is initialized
        // u is not initialized (u.size() == 0)
        setL(l);
        setU(u);
      }
    }
    else
    {
      ocra_assert( u.size()>0 && "l and u are not initialized, constraint is undefined");

      // l is not initialized (l.size() == 0)
      // u is initialized
      setU(u);
      setL(l);
    }
  }

  inline eConstraintType Constraint<Function>::getType() const
  {
    return _type;
  }

  inline const VectorXd& Constraint<Function>::getB() const
  {
    ocra_assert(isEquality() && "getB() can be called only on an equality constraint");
    return _u;
  }

  inline const VectorXd& Constraint<Function>::getL() const
  {
    ocra_assert(isInequality() && "getL() can be called only on an inequality constraint");
    return _l;
  }

  inline const VectorXd& Constraint<Function>::getU() const
  {
    ocra_assert(isInequality() && "getU() can be called only on an inequality constraint");
    return _u;
  }

  inline void Constraint<Function>::setViolationTolerance(double tol)
  {
    ocra_assert(tol>=0 && "tolerance must be a positive number");
    _violation = tol;
  }

  inline double Constraint<Function>::getViolationTolerance() const
  {
    return _violation;
  }



  //constructors
  template<class T>
  inline Constraint<T>::Constraint(T *function, bool equality, const VectorXd& v)
    :Constraint<typename T::functionType_t>(function, equality, v)
  {
      //force T to inherit of Function
                                        //TODO [mineur] ? could be put to T::functionType_t to verify
                                        //that functionType_t is defined according to the inheritance tree of Function
  }

  template<class T>
  inline Constraint<T>::Constraint(T* function, const VectorXd& l, const VectorXd& u)
    :Constraint<typename T::functionType_t>(function, l, u)
  {
      //force T to inherit of Function
                                        //TODO [mineur] ? could be put to T::functionType_t to verify
                                        //that functionType_t is defined according to the inheritance tree of Function
  }

  inline Constraint<Function>::Constraint(Function *function, bool equality, const VectorXd& v)
    :_function(*function)/*, _slack(NULL)*/, _u(v), _violation(1.e-7)
  {
    if (equality)
    {
      if (v.size() > 0)
      {
        ocra_assert(v.size() == getDimension() && "v has not the size of the function");
        _type = CSTR_EQUAL_B;
      }
      else
        _type = CSTR_EQUAL_ZERO;
    }
    else
    {
      if (v.size() > 0)
      {
        ocra_assert(v.size() == getDimension() && "v has not the size of the function");
        _type = CSTR_LOWER_U;
      }
      else
        _type = CSTR_LOWER_ZERO;
    }
  }

  inline Constraint<Function>::Constraint(Function* function, const VectorXd& l, const VectorXd& u)
    :_function(*function)/*, _slack(NULL)*/, _l(l), _u(u), _violation(1.e-7)
  {
    if (l.size() > 0)
    {
      ocra_assert(l.size() == getDimension() && "l has not the size of the function");
      if (u.size() > 0)
      {
        ocra_assert(_l.size() == _u.size() && "l and u don't have the same size");
        ocra_assert((l.array()-_violation < u.array()).all() && "l must be lower than u");
        if (l.isApprox(u))
          _type = CSTR_EQUAL_B;
        else
          _type = CSTR_LOWER_AND_GREATER;
      }
      else
        _type = CSTR_GREATER_L;
    }
    else
    {
      if (u.size() > 0)
      {
        ocra_assert(u.size() == getDimension() && "u has not the size of the function");
        _type = CSTR_LOWER_U;
      }
      else
        _type = CSTR_GREATER_ZERO;
    }
  }
/*
  inline bool Constraint<Function>::isSlacked(void) const
  {
    return _slack != NULL;
  }

  inline Variable* Constraint<Function>::getSlackVariable(void)
  {
    if (_slack != NULL)
      return _slack->getSlackVariable();
    else
      return NULL;
  }*/
}

#include "ocra/optim/LinearFunction.h"
#include "ocra/optim/BoundFunction.h"
#include "ocra/optim/IdentityFunction.h"
#include "ocra/optim/DiagonalLinearFunction.h"
//#include "ocra/optim/LinearTask.h"
//#include "ocra/optim/SecondOrderLinearTask.h"
//#include "ocra/optim/QuadraticFunction.h"

namespace ocra
{
  typedef Constraint<Function>                 GenericConstraint;
  typedef Constraint<LinearFunction>           LinearConstraint;
  typedef Constraint<BoundFunction>            BoundConstraint;
  typedef Constraint<IdentityFunction>         IdentityConstraint;
  typedef Constraint<DiagonalLinearFunction>   DiagonalLinearConstraint;
//  typedef Constraint<LinearTask>            TaskConstraint;
//  typedef Constraint<SecondOrderLinearTask> SecondOrderTaskConstraint;
//  typedef Constraint<QuadraticFunction>     QuadraticConstraint;
}

#endif  //_OCRABASE_CONSTRAINT_H_

// cmake:sourcegroup=Constraint