Frame.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.
******************************************************************************/
/*
Frame header file
 
Author: Alex Reynolds
*/
#ifndef FRAME_HPP
#define FRAME_HPP
 
#include "core/clockwerkerrors.h"
#include "core/vectormath.hpp"
#include "data_management/GraphTreeObject.h"
#include "data_management/DataIO.hpp"
#include "Joint.hpp"
#include "DCM.hpp"
#include "Quaternion.hpp"
#include "MRP.hpp"
#include "Euler321.hpp"
#include "attitudemath.hpp"
 
#define NUM_INTEGRATED_STATES 13
 
namespace clockwerk {
 
    /// Forward declaration of frame types
    template <typename T> class Body;
    template <typename T> class Node;
 
    /// @brief  Frame class definition
    /// 
    /// This file defines the frame class, which is the base of all 6-DOF
    /// kinematics and dynamics. This frame class is designed for recursive
    /// chaining, and represents the relationship of a frame to its parent.
    ///
    /// The frame class tracks the frame's relationship to its parent via the 
    /// following four key parameters:
    /// pos_f_p__p - The position of the child frame relative to its parent, in PARENT coords
    /// vel_f_p__p - The velocity of the child frame IN THE PARENT FRAME, in PARENT coords.
    /// quat_f_p - The attitude quaternion of the child frame relative to its parent
    /// ang_vel_f_p__f - The angular velocity of the child frame relative to its parent, in FRAME coords
    /// 
    /// Frame relationships to their parents are also defined by the Joint class, 
    /// which is a method for tracking the degrees of freedom (and thus, which
    /// channels can have a nonzero velocity value). Each frame has a translational
    /// and rotational joint, which sets degrees of freedom in the velocity (represented
    /// in parent coordinates) and angular velocity (represented in frame coordinates).
    /// The frame joint is set to fully fixed by default, but can be set to free or 
    /// modified later.
    ///
    /// Frame states may be set by calling the parent relative states directly (they are
    /// DataIO objects, but buyer beware that you can set the frame states with almost
    /// no restrictions.)
    ///
    /// Alternatively, users may get and set frame states relative to the root frame
    /// by calling the root relative functions which are provided by this class. The 
    /// functions rootRelPosition, rootRelVelocity, rootRel... and so on all return
    /// the frame state relative to root. The functions setRootRel... will set the 
    /// frame's state relative to the root frame. For many applications it is better
    /// to work with the root functions, rather than parent relative functions.
    ///
    /// Naming conventions
    /// ------------------
    /// - The naming convention for all vectors is:
    ///   variablename_obj1_obj2__frameN       (note the double underscore preceding frame)
    ///   and reads back as:
    ///   "variable variablename representing the relationship between obj1 and obj2
    ///   represented in frameN"
    /// - The naming convention for attitude representations is the same as the above,
    ///   with the trailing frame representation omitted.
    /// - The omega vector is sometimes denoted by w and assumed frame consistent with 
    ///   the attitude representation. For example, if a DCM represents the 
    ///   relative orientation between two frames [BN] the omega vector is 
    ///   assumed to be w_(B/N)
    /// - The frame represented by this class is denoted with "f" for simplicity
    /// - Unless otherwise noted angles and angular rates are in RADIANS
    /// - Where relevant and unless otherwise noted, all units are metric
    template <typename T=double>
    class Frame : public GraphTreeObject {
    public:
        /// @brief  Constructor for the frame object
        /// @param  name   Name of the frame
        /// @param  par    Parent of the frame. Defaults to nullptr
        /// @param  free   Boolean indicating whether frame should be free or fixed.
        Frame(const std::string &name, Frame<T>* par=nullptr, bool free = false);
 
        /// @brief   Function to get reference to the frame's translational joint
        /// @return  The frame's translational joint
        Joint<T>& tJoint() {return _t_joint;}
 
        /// @brief   Function to get reference to the frame's rotational joint
        /// @return  The frame's rotational joint
        Joint<T>& rJoint() {return _r_joint;}
 
        /// @brief   Function to assign the frame's parent via pointer
        /// @param   new_parent  The new parent to assign the node to
        /// @return  Error code corresponding to success/failure
        /// @note    This function is the only method to ADD or REMOVE a node
        ///          from a tree. To add, pass the address to a GraphTreeObject
        ///          to this function. To remove, pass a nullptr. Note that
        ///          parents for objects on an existing tree can be reassigned
        int parent(Frame<T>* new_parent);
        Frame<T>* parent() {return _parent;}
 
