system_identification.h

Go to the documentation of this file.

#pragma once
 
#include <vector>
 
#include "cia402_drive.h"
#include "motion-master.pb.h"
#include "si_unit_velocity.h"
#include "util.h"
 
class SystemIdentification {
 public:
  SystemIdentification(
      uint32_t device_address, Cia402Drive& cia402_drive,
      TuningParameters tuning_params,
      const std::function<void()>& started_callback = {},
      const std::function<void(motionmaster::MotionMasterMessage::Status::
                                   SystemIdentification::Warning::Code,
                               std::string)>& warning_callback = {},
      bool skip_chirp_signal = false);
 
  ~SystemIdentification();

  std::variant<MotionMasterError, PlantModel> start();
 
 private:
  uint32_t device_address_;
  Cia402Drive& cia402_drive_;
 
  TuningParameters tuning_params_;
  SystemParameters system_params_;
 
  const std::function<void()>& started_callback_;
  const std::function<void(motionmaster::MotionMasterMessage::Status::
                               SystemIdentification::Warning::Code,
                           std::string)>& warning_callback_;
 
  SiUnitVelocity si_unit_velocity_;
 
  bool skip_chirp_signal_ = false;
 
  double gear_ratio_ = 1.0;
  double feed_constant_ = 1.0;

  MotionMasterError init();

  static std::vector<int16_t> chirp(TuningParameters tuning_params);

  static double missing_data(std::vector<int32_t>& raw_positions,
                             std::vector<int32_t>& raw_velocities,
                             std::vector<int32_t>& raw_torques);

  static int32_t calculate_linear_interpolation_step_value(
      std::vector<int32_t> data, uint32_t first_zero_index,
      uint32_t last_zero_index);

  static void linear_interpolation(std::vector<int32_t>& raw_positions,
                                   std::vector<int32_t>& raw_velocities,
                                   std::vector<int32_t>& raw_torques);

  static std::vector<double> convert_velocity_to_si(
      const std::vector<int32_t>& raw_velocities, double gear_ratio = 1.0,
      double velocity_unit_scaling = 1, double feed_constant = 1.0);

  MotionMasterError check_input_signal_signs(
      const std::vector<int16_t>& chirp_target_torques,
      const std::vector<double>& velocities,
      const std::vector<int32_t>& raw_positions);

  static int32_t find_torque_sign_change(
      const std::vector<int16_t>& chirp_target_torques);

  MotionMasterError save_plant_model_to_flash(PlantModel m);

  MotionMasterError check_input_signal_amplitudes(
      const std::vector<int32_t>& torques,
      const std::vector<double>& velocities);

  MotionMasterError set_integral_limits();

  std::vector<double> convert_velocity_q15_to_si(
      const std::vector<int32_t>& q15_velocities);

  static std::vector<double> convert_torque_to_si(
      const std::vector<int32_t>& raw_torques, uint32_t nominal_torque);

  static bool check_model_stability(const PlantModel& m);
 
  static void write_sys_id_csv(const std::string& filename,
                               const std::vector<double>& torques_si,
                               const std::vector<double>& velocities_si);
};