Joint.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.
******************************************************************************/
/*
Joint header file
 
Alex Reynolds
*/
#ifndef JOINT_HPP
#define JOINT_HPP
 
namespace clockwerk {
 
    enum joint_type_e {
        NOT_SET,
        MIXED,
        FULLY_FREE,
        FULLY_LOCKED
    };
 
    /// @brief  Joint class defining relationship between frames
    /// 
    /// The joint class defines the relationship between a frame and its parent for
    /// a given motion type (translational or rotational). It defines two matrices:
    /// the freedom matrix, which defines whether each axis is a degree of freedom,
    /// and the dependency matrix, which is the opposite of the freedom matrix
    /// defined as D = I - F.
    /// 
    /// For now, the joint class is assumed to be fully free or fully locked for simplicity.
    /// Updating to reflect configurable degrees of freedom is a TODO
    template <typename T>
    class Joint {
    public:
        /// @brief   Default constructor for the joint object
        /// @note    Assumes fully locked by default
        Joint();
 
        /// @brief   Constructor to set joint type
        /// @param   joint_type  The type of joint to use
        Joint(joint_type_e joint_type);
 
        /// @brief   Constructor to set free axes on the joint for the object
        /// @param   x_free  Bool indicating whether x should be a degree of freedom (true)
        /// @param   y_free  Bool indicating whether y should be a degree of freedom (true)
        /// @param   z_free  Bool indicating whether z should be a degree of freedom (true)
        Joint(bool x_free, bool y_free, bool z_free);
 
        /// @brief   Function to set the axes free on this joint
        /// @param   x_free  Bool indicating whether x should be a degree of freedom (true)
        /// @param   y_free  Bool indicating whether y should be a degree of freedom (true)
        /// @param   z_free  Bool indicating whether z should be a degree of freedom (true)
        void setFreeAxes(bool x_free, bool y_free, bool z_free);
 
        /// @brief   Function to set the joint type
        /// @param   joint_type  The type of joint to use
        void setJointType(joint_type_e joint_type);
 
        /// Functions to return freedom or dependency matrix
        const clockwerk::Matrix<T, 3, 3>& freedom() {return _freedom;}
        const clockwerk::Matrix<T, 3, 3>& dependency() {return _dependency;}
 
        /// Getter/setter for joint type
        clockwerk::joint_type_e jointType() {return _joint_type;}
        void jointType(joint_type_e joint_type) {_joint_type = joint_type;}
    protected:
        joint_type_e _joint_type;
 
        /// Freedom and dependency matrix 
        Matrix<T, 3, 3> _freedom;
        Matrix<T, 3, 3> _dependency;
    };
 
    template <typename T>
    Joint<T>::Joint()
        : _joint_type(FULLY_LOCKED),
        _freedom({{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}}),
        _dependency({{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}) {}
 
    template <typename T>
    Joint<T>::Joint(joint_type_e joint_type) {
        setJointType(joint_type);
    }
 
    template <typename T>
    Joint<T>::Joint(bool x_free, bool y_free, bool z_free)
        : _freedom({{(T)x_free, 0.0, 0.0}, {0.0, (T)y_free, 0.0}, {0.0, 0.0, (T)z_free}}),
        _dependency({{(T)!x_free, 0.0, 0.0}, {0.0, (T)!y_free, 0.0}, {0.0, 0.0, (T)!z_free}}) {
        /// Set our joint type based on the values passed in
        if(x_free && y_free && z_free) {
            _joint_type = FULLY_FREE;
        } else if(!x_free && !y_free && !z_free) {
            _joint_type = FULLY_LOCKED;
        } else {
            _joint_type = MIXED;
        }
    }
 
    template <typename T>
    void Joint<T>::setFreeAxes(bool x_free, bool y_free, bool z_free) {
        _freedom.set(0, 0, (T)x_free);
        _freedom.set(1, 1, (T)y_free);
        _freedom.set(2, 2, (T)z_free);
        _dependency.set(0, 0, (T)!x_free);
        _dependency.set(1, 1, (T)!y_free);
        _dependency.set(2, 2, (T)!z_free);
 
        /// Set our joint type based on the values passed in
        if(x_free && y_free && z_free) {
            _joint_type = FULLY_FREE;
        } else if(!x_free && !y_free && !z_free) {
            _joint_type = FULLY_LOCKED;
        } else {
            _joint_type = MIXED;
        }
    }
 
    template <typename T>
    void Joint<T>::setJointType(joint_type_e joint_type) {
        switch(joint_type) {
            case FULLY_FREE:
                _freedom.set(0, 0, 1.0);
                _freedom.set(1, 1, 1.0);
                _freedom.set(2, 2, 1.0);
                _dependency.set(0, 0, 0.0);
                _dependency.set(1, 1, 0.0);
                _dependency.set(2, 2, 0.0);
                _joint_type = FULLY_FREE;
                break;
            case FULLY_LOCKED:
                _freedom.set(0, 0, 0.0);
                _freedom.set(1, 1, 0.0);
                _freedom.set(2, 2, 0.0);
                _dependency.set(0, 0, 1.0);
                _dependency.set(1, 1, 1.0);
                _dependency.set(2, 2, 1.0);
                _joint_type = FULLY_LOCKED;
                break;
            default:
                /// Note: this includes mixed because we do not have
                /// enough info to set joint
                _joint_type = NOT_SET;
                break;
        };
    }
 
}
 
#endif