Ring.hpp

#pragma once
 
#include "dynamical_systems/IDynamicalSystem.hpp"
#include "state_representation/parameters/Parameter.hpp"
#include "state_representation/space/cartesian/CartesianPose.hpp"
#include "state_representation/space/cartesian/CartesianState.hpp"
#include <cmath>
#include <vector>
 
namespace dynamical_systems {
 
class Ring : public IDynamicalSystem<state_representation::CartesianState> {
public:
  Ring();
 
  explicit Ring(const std::list<std::shared_ptr<state_representation::ParameterInterface>>& parameters);
 
  void set_base_frame(const state_representation::CartesianState& base_frame) override;
 
  [[nodiscard]] state_representation::CartesianState
  compute_dynamics(const state_representation::CartesianState& state) const override;
 
private:
  typedef std::shared_ptr<state_representation::Parameter<double>> ptr_param_double_t;
 
  void validate_and_set_parameter(const std::shared_ptr<state_representation::ParameterInterface>& parameter) override;
 
  void set_center(const state_representation::CartesianPose& center);
 
  void set_rotation_offset(const Eigen::Quaterniond& rotation);
 
  [[nodiscard]] Eigen::Quaterniond get_rotation_offset() const;
 
  [[nodiscard]] Eigen::Vector3d calculate_local_linear_velocity(
      const state_representation::CartesianPose& pose, double& local_field_strength
  ) const;
 
  [[nodiscard]] Eigen::Vector3d calculate_local_angular_velocity(
      const state_representation::CartesianPose& pose, const Eigen::Vector3d& linear_velocity,
      double local_field_strength
  ) const;
 
  std::shared_ptr<state_representation::Parameter<state_representation::CartesianPose>> center_; 
  std::shared_ptr<state_representation::Parameter<state_representation::CartesianPose>>
      rotation_offset_; 
  ptr_param_double_t radius_; 
  ptr_param_double_t width_; 
  ptr_param_double_t speed_; 
  ptr_param_double_t field_strength_; 
  ptr_param_double_t normal_gain_; 
  ptr_param_double_t angular_gain_; 
};
}// namespace dynamical_systems