motionprimitives.cpp

Go to the documentation of this file.

#include "motionprimitives.h"
 
// #include <iostream>
// #include <fstream>
 
#include <SimoxUtility/algorithm/string.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
 
#include <dmp/representation/trajectory.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;
        }
    }
 
 
} // namespace armarx::armem::server::motions::mps::segment::util