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/PackagePath.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <armarx/control/common/aron_conversions.h>
 
namespace armarx::control::common
{
 
    /// helpers
 
    Eigen::Vector4f
    control_law::helper::calculateTransitionWeights(float z1, float z2)
    {
        /// Define the constraint matrix for the null space transition function
        ///
        /// in transition state (state between full locking and unrestricted movement for a contin-
        /// uous control law) a third-order polynomial with four weight values is required
        /// In this method the weights are calculated in the preactivate method based on the set
        /// threshold parameters z1 and z2
        ///
        /// z1: state of full locking starts with this value
        /// z2: state of unrestricted movement starts with this value
        Eigen::Matrix4f constraintMatrix;
        constraintMatrix << std::pow(z1, 3), std::pow(z1, 2), z1, 1, std::pow(z2, 3),
            std::pow(z2, 2), z2, 1, 3 * std::pow(z1, 2), 2 * z1, 1, 0, 3 * std::pow(z2, 2), 2 * z2,
            1, 0;
 
        /// Define the constraint result values for the null space transition function
        Eigen::Vector4f b;
        b << 0, 1, 0, 0;
 
        /// weights for continuous task transition
        return constraintMatrix.colPivHouseholderQr().solve(b);
    }
 
    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_VERBOSE << 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
    debugEigenMat(StringVariantBaseMap& datafields,
                  const std::string& name,
                  const Eigen::MatrixXd& mat)
    {
        // Pass by const reference (&) to avoid copying the matrix data
        for (int r = 0; r < mat.rows(); ++r)
        {
            for (int c = 0; c < mat.cols(); ++c)
            {
                // Construct key: "name_row_col" -> e.g., "covariance_0_1"
                std::string key = name + "_" + std::to_string(r) + "_" + std::to_string(c);
 
                // Access matrix element at (row, col)
                datafields[key] = new Variant(mat(r, c));
            }
        }
    }
 
