Dissipative.hpp

#pragma once
 
#include "controllers/impedance/Impedance.hpp"
#include "state_representation/parameters/Parameter.hpp"
#include "state_representation/space/cartesian/CartesianState.hpp"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
 
namespace controllers::impedance {
 
enum class ComputationalSpaceType {
  LINEAR, ANGULAR, DECOUPLED_TWIST, FULL
};
 
template<class S>
class Dissipative : public Impedance<S> {
 
public:
 
  explicit Dissipative(const ComputationalSpaceType& computational_space, unsigned int dimensions = 6);
 
  explicit Dissipative(
      const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters,
      const ComputationalSpaceType& computational_space, unsigned int dimensions = 6
  );
 
  S compute_command(const S& command_state, const S& feedback_state) override;
 
protected:
 
  void validate_and_set_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
 
  static Eigen::MatrixXd orthonormalize_basis(const Eigen::MatrixXd& basis, const Eigen::VectorXd& main_eigenvector);
 
  Eigen::MatrixXd compute_orthonormal_basis(const S& desired_velocity);
 
  void compute_damping(const S& desired_velocity);
 
  std::shared_ptr<state_representation::Parameter<Eigen::VectorXd>>
      damping_eigenvalues_; 
 
  const ComputationalSpaceType computational_space_; 
 
  Eigen::MatrixXd basis_; 
 
};
 
template<class S>
Dissipative<S>::Dissipative(const ComputationalSpaceType& computational_space, unsigned int dimensions) :
    Impedance<S>(dimensions),
    damping_eigenvalues_(
        state_representation::make_shared_parameter<Eigen::VectorXd>(
            "damping_eigenvalues", Eigen::ArrayXd::Ones(dimensions))),
    computational_space_(computational_space),
    basis_(Eigen::MatrixXd::Random(dimensions, dimensions)) {
  this->parameters_.erase("stiffness");
  this->stiffness_->set_value(Eigen::MatrixXd::Zero(dimensions, dimensions));
  this->parameters_.erase("inertia");
  this->inertia_->set_value(Eigen::MatrixXd::Zero(dimensions, dimensions));
 
  this->damping_->set_value(Eigen::MatrixXd::Identity(dimensions, dimensions));
  this->parameters_.insert(std::make_pair("damping_eigenvalues", damping_eigenvalues_));
}
 
template<class S>
Dissipative<S>::Dissipative(
    const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters,
    const ComputationalSpaceType& computational_space, unsigned int dimensions
) :
    Dissipative<S>(computational_space, dimensions) {
  this->set_parameters(parameters);
}
 
template<class S>
void Dissipative<S>::validate_and_set_parameter(
    const std::shared_ptr<state_representation::ParameterInterface>& parameter
) {
  if (parameter->get_name() == "damping_eigenvalues") {
    this->damping_eigenvalues_->set_value(this->gain_matrix_from_parameter(parameter).diagonal());
  }
}
 
template<class S>
Eigen::MatrixXd Dissipative<S>::orthonormalize_basis(
    const Eigen::MatrixXd& basis, const Eigen::VectorXd& main_eigenvector
) {
  Eigen::MatrixXd orthonormal_basis = basis;
  uint dim = basis.rows();
  orthonormal_basis.col(0) = main_eigenvector.normalized();
  for (uint i = 1; i < dim; i++) {
    for (uint j = 0; j < i; j++) {
      orthonormal_basis.col(i) -= orthonormal_basis.col(j).dot(orthonormal_basis.col(i)) * orthonormal_basis.col(j);
    }
    orthonormal_basis.col(i).normalize();
  }
  return orthonormal_basis;
}
 
template<class S>
S Dissipative<S>::compute_command(
    const S& command_state, const S& feedback_state
) {
  // compute the damping matrix out of the command_state twist
  this->compute_damping(command_state);
  // apply the impedance control law
  return this->Impedance<S>::compute_command(command_state, feedback_state);
}
 
template<class S>
void Dissipative<S>::compute_damping(const S& desired_velocity) {
  this->basis_ = this->compute_orthonormal_basis(desired_velocity);
  auto diagonal_eigenvalues = this->damping_eigenvalues_->get_value().asDiagonal();
  this->damping_->set_value(this->basis_ * diagonal_eigenvalues * this->basis_.transpose());
}
 
}// namespace controllers