        /** Position of the frame with respect to parent, in parent coordinates */
        clockwerk::DataIO<CartesianVector<T, 3>> pos_f_p__p =
            clockwerk::DataIO<CartesianVector<T, 3>>(nullptr, "pos_f_p__p", CartesianVector<T, 3>());
        /** Velocity of the frame with respect to parent, in parent coordinates.
         * Note that this is the velocity IN the parent frame, which takes into account rotation. */
        clockwerk::DataIO<CartesianVector<T, 3>> vel_f_p__p =
            clockwerk::DataIO<CartesianVector<T, 3>>(nullptr, "vel_f_p__p", CartesianVector<T, 3>());
        /** Attitude quaternion of the frame with respect to parent */
        clockwerk::DataIO<Quaternion<T>> quat_f_p =
            clockwerk::DataIO<Quaternion<T>>(nullptr, "quat_f_p", Quaternion<T>());
        /** Angular velocity of the frame with respect to parent, in frame coordinates */
        clockwerk::DataIO<CartesianVector<T, 3>> ang_vel_f_p__f =
            clockwerk::DataIO<CartesianVector<T, 3>>(nullptr, "ang_vel_f_p__f", CartesianVector<T, 3>());
 
        /* Nice handle to the frame itself for mapping*/
        clockwerk::DataIO<Frame<T>*> self_id = clockwerk::DataIO<Frame<T>*>(nullptr, "self_id", this);
 
        /// @brief Function to set parent relative acceleration of frame
        /// @param accel acceleration of frame relative to parent in parent frame coords
        void setParentRelAcceleration(CartesianVector<T, 3> accel) {_acc_f_p__p = _t_joint.freedom()*accel;}
 
        /// @brief Function to set parent relative angular acceleration
        /// @param alpha angular accel of the frame relative to parent in parent frame coords
        void setParentRelAngularAcceleration(CartesianVector<T, 3> alpha) {_alpha_f_p__f = _r_joint.freedom()*alpha;}
 
        /// @brief Function to get the acceleration of the frame relative to parent expressed in parent coords
        /// @return Accel relative to parent in parent coords
        CartesianVector<T, 3> parentRelAcceleration() {return _t_joint.freedom()*_acc_f_p__p;}
 
        /// @brief Function to get angular acceleration relative to parent in frame coords
        /// @return Angular accel relative to parent in frame coords 
        CartesianVector<T, 3> parentRelAngularAcceleration() {return _r_joint.freedom()*_alpha_f_p__f;}
 
        /// @brief   Returns the position of the frame origin wrt the root origin, in root coordinates
        /// @param   pos_o_root_root    Implicit return of position of origin relative to root, in root
        /// @return  Error code corresponding to success/failure
        /// @note    Overloaded for explicit and implicit return
        void rootRelPosition(CartesianVector<T, 3> &pos_o_root__root);
        CartesianVector<T, 3> rootRelPosition();
 
        /// @brief   Returns the velocity of the frame origin wrt the root origin, in root coordinates
        /// @param   vel_o_root_root    Implicit return of velocity of origin relative to root in root
        /// @return  Error code corresponding to success/failure
        /// @note    Overloaded for explicit and implicit return
        void rootRelVelocity(CartesianVector<T, 3> &vel_o_root__root);
        CartesianVector<T, 3> rootRelVelocity();
 
        /// @brief   Returns the acceleration of the frame origin wrt the root origin, in root coordinates
        /// @param   acc_o_root_root    Implicit return of acceleration of origin relative to root, in root
        /// @return  Error code corresponding to success/failure
        /// @note    Overloaded for explicit and implicit return
        void rootRelAcceleration(CartesianVector<T, 3> &acc_o_root__root);
        CartesianVector<T, 3> rootRelAcceleration();
 
        /// @brief   Returns the quaternion representing the frame origin wrt the root frame
        /// @param   att_f_root  Implicit return of attitude relative to root
        /// @return  Error code corresponding to success/failure
        /// @note    Overloaded for explicit and implicit return
        void rootRelQuaternion(Quaternion<T> &quat_f_root);
        Quaternion<T> rootRelQuaternion();
 
