TrafoMatrix.hpp

////*****************************************************************
//  TrafoMatrix for EEROS 
//
//  Author: Romano Hauser
//  Date: 18.07.2023
//  Mail: romano.hauser@ost.ch
//
//  Labeled TrafoMatrix based on EEROS Matrix 
//
////*****************************************************************

#pragma once

#include <string>
#include <vector>

#include <eeros/math/Matrix.hpp>
#include <eeros/logger/Logger.hpp>

namespace eeros { 
namespace tf {
    
    class TrafoMatrix : public eeros::math::Matrix<4,4,double> {
    public: 
        
    // Constructor 
    ////////////////////////////////////////////////////////////////////////////////////////
        
        TrafoMatrix(std::string _name, std::string _base= "global" , Matrix<4,4,double> _mat= { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}); 
        
    // setter & getter 
    ////////////////////////////////////////////////////////////////////////////////////////
        virtual void setTrans(eeros::math::Vector3 _pos);
        virtual Matrix<3,1,double> getTrans();
        
        virtual void setRot(Matrix<3,3,double> _rot);
        virtual Matrix<3,3,double> getRot();
        
        virtual Matrix<4,4,double> getMatrix(); 
        virtual void setMatrix( Matrix<4,4,double> mat ) ;

        virtual std::string getName(){ return name; }; 
        virtual std::string getBaseName() { return baseName; }; 
        
        
    // orientation 
    ////////////////////////////////////////////////////////////////////////////////////////
        virtual void calcRotFromRPY(); 
        virtual void calcRPYfromRot();
        
        virtual void setRPY(double _a, double _b, double _g); 
        virtual void setRPY(Matrix<3,1,double> _rpy); 
        
        virtual Matrix<3,1,double> getRPY(); 
        
    ////////////////////////////////////////////////////////////////////////////////   
    //// Jacobi Matrix for TrafoMatrix 
        virtual Matrix<6,6,double> getJacobiMatrix();

    /////////////////////////////////////////////////////////////////////////////////
    //// Jacobi Matrix for TrafoMatrix as static method for general use:   
        static Matrix<6,6,double> getJacobiMatrix(Matrix<4,4,double> _tf); 

    /////////////////////////////////////////////////////////////////////////////////
    //// inverse matrix 
        virtual Matrix<4,4,double> inv(); 
     
    private: 
  
    std::string name, baseName;