From 9b69df8b7881968db0e5945e1c72df46804a2ab2 Mon Sep 17 00:00:00 2001
From: Franziska Krebs <franzikrebs@gmx.de>
Date: Tue, 12 Oct 2021 13:28:13 +0200
Subject: [PATCH] added from and toAron
---
.../libraries/armem_mps/CMakeLists.txt | 4 +-
.../libraries/armem_mps/aron_conversions.cpp | 62 +++++++++++++++++++
.../libraries/armem_mps/aron_conversions.h | 11 ++--
3 files changed, 70 insertions(+), 7 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
index dc5a3f37d..44dfe5155 100644
--- a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
index 5311e5ca9..4451bc835 100644
--- a/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_mps/aron_conversions.cpp
@@ -1,8 +1,70 @@
#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);
+ }
+
+}
+
}
diff --git a/source/RobotAPI/libraries/armem_mps/aron_conversions.h b/source/RobotAPI/libraries/armem_mps/aron_conversions.h
index a4a9f9f62..2e5656927 100644
--- a/source/RobotAPI/libraries/armem_mps/aron_conversions.h
+++ b/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)
}
--
GitLab