GenericConstraint.hpp

// This file is part of Qontrol, a quadratic optimization library to
// control robot.
//
// Copyright (C) 2023 Lucas Joseph <lucas.joseph@inria.fr>
//
// Qontrol 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.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 3 of
// the License, or (at your option) any later version.
//
// Qontrol 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 Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Qontrol. If not, see <http://www.gnu.org/licenses/>.
 
#pragma once
#include <memory>
 
#include <Qontrol/Model/GenericModel.hpp>
#include <Qontrol/Utils/Size.hpp>
#include "Eigen/Dense"
namespace Qontrol {
namespace Constraint {
class GenericConstraint {
public:
  GenericConstraint(std::string name, int constraint_size,int optimization_vector_size)
      : name_{name}, constraint_size_{constraint_size}, optimization_vector_size_{optimization_vector_size} {
        resize(constraint_size_,optimization_vector_size_); 
      }
 
  GenericConstraint(std::string name,
                    int constraint_size,
                    std::shared_ptr<Model::GenericModel> model_ptr)
      : name_{name}, model_ptr_{model_ptr},
        constraint_size_{constraint_size},
        optimization_vector_size_{model_ptr->getNrOfDegreesOfFreedom()} {
          resize(constraint_size_,optimization_vector_size_);
        }
 
  virtual void update(double dt){};
 
  int getSize();
 
  std::string getName();
 
  void setConstraintMatrix(Eigen::MatrixXd constraint_matrix);
 
  void setUpperBounds(Eigen::VectorXd upper_bound);
 
  void setLowerBounds(Eigen::VectorXd lower_bound);
 
  void resize(int task_size, int constraint_size);
 
  Eigen::MatrixXd getConstraintMatrix();
 
  Eigen::VectorXd getUpperBounds();
 
  Eigen::VectorXd getLowerBounds();
 
  Eigen::VectorXd lower_limit;
  Eigen::VectorXd upper_limit;
 
protected:
  std::shared_ptr<Model::GenericModel> model_ptr_;
  int constraint_size_;
  int optimization_vector_size_;
 
private:
  std::string name_;
  Eigen::MatrixXd constraint_matrix_;
  Eigen::VectorXd upper_bound_;
  Eigen::VectorXd lower_bound_;
};
} // namespace Constraint
} // namespace Qontrol