added from and toAron

source/RobotAPI/libraries/armem_mps/CMakeLists.txt

@@ -17,12 +17,12 @@ armarx_add_library(
         RobotAPI::PriorKnowledge::Motions
         VirtualRobot
     SOURCES  
-        #./aron_conversions.cpp
+        ./aron_conversions.cpp
         #./traj_conversions.cpp
         ./server/MotionPrimitives/motionprimitives.cpp
         ./server/MotionPrimitives/Segment.cpp
     HEADERS  
-        #./aron_conversions.h
+        ./aron_conversions.h
         #./traj_conversions.h
         ./server/MotionPrimitives/motionprimitives.h
         ./server/MotionPrimitives/Segment.h

source/RobotAPI/libraries/armem_mps/aron_conversions.cpp

 #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);
+    }
+
+}
+
 }

source/RobotAPI/libraries/armem_mps/aron_conversions.h

@@ -5,9 +5,12 @@
 
 //#include <RobotAPI/libraries/armem_skills/aron/Statechart.aron.generated.h>
 #include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+#include <RobotAPI/libraries/armem_mps/aron/Trajectory.aron.generated.h>
+
+#include <dmp/representation/trajectory.h>
 //#include <dmp
 
-namespace armarx::armem
+namespace armarx
 {
     /*void fromAron(const armarx::armem::arondto::Statechart::StateType& dto, armarx::eStateType& bo);
     void toAron(armarx::armem::arondto::Statechart::StateType& dto, const armarx::eStateType& bo);
@@ -20,10 +23,8 @@ namespace armarx::armem
 
     //void fromAron(const armarx::armem_mps::arondto::Trajectory& dto, armarx::Trajectory& bo);
     //void toAron(arondto::ObjectClass& dto, const RobotDescription& bo);
-
-    void fromAronToTraj();
-    void toAronFromTraj();
-    void toAronFromPath();
+    void fromAron(const armem::arondto::Trajectory& dto, DMP::SampledTrajectoryV2& bo, bool taskspace);
+    void toAron(armem::arondto::Trajectory& dto, const DMP::SampledTrajectoryV2& bo_taskspace, const DMP::SampledTrajectoryV2& bo_jointspace, const std::string name);
 
     //todo: trajbo zu dmptraj (und umgekehrt)
 }