        /// @brief   Returns the DCM of the frame origin wrt the root frame
        /// @param   att_f_root  Implicit return of attitude relative to root
        /// @return  Error code corresponding to success/failure
        /// @note    Overloaded for explicit and implicit return
        void rootRelDCM(DCM<T> &dcm_f_root);
        DCM<T> rootRelDCM();
 
        /// @brief   Returns the angular velocity of the frame origin wrt the root, expressed
        ///          in this frame's coordinates 
        /// @param   w_f_root__f    Implicit return of angular velocity relative to root, 
        ///                         expressed in f frame
        /// @return  Error code corresponding to success/failure
        /// @note    Overloaded for explicit and implicit return
        void rootRelAngularVelocity(CartesianVector<T, 3> &w_f_root__f);
        CartesianVector<T, 3> rootRelAngularVelocity();
 
        /// @brief   Returns the angular acceleration of the frame origin wrt the root, expressed
        ///          in this frame's coordinates 
        /// @param   alpha_f_root__f    Implicit return of angular acceleration relative to root, 
        ///                             expressed in f frame
        /// @return  Error code corresponding to success/failure
        /// @note    Overloaded for explicit and implicit return
        void rootRelAngularAcceleration(CartesianVector<T, 3> &alpha_f_root__f);
        CartesianVector<T, 3> rootRelAngularAcceleration();
 
        /// @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();
 
        /// Getter and setter for integrator pointer
        void integrator(void* integ_ptr) {_integrator_ptr = integ_ptr;}
        void* integrator() {return _integrator_ptr;}
 
        /// @brief   Function to set frame state based on NUM_INTEGRATED_STATES-element 
        ///          state vector
        /// @param   state   NUM_INTEGRATED_STATES-element state vector as [pos][vel][quat][omega]
        void setStateFromStateVector(const std::array<T, NUM_INTEGRATED_STATES> &state_f_p__f);
 
        /// @brief   Function to return frame state as a NUM_INTEGRATED_STATES-element state vector
        /// @param   state   Return of NUM_INTEGRATED_STATES-element state vector as [pos][vel][quat][omega]
        void getStateAsStateVector(std::array<T, NUM_INTEGRATED_STATES> &state_f_p__f);
 
        /// @brief Function to get the rate of change in the frame state vector
        /// @param state_dot Rate of change in each element of the state vector
        void getStateVectorDot(std::array<T, NUM_INTEGRATED_STATES> &state_dot);
 
        /// @brief   Function to set the frame's position relative to root
        /// @param   pos_f_root__root    Position of this frame wrt root frame origin,
        ///                              as specified in the root frame
        /// @note    If parent is not yet assigned, will throw error
        /// @note    With great power comes great responsibility. This thing will set
        ///          position to whatever you want, minimal questions asked, independent
        ///          of if you later change attitude, etc.
        int setRootRelPosition(const CartesianVector<T, 3> &pos_f_root__root);
 
        /// @brief   Function to set the frame's velocity relative to root
        /// @param   vel_f_root__root    Velocity of this frame wrt root frame origin,
        ///                              as specified in the root frame
        /// @note    If parent is not yet assigned, will throw error
        /// @note    With great power comes great responsibility. This thing will set
        ///          velocity to whatever you want, minimal questions asked, independent
        ///          of if you later change attitude, etc.
        int setRootRelVelocity(const CartesianVector<T, 3> &vel_f_root__root);
 
        /// @brief   Function to set the frame's attitude relative to root
        /// @param   quat_f_root         Attitude of this frame wrt root frame
        /// @note    If parent is not yet assigned, will throw error
        /// @note    With great power comes great responsibility. This thing will set
        ///          attitude to whatever you want, minimal questions asked, independent
        ///          of if you later change attitude, etc.
        int setRootRelAttitude(const Quaternion<T> &quat_f_root);
 
        /// @brief   Function to set the frame's position relative to root
        /// @param   w_f_root__f         Angular velocity of this frame wrt root frame origin,
        ///                              as specified in this frame
        /// @note    If parent is not yet assigned, will throw error
        /// @note    With great power comes great responsibility. This thing will set
        ///          ang vel to whatever you want, minimal questions asked, independent
        ///          of if you later change attitude, etc.
        int setRootRelAngularVelocity(const CartesianVector<T, 3> &w_f_root__f);
 
