motionprimitives.cpp

Go to the documentation of this file.

#include "motionprimitives.h"
 
// #include <iostream>
// #include <fstream>
 
#include <SimoxUtility/algorithm/string.h>
 
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
 
#include <dmp/representation/trajectory.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <VirtualRobot/MathTools.h>
 
namespace armarx::armem::server::motions::mps::segment::util
{
 
std::optional<armarx::arondto::Trajectory> createFromFile(const std::filesystem::__cxx11::path &pathToInfoJson, bool taskspace)
{
 
    if (std::filesystem::exists(pathToInfoJson) && std::filesystem::is_regular_file(pathToInfoJson))
    {
        DMP::Vec<DMP::SampledTrajectoryV2 > trajs;
        DMP::SampledTrajectoryV2 traj;
        std::string absPath;
        ArmarXDataPath::getAbsolutePath(pathToInfoJson, absPath);
        traj.readFromCSVFile(absPath);
 
        //traj = DMP::SampledTrajectoryV2::normalizeTimestamps(traj, 0, 1);
        std::map<double, DMP::DVec> currentTraj = traj.getPositionData();//todo
        trajs.push_back(traj);
 
        armarx::arondto::Trajectory trajectory;
        std::string name = pathToInfoJson.filename();
        std::string toErase = "taskspace-trajectory-";
        size_t pos = name.find(toErase);
        if (pos != std::string::npos)
        {
            name.erase(pos, toErase.length());
        }
        trajectory.name = name;
        std::map<std::string, std::vector<float>> mapJointSpace;
        for(DMP::SampledTrajectoryV2 traj: trajs)
    {
            std::map<double, DMP::DVec> currentTraj = traj.getPositionData(); // todo: add config making data structure clear
 
            if(taskspace)
        {
                    for(std::pair<double, DMP::DVec> element: currentTraj)
            {
                        Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
                        Eigen::Matrix<float, 4, 4> poseMatrix = VirtualRobot::MathTools::quat2eigen4f(element.second.at(4), element.second.at(5), element.second.at(6), element.second.at(3));
                        poseMatrix.block<3, 1>(0, 3) = vec;
                        armarx::arondto::TSElement tselement;
                        tselement.timestep = element.first;
                        tselement.pose = poseMatrix;
                        trajectory.taskSpace.steps.push_back(tselement);
 
                    }
 
 
            }
        else
        {
                for(std::pair<double, DMP::DVec> element: currentTraj)
        {
                    std::vector<float> configvec;
                    for(double el: element.second)
            {
                        configvec.push_back(float(el));
                    }
                    armarx::arondto::JSElement jselement;
                    jselement.timestep = element.first;
                    jselement.jointValues = configvec;
                    trajectory.jointSpace.steps.push_back(jselement);
                }
            }
 
        }
 
        return trajectory;
    }
    else
    {
        return std::nullopt;
    }
}
 
 
 
}