ConstraintSet.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 <Qontrol/Constraints/GenericConstraint.hpp>
#include <Qontrol/Model/GenericModel.hpp>
#include <Qontrol/Problem/ControlOutput.hpp>
 
#include <algorithm>
#include <memory>
#include <vector>
 
namespace Qontrol {
namespace Constraint {
template <Qontrol::ControlOutput output> class ConstraintSet {
public:
  ConstraintSet(std::shared_ptr<Model::GenericModel> model_ptr)
      : model_ptr_{model_ptr} {
  }
 
  template <template <Qontrol::ControlOutput output2> class ControlInput>
  std::shared_ptr<ControlInput<output>> add(std::string constraint_name) {
    if (!exist(constraint_name)) {
      auto constraint_ptr = std::shared_ptr<ControlInput<output>>(
          new ControlInput<output>(constraint_name, model_ptr_));
      // Implicit conversion to GenericConstraint when pushing
      constraints_ptrs_.push_back(constraint_ptr);
      constraints_names_.push_back(constraint_name);
      return constraint_ptr;
    } else {
      throw std::logic_error("Constraint with name " + constraint_name +
                             " already exist.");
    }
  }
 
  std::shared_ptr<Constraint::GenericConstraint> add(std::string constraint_name, int constraint_size) {
    if (!exist(constraint_name)) {
      auto base_constraint_ptr = std::make_shared<Constraint::GenericConstraint>(
          constraint_name,constraint_size, model_ptr_->getNrOfDegreesOfFreedom());
      constraints_ptrs_.push_back(base_constraint_ptr);
      constraints_names_.push_back(constraint_name);
      return base_constraint_ptr;
    } else {
      throw std::logic_error("Constraint with name " + constraint_name +
                             " already exist.");
    }
  }
 
  std::vector<std::string>::iterator
  findConstraintIterator(std::string constraint_name) {
    return std::find(constraints_names_.begin(), constraints_names_.end(),
                     constraint_name);
  }
 
  void remove(std::shared_ptr<Constraint::GenericConstraint> constraint_ptr) {
    remove(constraint_ptr->getName());
  }
 
  void remove(std::string constraint_name) {
    if (exist(constraint_name)) {
      auto index = getConstraintIndex(constraint_name);
      constraints_ptrs_.erase(constraints_ptrs_.begin() + index);
      constraints_names_.erase(constraints_names_.begin() + index);
    }
  }
 
  bool exist(std::string constraint_name) {
    return findConstraintIterator(constraint_name) != constraints_names_.end();
  }
 
  int getConstraintIndex(std::string constraint_name) {
    if (exist(constraint_name)) {
      auto itr = findConstraintIterator(constraint_name);
      return std::distance(constraints_names_.begin(), itr);
    } else {
      PLOGI << "Constraint " << constraint_name
            << " doesn't exist. Cannot get it's index.";
      return -1;
    }
  }
 
 
  void update(double dt) {
    for (auto constraint : constraints_ptrs_) {
      constraint->update(dt);
    }
  }
  
  Constraint::GenericConstraint getConstraint(std::string constraint_name) {
    if (exist(constraint_name)) {
      auto index = getConstraintIndex(constraint_name);
      return *constraints_ptrs_[index];
    } else {
      throw std::logic_error("Constraint with name " + constraint_name +
                             " doesn't exist.");
    }
  }
 
  std::vector<std::shared_ptr<Constraint::GenericConstraint>> getConstraints() {
    return constraints_ptrs_;
  }
 
private:
  std::shared_ptr<Model::GenericModel> model_ptr_;
  std::vector<std::string> constraints_names_;
  std::vector<std::shared_ptr<Constraint::GenericConstraint>> constraints_ptrs_;
};
} // namespace Constraint
} // namespace Qontrol