        /// @brief   Function to get a pointer to this frame's root frame
        /// @return  Pointer to root frame. Pointer to self if this frame is root
        Frame<T>* getFrameRootPointer();
    protected:
        /// Internal variables to store accelerations applied to this frame. Note: these
        /// are not states. They are temporary variables and treated slightly differently.
        CartesianVector<T, 3> _acc_f_p__p;
        CartesianVector<T, 3> _alpha_f_p__f;
 
        /// Translational and rotational joints relating this frame to its
        /// parent. Set locked by default.
        Joint<T> _t_joint;  /// Translational
        Joint<T> _r_joint;  /// Rotational
 
        /// Pointer to the integrator that is integrating this frame.
        /// Nullptr if unused
        void* _integrator_ptr = nullptr;
 
        /// Variable to hold our parent
        Frame<T>* _parent;
    };
 
    template<typename T>
    Frame<T>::Frame(const std::string &name, Frame<T>* par, bool free)
        : GraphTreeObject(name),
        _t_joint(free,free,free),
        _r_joint(free,free,free) {
 
        GraphTreeObject::_graph_tree_type = FRAME;
 
        _parent = nullptr;
        if(par != nullptr) {
            parent(par);
        }
    }
 
    template <typename T>
    int Frame<T>::parent(Frame<T>* new_parent) {
        /// First, check if our current parent is a nullptr. If not, we need to remove
        /// this node from it first. Temporarily assign our parent to nullptr once we've
        /// done this to prevent a possible broken operation where parent is removed
        /// but can't be reassigned
 
        /// Ensure we don't have a circular reference
        GraphTreeObject* ptr = new_parent;
        while(ptr != nullptr) {
            /// Ensure that we don't have a circular reference
            if(ptr == this) {
                return ERROR_CIRCULAR_REFERENCE;
            }
 
            /// Point further up the tree
            ptr = ptr->parent();
        }
 
        /// Type check our frame and the desired parent to ensure
        /// our setup is valid
        if(new_parent != nullptr) {
            switch(_graph_tree_type) {
                case FRAME:
                    /// Framess can be the children of anything. We're good here
                    break;
                case BODY:
                    /// Bodies can be the children of frames and other bodies,
                    /// but not of nodes. Type check and verify our parent
                    /// is not a node
                    if(new_parent->type() == NODE) {
                        return ERROR_FRAME_PARENT_TYPE;
                    }
                    break;
                case NODE:
                    /// Nodes can be the children of bodies only. If our parent
                    /// is a frame or another node, raise an error
                    if(new_parent->type() == FRAME || new_parent->type() == NODE) {
                        return ERROR_FRAME_PARENT_TYPE;
                    }
                    break;
                default:
                    /// This should be impossible, but return an error
                    return ERROR_FRAME_PARENT_TYPE;
                    break;
            }
        }
 
        /// Remove child from our parent if we're resetting our
        /// parent
        int err;
        if(_parent != nullptr) {
            err = _parent->removeChild(this);
            if(err) {return err;}
            _parent = nullptr;
        }
 
        /// Now add our node to its new parent, so long as our new parent is not a nullptr
        if(new_parent != nullptr) {
            err = new_parent->addChild(this);
            if(err) {return err;}
        }
 
        /// Otherwise, we're good to go to add this child to parent -- go ahead 
        _parent = new_parent;
 
        /// Now update frame stuff for nullpointer parent, which means we're root
        if(_parent == nullptr) {
            _t_joint.setFreeAxes(false, false, false);
            _r_joint.setFreeAxes(false, false, false);
            pos_f_p__p(CartesianVector<T, 3>());
            vel_f_p__p(CartesianVector<T, 3>());
            _acc_f_p__p = CartesianVector<T, 3>({0.0, 0.0, 0.0});
            quat_f_p(Quaternion<T>());
            ang_vel_f_p__f(CartesianVector<T, 3>());
            _alpha_f_p__f = CartesianVector<T, 3>({0.0, 0.0, 0.0});
        }
 
        /// Finally, recalculate the rank of this object and its children
        recalculateRank();
 
        return NO_ERROR;
    }
 
