utility.hpp

Go to the documentation of this file.

 
#ifndef FLEXIV_RDK_UTILITY_HPP_
#define FLEXIV_RDK_UTILITY_HPP_
 
#include "data.hpp"
#include <Eigen/Eigen>
#include <algorithm>
#include <sstream>
 
namespace flexiv::rdk {
 
namespace utility {
 
inline bool IsSingleArmGroup(JointGroup group)
{
    return group == JointGroup::ARM_1 || group == JointGroup::ARM_2;
}
 
inline bool PrimitiveStateTrue(
    const PrimitiveStates& primitive_states, const std::string& state_name)
{
    const auto state_it = primitive_states.names_and_values.find(state_name);
    if (state_it == primitive_states.names_and_values.end()) {
        return false;
    }
 
    if (!std::holds_alternative<int>(state_it->second)) {
        return false;
    }
 
    return std::get<int>(state_it->second) != 0;
}
 
inline bool PrimitiveStateTrueForGroups(
    const std::map<JointGroup, PrimitiveStates>& primitive_states, const std::string& state_name)
{
    return std::all_of(primitive_states.begin(), primitive_states.end(),
        [&](const auto& entry) { return PrimitiveStateTrue(entry.second, state_name); });
}
 
inline bool PrimitiveStateTrueForGroups(
    const std::map<JointGroup, PrimitiveStates>& primitive_states,
    const std::map<JointGroup, std::string>& state_names)
{
    return std::all_of(primitive_states.begin(), primitive_states.end(), [&](const auto& entry) {
        if (!state_names.contains(entry.first)) {
            return false;
        }
        return PrimitiveStateTrue(entry.second, state_names.at(entry.first));
    });
}
 
inline std::array<double, 3> Quat2EulerZYX(const std::array<double, 4>& quat)
{
    // Form quaternion
    Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
 
    // The returned array is in [z,y,x] order
    auto euler_ZYX = q.toRotationMatrix().eulerAngles(2, 1, 0);
 
    // Convert to general [x,y,z] order
    return (std::array<double, 3> {euler_ZYX[2], euler_ZYX[1], euler_ZYX[0]});
}
 
inline double Rad2Deg(double rad)
{
    constexpr double kPi = 3.14159265358979323846;
    return (rad / kPi * 180.0);
}
 
template <size_t N>
inline std::array<double, N> Rad2Deg(const std::array<double, N>& rad_arr)
{
    std::array<double, N> deg_arr;
    for (size_t i = 0; i < N; i++) {
        deg_arr[i] = Rad2Deg(rad_arr[i]);
    }
    return deg_arr;
}
 
inline std::vector<double> Rad2Deg(const std::vector<double>& rad_vec)
{
    std::vector<double> deg_vec;
    for (const auto& v : rad_vec) {
        deg_vec.push_back(Rad2Deg(v));
    }
    return deg_vec;
}
 
template <typename T>
inline std::string Vec2Str(
    const std::vector<T>& vec, size_t decimal = 3, const std::string& separator = " ")
{
    std::string padding = "";
    std::ostringstream oss;
    oss.precision(decimal);
    oss << std::fixed;
 
    for (const auto& v : vec) {
        oss << padding << v;
        padding = separator;
    }
    return oss.str();
}
 
template <typename T, size_t N>
inline std::string Arr2Str(
    const std::array<T, N>& arr, size_t decimal = 3, const std::string& separator = " ")
{
    std::vector<T> vec(N);
    std::copy(arr.begin(), arr.end(), vec.begin());
    return Vec2Str(vec, decimal, separator);
}
 
inline std::string FlexivTypes2Str(
    const rdk::FlexivDataTypes& variant, size_t decimal = 3, const std::string& separator = " ")
{
    if (auto* val = std::get_if<int>(&variant)) {
        return Vec2Str(std::vector<int> {*val}, decimal);
    } else if (auto* val = std::get_if<double>(&variant)) {
        return Vec2Str(std::vector<double> {*val}, decimal);
    } else if (auto* val = std::get_if<std::string>(&variant)) {
        return *val;
    } else if (auto* val = std::get_if<rdk::Coord>(&variant)) {
        return (*val).str();
    } else if (auto* vec = std::get_if<std::vector<int>>(&variant)) {
        return Vec2Str(*vec, decimal, separator);
    } else if (auto* vec = std::get_if<std::vector<double>>(&variant)) {
        return Vec2Str(*vec, decimal, separator);
    } else if (auto* vec = std::get_if<std::vector<std::string>>(&variant)) {
        return Vec2Str(*vec, decimal, separator);
    } else if (auto* vec = std::get_if<std::vector<rdk::Coord>>(&variant)) {
        std::string ret;
        // Separate two Coord by " : "
        for (const auto& v : (*vec)) {
            ret += v.str() + " : ";
        }
        // Remove the trailing " : "
        if (!ret.empty()) {
            ret.erase(ret.size() - 3);
        }
        return ret;
    }
    return "";
}
 
inline bool ProgramArgsExistAny(int argc, char** argv, const std::vector<std::string>& ref_strings)
{
    for (int i = 0; i < argc; i++) {
        for (const auto& v : ref_strings) {
            if (v == std::string(argv[i])) {
                return true;
            }
        }
    }
    return false;
}
 
inline bool ProgramArgsExist(int argc, char** argv, const std::string& ref_strings)
{
    return ProgramArgsExistAny(argc, argv, {ref_strings});
}
 
} /* namespace utility */
} /* namespace flexiv::rdk */
 
#endif /* FLEXIV_RDK_UTILITY_HPP_ */