system_identification.h

Go to the documentation of this file.

#pragma once
 
#include <dlib/matrix.h>
 
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <vector>
 
#include "cia402_drive.h"
#include "motion-master.pb.h"
#include "si_unit_velocity.h"
#include "util.h"
 
class SystemIdentification {
 public:
  typedef std::vector<double> state_type;
 
  struct push_back_state_and_time {
    std::vector<state_type>& m_states;
    std::vector<double>& m_times;
    bool m_resample;
    double m_timeStep;
    size_t m_size;
 
    push_back_state_and_time(std::vector<state_type>& states,
                             std::vector<double>& times, bool resample = false,
                             double timeStep = 0, size_t size = 0)
        : m_states(states),
          m_times(times),
          m_resample(resample),
          m_timeStep(timeStep),
          m_size(size) {}
 
    void operator()(const state_type& x, double t) {
      if (m_resample) {
        if (m_times.size() > 0) {
          double invTS = 1.0 / m_timeStep;
          double bt = m_times.back();
          if (t >= bt + m_timeStep && m_times.size() < m_size) {
            state_type bx = m_states.back();
            state_type new_x(x.size());
            // perform linear interpolation
            for (size_t si = 0; si < x.size(); si++)
              new_x[si] = x[si] - (x[si] - bx[si]) * (t - bt) * invTS;
            m_states.push_back(new_x);
            m_times.push_back(bt + m_timeStep);
            // this is just preparation for the last value
            if (m_times.size() == m_size - 1) {
              m_states.push_back(new_x);
              m_times.push_back(bt + m_timeStep);
            }
          } else if (m_times.size() == m_size) {
            // the last value can be a problem because it never reaches above
            // condition so for the last value we just set whatever is the last
            // value
            m_states[m_size - 1] = x;
          }
        } else {
          m_states.push_back(x);
          m_times.push_back(t);
        }
      } else {
        m_states.push_back(x);
        m_times.push_back(t);
      }
    }
  };
 
  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 next_gen_sys_id = false, 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 next_gen_sys_id_ = false;
  bool skip_chirp_signal_ = false;
 
  double gear_ratio_ = 1.0;     // initialized in init()
  double feed_constant_ = 1.0;  // initialized in init()

  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);

  MotionMasterError save_plant_model_to_flash(PlantModel m);

  static std::vector<double> convert_position_to_si(
      std::vector<int32_t> raw_positions, uint32_t singleturn_resolution);

  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);

  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);

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

  static 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);

  MotionMasterError set_integral_limits();

  static bool check_model_stability(const PlantModel& m);

  static PlantModel identify_plant_model(std::vector<double> velocities_si,
                                         std::vector<double> torques_si,
                                         SystemParameters system_params,
                                         TuningParameters tuning_params);

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

  static std::tuple<PlantModel, double> get_optimal_model(
      std::vector<double> velocities_si, std::vector<double> torques_si,
      SystemParameters system_params, TuningParameters tuning_params,
      const std::string& fileName = "");

  static double calculate_stability_margin(PlantModel m);

  static double validate_sys_ident(PlantModel m,
                                   const std::vector<double>& velocities_si,
                                   const std::vector<double>& torques_si,
                                   SystemParameters system_params,
                                   TuningParameters tuning_params,
                                   const std::string& = "");

  static PlantModel do_least_squares(std::vector<double> y,
                                     std::vector<double> tau, double time_step,
                                     uint8_t order, uint16_t cutoff_frequency);

  static std::vector<double> gpmf_filtering(std::vector<double> data,
                                            uint16_t cutoff_frequency,
                                            double time_step);

  static std::tuple<dlib::matrix<double>, std::vector<double>,
                    std::vector<double>>
  create_data_matrix(std::vector<double> y, std::vector<double> tau,
                     double time_step, uint8_t order);

  static std::vector<double> numerical_differentiation(std::vector<double> data,
                                                       double time_step);
 
  static void write_sys_id_csv(const std::string& filename,
                               const std::vector<double>& torques_si,
                               const std::vector<double>& velocities_si);
};