Skip to content

Commit

Permalink
[update] use the library libra::numerical_integrator
Browse files Browse the repository at this point in the history
  • Loading branch information
TomokiMochizuki committed Jun 5, 2024
1 parent c2f09e9 commit 00efcf3
Show file tree
Hide file tree
Showing 5 changed files with 224 additions and 106 deletions.
2 changes: 1 addition & 1 deletion src/dynamics/attitude/attitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ 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> Attitude::CalcAngularVelocityMatrix(libra::Vector<3> 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;
Expand Down
14 changes: 7 additions & 7 deletions src/dynamics/attitude/attitude.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,13 +134,13 @@ class Attitude : public ILoggable, public SimulationObject {
* @brief Calculate angular momentum
*/
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);
};

/**
* @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_
123 changes: 37 additions & 86 deletions src/dynamics/attitude/attitude_with_cantilever_vibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,14 +5,16 @@
#include "attitude_with_cantilever_vibration.hpp"

#include <logger/log_utility.hpp>
#include <math_physics/numerical_integration/runge_kutta_4.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) {
: Attitude(inertia_tensor_kgm2, simulation_object_name),
numerical_integrator_(propagation_step_s, attitude_ode_, libra::numerical_integration::NumericalIntegrationMethod::kRk4) {
angular_velocity_b_rad_s_ = angular_velocity_b_rad_s;
quaternion_i2b_ = quaternion_i2b;
torque_b_Nm_ = torque_b_Nm;
Expand All @@ -21,11 +23,12 @@ AttitudeWithCantileverVibration::AttitudeWithCantileverVibration(
angular_momentum_reaction_wheel_b_Nms_ = libra::Vector<3>(0.0);
previous_inertia_tensor_kgm2_ = inertia_tensor_kgm2_;
inertia_tensor_cantilever_kgm2_ = inertia_tensor_cantilever_kgm2;
attenuateion_coefficient_ = 2 * damping_ratio_cantilever * intrinsic_angular_velocity_cantilever_rad_s;
attenuation_coefficient_ = 2 * damping_ratio_cantilever * intrinsic_angular_velocity_cantilever_rad_s;
spring_coefficient_ = pow(intrinsic_angular_velocity_cantilever_rad_s, 2.0);
inverse_inertia_tensor_ = CalcInverseMatrix(inertia_tensor_kgm2_);
inverse_equivalent_inertia_tensor_cantilever_ = CalcInverseMatrix(inertia_tensor_kgm2_ - inertia_tensor_cantilever_kgm2_) * inertia_tensor_kgm2_;
CalcAngularMomentum();
SetOdeParameters();
}

AttitudeWithCantileverVibration::~AttitudeWithCantileverVibration() {}
Expand Down Expand Up @@ -58,124 +61,72 @@ void AttitudeWithCantileverVibration::SetParameters(const MonteCarloSimulationEx
CalcAngularMomentum();
}

void AttitudeWithCantileverVibration::SetOdeParameters() {
attitude_ode_.SetAttenuationCoefficient(attenuation_coefficient_);
attitude_ode_.SetSpringCoefficient(spring_coefficient_);
attitude_ode_.SetTorque_b_Nm(torque_b_Nm_);
attitude_ode_.SetTorqueInertiaTensor_b_Nm(torque_inertia_tensor_b_Nm_);
attitude_ode_.SetAngularMomentumReactionWheel_b_Nms(angular_momentum_reaction_wheel_b_Nms_);
attitude_ode_.SetInverseInertiaTensor(inverse_inertia_tensor_);
attitude_ode_.SetPreviousInertiaTensor_kgm2(previous_inertia_tensor_kgm2_);
attitude_ode_.SetInertiaTensorCantilever_kgm2(inertia_tensor_cantilever_kgm2_);
attitude_ode_.SetInverseEquivalentInertiaTensorCantilever(inverse_equivalent_inertia_tensor_cantilever_);
}

void AttitudeWithCantileverVibration::Propagate(const double end_time_s) {
if (!is_calc_enabled_) return;

libra::Matrix<3, 3> dot_inertia_tensor =
(1.0 / (end_time_s - current_propagation_time_s_)) * (inertia_tensor_kgm2_ - previous_inertia_tensor_kgm2_);
torque_inertia_tensor_change_b_Nm_ = dot_inertia_tensor * angular_velocity_b_rad_s_;
torque_inertia_tensor_b_Nm_ = dot_inertia_tensor * angular_velocity_b_rad_s_;
inverse_inertia_tensor_ = CalcInverseMatrix(inertia_tensor_kgm2_);
SetOdeParameters();

libra::Vector<13> state = SetStateFromPhysicalQuantities();
numerical_integrator_.GetIntegrator()->SetState(propagation_step_s_, state);
while (end_time_s - current_propagation_time_s_ - propagation_step_s_ > 1.0e-6) {
RungeKuttaOneStep(current_propagation_time_s_, propagation_step_s_);
numerical_integrator_.GetIntegrator()->Integrate();
current_propagation_time_s_ += propagation_step_s_;
}
RungeKuttaOneStep(current_propagation_time_s_, end_time_s - current_propagation_time_s_);
numerical_integrator_.GetIntegrator()->SetState(end_time_s - current_propagation_time_s_, numerical_integrator_.GetIntegrator()->GetState());
numerical_integrator_.GetIntegrator()->Integrate();
SetPhysicalQuantitiesFromState(numerical_integrator_.GetIntegrator()->GetState());

// Update information
current_propagation_time_s_ = end_time_s;
previous_inertia_tensor_kgm2_ = inertia_tensor_kgm2_;
CalcAngularMomentum();
}

libra::Vector<13> AttitudeWithCantileverVibration::AttitudeDynamicsAndKinematics(libra::Vector<13> x, double t) {
UNUSED(t);

libra::Vector<13> dxdt;

libra::Vector<3> omega_b_rad_s;
for (int i = 0; i < 3; i++) {
omega_b_rad_s[i] = x[i];
}
libra::Vector<3> omega_cantilever_rad_s;
libra::Vector<13> AttitudeWithCantileverVibration::SetStateFromPhysicalQuantities() {
libra::Vector<13> state;
for (int i = 0; i < 3; i++) {
omega_cantilever_rad_s[i] = x[i + 3];
state[i] = angular_velocity_b_rad_s_[i];
}

libra::Vector<3> euler_angle_cantilever_rad;
for (int i = 0; i < 3; i++) {
euler_angle_cantilever_rad[i] = x[i + 10];
state[i + 3] = angular_velocity_cantilever_rad_s_[i];
}

libra::Vector<3> angular_momentum_total_b_Nms = (previous_inertia_tensor_kgm2_ * omega_b_rad_s) + angular_momentum_reaction_wheel_b_Nms_;
libra::Vector<3> net_torque_b_Nm =
torque_b_Nm_ - libra::OuterProduct(omega_b_rad_s, angular_momentum_total_b_Nms) - torque_inertia_tensor_change_b_Nm_;

libra::Vector<3> angular_accelaration_cantilever_rad_s2 =
-(inverse_equivalent_inertia_tensor_cantilever_ *
(attenuateion_coefficient_ * omega_cantilever_rad_s + spring_coefficient_ * euler_angle_cantilever_rad)) -
inverse_inertia_tensor_ * net_torque_b_Nm;

libra::Vector<3> rhs = inverse_inertia_tensor_ * (net_torque_b_Nm - inertia_tensor_cantilever_kgm2_ * angular_accelaration_cantilever_rad_s2);

for (int i = 0; i < 3; ++i) {
dxdt[i] = rhs[i];
}

for (int i = 0; i < 3; i++) {
dxdt[i + 3] = angular_accelaration_cantilever_rad_s2[i];
}

libra::Vector<4> quaternion_i2b;
for (int i = 0; i < 4; i++) {
quaternion_i2b[i] = x[i + 6];
state[i + 6] = quaternion_i2b_[i];
}

libra::Vector<4> d_quaternion = 0.5 * CalcAngularVelocityMatrix(omega_b_rad_s) * quaternion_i2b;

for (int i = 0; i < 4; i++) {
dxdt[i + 6] = d_quaternion[i];
}

for (int i = 0; i < 3; i++) {
dxdt[i + 10] = omega_cantilever_rad_s[i];
state[i + 10] = euler_angular_cantilever_rad_[i];
}

return dxdt;
return state;
}

void AttitudeWithCantileverVibration::RungeKuttaOneStep(double t, double dt) {
libra::Vector<13> x;
for (int i = 0; i < 3; i++) {
x[i] = angular_velocity_b_rad_s_[i];
}
for (int i = 0; i < 3; i++) {
x[i + 3] = angular_velocity_cantilever_rad_s_[i];
}
for (int i = 0; i < 4; i++) {
x[i + 6] = quaternion_i2b_[i];
}
for (int i = 0; i < 3; i++) {
x[i + 10] = euler_angular_cantilever_rad_[i];
}

libra::Vector<13> k1, k2, k3, k4;
libra::Vector<13> xk2, xk3, xk4;

k1 = AttitudeDynamicsAndKinematics(x, t);
xk2 = x + (dt / 2.0) * k1;

k2 = AttitudeDynamicsAndKinematics(xk2, (t + dt / 2.0));
xk3 = x + (dt / 2.0) * k2;

k3 = AttitudeDynamicsAndKinematics(xk3, (t + dt / 2.0));
xk4 = x + dt * k3;

k4 = AttitudeDynamicsAndKinematics(xk4, (t + dt));

libra::Vector<13> next_x = x + (dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4);

void AttitudeWithCantileverVibration::SetPhysicalQuantitiesFromState(libra::Vector<13> state) {
for (int i = 0; i < 3; i++) {
angular_velocity_b_rad_s_[i] = next_x[i];
angular_velocity_b_rad_s_[i] = state[i];
}
for (int i = 0; i < 3; i++) {
angular_velocity_cantilever_rad_s_[i] = next_x[i + 3];
angular_velocity_cantilever_rad_s_[i] = state[i + 3];
}
for (int i = 0; i < 4; i++) {
quaternion_i2b_[i] = next_x[i + 6];
quaternion_i2b_[i] = state[i + 6];
}
quaternion_i2b_.Normalize();
for (int i = 0; i < 3; i++) {
euler_angular_cantilever_rad_[i] = next_x[i + 10];
euler_angular_cantilever_rad_[i] = state[i + 10];
}
}
44 changes: 32 additions & 12 deletions src/dynamics/attitude/attitude_with_cantilever_vibration.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,11 @@
#ifndef S2E_DYNAMICS_ATTITUDE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_
#define S2E_DYNAMICS_ATTITUDE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_

#include <math_physics/numerical_integration/numerical_integrator_manager.hpp>
#include <utilities/macros.hpp>

#include "attitude.hpp"
#include "ode_attitude_with_cantilever_vibration.hpp"

/**
* @class AttitudeWithCantileverVibration
Expand Down Expand Up @@ -56,6 +60,16 @@ class AttitudeWithCantileverVibration : public Attitude {
* @brief Override GetLogValue function of ILoggable
*/
virtual std::string GetLogValue() const;
/**
* @fn GetPreviousInertiaTensor_b_kgm2
* @brief Return previous inertia tensor [kg m^2]
*/
inline libra::Matrix<3, 3> GetPreviousInertiaTensor_b_kgm2() const { return previous_inertia_tensor_kgm2_; }
/**
* @fn GetTorqueInertiaTensor_b_Nm
* @brief Return torque generated by inertia tensor [Nm]
*/
inline libra::Vector<3> GetTorqueInertiaTensor_b_Nm() const { return torque_inertia_tensor_b_Nm_; }