    template <typename T>
    Frame<T>* Frame<T>::getFrameRootPointer() {
        if(_parent == nullptr) {
            return this;
        } else {
            return _parent->getFrameRootPointer();
        }
    }
 
    template <typename T>
    void Frame<T>::getStateVectorDot(std::array<T, NUM_INTEGRATED_STATES> &state_dot) {
        // First evaluate our rotational degrees of freedom and integrate
        if(_r_joint.jointType() == FULLY_LOCKED) {
            // If our joint is locked all state dot values should be zero
            for(unsigned int i = 6; i < NUM_INTEGRATED_STATES; i++) {
                state_dot[i] = 0.0;
            }
        } else {
            // Set our quaternion rate of change from 
            Matrix<T, 4, 1> qdot_f_p;
            quat_f_p().rate(_r_joint.freedom()*ang_vel_f_p__f(), qdot_f_p);
            state_dot[6] = qdot_f_p[0][0];
            state_dot[7] = qdot_f_p[1][0];
            state_dot[8] = qdot_f_p[2][0];
            state_dot[9] = qdot_f_p[3][0];
 
            // Now set our omega rate of change from alpha and our parent
            CartesianVector<T, 3> omega_dot = _alpha_f_p__f;
            if(_parent != nullptr) {
                // If our parent is not nothing, subtract parent's angular acceleration if
                // we're free to account for case where parent accelerates and we do not.
                omega_dot = omega_dot - _r_joint.freedom()*(quat_f_p()*_parent->rootRelAngularAcceleration());
            }
            state_dot[10] = omega_dot[0];
            state_dot[11] = omega_dot[1];
            state_dot[12] = omega_dot[2];
        }
 
        // Evaluate our translational degrees of freedom and integrate our velocity/accel
        if(_t_joint.jointType() == FULLY_LOCKED) {
            // If our joint is locked all state dot values should be zero
            for(unsigned int i = 0; i < 6; i++) {
                state_dot[i] = 0.0;
            }
        } else {
            // Our rate of change in position is just an integration of our velocity
            vel_f_p__p().get(0, state_dot[0]);
            vel_f_p__p().get(1, state_dot[1]);
            vel_f_p__p().get(2, state_dot[2]);
 
            // Calculate rate of change in velo from in-frame acceleration
            CartesianVector<T, 3> v_dot = _acc_f_p__p;
 
            // Now account for cases where our frame is free to its parent and the 
            // parent moves "underneath" it. This includes two cases:
            // 1. Translational acceleration experienced by the parent 
            // 2. Acceleration due to parent's rotation (because we're tracking state IN parent frame)
            if(_parent != nullptr) {
                // Get our dcm parent/root
                DCM<T> dcm_p_root = _parent->rootRelDCM();
 
                // Account for case 1 - translational accelearation of parent frame origin
                v_dot = v_dot - dcm_p_root*_parent->rootRelAcceleration();
 
                // Account for case 2 - acceleration due to rotating parent frame
                // Angular acceleration
                v_dot = v_dot - cross(_parent->rootRelAngularAcceleration(), pos_f_p__p());
 
                // Coriolis acceleration
                CartesianVector<T, 3> parent_omega = _parent->rootRelAngularVelocity();
                v_dot = v_dot - 2.0*cross(parent_omega, vel_f_p__p());
 
                // Centripetal acceleration
                v_dot = v_dot - cross(parent_omega, cross(parent_omega, pos_f_p__p()));
            }
            v_dot = _t_joint.freedom()*v_dot;
            v_dot.get(0, state_dot[3]);
            v_dot.get(1, state_dot[4]);
            v_dot.get(2, state_dot[5]);
        }
    }
 
    template <typename T>
    void Frame<T>::setStateFromStateVector(const std::array<T, NUM_INTEGRATED_STATES> &state) {
        pos_f_p__p(CartesianVector<T, 3>({state[0], state[1], state[2]}));
        vel_f_p__p(_t_joint.freedom()*CartesianVector<T, 3>({state[3], state[4], state[5]}));
        Quaternion<T> tmp({state[6], state[7], state[8], state[9]});
        tmp.unitize();
        quat_f_p(tmp);
        ang_vel_f_p__f(_r_joint.freedom()*CartesianVector<T, 3>({state[10], state[11], state[12]}));
    }
 
