aron_conversions.cpp

Go to the documentation of this file.

#include "aron_conversions.h"
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <dmp/representation/dmp/umitsmp.h>
#include <VirtualRobot/MathTools.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);
    }
 
}
 
}