armadillo_twist_to_jog_frame.cpp

#include <armadillo>

using namespace arma {
    vec v_twist = vec6({twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z});

    // hasDuplicate
    vec tmp = unique(v_twist);
    if(size(tmp) != size(v_twist))
    {
        v_twist.zeros();
    }

    //Check if any value is non zero
    if(any(v_twist)){
        //publish
    }

    //Rotate axes
    vec linear = vec3({v_twist.at(0), v_twist.at(1), v_twist.at(2)});
    vec angular = vec3({v_twist.at(3), v_twist.at(4), v_twist.at(5)});
    linear = dot(rotation_matrix_,linear);
    angular = dot(rotation_matrix_,angular);
    
}