Controller.hpp

/*
    Copyright (c) Tobias Löw
    Written by Tobias Löw <https://tobiloew.ch>
 
    This file is part of orwell.
 
    gafro is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License version 3 as
    published by the Free Software Foundation.
 
    gafro 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 General Public License for more details.
 
    You should have received a copy of the GNU General Public License
    along with gafro. If not, see <http://www.gnu.org/licenses/>.
*/
 
#pragma once
 
#include <Eigen/Core>
//
#include <sackmesser/FactoryClass.hpp>
#include <sackmesser/Interface.hpp>
//
#include <orwell/RobotModel.hpp>
#include <orwell/RobotState.hpp>
#include <orwell/StoppingCriterion.hpp>
 
namespace orwell
{
 
    template <int dof>
    class Controller : public std::enable_shared_from_this<Controller<dof>>
    {
      public:
        Controller(const sackmesser::Interface::Ptr &interface, const std::string &name);
 
        virtual ~Controller() = default;
 
        typename RobotState<dof>::Vector getControlCommand(const RobotState<dof> &robot_state);
 
        void setRobotModel(const typename RobotModel<dof>::Ptr &robot_model);
 
        void setStoppingCriterion(const std::shared_ptr<StoppingCriterion<dof>> &stopping_criterion);
 
        bool isFinished(const RobotState<dof> &robot_state) const;
 
        template <class Derived>
        typename Derived::Ptr cast();
 
      protected:
        virtual typename RobotState<dof>::Vector computeCommand() = 0;
 
        typename RobotModel<dof>::Ptr getRobotModel();
 
        const RobotState<dof> &getRobotState() const;
 
      private:
        typename RobotState<dof>::Vector limitCommand(const typename RobotState<dof>::Vector &command);
 
      private:
        typename RobotModel<dof>::Ptr robot_model_;
 
        RobotState<dof> robot_state_;
 
        struct Configuration : public sackmesser::Configuration
        {
            bool load(const std::string &ns, const std::shared_ptr<sackmesser::Configurations> &server);
 
            typename RobotState<dof>::Vector command_limits_;
        } config_;
 
        std::shared_ptr<StoppingCriterion<dof>> stopping_criterion_;
 
      private:
        // declare ParallelController as friend class such that it can access protected method computeCommand
        template <int, template <int> class>
        friend class ParallelController;
    };
 
}  // namespace orwell
 
#include <orwell/Controller.hxx>