GenericTask.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 <iostream>
#include <memory>
 
#include "Eigen/Dense"
#include <Qontrol/Model/GenericModel.hpp>
#include <Qontrol/Utils/Size.hpp>
namespace Qontrol {
namespace Task {
class GenericTask {
public:
  GenericTask(std::string name, int task_dimension, int optim_vector_dimension)
      : name_{name}, task_dimension_{task_dimension},
        optim_vector_dimension_{optim_vector_dimension} {
    resize(task_dimension_, optim_vector_dimension);
    setSelectionMatrix(Eigen::MatrixXd::Identity(optim_vector_dimension,
                                                 optim_vector_dimension));
    setWeightingMatrix(Eigen::MatrixXd::Identity(optim_vector_dimension,
                                                 optim_vector_dimension));
  }
 
  GenericTask(std::string name, int task_dimension,
           std::shared_ptr<Model::GenericModel> model_ptr)
      : name_{name}, task_dimension_{task_dimension},
        optim_vector_dimension_{model_ptr_->getNrOfDegreesOfFreedom()},
        model_ptr_{model_ptr} {
    resize(task_dimension_, optim_vector_dimension_);
    setSelectionMatrix(Eigen::MatrixXd::Identity(optim_vector_dimension_,
                                                 optim_vector_dimension_));
    setWeightingMatrix(Eigen::MatrixXd::Identity(optim_vector_dimension_,
                                                 optim_vector_dimension_));
  }
  std::string getName();
 
  void resize(int input_size, int output_size);
 
  void setE(Eigen::MatrixXd E);
 
  void setf(Eigen::VectorXd f);
 
  virtual void update(double dt){};
 
  Eigen::MatrixXd getHessian();
 
  Eigen::VectorXd getGradient();
 
  void setSelectionMatrix(Eigen::MatrixXd selection_matrix);
 
  void setWeightingMatrix(Eigen::MatrixXd weighting_matrix);
 
  Eigen::MatrixXd getSelectionMatrix();
 
  Eigen::MatrixXd getWeightingMatrix();
 
protected:
  std::shared_ptr<Model::GenericModel> model_ptr_;
  std::string name_;
  Eigen::MatrixXd E_;
  Eigen::VectorXd f_;
  Eigen::MatrixXd selection_matrix_;
  Eigen::MatrixXd weighting_matrix_;
  int task_dimension_;
  int optim_vector_dimension_;
};
} // namespace Task
} // namespace Qontrol