    template <typename T>
    void Frame<T>::getStateAsStateVector(std::array<T, NUM_INTEGRATED_STATES> &state) {
        /// Position
        pos_f_p__p().get(0, state[0]);
        pos_f_p__p().get(1, state[1]);
        pos_f_p__p().get(2, state[2]);
 
        /// Velocity
        vel_f_p__p().get(0, state[3]);
        vel_f_p__p().get(1, state[4]);
        vel_f_p__p().get(2, state[5]);
 
        /// Attitude - includes normalization
        quat_f_p().get(0, state[6]);
        quat_f_p().get(1, state[7]);
        quat_f_p().get(2, state[8]);
        quat_f_p().get(3, state[9]);
 
        /// Angular velocity
        ang_vel_f_p__f().get(0, state[10]);
        ang_vel_f_p__f().get(1, state[11]);
        ang_vel_f_p__f().get(2, state[12]);
    }
 
    template <typename T>
    void Frame<T>::rootRelPosition(CartesianVector<T, 3> &pos_f_root__root) {
        /// First, evaluate if the current frame is root and continue to recurse up the chain if not
        if(_parent == nullptr) {
            /// We're the root frame! Set our position to zero and return
            pos_f_root__root.set(0, 0.0);
            pos_f_root__root.set(1, 0.0);
            pos_f_root__root.set(2, 0.0);
        } else {
            /// Recursively run up our tree until we find root
            _parent->rootRelPosition(pos_f_root__root);
 
            // We're on the return now. Take our contribution and add this frame's
            DCM<T> dcm_p_root = _parent->rootRelDCM();                                  // Get our dcm parent/root
            pos_f_root__root = pos_f_root__root + dcm_p_root.inverse()*pos_f_p__p();    // Add our position in root to total
        }
    }
    template <typename T>
    CartesianVector<T, 3> Frame<T>::rootRelPosition() {
        CartesianVector<T, 3> tmp;
        rootRelPosition(tmp);
        return tmp;
    }
 
    template <typename T>
    void Frame<T>::rootRelVelocity(CartesianVector<T, 3> &vel_f_root__root) {
        /// First, evaluate if the current frame is root and continue to recurse up the chain if not
        if(_parent == nullptr) {
            /// We're the root frame! Set our velocity to zero and return
            vel_f_root__root.set(0, 0.0);
            vel_f_root__root.set(1, 0.0);
            vel_f_root__root.set(2, 0.0);
        } else {
            // Recurse further up the tree until we find root
            _parent->rootRelVelocity(vel_f_root__root);
 
            // Now we're on the return. Calculate contribution from this frame in parent coords
            CartesianVector<T, 3> frame_contrib__p = cross(_parent->rootRelAngularVelocity(), pos_f_p__p());
            if(_t_joint.jointType() != FULLY_LOCKED) {
                eAdd(frame_contrib__p, _t_joint.freedom()*vel_f_p__p(), frame_contrib__p);
            }
 
            // And add contribution to the total
            DCM<T> dcm_p_root = _parent->rootRelDCM();
            vel_f_root__root = vel_f_root__root + dcm_p_root.inverse()*frame_contrib__p;
        }
    }
    template <typename T>
    CartesianVector<T, 3> Frame<T>::rootRelVelocity() {
        CartesianVector<T, 3> tmp;
        rootRelVelocity(tmp);
        return tmp;
    }
 
