framed.cpp
Go to the documentation of this file.
1#include "framed.h"
2
3#include <RobotAPI/libraries/aron/common/aron/framed.aron.generated.h>
4
5namespace armarx::aron::framed
6{
7 void
8 fromAron(const arondto::FramedPosition& dto, armarx::FramedPosition& bo)
9 {
10 bo.frame = dto.header.frame;
11 bo.agent = dto.header.agent;
12 bo.x = dto.position.x();
13 bo.y = dto.position.y();
14 bo.z = dto.position.z();
15 }
16
17 void
18 toAron(arondto::FramedPosition& dto, const armarx::FramedPosition& bo)
19 {
20 dto.header.frame = bo.frame;
21 dto.header.agent = bo.agent;
22 dto.position.x() = bo.x;
23 dto.position.y() = bo.y;
24 dto.position.z() = bo.z;
25 }
26
27 void
28 fromAron(const arondto::FramedDirection& dto, armarx::FramedDirection& bo)
29 {
30 bo.frame = dto.header.frame;
31 bo.agent = dto.header.agent;
32 bo.x = dto.direction.x();
33 bo.y = dto.direction.y();
34 bo.z = dto.direction.z();
35 }
36
37 void
38 toAron(arondto::FramedDirection& dto, const armarx::FramedDirection& bo)
39 {
40 dto.header.frame = bo.frame;
41 dto.header.agent = bo.agent;
42 dto.direction.x() = bo.x;
43 dto.direction.y() = bo.y;
44 dto.direction.z() = bo.z;
45 }
46
47 void
48 fromAron(const arondto::FramedOrientation& dto, armarx::FramedOrientation& bo)
49 {
50 bo.frame = dto.header.frame;
51 bo.agent = dto.header.agent;
52 bo.qw = dto.orientation.w();
53 bo.qx = dto.orientation.x();
54 bo.qy = dto.orientation.y();
55 bo.qz = dto.orientation.z();
56 }
57
58 void
59 toAron(arondto::FramedOrientation& dto, const armarx::FramedOrientation& bo)
60 {
61 dto.header.frame = bo.frame;
62 dto.header.agent = bo.agent;
63 dto.orientation.w() = bo.qw;
64 dto.orientation.x() = bo.qx;
65 dto.orientation.y() = bo.qy;
66 dto.orientation.z() = bo.qz;
67 }
68
69 void
70 fromAron(const arondto::FramedPose& dto, armarx::FramedPose& bo)
71 {
72 bo.frame = dto.header.frame;
73 bo.agent = dto.header.agent;
74 auto vec = Eigen::Vector3f(dto.pose.block<3, 1>(0, 3));
75 bo.position->x = vec.x();
76 bo.position->y = vec.y();
77 bo.position->z = vec.z();
78 auto quat = Eigen::Quaternionf(dto.pose.block<3, 3>(0, 0));
79 bo.orientation->qw = quat.w();
80 bo.orientation->qx = quat.x();
81 bo.orientation->qy = quat.y();
82 bo.orientation->qz = quat.z();
83 }
84
85 void
86 toAron(arondto::FramedPose& dto, const armarx::FramedPose& bo)
87 {
88 dto.header.frame = bo.frame;
89 dto.header.agent = bo.agent;
90 dto.pose = Eigen::Matrix4f::Identity();
91 Eigen::Vector3f vec;
92 vec.x() = bo.position->x;
93 vec.y() = bo.position->y;
94 vec.z() = bo.position->z;
95 Eigen::Quaternionf quat;
96 quat.w() = bo.orientation->qw;
97 quat.x() = bo.orientation->qx;
98 quat.y() = bo.orientation->qy;
99 quat.z() = bo.orientation->qz;
100 dto.pose.block<3, 1>(0, 3) = vec;
101 dto.pose.block<3, 3>(0, 0) = quat.toRotationMatrix();
102 }
103} // namespace armarx::aron::framed
framed.h
armarx::FramedDirection
FramedDirection is a 3 dimensional direction vector with a reference frame.
Definition FramedPose.h:87
armarx::FramedOrientation
The FramedOrientation class.
Definition FramedPose.h:216
armarx::FramedPose
The FramedPose class.
Definition FramedPose.h:281
armarx::FramedPosition
The FramedPosition class.
Definition FramedPose.h:158
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition EigenForwardDeclarations.h:61
armarx::aron::framed::toAron
void toAron(arondto::FramedPosition &dto, const armarx::FramedPosition &bo)
Definition framed.cpp:18
armarx::aron::framed::fromAron
void fromAron(const arondto::FramedPosition &dto, armarx::FramedPosition &bo)
Definition framed.cpp:8