aron_conversions.cpp
Go to the documentation of this file.
1 #include "aron_conversions.h"
3 #include <dmp/representation/dmp/umitsmp.h>
4 #include <VirtualRobot/MathTools.h>
5 
6 namespace armarx::armem
7 {
8 
9 void fromAron(const arondto::Trajectory &dto, DMP::SampledTrajectoryV2 &bo, bool taskspace)
10 {
11  std::map<double, DMP::DVec> traj_map;
12  if(taskspace){
13  for(auto element : dto.taskSpace.steps){
14  DMP::DVec pose;
15  pose.push_back(element.pose(0,3));
16  pose.push_back(element.pose(1,3));
17  pose.push_back(element.pose(2,3));
18  VirtualRobot::MathTools::Quaternion quat = VirtualRobot::MathTools::eigen4f2quat(element.pose);
19  pose.push_back(quat.w);
20  pose.push_back(quat.x);
21  pose.push_back(quat.y);
22  pose.push_back(quat.z);
23  traj_map.insert(std::make_pair(element.timestep, pose));
24  }
25 
26  }else{
27  for(auto element : dto.jointSpace.steps){
28  DMP::DVec jointvalues;
29  for(auto angle: element.jointValues){
30  jointvalues.push_back(double(angle));
31  }
32  traj_map.insert(std::make_pair(element.timestep, jointvalues));
33  }
34  }
35 }
36 
37 void toAron(arondto::Trajectory &dto, const DMP::SampledTrajectoryV2 &bo_taskspace, const DMP::SampledTrajectoryV2 &bo_jointspace, const std::string name)
38 {
39  dto.name = name;
40  std::map<std::string, std::vector<float>> mapJointSpace;
41 
42  // taskspace
43  std::map<double, DMP::DVec> ts_map = bo_taskspace.getPositionData();
44  for(std::pair<double, DMP::DVec> element: ts_map){
45  Eigen::Vector3f vec(element.second.at(0), element.second.at(1), element.second.at(2));
46  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));
47  poseMatrix.block<3, 1>(0, 3) = vec;
48  arondto::TSElement tselement;
49  tselement.timestep = element.first;
50  tselement.pose = poseMatrix;
51  dto.taskSpace.steps.push_back(tselement);
52 
53  }
54 
55  // jointspace
56  std::map<double, DMP::DVec> js_map = bo_jointspace.getPositionData();
57  for(std::pair<double, DMP::DVec> element: js_map){
58  std::vector<float> configvec;
59  for(double el: element.second){
60  configvec.push_back(float(el));
61  }
62  arondto::JSElement jselement;
63  jselement.timestep = element.first;
64  jselement.jointValues = configvec;
65  dto.jointSpace.steps.push_back(jselement);
66  }
67 
68 }
69 
70 }
aron_conversions.h
armarx::armem::toAron
void toAron(arondto::MemoryID &dto, const MemoryID &bo)
Definition: aron_conversions.cpp:19
armarx::armem
Definition: LegacyRobotStateMemoryAdapter.cpp:31
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
armarx::VariantType::Trajectory
const VariantTypeId Trajectory
Definition: Trajectory.h:44
aron_conversions.h
armarx::armem::fromAron
void fromAron(const arondto::MemoryID &dto, MemoryID &bo)
Definition: aron_conversions.cpp:8
angle
double angle(const Point &a, const Point &b, const Point &c)
Definition: point.hpp:100
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
armarx::aron::bo
const std::optional< BoT > & bo
Definition: aron_conversions.h:168