Merge pull request #639 from ut-issl/feature/add_simple_flexible_stru…

…cture

Add flexible structure model

data/sample/initialize_files/sample_satellite.ini

@@ -1,12 +1,13 @@
 [ATTITUDE]
 // Attitude propagation mode
 // RK4 : Attitude Propagation with RK4 including disturbances and control torque
+// CANTILEVER_VIBRATION : Attitude Propagation with the consideration of the cantilever vibration (flexible structure) including disturbances and control torque.
 // CONTROLLED : Attitude Calculation with Controlled Attitude mode. All disturbances and control torque are ignored.
 propagate_mode = RK4
 
 // Initialize Attitude mode
 // MANUAL : Initialize Quaternion_i2b manually below 
-// CONTROLLED : Initialize attitude with given condition. Valid only when Attitude propagation mode is RK4.
+// CONTROLLED : Initialize attitude with given condition. Valid except when Attitude propagation mode is CONTROLLED.
 initialize_mode = MANUAL
 
 // Initial angular velocity at body frame [rad/s]

data/sample/initialize_files/sample_structure.ini

@@ -24,6 +24,25 @@ center_of_gravity_b_m(0) = 0.01
 center_of_gravity_b_m(1) = 0.01
 center_of_gravity_b_m(2) = 0.01
 
+[CANTILEVER_PARAMETERS]
+// Currently, the cantilever frame has the same coordinate system as the body frame of the satellite
+// Inertia Tensor of cantilever (flexible structure) @ body fixed frame [kg・m2]
+inertia_tensor_cantilever_kgm2(0) = 0.1	// I_xx
+inertia_tensor_cantilever_kgm2(1) = 0.0	// I_xy
+inertia_tensor_cantilever_kgm2(2) = 0.0	// I_xz
+inertia_tensor_cantilever_kgm2(3) = 0.0	// I_yx
+inertia_tensor_cantilever_kgm2(4) = 0.1	// I_yy
+inertia_tensor_cantilever_kgm2(5) = 0.0	// I_yz
+inertia_tensor_cantilever_kgm2(6) = 0.0	// I_zx
+inertia_tensor_cantilever_kgm2(7) = 0.0	// I_zy
+inertia_tensor_cantilever_kgm2(8) = 0.1	// I_zz
+
+// Damping ratio of the cantilever
+damping_ratio_cantilever = 0.01
+
+// Intrinsic angular velocity of the cantilever [rad/s]
+intrinsic_angular_velocity_cantilever_rad_s = 25.5097 // 4.06 Hz
+
 [SURFACES]
 number_of_surfaces = 6
 

scripts/Plot/plot_euler_angle_cantilever_observer.py

@@ -0,0 +1,75 @@
+#
+# Plot Euler Angle of Cantilever Observer
+#
+# arg[1] : read_file_tag : time tag for default CSV output log file. ex. 220627_142946
+#
+
+#
+# Import
+#
+# plots
+import matplotlib.pyplot as plt
+# local function
+from common import find_latest_log_tag
+from common import add_log_file_arguments
+from common import read_3d_vector_from_csv
+from common import read_scalar_from_csv
+# arguments
+import argparse
+
+# Arguments
+aparser = argparse.ArgumentParser()
+aparser = add_log_file_arguments(aparser)
+aparser.add_argument('--no-gui', action='store_true')
+args = aparser.parse_args()
+
+#
+# Read Arguments
+#
+# log file path
+path_to_logs = args.logs_dir
+
+read_file_tag = args.file_tag
+if read_file_tag == None:
+  print("file tag does not found. use latest.")
+  read_file_tag = find_latest_log_tag(path_to_logs)
+
+print("log: " + read_file_tag)
+
+#
+# CSV file name
+#
+read_file_name  = path_to_logs + '/' + 'logs_' + read_file_tag + '/' + read_file_tag + '_default.csv'
+
+#
+# Data read and edit
+#
+# Read S2E CSV
+time = read_scalar_from_csv(read_file_name, 'elapsed_time[s]')
+
+euler_angle_cantilever_rad = read_3d_vector_from_csv(read_file_name, 'euler_angular_cantilever_c', 'rad')
+
+#
+# Plot
+#
+unit = ' rad'
+
+fig, axis = plt.subplots(3, 1, squeeze = False, tight_layout = True, sharex = True)
+axis[0, 0].plot(time[0][0:1200], euler_angle_cantilever_rad[0][0:1200], c="orange",  label="TRUE-X")
+axis[0, 0].legend(loc = 'upper right')
+
+axis[1, 0].plot(time[0][0:1200], euler_angle_cantilever_rad[1][0:1200], c="green",  label="TRUE-Y")
+axis[1, 0].legend(loc = 'upper right')
+
+axis[2, 0].plot(time[0][0:1200], euler_angle_cantilever_rad[2][0:1200], c="blue",   label="TRUE-Z")
+axis[2, 0].legend(loc = 'upper right')
+
+fig.suptitle("Euler Angle Observer")
+fig.supylabel("Euler Angle [" + unit + "]")
+fig.supxlabel("Time [s]")
+
+# Data save
+if args.no_gui:
+  plt.savefig(read_file_tag + "_angular_velocity_observer.png") # save last figure only
+else:
+  plt.show()

