TaskSet.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/Model/GenericModel.hpp>
#include <Qontrol/Tasks/GenericTask.hpp>
#include <Qontrol/Problem/ControlOutput.hpp>
 
#include <algorithm>
#include <memory>
#include <vector>
 
namespace Qontrol {
namespace Task {
template <Qontrol::ControlOutput output> class TaskSet {
public:
  TaskSet(std::shared_ptr<Model::GenericModel> model_library)
      : model_ptr_{model_library} {
  }
 
  std::shared_ptr<Task::GenericTask> add(std::string task_name, int task_dimension,
                                      double task_weight = 1.0) {
    if (!exist(task_name)) {
      auto base_task_ptr = std::make_shared<Task::GenericTask>(
          task_name, task_dimension, model_ptr_->getNrOfDegreesOfFreedom());
      tasks_ptrs_.push_back(base_task_ptr);
      tasks_names_.push_back(task_name);
      tasks_weight_.push_back(task_weight);
      return base_task_ptr;
    } else {
      throw std::logic_error("Task with name " + task_name + " already exist.");
    }
  }
 
  template <template <Qontrol::ControlOutput output2> class ControlInput>
  std::shared_ptr<ControlInput<output>> add(std::string task_name,
                                            double task_weight = 1.0) {
    if (!exist(task_name)) {
      auto task_ptr = std::shared_ptr<ControlInput<output>>(
          new ControlInput<output>(task_name, model_ptr_));
      // Implicit conversion to GenericTask when pushing
      tasks_ptrs_.push_back(task_ptr);
      tasks_names_.push_back(task_name);
      tasks_weight_.push_back(task_weight);
      return task_ptr;
    } else {
      throw std::logic_error("Task with name " + task_name + " already exist.");
    }
  }
 
  bool exist(std::string task_name) {
    return (findTaskIterator(task_name) != tasks_names_.end());
  }
 
  void getNames() {
    for (auto task : tasks_ptrs_)
      PLOGD << "Task name " << task->getName();
  }
 
  void update(double dt) {
    for (auto task : tasks_ptrs_) {
      task->update(dt);
    }
  }
 
  std::vector<std::shared_ptr<Task::GenericTask>> getTasks() {
    return tasks_ptrs_;
  }
 
  std::vector<double> getTasksWeight() { return tasks_weight_; }
 
 
 
  int getTaskIndex(std::string task_name) {
    if (exist(task_name)) {
      auto itr = findTaskIterator(task_name);
      return std::distance(tasks_names_.begin(), itr);
    } else {
      PLOGI << "Task " << task_name << " doesn't exist. Cannot get it's index.";
      return -1;
    }
  }
 
  void remove(std::shared_ptr<Task::GenericTask> task_ptr) {
    remove(task_ptr->getName());
  }
 
  void remove(std::string task_name) {
    if (exist(task_name)) {
      auto index = getTaskIndex(task_name);
      tasks_ptrs_.erase(tasks_ptrs_.begin() + index);
      tasks_names_.erase(tasks_names_.begin() + index);
      tasks_weight_.erase(tasks_weight_.begin() + index);
    }
  }
 
  bool setTaskWeight(std::shared_ptr<Task::GenericTask> task, double task_weight) {
    return setTaskWeight(task->getName(), task_weight);
  }
 
  bool setTaskWeight(std::string task_name, double task_weight) {
    if (exist(task_name)) {
      tasks_weight_.at(getTaskIndex(task_name)) = task_weight;
      return true;
    } else {
      PLOGW << "Task " << task_name << " doesn't exist. Cannot set its weight";
      return false;
    }
  }
 
protected:
  std::shared_ptr<Model::GenericModel> model_ptr_;
  std::vector<std::string> tasks_names_;
 
private:
  std::vector<std::string>::iterator findTaskIterator(std::string task_name) {
    return std::find(tasks_names_.begin(), tasks_names_.end(), task_name);
  }
 
  std::vector<std::shared_ptr<Task::GenericTask>> tasks_ptrs_;
  std::vector<double> tasks_weight_;
};
} // namespace Task
} // namespace Qontrol