/**
* @fn SetParameters
Expand All @@ -64,32 +78,38 @@ class AttitudeWithCantileverVibration : public Attitude {
*/
virtual void SetParameters(const MonteCarloSimulationExecutor& mc_simulator);

/**
* @fn SetOdeParameters
* @brief Set parameters for ordinary differential equations
*/
void SetOdeParameters();

private:
double current_propagation_time_s_; //!< current time [sec]
libra::Matrix<3, 3> inverse_inertia_tensor_; //!< Inverse of inertia tensor
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]
libra::Vector<3> torque_inertia_tensor_b_Nm_; //!< Torque generated by inertia tensor [Nm]
libra::Matrix<3, 3> inertia_tensor_cantilever_kgm2_; //!< Inertia tensor of the cantilever [kgm2]
libra::Matrix<3, 3> inverse_equivalent_inertia_tensor_cantilever_; //!< Inverse of inertia tensor of the cantilever
double attenuateion_coefficient_; //!< Attenuation coefficient
double attenuation_coefficient_; //!< Attenuation coefficient
double spring_coefficient_; //!< Spring coefficient
libra::Vector<3> angular_velocity_cantilever_rad_s_; //!< Angular velocity of the cantilever with respect to the body frame [rad/s]
libra::Vector<3> euler_angular_cantilever_rad_; //!< Euler angle of the cantilever with respect to the body frame [rad/s]