src/dynamics/CMakeLists.txt

@@ -18,6 +18,7 @@ add_library(${PROJECT_NAME} STATIC
 
   attitude/attitude.cpp
   attitude/attitude_rk4.cpp
+  attitude/attitude_with_cantilever_vibration.cpp
   attitude/controlled_attitude.cpp
   attitude/initialize_attitude.cpp
 

src/dynamics/attitude/attitude.cpp

@@ -56,3 +56,26 @@ void Attitude::CalcAngularMomentum(void) {
 
   kinetic_energy_J_ = 0.5 * libra::InnerProduct(angular_momentum_spacecraft_b_Nms_, angular_velocity_b_rad_s_);
 }
+
+libra::Matrix<4, 4> CalcAngularVelocityMatrix(libra::Vector<3> angular_velocity_b_rad_s) {
+  libra::Matrix<4, 4> angular_velocity_matrix;
+
+  angular_velocity_matrix[0][0] = 0.0f;
+  angular_velocity_matrix[0][1] = angular_velocity_b_rad_s[2];
+  angular_velocity_matrix[0][2] = -angular_velocity_b_rad_s[1];
+  angular_velocity_matrix[0][3] = angular_velocity_b_rad_s[0];
+  angular_velocity_matrix[1][0] = -angular_velocity_b_rad_s[2];
+  angular_velocity_matrix[1][1] = 0.0f;
+  angular_velocity_matrix[1][2] = angular_velocity_b_rad_s[0];
+  angular_velocity_matrix[1][3] = angular_velocity_b_rad_s[1];
+  angular_velocity_matrix[2][0] = angular_velocity_b_rad_s[1];
+  angular_velocity_matrix[2][1] = -angular_velocity_b_rad_s[0];
+  angular_velocity_matrix[2][2] = 0.0f;
+  angular_velocity_matrix[2][3] = angular_velocity_b_rad_s[2];
+  angular_velocity_matrix[3][0] = -angular_velocity_b_rad_s[0];
+  angular_velocity_matrix[3][1] = -angular_velocity_b_rad_s[1];
+  angular_velocity_matrix[3][2] = -angular_velocity_b_rad_s[2];
+  angular_velocity_matrix[3][3] = 0.0f;
+
+  return angular_velocity_matrix;
+}

src/dynamics/attitude/attitude.hpp

@@ -114,11 +114,11 @@ class Attitude : public ILoggable, public SimulationObject {
   virtual void SetParameters(const MonteCarloSimulationExecutor& mc_simulator);
 
  protected:
-  bool is_calc_enabled_ = true;                //!< Calculation flag
-  double propagation_step_s_;                  //!< Propagation step [sec]
-  libra::Vector<3> angular_velocity_b_rad_s_;  //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s]
-  libra::Quaternion quaternion_i2b_;           //!< Attitude quaternion from the inertial frame to the body fixed frame
-  libra::Vector<3> torque_b_Nm_;               //!< Torque in the body fixed frame [Nm]
+  bool is_calc_enabled_ = true;                     //!< Calculation flag
+  double propagation_step_s_;                       //!< Propagation step [sec]
+  libra::Vector<3> angular_velocity_b_rad_s_;       //!< Angular velocity of spacecraft body fixed frame with respect to the inertial frame [rad/s]
+  libra::Quaternion quaternion_i2b_;                //!< Attitude quaternion from the inertial frame to the body fixed frame
+  libra::Vector<3> torque_b_Nm_;                    //!< Torque in the body fixed frame [Nm]
 
   const libra::Matrix<3, 3>& inertia_tensor_kgm2_;  //!< Inertia tensor of the spacecraft [kg m^2]
 
@@ -136,4 +136,11 @@ class Attitude : public ILoggable, public SimulationObject {
   void CalcAngularMomentum(void);
 };
 
+/**
+ * @fn CalcAngularVelocityMatrix
+ * @brief Generate angular velocity matrix for kinematics calculation
+ * @param [in] angular_velocity_b_rad_s: Angular velocity [rad/s]
+ */
+libra::Matrix<4, 4> CalcAngularVelocityMatrix(libra::Vector<3> angular_velocity_b_rad_s);
+
 #endif  // S2E_DYNAMICS_ATTITUDE_ATTITUDE_HPP_

src/dynamics/attitude/attitude_rk4.cpp

@@ -56,29 +56,6 @@ void AttitudeRk4::Propagate(const double end_time_s) {
   CalcAngularMomentum();
 }
 
-libra::Matrix<4, 4> AttitudeRk4::CalcAngularVelocityMatrix(libra::Vector<3> angular_velocity_b_rad_s) {
-  libra::Matrix<4, 4> angular_velocity_matrix;
-
-  angular_velocity_matrix[0][0] = 0.0f;
-  angular_velocity_matrix[0][1] = angular_velocity_b_rad_s[2];
-  angular_velocity_matrix[0][2] = -angular_velocity_b_rad_s[1];
-  angular_velocity_matrix[0][3] = angular_velocity_b_rad_s[0];
-  angular_velocity_matrix[1][0] = -angular_velocity_b_rad_s[2];
-  angular_velocity_matrix[1][1] = 0.0f;
-  angular_velocity_matrix[1][2] = angular_velocity_b_rad_s[0];
-  angular_velocity_matrix[1][3] = angular_velocity_b_rad_s[1];
-  angular_velocity_matrix[2][0] = angular_velocity_b_rad_s[1];
-  angular_velocity_matrix[2][1] = -angular_velocity_b_rad_s[0];
-  angular_velocity_matrix[2][2] = 0.0f;
-  angular_velocity_matrix[2][3] = angular_velocity_b_rad_s[2];
-  angular_velocity_matrix[3][0] = -angular_velocity_b_rad_s[0];
-  angular_velocity_matrix[3][1] = -angular_velocity_b_rad_s[1];
-  angular_velocity_matrix[3][2] = -angular_velocity_b_rad_s[2];
-  angular_velocity_matrix[3][3] = 0.0f;
-
-  return angular_velocity_matrix;
-}
-
 libra::Vector<7> AttitudeRk4::AttitudeDynamicsAndKinematics(libra::Vector<7> x, double t) {
   UNUSED(t);
 

src/dynamics/attitude/attitude_rk4.hpp

@@ -53,12 +53,6 @@ class AttitudeRk4 : public Attitude {
   libra::Matrix<3, 3> previous_inertia_tensor_kgm2_;    //!< Previous inertia tensor [kgm2]
   libra::Vector<3> torque_inertia_tensor_change_b_Nm_;  //!< Torque generated by inertia tensor change [Nm]
 
-  /**
-   * @fn CalcAngularVelocityMatrix
-   * @brief Generate angular velocity matrix for kinematics calculation
-   * @param [in] angular_velocity_b_rad_s: Angular velocity [rad/s]
-   */
-  libra::Matrix<4, 4> CalcAngularVelocityMatrix(libra::Vector<3> angular_velocity_b_rad_s);
   /**
    * @fn AttitudeDynamicsAndKinematics
    * @brief Dynamics equation with kinematics

src/dynamics/attitude/attitude_with_cantilever_vibration.cpp

@@ -0,0 +1,101 @@
+/**
+ * @file attitude_with_cantilever_vibration.cpp
+ * @brief Class to calculate spacecraft attitude with cantilever vibration
+ */
+#include "attitude_with_cantilever_vibration.hpp"
+
+#include <cassert>
+#include <logger/log_utility.hpp>
+#include <utilities/macros.hpp>
+
+AttitudeWithCantileverVibration::AttitudeWithCantileverVibration(
+    const libra::Vector<3>& angular_velocity_b_rad_s, const libra::Quaternion& quaternion_i2b, const libra::Matrix<3, 3>& inertia_tensor_kgm2,
+    const libra::Matrix<3, 3>& inertia_tensor_cantilever_kgm2, const double damping_ratio_cantilever,
+    const double intrinsic_angular_velocity_cantilever_rad_s, const libra::Vector<3>& torque_b_Nm, const double propagation_step_s,
+    const std::string& simulation_object_name)
+    : Attitude(inertia_tensor_kgm2, simulation_object_name),
+      numerical_integrator_(propagation_step_s, attitude_ode_,
+                            libra::numerical_integration::NumericalIntegrationMethod::kRk4) {  //!< TODO: Set NumericalIntegrationMethod in *.ini file
+  angular_velocity_b_rad_s_ = angular_velocity_b_rad_s;
+  quaternion_i2b_ = quaternion_i2b;
+  torque_b_Nm_ = torque_b_Nm;
+  propagation_step_s_ = propagation_step_s;
+  current_propagation_time_s_ = 0.0;
+  angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0);
+
+  attitude_ode_.SetInertiaTensorCantilever_kgm2(inertia_tensor_cantilever_kgm2);
+  attitude_ode_.SetPreviousInertiaTensor_kgm2(inertia_tensor_kgm2_);
+  double attenuation_coefficient = 2 * damping_ratio_cantilever * intrinsic_angular_velocity_cantilever_rad_s;
+  attitude_ode_.SetAttenuationCoefficient(attenuation_coefficient);
+  double spring_coefficient = pow(intrinsic_angular_velocity_cantilever_rad_s, 2.0);
+  attitude_ode_.SetSpringCoefficient(spring_coefficient);
+  attitude_ode_.SetInverseInertiaTensor(CalcInverseMatrix(inertia_tensor_kgm2_));
+  libra::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever =
+      CalcInverseMatrix(inertia_tensor_kgm2_ - inertia_tensor_cantilever_kgm2) * inertia_tensor_kgm2_;
+  attitude_ode_.SetInverseEquivalentInertiaTensorCantilever(inverse_equivalent_inertia_tensor_cantilever);
+  attitude_ode_.SetTorque_b_Nm(torque_b_Nm_);
+  attitude_ode_.SetAngularMomentumReactionWheel_b_Nms(angular_momentum_reaction_wheel_b_Nms_);
+
+  CalcAngularMomentum();
+}
+
+AttitudeWithCantileverVibration::~AttitudeWithCantileverVibration() {}
+
+std::string AttitudeWithCantileverVibration::GetLogHeader() const {
+  std::string str_tmp = Attitude::GetLogHeader();
+
+  str_tmp += WriteVector("euler_angular_cantilever", "c", "rad", 3);
+  str_tmp += WriteVector("angular_velocity_cantilever", "c", "rad/s", 3);
+
+  return str_tmp;
+}
+
+std::string AttitudeWithCantileverVibration::GetLogValue() const {
+  std::string str_tmp = Attitude::GetLogValue();
+
+  str_tmp += WriteVector(euler_angular_cantilever_rad_);
+  str_tmp += WriteVector(angular_velocity_cantilever_rad_s_);
+
+  return str_tmp;
+}
+
+void AttitudeWithCantileverVibration::SetParameters(const MonteCarloSimulationExecutor& mc_simulator) {
+  Attitude::SetParameters(mc_simulator);
+  GetInitializedMonteCarloParameterVector(mc_simulator, "angular_velocity_b_rad_s", angular_velocity_b_rad_s_);
+
+  // TODO: Consider the following calculation is needed here?
+  current_propagation_time_s_ = 0.0;
+  angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0);  //!< TODO: Consider how to handle this variable
+  CalcAngularMomentum();
+}
+
+void AttitudeWithCantileverVibration::Propagate(const double end_time_s) {
+  if (!is_calc_enabled_) return;
+
+  libra::Matrix<3, 3> previous_inertia_tensor_kgm2 = attitude_ode_.GetPreviousInertiaTensor_kgm2();
+  assert(end_time_s - current_propagation_time_s_ > 1e-6);
+  libra::Matrix<3, 3> dot_inertia_tensor = (1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2);
+  libra::Vector<3> torque_inertia_tensor_b_Nm = dot_inertia_tensor * angular_velocity_b_rad_s_;
+  attitude_ode_.SetTorqueInertiaTensor_b_Nm(torque_inertia_tensor_b_Nm);
+  attitude_ode_.SetInverseInertiaTensor(CalcInverseMatrix(inertia_tensor_kgm2_));
+  attitude_ode_.SetTorque_b_Nm(torque_b_Nm_);
+  attitude_ode_.SetAngularMomentumReactionWheel_b_Nms(angular_momentum_reaction_wheel_b_Nms_);
+
+  libra::Vector<13> state = attitude_ode_.SetStateFromPhysicalQuantities(angular_velocity_b_rad_s_, angular_velocity_cantilever_rad_s_,
+                                                                         quaternion_i2b_, euler_angular_cantilever_rad_);
+  numerical_integrator_.GetIntegrator()->SetState(propagation_step_s_, state);
+  while (end_time_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) {
+    numerical_integrator_.GetIntegrator()->Integrate();
+    current_propagation_time_s_ += propagation_step_s_;
+  }
+  numerical_integrator_.GetIntegrator()->SetState(end_time_s - current_propagation_time_s_, numerical_integrator_.GetIntegrator()->GetState());
+  numerical_integrator_.GetIntegrator()->Integrate();
+  attitude_ode_.SetPhysicalQuantitiesFromState(numerical_integrator_.GetIntegrator()->GetState(), angular_velocity_b_rad_s_,
+                                               angular_velocity_cantilever_rad_s_, quaternion_i2b_, euler_angular_cantilever_rad_);
+  quaternion_i2b_.Normalize();
+
+  // Update information
+  current_propagation_time_s_ = end_time_s;
+  attitude_ode_.SetPreviousInertiaTensor_kgm2(inertia_tensor_kgm2_);
+  CalcAngularMomentum();
+}