Body.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.
******************************************************************************/
/*
Body header file
 
Author: Alex Reynolds
*/
#ifndef BODY_HPP
#define BODY_HPP
 
#include "data_management/DataIO.hpp"
#include "Frame.hpp"
#include "Node.hpp"
 
namespace clockwerk {
 
    /// @brief  Class to define a body as a frame with mass and inertia
    /// 
    /// This file defines the base body class which forms the basis for 6-DOF
    /// dynaics. Bodies are special invocations of the Frame object with mass
    /// and inertia, which allows them to interact with frames freely.
    /// 
    /// For the individual body, inertia is assumed to be about the frame center.
    /// 
    /// TODO
    /// For now, the body class enforces the following constraints in order to
    /// make things simpler and verify dynamics.
    /// - Bodies may only be children of frames
    /// - Nodes may only be children of bodies
    /// - Joints are restricted to fully free or fully locked
    /// 
    /// Unless otherwise noted units should be assumed metric
    template <typename T>
    class Body : public Frame<T> {
    public:
        /// @brief   Parent constructor for the body object 
        /// @param  name    The name of the body
        /// @param  par     The frame parent. Default is nullptr
        Body(const std::string &name, Frame<T>* par=nullptr);
 
        /// @brief  Signal/parameter for the body's mass
        clockwerk::DataIO<T> mass = clockwerk::DataIO<T>(nullptr, "mass", 1.0);
        /// @brief  Signal/parameter for the body's inertia
        clockwerk::DataIO<Matrix<T, 3, 3>> inertia = clockwerk::DataIO<Matrix<T, 3, 3>>(nullptr, "inertia",
                                        clockwerk::Matrix<T, 3, 3>({{1.0,0.0,0.0},{0.0,1.0,0.0},{0.0,0.0,1.0}}));
        /// @brief  Signal/parameter for the body's composite mass.
        /// @note Is calculated -- should not be modified
        clockwerk::DataIO<T> composite_mass = clockwerk::DataIO<T>(nullptr, "composite_mass", 1.0);
        /// @brief  Signal/parameter for the body's composite inertia.
        /// @note Is calculated -- should not be modified
        clockwerk::DataIO<Matrix<T, 3, 3>> composite_inertia = clockwerk::DataIO<Matrix<T, 3, 3>>(nullptr, "composite_inertia",
                                                    clockwerk::Matrix<T, 3, 3>({{1.0,0.0,0.0},{0.0,1.0,0.0},{0.0,0.0,1.0}}));
 
        /* Nice handle to the body itself for mapping*/
        clockwerk::DataIO<Body<T>*> self_id = clockwerk::DataIO<Body<T>*>(nullptr, "self_id", this);
 
        /// @brief   Function to recurse through the body and its children to apply external
        ///          forces and moments to the "correct" body by resolving them to locations
        ///          where there are degrees of freedom.
        /// @note    Specific implementation of this function for body class. Overwrites
        ///          frame class implementation
        int calcFrameTreeExtForcesMoments();
 
        /// @brief   Function to recurse through the body and its children to resolve applied
        ///          external forces and moments into acceleration and angular acceleration
        /// @note    Implicitly applies acceleration/ang accel to the body frame 
        /// @note    Assumes calcBodyTreeForcesMoments has been called to calculate forces/moments
        /// @note    Specific implementation of this function for body class. Overwrites
        ///          frame class implementation
        int calcFrameTreeExtAcceleration();
 
        /// @brief   Function to resolve passthrough forces/moments into a force/moment pair in parent body frame origin
        /// @param   force_parentcg__p      Implicit return of passthrough force applied to this body in the parent body frame
        /// @param   moment_parentcg__p     Implicit return of passthrough moment applied to this body 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);
 
        /// Getters for force and moment stuff
        CartesianVector<T, 3> externalForce() {return _ext_force__b;}
        CartesianVector<T, 3> externalMoment() {return _ext_moment__b;}
        CartesianVector<T, 3> passthroughForce() {return _passthrough_force__b;}
        CartesianVector<T, 3> passthroughMoment() {return _passthrough_moment__b;}
 
    protected:
        /// @brief   Function to recurse through children and calculate body composite inertia
        void _calcCompositeMassInertiaCM();
 
        /// @brief   Function to resolve forces and moments to body frame as applied and passed through
        void _calcAppliedForceMoment(const CartesianVector<T, 3> &force, const CartesianVector<T, 3> &moment);
 
        /// Applied external force and moment on the body
        CartesianVector<T, 3> _ext_force__b;
        CartesianVector<T, 3> _ext_moment__b;
 
        /// Passthrough force on the body -- force not applied here but passed on to parent body
        CartesianVector<T, 3> _passthrough_force__b;
        CartesianVector<T, 3> _passthrough_moment__b;
    };
 
