Node.hpp

/******************************************************************************
* 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.
******************************************************************************/
/*
Node header file
 
Author: Alex Reynolds
*/
#ifndef NODE_HPP
#define NODE_HPP
 
#include "Frame.hpp"
#include "data_management/DataIO.hpp"
#include "core/matrixmath.hpp"
#include "core/vectormath.hpp"
#include "utils/frameutils.hpp"
 
namespace clockwerk {
 
    /// @brief  Node class to apply forces and moments to frames
    /// 
    /// This file defines the base node class which is used to apply forces and
    /// sense accelerations at fixed locations on bodies.
    /// 
    /// The node class is a special Frame that is fully fixed to its parent
    /// (i.e. cannot be fully free) and has special handles for applying forces
    /// and moments. Because it is a simple fixed frame, it is also ideal for 
    /// sensing accelerations and states, in applications such as an accelerometer.
    ///
    /// Applying forces in the node frame is done via the force() and moment() DataIO
    /// objects. By default, forces and moments are applied in the node frame. However,
    /// passing a frame argument to the force_frame or moment_frame objects causes forces
    /// and moments to be applied in that frame -- this is particularly useful for cases
    /// where the node frame is rotating but the force is not, such as gravity applied
    /// to a rotating spacecraft node.
    template <typename T=double>
    class Node : public Frame<T> {
    public:
        /// @brief   Basic constructor for the node object
        /// @param  name    The name of the node
        /// @param  par     The node's parent. Nullptr by default
        Node(const std::string &name, Frame<T> *par=nullptr);
 
        /// @brief   Function to get the frame's translational joint
        /// @return  The frame's translational joint
        clockwerk::Joint<T> tJoint() {return Frame<T>::_t_joint;}
 
        /// @brief   Function to get the frame's rotational joint
        /// @return  The frame's rotational joint
        clockwerk::Joint<T> rJoint() {return Frame<T>::_r_joint;}
 
        /// @brief The force applied at the node
        clockwerk::DataIO<CartesianVector<T,3>> force
            = clockwerk::DataIO<CartesianVector<T,3>>(nullptr, "force", CartesianVector<T,3>({0.0,0.0,0.0}));
        /// @brief The frame in which the force is applied.
        /// Using this utility abstracts away framerotate commands and also allows forces
        /// to be applied inertially or frame locally, because rotation will occur every step
        /// @note Our node only rotates from the force frame -- no translation or coupling
        clockwerk::DataIO<Frame<T>*> force_frame = clockwerk::DataIO<Frame<T>*>(nullptr, "force_frame", nullptr);
 
        /// @brief The moment applied at the node
        clockwerk::DataIO<CartesianVector<T,3>> moment = clockwerk::DataIO<CartesianVector<T,3>>(nullptr, "moment", CartesianVector<T,3>({0.0,0.0,0.0}));
        /// @brief The frame in which the moment is applied
        /// Using this utility abstracts away framerotate commands and also allows moments
        /// to be applied inertially or frame locally, because rotation will occur every step
        /// @note Our node only rotates from the moment frame -- no translation or coupling
        clockwerk::DataIO<Frame<T>*> moment_frame = clockwerk::DataIO<Frame<T>*>(nullptr, "moment_frame", nullptr);
 
        /* Nice handle to the node itself for mapping*/
        clockwerk::DataIO<Node<T>*> self_id = clockwerk::DataIO<Node<T>*>(nullptr, "self_id", this);
 
        /// @brief   Function to resolve applied forces/moments into a force/moment pair in parent body frame origin
        /// @param   force_parentcg__p      Implicit return of force applied to this node in the parent body frame
        /// @param   moment_parentcg__p     Implicit return of moment applied to this node in the parent body frame,
        ///                                 RESOLVED TO A FORCE/MOMENT COUPLE AT THE PARENT FRAME LOCATION
        void getFMPairAtParentOrigin(CartesianVector<T, 3> &force_parentcg__p, CartesianVector<T, 3> &moment_parentcg__p);
 
        CartesianVector<T, 3> passthroughForce() {return _passthrough_force__b;}
        CartesianVector<T, 3> passthroughMoment() {return _passthrough_moment__b;}
    protected:
        /// Passthrough force on the node -- force not applied here but passed on to parent body
        /// All forces and moments should be passed through for nodes
        CartesianVector<T, 3> _passthrough_force__b;
        CartesianVector<T, 3> _passthrough_moment__b;
    };
 
    template <typename T>
    Node<T>::Node(const std::string &name, Frame<T>* par)
        : Frame<T>(name, par, false) {
 
        GraphTreeObject::_graph_tree_type = NODE;
    }
 
    template <typename T>
    void Node<T>::getFMPairAtParentOrigin(CartesianVector<T, 3> &force_parentcg__p,
                                        CartesianVector<T, 3> &moment_parentcg__p) {
        /// First, we need to get our force and moment as applied in the node frame
        // std::cout<<Frame<T>::name()<<std::endl;
        if(force_frame() == nullptr || force_frame() == this) {
            _passthrough_force__b = force();
        } else {
            frameRotate(force(), *force_frame(), *this, _passthrough_force__b);
        }
        if(moment_frame() == nullptr || moment_frame() == this) {
            _passthrough_moment__b = moment();
        } else {
            frameRotate(moment(), *moment_frame(), *this, _passthrough_moment__b);
        }
 
        // First calculate our passthrough force in parent frame
        DCM<T> dcm_p_f = Frame<T>::quat_f_p().toDCM().inverse();
        force_parentcg__p = dcm_p_f*_passthrough_force__b;
 
        /// Now add our moment couple to our already applied moment
        moment_parentcg__p = dcm_p_f*_passthrough_moment__b + cross(Frame<T>::pos_f_p__p(), force_parentcg__p);
    }
 
}
 
#endif