Impedance.hpp

#pragma once
 
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
 
#include "controllers/IController.hpp"
#include "state_representation/parameters/Parameter.hpp"
#include "state_representation/State.hpp"
 
namespace controllers::impedance {
 
template<class S>
class Impedance : public IController<S> {
public:
 
  explicit Impedance(unsigned int dimensions = 6);
 
  explicit Impedance(
      const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters,
      unsigned int dimensions = 6
  );
 
  S compute_command(const S& command_state, const S& feedback_state) override;
 
protected:
 
  void clamp_force(Eigen::VectorXd& force);
 
  void validate_and_set_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
 
  Eigen::MatrixXd
  gain_matrix_from_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter);
 
  std::shared_ptr<state_representation::Parameter<Eigen::MatrixXd>>
      stiffness_; 
  std::shared_ptr<state_representation::Parameter<Eigen::MatrixXd>>
      damping_; 
  std::shared_ptr<state_representation::Parameter<Eigen::MatrixXd>>
      inertia_; 
  std::shared_ptr<state_representation::Parameter<bool>>
      feed_forward_force_; 
  std::shared_ptr<state_representation::Parameter<Eigen::VectorXd>>
      force_limit_; 
 
  const unsigned int dimensions_; 
};
 
template<class S>
Impedance<S>::Impedance(unsigned int dimensions) :
    stiffness_(
        state_representation::make_shared_parameter<Eigen::MatrixXd>(
            "stiffness", Eigen::MatrixXd::Identity(dimensions, dimensions))), damping_(
    state_representation::make_shared_parameter<Eigen::MatrixXd>(
        "damping", Eigen::MatrixXd::Identity(dimensions, dimensions))), inertia_(
    state_representation::make_shared_parameter<Eigen::MatrixXd>(
        "inertia", Eigen::MatrixXd::Identity(dimensions, dimensions))), feed_forward_force_(
            state_representation::make_shared_parameter<bool>("feed_forward_force", false)), force_limit_(
    state_representation::make_shared_parameter<Eigen::VectorXd>("force_limit")), dimensions_(dimensions) {
  this->parameters_.insert(std::make_pair("stiffness", stiffness_));
  this->parameters_.insert(std::make_pair("damping", damping_));
  this->parameters_.insert(std::make_pair("inertia", inertia_));
  this->parameters_.insert(std::make_pair("feed_forward_force", feed_forward_force_));
  this->parameters_.insert(std::make_pair("force_limit", force_limit_));
}
 
template<class S>
Impedance<S>::Impedance(
    const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters, unsigned int dimensions
) :
    Impedance(dimensions) {
  this->set_parameters(parameters);
}
 
template<class S>
void Impedance<S>::clamp_force(Eigen::VectorXd& force) {
  if (*this->force_limit_) {
    force = force.cwiseMax(-this->force_limit_->get_value()).cwiseMin(this->force_limit_->get_value());
  }
}
 
template<class S>
void Impedance<S>::validate_and_set_parameter(
    const std::shared_ptr<state_representation::ParameterInterface>& parameter
) {
  if (parameter->get_name() == "stiffness") {
    this->stiffness_->set_value(this->gain_matrix_from_parameter(parameter));
  } else if (parameter->get_name() == "damping") {
    this->damping_->set_value(this->gain_matrix_from_parameter(parameter));
  } else if (parameter->get_name() == "inertia") {
    this->inertia_->set_value(this->gain_matrix_from_parameter(parameter));
  } else if (parameter->get_name() == "feed_forward_force") {
    this->feed_forward_force_->set_value(parameter->get_parameter_value<bool>());
  } else if (parameter->get_name() == "force_limit") {
    if (parameter->get_parameter_type() == state_representation::ParameterType::MATRIX) {
      throw state_representation::exceptions::InvalidParameterException(
          "Parameter " + parameter->get_name() + " has incorrect type");
    }
    auto limit_matrix = this->gain_matrix_from_parameter(parameter);
    this->force_limit_->set_value(limit_matrix.diagonal());
  } else {
    throw state_representation::exceptions::InvalidParameterException(
        "No parameter with name '" + parameter->get_name() + "' found"
    );
  }
}
 
template<class S>
Eigen::MatrixXd Impedance<S>::gain_matrix_from_parameter(
    const std::shared_ptr<state_representation::ParameterInterface>& parameter
) {
  Eigen::MatrixXd matrix;
  if (parameter->get_parameter_type() == state_representation::ParameterType::DOUBLE) {
    auto gain = std::static_pointer_cast<state_representation::Parameter<double>>(parameter);
    matrix = gain->get_value() * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_);
  } else if (parameter->get_parameter_type() == state_representation::ParameterType::DOUBLE_ARRAY) {
    auto gain = std::static_pointer_cast<state_representation::Parameter<std::vector<double>>>(parameter);
    if (gain->get_value().size() == 1) {
      matrix = gain->get_value().at(0) * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_);
    } else if (gain->get_value().size() == this->dimensions_) {
      Eigen::VectorXd diagonal = Eigen::VectorXd::Map(gain->get_value().data(), this->dimensions_);
      matrix = diagonal.asDiagonal();
    } else {
      throw state_representation::exceptions::IncompatibleSizeException(
          "The provided diagonal coefficients do not match the dimensionality of the controller ("
              + std::to_string(this->dimensions_) + ")");
    }
  } else if (parameter->get_parameter_type() == state_representation::ParameterType::VECTOR) {
    auto gain = std::static_pointer_cast<state_representation::Parameter<Eigen::VectorXd>>(parameter);
    if (gain->get_value().size() == 1) {
      matrix = gain->get_value()(0) * Eigen::MatrixXd::Identity(this->dimensions_, this->dimensions_);
    } else if (gain->get_value().size() == this->dimensions_) {
      matrix = gain->get_value().asDiagonal();
    } else {
      throw state_representation::exceptions::IncompatibleSizeException(
          "The provided diagonal coefficients do not match the dimensionality of the controller ("
              + std::to_string(this->dimensions_) + ")");
    }
  } else if (parameter->get_parameter_type() == state_representation::ParameterType::MATRIX) {
    auto gain = std::static_pointer_cast<state_representation::Parameter<Eigen::MatrixXd>>(parameter);
    if (gain->get_value().rows() != this->dimensions_ || gain->get_value().cols() != this->dimensions_) {
      auto dim = std::to_string(this->dimensions_);
      throw state_representation::exceptions::IncompatibleSizeException(
          "The provided matrix does not have the expected size (" + dim + "x" + dim + ")");
    }
    matrix = gain->get_value();
  } else {
    throw state_representation::exceptions::InvalidParameterException(
        "Parameter " + parameter->get_name() + " has incorrect type");
  }
  if ((matrix.array() < 0).any()) {
    throw state_representation::exceptions::InvalidParameterException(
        "Parameter " + parameter->get_name() + " cannot have negative elements");
  }
  return matrix;
}
 
}// namespace controllers