IController.hpp

#pragma once
 
#include <list>
#include <map>
#include <memory>
 
#include "controllers/exceptions/NoRobotModelException.hpp"
 
#include "robot_model/Model.hpp"
#include "state_representation/parameters/ParameterMap.hpp"
#include "state_representation/space/Jacobian.hpp"
#include "state_representation/space/joint/JointPositions.hpp"
#include "state_representation/space/joint/JointState.hpp"
 
namespace controllers {
template<class S>
class IController : public state_representation::ParameterMap {
public:
  IController() = default;
 
  virtual ~IController() = default;
 
  [[nodiscard]] virtual S compute_command(const S& command_state, const S& feedback_state) = 0;
 
  [[nodiscard]] state_representation::JointState compute_command(
      const S& command_state, const S& feedback_state, const state_representation::Jacobian& jacobian
  );
 
  [[nodiscard]] state_representation::JointState compute_command(
      const S& command_state, const S& feedback_state, const state_representation::JointPositions& joint_positions,
      const std::string& frame = ""
  );
 
  [[nodiscard]] const robot_model::Model& get_robot_model();
 
  void set_robot_model(const robot_model::Model& robot_model);
 
protected:
  std::shared_ptr<robot_model::Model> robot_model_; 
};
 
template<class S>
const robot_model::Model& IController<S>::get_robot_model() {
  if (this->robot_model_ == nullptr) {
    throw exceptions::NoRobotModelException("No robot model");
  }
  return *this->robot_model_;
}
 
template<class S>
void IController<S>::set_robot_model(const robot_model::Model& robot_model) {
  this->robot_model_ = std::make_shared<robot_model::Model>(robot_model);
}
}// namespace controllers