utils.cpp

Go to the documentation of this file.

#include "utils.h"
 
#include <filesystem>
#include <fstream>
#include <iostream>
#include <memory>
#include <optional>
#include <type_traits>
 
#include <boost/tokenizer.hpp>
 
#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() == static_cast<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
    debugStdVec(StringVariantBaseMap& datafields, const std::string& name, std::vector<float> vec)
    {
        for (size_t 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;
    }
 
    mp::arondto::MPTraj
    mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
    {
        std::vector<double> timesteps;
        std::vector<std::vector<double>> trajPoses;
 
        std::ifstream f(filepath.c_str());
 
        if (!f.is_open())
        {
            ARMARX_ERROR << "Could not open file: " + filepath;
        }
 
        using Tokenizer = boost::tokenizer<boost::escaped_list_separator<char>>;
 
 
        //        unsigned int stateDim = double().size();
        unsigned int i = 1;
 
        double current_time = 0;
        std::string line;
        std::vector<std::string> stringVec;
        while (getline(f, line))
        {
            if (containsHeader && i == 1)
            {
                i++;
                continue;
            }
 
            Tokenizer tok(line);
            stringVec.assign(tok.begin(), tok.end());
 
            if (stringVec.size() == 0)
            {
                std::cout << "Line " << i << " is invalid  skipping: " << line << std::endl;
                continue;
            }
 
            unsigned int j = 1;
 
            armarx::control::common::mp::arondto::MPTraj traj;
 
            //TrajData entry(*this);
            int size = stringVec.size() - 1;
            if (noTimeStamp)
            {
                j = 0;
                //entry.timestamp = current_time;
                timesteps.push_back(current_time);
                current_time += 0.01;
                size = stringVec.size();
            }
            else
            {
                //entry.timestamp = math::util::fromString<double>(stringVec[0]);
                timesteps.push_back(std::stod(stringVec[0]));
            }
 
            //            DVec values(int(ceil(size/stateDim)*stateDim), 0.0);
            //DVec values(size, 0.0);
            std::vector<double> currentPose;
 
            for (unsigned int k = j; k < stringVec.size(); ++k)
            {
                currentPose.push_back(std::stod(stringVec[k]));
                //values.at(sd) = (math::util::fromString<double>(stringVec[j]));
                //math::util::checkValue(values.at(sd));
            }
            trajPoses.push_back(currentPose);
 
            i++;
        }
 
        // convert to aron types
        if (timesteps.size() != trajPoses.size())
        {
            ARMARX_ERROR << "consistency error in reading trajectory";
        }
        armarx::control::common::mp::arondto::MPTraj taskTraj;
        taskTraj.time.setZero(timesteps.size());
        taskTraj.traj.setZero(timesteps.size(), trajPoses.at(0).size());
        ARMARX_INFO << "no timesteps" << timesteps.size();
        ARMARX_INFO << "traj pose dim " << trajPoses.at(0).size();
        for (unsigned int l = 0; l < timesteps.size(); l++)
        {
            taskTraj.time(l) = timesteps.at(l);
            for (size_t j = 0; j < trajPoses.at(0).size(); ++j)
                taskTraj.traj(l, j) = trajPoses.at(l).at(j);
        }
        return taskTraj;
    }
 
 
} // namespace armarx::control::common