CompliantTwist.hpp

#pragma once
 
#include "controllers/IController.hpp"
#include "controllers/impedance/Dissipative.hpp"
#include "controllers/impedance/VelocityImpedance.hpp"
#include "state_representation/parameters/Parameter.hpp"
 
namespace controllers::impedance {
class CompliantTwist : public IController<state_representation::CartesianState> {
 
public:
 
  explicit CompliantTwist(
      const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters
  );
 
  CompliantTwist(
      double linear_principle_damping, double linear_orthogonal_damping, double angular_stiffness,
      double angular_damping
  );
 
  state_representation::CartesianState compute_command(
      const state_representation::CartesianState& desired_state,
      const state_representation::CartesianState& feedback_state
  ) override;
 
protected:
 
  void validate_and_set_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
 
  void set_linear_principle_damping(double linear_principle_damping);
 
  void set_linear_orthogonal_damping(double linear_orthogonal_damping);
 
  void set_linear_gains(double linear_principle_damping, double linear_orthogonal_damping);
 
  void set_angular_stiffness(double angular_stiffness);
 
  void set_angular_damping(double angular_damping);
 
  void set_angular_gains(double angular_stiffness, double angular_damping);
 
  std::shared_ptr<state_representation::Parameter<double>>
      linear_principle_damping_; 
  std::shared_ptr<state_representation::Parameter<double>>
      linear_orthogonal_damping_; 
  std::shared_ptr<state_representation::Parameter<double>> angular_stiffness_; 
  std::shared_ptr<state_representation::Parameter<double>> angular_damping_; 
 
  Dissipative<state_representation::CartesianState> dissipative_ctrl_; 
  VelocityImpedance<state_representation::CartesianState> velocity_impedance_ctrl_; 
};
 
}// namespace controllers