    void
    debugEigenMat(StringVariantBaseMap& datafields,
                  const std::string& name,
                  const Eigen::MatrixXf& mat)
    {
        // Pass by const reference (&) to avoid copying the matrix data
        for (int r = 0; r < mat.rows(); ++r)
        {
            for (int c = 0; c < mat.cols(); ++c)
            {
                // Construct key: "name_row_col" -> e.g., "covariance_0_1"
                std::string key = name + "_" + std::to_string(r) + "_" + std::to_string(c);
 
                // Access matrix element at (row, col)
                datafields[key] = new Variant(mat(r, c));
            }
        }
    }
 
    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);
    }
 
    Eigen::Block<Eigen::Matrix4f, 3, 3>
    getOri(Eigen::Matrix4f& matrix)
    {
        return matrix.block<3, 3>(0, 0);
    }
 
    Eigen::Matrix3f
    getOri(const Eigen::Matrix4f& matrix)
    {
        return matrix.block<3, 3>(0, 0);
    }
 
    void
    skew(const Eigen::Vector3f& vec, Eigen::Ref<Eigen::Matrix3f> result)
    {
        result.setZero();
        result(1, 2) = -vec(0);
        result(0, 2) = vec(1);
        result(0, 1) = -vec(2);
        result(2, 1) = vec(0);
        result(2, 0) = -vec(1);
        result(1, 0) = vec(2);
    }
 
    Eigen::Matrix3f
    getOriT(const Eigen::Matrix4f& matrix)
    {
        return matrix.block<3, 3>(0, 0).transpose();
    }
 
    void
    setOri(Eigen::Matrix4f& matrix, const Eigen::Ref<const Eigen::Matrix3f>& ori)
    {
        matrix.block<3, 3>(0, 0) = ori;
    }
 
    Eigen::Block<Eigen::Matrix4f, 3, 1>
    getPos(Eigen::Matrix4f& matrix)
    {
        return matrix.block<3, 1>(0, 3);
    }
 
    void
    setPos(Eigen::Matrix4f& matrix, const Eigen::Ref<const Eigen::Vector3f>& pos)
    {
        matrix.block<3, 1>(0, 3) = pos;
    }
 
    Eigen::Vector3f
    getPos(const Eigen::Matrix4f& matrix)
    {
        return matrix.block<3, 1>(0, 3);
    }
 
    PosePtr
    dVecToPose(const DVec& dvec)
    {
        return new Pose(dVecToMat4(dvec));
    }
 
    Eigen::Matrix4f
    dVecToMat4(const 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;
    }
 
    Eigen::Matrix4f
    vec7fToMat4f(const Eigen::VectorXf& vec)
    {
        ARMARX_CHECK_EQUAL(vec.size(), 7);
        Eigen::Matrix4f mat = VirtualRobot::MathTools::quat2eigen4f(vec[4], vec[5], vec[6], vec[3]);
        mat(0, 3) = vec[0];
        mat(1, 3) = vec[1];
        mat(2, 3) = vec[2];
 
        return mat;
    }
 
    DVec
    poseToDVec(PosePtr& pose)
    {
        Eigen::Matrix4f poseMat = pose->toEigen();
        return mat4ToDVec(poseMat);
    }
 
    DVec
    mat4ToDVec(const 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(const 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 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());
    }
 
    void
    dvecToEigenf(std::vector<double>& vec, Eigen::VectorXf& result)
    {
        // Check size match
        ARMARX_CHECK_EQUAL(result.size(), static_cast<Eigen::Index>(vec.size()))
            << "Size of result VectorXf does not match input vector size.";
 
        // Copy and cast
        for (size_t i = 0; i < vec.size(); ++i)
        {
            result[i] = static_cast<float>(vec[i]);
        }
    }
 
    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 += 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;
    }
 
    mp::arondto::MPTraj
    createDummyTraj(const int dimension, const int timesteps)
    {
        mp::arondto::MPTraj traj;
        traj.time = Eigen::VectorXd::LinSpaced(timesteps, 0, 1);
        traj.traj = Eigen::MatrixXd::Zero(timesteps, dimension);
        return traj;
    }
 
    mp::arondto::MPConfig
    getDefaultMPConfig(MPType mpType,
                       const std::string& name,
                       const std::string& nodeSetName,
                       const double& durationSec,
                       const std::vector<mp::arondto::MPTraj>& mpTraj,
                       const std::vector<mp::arondto::ListViaPoint>& viaPointList)
    {
        const std::string className = mpTypeToString.to_name(mpType);
        const armarx::PackagePath configPath(
            "armarx_control", "controller_config/mp/default_mp_" + className + ".json");
        ARMARX_INFO << "using default config as base: " << configPath.toSystemPath();
 
        auto cfg = armarx::readFromJson<mp::arondto::MPConfig>(configPath.toSystemPath());
        cfg.name = name;
        cfg.nodeSetName = nodeSetName;
        if (durationSec > 0.0)
        {
            ARMARX_INFO << "set duration to " << durationSec << " sec";
            cfg.durationSec = durationSec;
        }
        if (mpTraj.size() > 0)
        {
            ARMARX_INFO << "set trajectory with " << mpTraj.size() << " segments";
            cfg.trajectoryList = mpTraj;
        }
        if (viaPointList.size() > 0)
        {
            ARMARX_INFO << "set " << viaPointList.size() << " via points";
            cfg.viaPoints = viaPointList;
        }
        return cfg;
    }
 
    mp::arondto::MPConfig
    getDefaultMPConfig(MPType mpType,
                       const std::string& name,
                       const std::string& nodeSetName,
                       const double& durationSec,
                       const std::vector<std::string>& mpTraj,
                       const std::vector<mp::arondto::ListViaPoint>& viaPointList)
    {
        std::vector<mp::arondto::MPTraj> mpTrajList{};
        for (auto& f : mpTraj)
        {
            mpTrajList.push_back(mpTrajFromFile(f));
        }
        return getDefaultMPConfig(mpType, name, nodeSetName, durationSec, mpTrajList, viaPointList);
    }
 
    void
    addViaPoint(mp::arondto::MPConfig& cfg,
                const double& canValue,
                const std::vector<double>& viaPoint)
    {
        auto vp = mp::arondto::ListViaPoint();
        vp.canonicalValue = canValue;
        vp.viaPointValue = viaPoint;
        cfg.viaPoints.push_back(vp);
    }
 
    void
    addViaPoint(mp::arondto::MPConfig& cfg,
                const double& canValue,
                const std::vector<float>& viaPoint)
    {
        std::vector<double> viaPointVec;
        std::transform(viaPoint.begin(),
                       viaPoint.end(),
                       std::back_inserter(viaPointVec),
                       [](float value) { return static_cast<double>(value); });
        addViaPoint(cfg, canValue, viaPointVec);
    }
 
    void
    addViaPoint(mp::arondto::MPConfig& cfg, const double& canValue, const Eigen::VectorXf& viaPoint)
    {
        std::vector<double> viaPointVec;
        std::transform(viaPoint.data(),
                       viaPoint.data() + viaPoint.size(),
                       std::back_inserter(viaPointVec),
                       [](float value) { return static_cast<double>(value); });
        addViaPoint(cfg, canValue, viaPointVec);
    }
 
    void
    addViaPoint(mp::arondto::MPConfig& cfg, const double& canValue, const Eigen::Matrix4f& viaPose)
    {
        auto viaPointVec = mat4ToDVec(viaPose);
        addViaPoint(cfg, canValue, viaPointVec);
    }
 
    void
    validateTSViaPoint(std::vector<double>& viaPoint, std::vector<double>& viaPointReference)
    {
        if (viaPoint.size() != 7 || viaPointReference.size() != 7)
        {
            ARMARX_ERROR << "Both viapoints must have exactly 7 elements";
        }
 
        bool allDifferentSigns = true;
 
        // Check signs for the last 4 elements
        for (int i = 3; i < 7; ++i)
        {
            if ((viaPoint[i] * viaPointReference[i]) >= 0)
            {
                allDifferentSigns = false;
                break;
            }
        }
 
        // If all last 4 elements have different signs, flip their signs
        if (allDifferentSigns)
        {
            ARMARX_VERBOSE << "===> Viapoints for TS Orientation on two hemisphere, flipping sign";
            for (int i = 3; i < 7; ++i)
            {
                viaPoint[i] = -viaPoint[i];
            }
        }
    }
 
    void
    appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
                         const double canonicalValue,
                         const std::vector<double>& viaPoint)
    {
        mp::arondto::ListViaPoint vp;
        vp.canonicalValue = canonicalValue;
        vp.viaPointValue = viaPoint;
        vpList.push_back(vp);
    }
 
    void
    appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
                         const double canonicalValue,
                         const std::vector<float>& viaPoint)
    {
        std::vector<double> viaPointVec;
        std::transform(viaPoint.begin(),
                       viaPoint.end(),
                       std::back_inserter(viaPointVec),
                       [](float value) { return static_cast<double>(value); });
        appendToViapointList(vpList, canonicalValue, viaPointVec);
    }
 
    void
    appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
                         const double canonicalValue,
                         const Eigen::VectorXf& viaPoint)
    {
        std::vector<double> viaPointVec;
        std::transform(viaPoint.data(),
                       viaPoint.data() + viaPoint.size(),
                       std::back_inserter(viaPointVec),
                       [](float value) { return static_cast<double>(value); });
        appendToViapointList(vpList, canonicalValue, viaPointVec);
    }
 
    void
    appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
                         const double canonicalValue,
                         const Eigen::Matrix4f& viaPose)
    {
        auto viaPointVec = mat4ToDVec(viaPose);
        appendToViapointList(vpList, canonicalValue, viaPointVec);
    }
 
    float
    getDeltaAngleBetweenRotMat(const Eigen::Matrix3f& rotMat1, const Eigen::Matrix3f& rotMat2)
    {
        Eigen::Matrix3f poseDiffMatImp = rotMat1 * rotMat2.transpose();
        Eigen::AngleAxisf oriDiffAngleAxis;
        oriDiffAngleAxis.fromRotationMatrix(poseDiffMatImp);
        float angle = VirtualRobot::MathTools::angleModPI(oriDiffAngleAxis.angle());
        angle = std::fminf(angle, 2 * M_PI - angle);
        return angle;
    }
 
    float
    getDeltaAngleBetweenPose(const Eigen::Matrix4f& pose1, const Eigen::Matrix4f& pose2)
    {
        return getDeltaAngleBetweenRotMat(pose1.block<3, 3>(0, 0), pose2.block<3, 3>(0, 0));
    }
 
    bool
    poseDeviationTooLarge(const Eigen::Matrix4f& currentPose,
                          const Eigen::Matrix4f& desiredPose,
                          float positionThrMM,
                          float angleThrDeg)
    {
        float angle = getDeltaAngleBetweenPose(currentPose, desiredPose);
        return (desiredPose.block<3, 1>(0, 3) - currentPose.block<3, 1>(0, 3)).norm() >
                   positionThrMM or
               angle > VirtualRobot::MathTools::deg2rad(angleThrDeg);
    }
 
    void
    reportPoseDeviationWarning(const std::string& who,
                               const Eigen::Matrix4f& pose1,
                               const Eigen::Matrix4f& pose2,
                               const std::string& namePose1,
                               const std::string& namePose2)
    {
        auto quat1 = VirtualRobot::MathTools::eigen4f2quat(pose1);
        auto quat2 = VirtualRobot::MathTools::eigen4f2quat(pose2);
        ARMARX_INFO << deactivateSpam(2) << "-- " << who << " " << namePose2
                    << " (x, y, z, qw, qx, qy, qz) \n\n"
                    << " [ " << pose2(0, 3) << ", " << pose2(1, 3) << ", " << pose2(2, 3) << ", "
                    << quat2.w << ", " << quat2.x << ", " << quat2.y << ", " << quat2.z << " ]\n\n"
                    << " is too far away from the " << namePose1
                    << " (x, y, z, qw, qx, qy, qz) \n\n"
                    << " [ " << pose1(0, 3) << ", " << pose1(1, 3) << ", " << pose1(2, 3) << ", "
                    << quat1.w << ", " << quat1.x << ", " << quat1.y << ", " << quat1.z << " ].";
    }
 
    bool
    detectAndReportPoseDeviationWarning(const Eigen::Matrix4f& pose1,
                                        const Eigen::Matrix4f& pose2,
                                        const std::string& namePose1,
                                        const std::string& namePose2,
                                        float positionThrMM,
                                        float angleThrDeg,
                                        const std::string& who)
    {
        float angle = getDeltaAngleBetweenPose(pose1, pose2);
        float dist = (pose2.block<3, 1>(0, 3) - pose1.block<3, 1>(0, 3)).norm();
 
        bool flagToReport =
            dist > positionThrMM or angle > VirtualRobot::MathTools::deg2rad(angleThrDeg);
        if (flagToReport)
        {
            reportPoseDeviationWarning(who, pose1, pose2, namePose1, namePose2);
            ARMARX_INFO << deactivateSpam(2) << "-- Delta distance (mm) " << dist << " > "
                        << positionThrMM << " or angle (rad) " << angle << " > "
                        << VirtualRobot::MathTools::deg2rad(angleThrDeg);
        }
        return flagToReport;
    }
 
    Eigen::Matrix4f
    mirrorLeftRightPose(Eigen::Matrix4f& sourcePose)
    {
        Eigen::Matrix4f convert;
        convert << 1, -1, -1, -1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1;
        return sourcePose.cwiseProduct(convert);
    }
 
    Eigen::Matrix4f
    mirrorLeftRightOri(Eigen::Matrix4f& sourcePose)
    {
        Eigen::Matrix4f convert;
        convert << 1, -1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1;
        return sourcePose.cwiseProduct(convert);
    }
 
    Eigen::Matrix3f
    mirrorLeftRightOri(Eigen::Matrix3f& sourcePose)
    {
        Eigen::Matrix3f convert;
        convert << 1, -1, -1, -1, 1, 1, -1, 1, 1;
        return sourcePose.cwiseProduct(convert);
    }
 
} // namespace armarx::control::common