    template <typename T>
    void Frame<T>::rootRelAcceleration(CartesianVector<T, 3> &acc_f_root__root) {
        /// First, evaluate if the current frame is root and continue to recurse up the chain if not
        if(_parent == nullptr) {
            /// We're the root frame! Set our acceleration to zero and return
            acc_f_root__root.set(0, 0.0);
            acc_f_root__root.set(1, 0.0);
            acc_f_root__root.set(2, 0.0);
        } else {
            // Recurse further up the tree until we find root
            _parent->rootRelAcceleration(acc_f_root__root);
 
            // Now we're on the return. Calculate contribution from this frame in parent coords
            // Contribution due to angular acceleration term alpha cross r
            CartesianVector<T, 3> frame_contrib__p = cross(_parent->rootRelAngularAcceleration(), pos_f_p__p());
 
            // Contribution due to coriolis acceleration term 
            frame_contrib__p = frame_contrib__p + 2.0*cross(_parent->rootRelAngularVelocity(), _t_joint.freedom()*vel_f_p__p());
 
            // Contribution due to centripetal acceleration
            frame_contrib__p = frame_contrib__p
                + cross(_parent->rootRelAngularVelocity(), cross(_parent->rootRelAngularVelocity(), pos_f_p__p()));
 
            // Contribution due to in-frame acceleration
            if(_t_joint.jointType() != FULLY_LOCKED) {
                frame_contrib__p = frame_contrib__p + _t_joint.freedom()*_acc_f_p__p;
            }
 
            // And add contribution to the total
            DCM<T> dcm_p_root = _parent->rootRelDCM();
            acc_f_root__root = acc_f_root__root + dcm_p_root.inverse()*frame_contrib__p;
        }
    }
    template <typename T>
    CartesianVector<T, 3> Frame<T>::rootRelAcceleration() {
        CartesianVector<T, 3> tmp;
        rootRelAcceleration(tmp);
        return tmp;
    }
 
    template <typename T>
    void Frame<T>::rootRelAngularVelocity(CartesianVector<T, 3> &w_f_root__f) {
        /// First, evaluate if the current frame is root and continue to recurse up the chain if not
        if(_parent == nullptr) {
            /// We're the root frame! Set our angular velocity to zero and return
            w_f_root__f.set(0, 0.0);
            w_f_root__f.set(1, 0.0);
            w_f_root__f.set(2, 0.0);
        } else {
            // Recurse further up the tree until we find root
            _parent->rootRelAngularVelocity(w_f_root__f);
 
            /// Now, we're returning and our parent frame's result has been calculated. Add this
            /// frame's contribution (in this frame) and return
            /// First get the previous position calculation in this frame's coordinates
            w_f_root__f = quat_f_p()*w_f_root__f + _r_joint.freedom()*ang_vel_f_p__f();
        }
    }
    template <typename T>
    CartesianVector<T, 3> Frame<T>::rootRelAngularVelocity() {
        CartesianVector<T, 3> tmp;
        rootRelAngularVelocity(tmp);
        return tmp;
    }
 
    template <typename T>
    void Frame<T>::rootRelAngularAcceleration(CartesianVector<T, 3> &alpha_f_root__f) {
        /// First, evaluate if the current frame is root and continue to recurse up the chain if not
        if(_parent == nullptr) {
            /// We're the root frame! Set our angular velocity to zero and return
            alpha_f_root__f.set(0, 0.0);
            alpha_f_root__f.set(1, 0.0);
            alpha_f_root__f.set(2, 0.0);
        } else {
            // Recurse further up the tree until we find root
            _parent->rootRelAngularAcceleration(alpha_f_root__f);
 
            /// Now, we're returning and our parent frame's result has been calculated. Add this
            /// frame's contribution (in this frame) and return
            alpha_f_root__f = quat_f_p()*alpha_f_root__f + _r_joint.freedom()*_alpha_f_p__f;
        }
    }
    template <typename T>
    CartesianVector<T, 3> Frame<T>::rootRelAngularAcceleration() {
        CartesianVector<T, 3> tmp;
        rootRelAngularAcceleration(tmp);
        return tmp;
    }
 
    template <typename T>
    void Frame<T>::rootRelQuaternion(Quaternion<T> &quat_f_root) {
        /// Really this function is just a wrapper around our DCM getter... so do that
        rootRelDCM().toQuaternion(quat_f_root);
    }
    template <typename T>
    Quaternion<T> Frame<T>::rootRelQuaternion() {
        Quaternion<T> tmp;
        rootRelQuaternion(tmp);
        return tmp;
    }
 
    template <typename T>
    void Frame<T>::rootRelDCM(DCM<T> &dcm_f_root) {
        /// First, evaluate if the current frame is root and continue to recurse up the chain if not
        if(_parent == nullptr) {
            /// We're the root frame! Set our angular velocity to zero and return
            dcm_f_root = DCM<T>();
        } else {
            // Recurse further up the tree until we find root
            _parent->rootRelDCM(dcm_f_root);
 
            /// Now, we're returning. Add our frame's contribution to the total
            dcm_f_root = quat_f_p().toDCM()*dcm_f_root;
        }
    }
    template <typename T>
    DCM<T> Frame<T>::rootRelDCM() {
        DCM<T> tmp;
        rootRelDCM(tmp);
        return tmp;
    }
 