libra::numerical_integration::AttitudeWithCantileverVibrationOde attitude_ode_;
libra::numerical_integration::NumericalIntegratorManager<13> numerical_integrator_;

/**
* @fn AttitudeDynamicsAndKinematics
* @brief Dynamics equation with kinematics
* @param [in] x: State vector (angular velocity and quaternion)
* @param [in] t: Unused TODO: remove?
* @fn SetStateFromPhysicalQuantities
* @brief Set state for calculating the ordinary differential equation from physical quantities
*/
libra::Vector<13> AttitudeDynamicsAndKinematics(libra::Vector<13> x, double t);
libra::Vector<13> SetStateFromPhysicalQuantities();

/**
* @fn RungeKuttaOneStep
* @brief Equation for one step of Runge-Kutta method
* @param [in] t: Unused TODO: remove?
* @param [in] dt: Step width [sec]
* @fn SetPhysicalQuantitiesFromState
* @brief Set physical quantities from state acquired by calculation of the ordinary differential equation
*/
void RungeKuttaOneStep(double t, double dt);
void SetPhysicalQuantitiesFromState(libra::Vector<13> state);
};

#endif // S2E_DYNAMICS_ATTITUDE_ATTITUDE_WITH_CANTILEVER_VIBRATION_HPP_
Loading

0 comments on commit 00efcf3

Please sign in to comment.