utils.cpp

Go to the documentation of this file.

#include "utils.h"
 
#include <memory>
 
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace armarx::control::common
{
 
    Eigen::VectorXf
    getEigenVec(nlohmann::json& userConfig,
                nlohmann::json& defaultConfig,
                const std::string& entryName,
                int size,
                int value)
    {
        if (userConfig.find(entryName) != userConfig.end())
        {
            auto vec = userConfig[entryName].get<std::vector<float>>();
            if (vec.size() > 0)
            {
                return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
            }
            if (size > 0)
            {
                return Eigen::VectorXf::Ones(size) * value;
            }
            ARMARX_IMPORTANT << "user defined parameter " << entryName
                             << " have size 0, fall back to default";
        }
 
        auto vec = defaultConfig[entryName].get<std::vector<float>>();
        return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
    }
 
    void
    getEigenMatrix(nlohmann::json& userConfig, const std::string& entryName, Eigen::MatrixXf& mat)
    {
        if (userConfig.find(entryName) != userConfig.end())
        {
            auto vec = userConfig[entryName].get<std::vector<std::vector<float>>>();
            int rows = vec.size();
            int cols = vec[0].size();
            ARMARX_INFO << VAROUT(rows) << VAROUT(cols);
            ARMARX_CHECK_GREATER_EQUAL(rows, 1);
            ARMARX_CHECK_GREATER_EQUAL(cols, 1);
            mat.resize(rows, cols);
            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    mat(i, j) = vec[i][j];
                }
            }
            //        memcpy(mat.data(), vec.data(), rows * cols * sizeof(float));
        }
        else
        {
            ARMARX_ERROR << "values not found in user config";
        }
    }
 
    Eigen::VectorXf
    getEigenVecWithDefault(nlohmann::json& userConfig,
                           Eigen::VectorXf defaultValue,
                           const std::string& entryName)
    {
        if (userConfig.find(entryName) != userConfig.end())
        {
            auto vec = userConfig[entryName].get<std::vector<float>>();
            if (vec.size() == (unsigned)defaultValue.size())
            {
                return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
            }
            ARMARX_WARNING << "size of the user defined parameter " << entryName
                           << " doesn't correspond to default size, use default value instead";
        }
        return defaultValue;
    }
 
    void
    getError(Eigen::Matrix4f& currentPose,
             Eigen::Vector3d& targetPosition,
             Eigen::Quaterniond& targetQuaternion,
             double& posiError,
             double& oriError)
    {
        Eigen::Matrix4d currentPosed = currentPose.cast<double>();
        Eigen::Vector3d currentPosid = currentPosed.block<3, 1>(0, 3);
        posiError = (currentPosid - targetPosition).norm();
 
        Eigen::Quaterniond currentOrientation = Eigen::Quaterniond{currentPosed.block<3, 3>(0, 0)};
        oriError = targetQuaternion.angularDistance(currentOrientation);
        if (oriError > M_PI)
        {
            oriError = 2 * M_PI - oriError;
        }
    }
 
    void
    debugEigenPose(StringVariantBaseMap& datafields, const std::string& name, Eigen::Matrix4f pose)
    {
        Eigen::Vector3f rpy = VirtualRobot::MathTools::eigen4f2rpy(pose);
        datafields[name + "_x"] = new Variant(pose(0, 3));
        datafields[name + "_y"] = new Variant(pose(1, 3));
        datafields[name + "_z"] = new Variant(pose(2, 3));
        datafields[name + "_rx"] = new Variant(rpy(0));
        datafields[name + "_ry"] = new Variant(rpy(1));
        datafields[name + "_rz"] = new Variant(rpy(2));
    }
 
    void
    debugEigenVec(StringVariantBaseMap& datafields, const std::string& name, Eigen::VectorXf vec)
    {
        for (int i = 0; i < vec.size(); i++)
        {
            datafields[name + "_" + std::to_string(i)] = new Variant(vec(i));
        }
    }
 
    void
    to_json(nlohmann::json& j, const Pose& fp)
    {
        j = nlohmann::json{{"qw", fp.orientation->qw},
                           {"qx", fp.orientation->qx},
                           {"qy", fp.orientation->qy},
                           {"qz", fp.orientation->qz},
                           {"x", fp.position->x},
                           {"y", fp.position->y},
                           {"z", fp.position->z}};
    }
 
    void
    from_json(const nlohmann::json& j, Pose& fp)
    {
        j.at("qw").get_to(fp.orientation->qw);
        j.at("qx").get_to(fp.orientation->qx);
        j.at("qy").get_to(fp.orientation->qy);
        j.at("qz").get_to(fp.orientation->qz);
        j.at("x").get_to(fp.position->x);
        j.at("y").get_to(fp.position->y);
        j.at("z").get_to(fp.position->z);
    }
 
    void
    to_json(nlohmann::json& j, const PosePtr& fp)
    {
        if (fp)
        {
            to_json(j, *fp);
        }
    }
 
    void
    to_json(nlohmann::json& j, const PoseBasePtr& fp)
    {
        if (fp)
        {
            to_json(j, PosePtr::dynamicCast(fp));
        }
    }
 
    void
    from_json(const nlohmann::json& j, PoseBasePtr& fp)
    {
        PosePtr p = new Pose();
        from_json(j, p);
        fp = p;
    }
 
    void
    from_json(const nlohmann::json& j, PosePtr& fp)
    {
        fp = new Pose();
        from_json(j, *fp);
    }
 
    PosePtr
    dVecToPose(const mplib::core::DVec& dvec)
    {
        return new Pose(dVecToMat4(dvec));
    }
 
    Eigen::Matrix4f
    dVecToMat4(const mplib::core::DVec& dvec)
    {
        ARMARX_CHECK_EQUAL(dvec.size(), 7);
        Eigen::Matrix4f mat = VirtualRobot::MathTools::quat2eigen4f(static_cast<float>(dvec[4]),
                                                                    static_cast<float>(dvec[5]),
                                                                    static_cast<float>(dvec[6]),
                                                                    static_cast<float>(dvec[3]));
        mat(0, 3) = static_cast<float>(dvec[0]);
        mat(1, 3) = static_cast<float>(dvec[1]);
        mat(2, 3) = static_cast<float>(dvec[2]);
 
        return mat;
    }
 
    mplib::core::DVec
    poseToDVec(PosePtr& pose)
    {
        Eigen::Matrix4f poseMat = pose->toEigen();
        return mat4ToDVec(poseMat);
    }
 
    mplib::core::DVec
    mat4ToDVec(Eigen::Matrix4f& mat)
    {
        std::vector<double> result;
        result.resize(7);
 
        for (size_t i = 0; i < 3; ++i)
        {
            result.at(i) = static_cast<double>(mat(i, 3));
        }
 
        VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
 
        result.at(3) = static_cast<double>(quat.w);
        result.at(4) = static_cast<double>(quat.x);
        result.at(5) = static_cast<double>(quat.y);
        result.at(6) = static_cast<double>(quat.z);
 
        return result;
    }
 
    std::vector<float>
    poseToFVec(PosePtr& pose)
    {
        Eigen::Matrix4f poseMat = pose->toEigen();
        return mat4ToFVec(poseMat);
    }
 
    std::vector<float>
    mat4ToFVec(Eigen::Matrix4f& mat)
    {
        std::vector<float> result;
        result.resize(7);
 
        for (size_t i = 0; i < 3; ++i)
        {
            result.at(i) = mat(i, 3);
        }
 
        VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(mat);
 
        result.at(3) = quat.w;
        result.at(4) = quat.x;
        result.at(5) = quat.y;
        result.at(6) = quat.z;
 
        return result;
    }
 
    std::string
    dVecToString(const mplib::core::DVec& dvec)
    {
        std::string s = "";
        for (const double& d : dvec)
        {
            s = s + std::to_string(d) + " | ";
        }
        return s;
    }
 
    Eigen::VectorXf
    vecToEigen(std::vector<float>& vec)
    {
        return Eigen::Map<Eigen::VectorXf>(vec.data(), vec.size());
    }
 
    std::string
    sVecToString(const std::vector<std::string>& vec, const std::string& delimiter)
    {
        std::string s = "";
        for (size_t i = 0; i < vec.size(); ++i)
        {
            if (i != 0)
            {
                s = s + delimiter;
            }
            s + vec[i];
        }
        return s;
    }
 
 
} // namespace armarx::control::common