    template <typename T>
    int Frame<T>::calcFrameTreeExtForcesMoments() {
        /// 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()) {
            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, but now through body rather than frame
                    Body<T>* body_ptr = (Body<T>*)ptr;
                    body_ptr->calcFrameTreeExtForcesMoments();
                } else {
                    /// We're looking at a frame. Don't do anything, but recurse
                    /// further to see if its children do anything interesting
                    ptr->calcFrameTreeExtForcesMoments();
                }
                /// Note: we should never have a node as the child of a frame,
                /// but just in case the worst case scenario here is we'll try
                /// to recurse into it and treat it as a frame
            }
        }
 
        return NO_ERROR;
    }
 
    template <typename T>
    int Frame<T>::calcFrameTreeExtAcceleration() {
        /// Do nothing because frames only apply their internal kinematics.
        /// They do not have external forces/moments applied by definition
 
        /// Recurse through children
        if(this->nChildren()) {
            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, but now through body rather than frame
                    Body<T>* body_ptr = (Body<T>*)ptr;
                    body_ptr->calcFrameTreeExtAcceleration();
                } else {
                    /// We're looking at a frame. Don't do anything, but recurse
                    /// further to see if its children do anything interesting
                    ptr->calcFrameTreeExtAcceleration();
                }
                /// Note: we should never have a node as the child of a frame,
                /// but just in case the worst case scenario here is we'll try
                /// to recurse into it and treat it as a frame
            }
        }
 
        return NO_ERROR;
    }
 
    template <typename T>
    int Frame<T>::setRootRelPosition(const CartesianVector<T, 3> &pos_f_root__root) {
        /// First let's make sure we've got a valid parent and the frame isn't locked
        if(_parent == nullptr) {
            return ERROR_PARENT_NOT_FRAME;
        }
 
        // Now subtract parent position from desired and set value accordingly
        DCM<T> dcm_p_root = _parent->rootRelDCM();
        pos_f_p__p(dcm_p_root*(pos_f_root__root - _parent->rootRelPosition()));
 
        return NO_ERROR;
    }
 
    template <typename T>
    int Frame<T>::setRootRelVelocity(const CartesianVector<T, 3> &vel_f_root__root) {
        /// First let's make sure we've got a valid parent and the frame isn't locked
        if(_parent == nullptr) {
            return ERROR_PARENT_NOT_FRAME;
        }
 
        // Calculate what fixed frame velocity would be at current point
        DCM<T> dcm_p_root = _parent->rootRelDCM();
        CartesianVector<T, 3> ff_vel__p = dcm_p_root*_parent->rootRelVelocity();
        ff_vel__p = ff_vel__p + cross(_parent->rootRelAngularVelocity(), pos_f_p__p());
 
        // And set parent relative velocity accounting for that
        vel_f_p__p(_t_joint.freedom()*dcm_p_root*vel_f_root__root - ff_vel__p);
 
        return NO_ERROR;
    }
 
    template <typename T>
    int Frame<T>::setRootRelAttitude(const Quaternion<T> &quat_f_root) {
        /// First let's make sure we've got a valid parent and the frame isn't locked
        if(_parent == nullptr) {
            return ERROR_PARENT_NOT_FRAME;
        }
 
        // Now subtract parent position from desired and set value accordingly
        DCM<T> dcm_p_root = _parent->rootRelDCM();
        DCM<T> dcm_f_p = quat_f_root.toDCM()*dcm_p_root.inverse();
        quat_f_p(dcm_f_p.toQuaternion());
 
        return NO_ERROR;
    }
 
    template <typename T>
    int Frame<T>::setRootRelAngularVelocity(const CartesianVector<T, 3> &w_f_root__f) {
        /// First let's make sure we've got a valid parent and the frame isn't locked
        if(_parent == nullptr) {
            return ERROR_PARENT_NOT_FRAME;
        }
 
        // Now subtract parent angular velocity from desired and set value accordingly
        ang_vel_f_p__f(_r_joint.freedom()*(w_f_root__f - quat_f_p()*_parent->rootRelAngularVelocity()));
 
        return NO_ERROR;
    }
 
}
 
#endif