    template <typename T>
    Body<T>::Body(const std::string &name, Frame<T>* par)
        : Frame<T>(name, par, true) {
 
        /// Set frame type to body
        GraphTreeObject::_graph_tree_type = BODY;
    }
 
    template <typename T>
    int Body<T>::calcFrameTreeExtForcesMoments() {
        /// Set our internal forces/moments to zero for fresh calculation
        _ext_force__b.setToZeros();
        _ext_moment__b.setToZeros();
        _passthrough_force__b.setToZeros();
        _passthrough_moment__b.setToZeros();
 
        /// This calculation is executed from the tree "leaves" in, so we recurse first and execute
        /// calculations on our way back out
        /// If we have children, get their stuff
        if(this->nChildren()) {
            /// Temporary variables to hold force and moment calculations from parents
            CartesianVector<T, 3> force_tmp;
            CartesianVector<T, 3> moment_tmp;
 
            for(unsigned int i = 0; i < this->nChildren(); i++) {
                /// Our pointer is technically stored as graph tree object -- cast to frame
                Frame<T>* ptr = (Frame<T>*)GraphTreeObject::_children[i];
                /// We ignore anything that is not a node or a body -- handle both appropriately
                if(ptr->type() == BODY) {
                    /// Recurse further through body
                    Body<T>* body_ptr = (Body<T>*)ptr;
                    body_ptr->calcFrameTreeExtForcesMoments();
 
                    /// Now that we've calculated (implicitly) our child body's forces and moments,
                    /// resolve the passthrough forces into our body
                    body_ptr->getFMPairAtParentOrigin(force_tmp, moment_tmp);
 
                    /// Calculate the applied and passed through elements of our force and moment
                    _calcAppliedForceMoment(force_tmp, moment_tmp);
                } else if(ptr->type() == NODE) {
                    /// Nodes are not allowed to have children, so we can safely pull our forces
                    /// and moments from them without issue. Do that here
                    Node<T>* node_ptr = (Node<T>*)ptr;
                    node_ptr->getFMPairAtParentOrigin(force_tmp, moment_tmp);
 
                    /// Calculate the applied and passed through elements of our force and moment
                    _calcAppliedForceMoment(force_tmp, moment_tmp);
                } else {
                    /// We're looking at a frame. Don't do anything, but recurse
                    /// further to see if its children do anything interesting
                    ptr->calcFrameTreeExtForcesMoments();
                }
            }
        }
 
        return NO_ERROR;
    }
 
    template <typename T>
    int Body<T>::calcFrameTreeExtAcceleration() {
        /// First calculate our composite CM recursively by calling our calculation function
        _calcCompositeMassInertiaCM();
 
        /// TODO: Implement recursive and offset composite CM/inertia here
 
        /// Now calculate our acceleration based on mass
        Frame<T>::_acc_f_p__p = Frame<T>::quat_f_p().toDCM().inverse()*_ext_force__b;
        multiply((T)1.0/composite_mass(), Frame<T>::_acc_f_p__p, Frame<T>::_acc_f_p__p);
 
        /// Now calculate our angular acceleration on the basis of our applied moment and omega
        CartesianVector<T, 3> w_f_root_f = Frame<T>::rootRelAngularVelocity();
        CartesianVector<T, 3> tmp = _ext_moment__b - tilde(w_f_root_f)*composite_inertia()*w_f_root_f;
        Matrix<T, 3, 3> ci_inverse;
        int err = composite_inertia().inverse(ci_inverse); if(err) {return err;}
        Frame<T>::_alpha_f_p__f = ci_inverse*tmp;
 
        return NO_ERROR;
    }
 
    template <typename T>
    void Body<T>::_calcCompositeMassInertiaCM() {
        /// TODO: Update for multiple body case
        composite_inertia(inertia());
        composite_mass(mass());
    }
 
    template <typename T>
    void Body<T>::_calcAppliedForceMoment(const CartesianVector<T, 3> &force, const CartesianVector<T, 3> &moment) {
        /// Calculate which elements of the force and moment are applied (affect body motion)
        /// and which are passed through (affect parent body motion, make this body into a node)
        eAdd(Frame<T>::_t_joint.freedom()*force, _ext_force__b, _ext_force__b);
        eAdd(Frame<T>::_t_joint.dependency()*force, _passthrough_force__b, _passthrough_force__b);
        eAdd(Frame<T>::_r_joint.freedom()*moment, _ext_moment__b, _ext_moment__b);
        eAdd(Frame<T>::_r_joint.dependency()*moment, _passthrough_moment__b, _passthrough_moment__b);
    }
 
    template <typename T>
    void Body<T>::getFMPairAtParentOrigin(CartesianVector<T, 3> &force_parentcg__p,
                                        CartesianVector<T, 3> &moment_parentcg__p) {
        // 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