utils.cpp

Go to the documentation of this file.

#include <memory>
#include <VirtualRobot/MathTools.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "utils.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());
}
 
 
}