aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
 
#include <VirtualRobot/MathTools.h>
 
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 
#include <dmp/representation/dmp/umitsmp.h>
 
namespace armarx::armem
{
 
    void
    fromAron(const arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace)
    {
        std::map<double, DMP::DVec> traj_map;
        if (taskspace)
        {
            for (auto element : dto.taskSpace.steps)
            {
                DMP::DVec pose;
                pose.push_back(element.pose(0, 3));
                pose.push_back(element.pose(1, 3));
                pose.push_back(element.pose(2, 3));
                VirtualRobot::MathTools::Quaternion quat =
                    VirtualRobot::MathTools::eigen4f2quat(element.pose);
                pose.push_back(quat.w);
                pose.push_back(quat.x);
                pose.push_back(quat.y);
                pose.push_back(quat.z);
                traj_map.insert(std::make_pair(element.timestep, pose));
            }
        }
        else
        {
            for (auto element : dto.jointSpace.steps)
            {
                DMP::DVec jointvalues;
                for (auto angle : element.jointValues)
                {
                    jointvalues.push_back(double(angle));
                }
                traj_map.insert(std::make_pair(element.timestep, jointvalues));
            }
        }
    }
 
    void
    toAron(arondto::Trajectory& dto,
           const DMP::SampledTrajectoryV2& bo_taskspace,
           const DMP::SampledTrajectoryV2& bo_jointspace,
           const std::string name)
    {
        dto.name = name;
        std::map<std::string, std::vector<float>> mapJointSpace;
 
        // taskspace
        std::map<double, DMP::DVec> ts_map = bo_taskspace.getPositionData();
        for (std::pair<double, DMP::DVec> element : ts_map)
        {
            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;
            arondto::TSElement tselement;
            tselement.timestep = element.first;
            tselement.pose = poseMatrix;
            dto.taskSpace.steps.push_back(tselement);
        }
 
        // jointspace
        std::map<double, DMP::DVec> js_map = bo_jointspace.getPositionData();
        for (std::pair<double, DMP::DVec> element : js_map)
        {
            std::vector<float> configvec;
            for (double el : element.second)
            {
                configvec.push_back(float(el));
            }
            arondto::JSElement jselement;
            jselement.timestep = element.first;
            jselement.jointValues = configvec;
            dto.jointSpace.steps.push_back(jselement);
        }
    }
 
} // namespace armarx::armem