SixDOFDynamicsModel.h

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC.
******************************************************************************/
/*
6-DOF Dynamics header file
 
Author: Alex Reynolds
*/
 
#ifndef MODELS_SUPPORT_SIX_DOF_DYNAMICS_MODEL
#define MODELS_SUPPORT_SIX_DOF_DYNAMICS_MODEL
 
#include <vector>
#include <array>
#include <list>
 
#include "architecture/Time.h"
#include "architecture/Tasks.h"
#include "architecture/Executive.h"
#include "six_dof_dynamics/Frame.hpp"
#include "six_dof_dynamics/Node.hpp"
#include "six_dof_dynamics/Body.hpp"
#include "six_dof_dynamics/FrameDynamics.hpp"
#include "utils/ForwardEulerIntegrator.hpp"
#include "utils/RK4Integrator.hpp"
#include "simulation/SimulationSteps.h"
 
namespace modelspace {
 
    enum integrator_type_e {
        FORWARD_EULER = 1,
        RK4 = 4
    };
 
    /// @brief  Model to implement 6-DOF dynamics
    /// 
    /// This file defines the dynamics used for 6-DOF simulation.
    /// 
    /// It inherits from the classical model architecture, but is designed
    /// to execute in a manner that propagates dynamics forward in time.
    /// 
    /// Tasks executed and order of operations:
    /// Start step: Nothing
    /// Derivative: 
    /// - Ensures frames are up to date by evaluating current state
    /// - Articulates forces and moments applied at nodes through the
    ///   kinematics chain to the appropriate bodies
    /// - Determines frame motion on the basis of applied forces/moments
    /// - Integrates/updates frame states on the basis of kinematics --
    ///   note the critical difference in model scheme here -- this model
    ///   updates frame states in derivative, and therefore must run at 
    ///   the end of the model order after forces and moments have been 
    ///   calculated. It cannot be used as a standard model
    /// End step: Nothing
    /// 
    /// Key restrictions
    /// - Must run at the end of model chain
    /// - Must be one, and only one, instance of this model for every 
    ///   simulation instance that uses 6-DOF frame dynamics
    class SixDOFDynamicsModel : public clockwerk::Task {
    public:
        /// Model params
        ///         NAME                    TYPE                    DEFAULT VALUE
        START_PARAMS
            /** Input parameter describing the integrator type dynamics should use
             *  Valid choices are: "Forward Euler", "RK4" */
            SIGNAL(integrator_type,        modelspace::integrator_type_e,RK4);
            /** Pointer to the root frame for the simulation. This pointer is used
             *  as the starting point for recursive calls to dynamics calculations*/
            SIGNAL(root_frame_ptr,         clockwerk::Frame<double>*,nullptr);
        END_PARAMS
 
        /// Model inputs
        ///         NAME                    TYPE                    DEFAULT VALUE
        START_INPUTS
            /** Time at the start of the simulation step, in seconds/nsec as Time class */
            clockwerk::DataIO<clockwerk::Time> step_start_time = clockwerk::DataIO<clockwerk::Time>(this, "step_start_time");
            /** Time at the end of the simulation step, in seconds as a double */
            clockwerk::DataIO<clockwerk::Time> step_end_time = clockwerk::DataIO<clockwerk::Time>(this, "step_end_time");
        END_INPUTS
 
        /// Model outputs
        ///         NAME                    TYPE                    DEFAULT VALUE
        START_OUTPUTS
            /** The current simulation time as a time object */
            clockwerk::DataIO<clockwerk::Time> current_time = clockwerk::DataIO<clockwerk::Time>(this, "current_time");
        END_OUTPUTS
 
        /// Model-specific implementations of startup and derivative
        /// Must be scheduled manually to derivative
        SixDOFDynamicsModel(clockwerk::Task &pnt, int schedule_slot=-2, const std::string &m_name="six_dof_dynamics")
            : clockwerk::Task(pnt, schedule_slot, m_name) {}
        SixDOFDynamicsModel(clockwerk::Executive &e, int schedule_slot=-2, const std::string &m_name="six_dof_dynamics")
            : clockwerk::Task(e, schedule_slot, m_name) {}
        ~SixDOFDynamicsModel() {}
 
        /// @brief   Function to reset our integrator for next step
        void reset() {_integrator_step_num = 0;}
    protected:
        int start();
        int execute();
 
        /// @brief   Function to recursively integrate the frame tree via 
        ///          forward euler
        /// @param   frame_ptr   Pointer to the root frame
        /// @return  Error code corresponding to success/failure
        int _integrateRecursiveForwardEuler(clockwerk::Frame<double>* frame_ptr);
        /// @brief   Function to recursively integrate the frame tree via RK4
        /// @param   frame_ptr   Pointer to the root frame
        /// @return  Error code corresponding to success/failure
        int _integrateRecursiveRk4(clockwerk::Frame<double>* frame_ptr);
 
        /// @brief   Function to recursively loop through frames and set
        ///          their integrators
        /// @return  Error code corresponding to success/failure
        int _recurseRefreshFrameIntegratorSet(clockwerk::Frame<double>* frame_ptr);
 
        /// Variables to track integrator type and steps 
        unsigned int _integrator_step_num = 0;
        unsigned int _integrator_steps_per_step = 0;
 
        /// Vectors of integrators -- allows for dynamic integrator numbers
        std::list<clockwerk::ForwardEulerIntegrator<double, NUM_INTEGRATED_STATES>> _fe_integrators;
        std::list<clockwerk::RK4Integrator<double, NUM_INTEGRATED_STATES>> _rk4_integrators;
        clockwerk::RK4Integrator<double, NUM_INTEGRATED_STATES>* _rk4_ptr;
        clockwerk::ForwardEulerIntegrator<double, NUM_INTEGRATED_STATES>* _fe_ptr;
 
        /// Our dynamics model for frames
        clockwerk::FrameDynamics<double> _frame_dynamics;
 
        /// Temporary variables to hold state output from integrator
        /// and frame children for recursion
        std::array<double, NUM_INTEGRATED_STATES> _tmp_state;
        std::vector<GraphTreeObject*> _frame_children;
 
        /// Temporary variable for time calculation in RK4
        clockwerk::Time _half_step_size;
    };
 
}
 
#endif