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>
#include <Qontrol/Problem/ResolutionStrategy.hpp>
#include <Qontrol/Problem/ReferenceFrame.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},
        resolution_strategy_{ResolutionStrategy::Weighted},
        controled_frame_name_{""},
        subStateMatrix_{Eigen::MatrixXd::Zero(optim_vector_dimension_,optim_vector_dimension_)},
        reference_frame_{ReferenceFrame::LOCAL}  {
    resize(task_dimension_, optim_vector_dimension);
 
    setSelectionMatrix(Eigen::MatrixXd::Identity(task_dimension_,
                                                 task_dimension_));
    setWeightingMatrix(Eigen::MatrixXd::Identity(task_dimension_,
                                                 task_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},
        resolution_strategy_{ResolutionStrategy::Weighted} ,
        controled_frame_name_{model_ptr->getTipFrameName()},
        subStateMatrix_{Eigen::MatrixXd::Zero(optim_vector_dimension_,optim_vector_dimension_)},
        reference_frame_{ReferenceFrame::LOCAL} {
    resize(task_dimension_, optim_vector_dimension_);
    setSelectionMatrix(Eigen::MatrixXd::Identity(task_dimension_,
                                                 task_dimension_));
    setWeightingMatrix(Eigen::MatrixXd::Identity(task_dimension_,
                                                 task_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();
 
  Eigen::MatrixXd getJacobianGHC();
 
  int getTaskDimension();
 
  void setResolutionStrategy(ResolutionStrategy resolution_strategy);
 
  void setControledFrameName(std::string frame_name);
 
  void setReferenceFrame(ReferenceFrame reference_frame);
 
  void setSubStateMatrix(Eigen::MatrixXd &subStateMatrix);
 
 
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_;
  ResolutionStrategy resolution_strategy_;
  Eigen::MatrixXd subStateMatrix_;
  std::string controled_frame_name_;
  ReferenceFrame reference_frame_;
};
} // namespace Task
} // namespace Qontrol