Utility.hpp

Go to the documentation of this file.

 
#ifndef FLEXIVRDK_UTILITY_HPP_
#define FLEXIVRDK_UTILITY_HPP_
 
#include <flexiv/Exception.hpp>
#include <Eigen/Eigen>
#include <array>
#include <vector>
 
namespace flexiv {
namespace utility {
 
inline std::vector<double> quat2EulerZYX(const std::vector<double>& quat)
{
    // Check input size
    if (quat.size() != 4) {
        throw InputException(
            "[flexiv::utility] quat2EulerZYX: input vector is not size 4");
    }
 
    // Form quaternion
    Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
 
    // The returned vector is in [z,y,x] order
    auto eulerZYX = q.toRotationMatrix().eulerAngles(2, 1, 0);
 
    // Convert to general [x,y,z] order
    return (std::vector<double> {eulerZYX[2], eulerZYX[1], eulerZYX[0]});
}
 
inline double rad2Deg(double rad)
{
    constexpr double k_pi = 3.14159265358979323846;
    return (rad / k_pi * 180.0);
}
 
inline std::vector<double> rad2Deg(const std::vector<double>& radVec)
{
    std::vector<double> degVec = {};
    for (const auto& v : radVec) {
        degVec.push_back(rad2Deg(v));
    }
    return degVec;
}
 
template <typename T>
inline std::string vec2Str(
    const std::vector<T>& vec, size_t decimal = 3, bool trailingSpace = true)
{
    auto padding = "";
    std::stringstream ss;
    ss.precision(decimal);
    ss << std::fixed;
 
    for (const auto& v : vec) {
        ss << padding << v;
        padding = " ";
    }
 
    if (trailingSpace) {
        ss << " ";
    }
    return ss.str();
}
 
template <typename T, size_t N>
inline std::string arr2Str(
    const std::array<T, N>& arr, size_t decimal = 3, bool trailingSpace = true)
{
    auto padding = "";
    std::stringstream ss;
    ss.precision(decimal);
    ss << std::fixed;
 
    for (const auto& v : arr) {
        ss << padding << v;
        padding = " ";
    }
 
    if (trailingSpace) {
        ss << " ";
    }
    return ss.str();
}
 
inline bool programArgsExistAny(
    int argc, char** argv, const std::vector<std::string>& refStrs)
{
    for (int i = 0; i < argc; i++) {
        for (const auto& v : refStrs) {
            if (v == std::string(argv[i])) {
                return true;
            }
        }
    }
    return false;
}
 
inline bool programArgsExist(int argc, char** argv, const std::string& refStr)
{
    return programArgsExistAny(argc, argv, {refStr});
}
 
inline std::string parsePtStates(
    const std::vector<std::string>& ptStates, const std::string& parseTarget)
{
    for (const auto& state : ptStates) {
        // Skip if empty
        if (state.empty()) {
            continue;
        }
        std::stringstream ss(state);
        std::string buffer;
        std::vector<std::string> parsedState;
        while (ss >> buffer) {
            parsedState.push_back(buffer);
        }
        if (parsedState.front() == parseTarget) {
            return parsedState.back();
        }
    }
 
    return "";
}
 
} /* namespace utility */
} /* namespace flexiv */
 
#endif /* FLEXIVRDK_UTILITY_HPP_ */