eigen.h
Go to the documentation of this file.
1#pragma once
2
3#include <Eigen/Core>
4#include <Eigen/Geometry> // for Quaternion
5
6#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
7
8#include <RobotAPI/libraries/aron/codegeneration_util/TypeName.h>
9#include <RobotAPI/libraries/aron/core/Exception.h>
10#include <RobotAPI/libraries/aron/core/data/rw/Reader.h>
11#include <RobotAPI/libraries/aron/core/data/rw/Writer.h>
12#include <RobotAPI/libraries/aron/core/type/rw/Writer.h>
13
14namespace armarx
15{
16 // Helper methods for code generation to convert json/aron to bo and vice versa
17 namespace aron
18 {
19 template <class ReaderT, class EigenT, int rows, int cols, int options>
20 requires data::isReader<ReaderT>
21
22 void
23 read(ReaderT& aron_r,
24 typename ReaderT::InputType& input,
25 Eigen::Matrix<EigenT, rows, cols, options>& ret)
26 {
27 std::string typeAsString;
28 std::vector<int> shape;
29 std::vector<unsigned char> data;
30 aron_r.readNDArray(input, shape, typeAsString, data);
31
32 std::stringstream ss;
33 ss << "Received wrong dimensions for matrix member. Dimensions are (" << shape.at(0)
34 << "," << shape.at(1) << ") but should be (" << ret.rows() << "," << ret.cols()
35 << ").";
36
37 ARMARX_CHECK_AND_THROW(
38 typeAsString == TypeName<EigenT>::Get(),
39 ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
40 "Received wrong typename",
41 typeAsString,
42 TypeName<EigenT>::Get()));
43 ARMARX_CHECK_AND_THROW(
44 ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
45 ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, ss.str()));
46
47 std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
48 }
49
50 template <class ReaderT, class EigenT, int cols, int options>
51 requires data::isReader<ReaderT>
52
53 void
54 read(ReaderT& aron_r,
55 typename ReaderT::InputType& input,
56 Eigen::Matrix<EigenT, Eigen::Dynamic, cols, options>& ret)
57 {
58 std::string typeAsString;
59 std::vector<int> shape;
60 std::vector<unsigned char> data;
61 aron_r.readNDArray(input, shape, typeAsString, data);
62
63 ret.resize(shape.at(0));
64
65 ARMARX_CHECK_AND_THROW(
66 ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
67 ::armarx::aron::error::AronException(
68 __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
69 ARMARX_CHECK_AND_THROW(
70 typeAsString == TypeName<EigenT>::Get(),
71 ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
72 "Received wrong typename",
73 typeAsString,
74 TypeName<EigenT>::Get()));
75 std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
76 }
77
78 template <class ReaderT, class EigenT, int rows, int options>
79 requires data::isReader<ReaderT>
80
81 void
82 read(ReaderT& aron_r,
83 typename ReaderT::InputType& input,
84 Eigen::Matrix<EigenT, rows, Eigen::Dynamic, options>& ret)
85 {
86 std::string typeAsString;
87 std::vector<int> shape;
88 std::vector<unsigned char> data;
89 aron_r.readNDArray(input, shape, typeAsString, data);
90
91 ret.resize(shape.at(1));
92
93 ARMARX_CHECK_AND_THROW(
94 ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
95 ::armarx::aron::error::AronException(
96 __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
97 ARMARX_CHECK_AND_THROW(
98 typeAsString == TypeName<EigenT>::Get(),
99 ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
100 "Received wrong typename",
101 typeAsString,
102 TypeName<EigenT>::Get()));
103 std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
104 }
105
106 template <class ReaderT, class EigenT, int options>
107 requires data::isReader<ReaderT>
108
109 void
110 read(ReaderT& aron_r,
111 typename ReaderT::InputType& input,
112 Eigen::Matrix<EigenT, Eigen::Dynamic, Eigen::Dynamic, options>& ret)
113 {
114 std::string typeAsString;
115 std::vector<int> shape;
116 std::vector<unsigned char> data;
117 aron_r.readNDArray(input, shape, typeAsString, data);
118
119 ret.resize(shape.at(0), shape.at(1));
120
121 ARMARX_CHECK_AND_THROW(
122 ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
123 ::armarx::aron::error::AronException(
124 __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
125 ARMARX_CHECK_AND_THROW(
126 typeAsString == TypeName<EigenT>::Get(),
127 ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
128 "Received wrong typename",
129 typeAsString,
130 TypeName<EigenT>::Get()));
131 std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
132 }
133
134 template <class WriterT, class EigenT, int rows, int cols, int options>
135 requires data::isWriter<WriterT>
136
137 void
138 write(WriterT& aron_w,
139 const Eigen::Matrix<EigenT, rows, cols, options>& input,
140 typename WriterT::ReturnType& ret,
141 const armarx::aron::Path& aron_p = armarx::aron::Path())
142 {
143 ret = aron_w.writeNDArray(
144 {static_cast<int>(input.rows()), static_cast<int>(input.cols()), sizeof(EigenT)},
145 TypeName<EigenT>::Get(),
146 reinterpret_cast<const unsigned char*>(input.data()),
147 aron_p);
148 }
149
150 template <class ReaderT, class EigenT>
151 requires data::isReader<ReaderT>
152
153 void
154 read(ReaderT& aron_r, typename ReaderT::InputType& input, Eigen::Quaternion<EigenT>& ret)
155 {
156 std::string typeAsString;
157 std::vector<int> shape;
158 std::vector<unsigned char> data;
159 aron_r.readNDArray(input, shape, typeAsString, data);
160 std::memcpy(
161 reinterpret_cast<unsigned char*>(ret.coeffs().data()), data.data(), data.size());
162 }
163
164 template <class WriterT, class EigenT>
165 requires data::isWriter<WriterT>
166
167 void
168 write(WriterT& aron_w,
169 const Eigen::Quaternion<EigenT>& input,
170 typename WriterT::ReturnType& ret,
171 const armarx::aron::Path& aron_p = armarx::aron::Path())
172 {
173 ret = aron_w.writeNDArray({1, 4, sizeof(EigenT)},
174 TypeName<EigenT>::Get(),
175 reinterpret_cast<const unsigned char*>(input.coeffs().data()),
176 aron_p);
177 }
178 } // namespace aron
179
180} // namespace armarx
armarx::aron::Path
The Path class.
Definition Path.h:36
armarx::aron::error::AronException
A base class for aron exceptions.
Definition Exception.h:37
armarx::aron::error::ValueNotValidException
The ValueNotValidException class.
Definition Exception.h:99
ARMARX_CHECK_AND_THROW
#define ARMARX_CHECK_AND_THROW(expression, ExceptionType)
This macro evaluates the expression and if it turns out to be false it will throw an exception of the...
Definition ExpressionException.h:245
armarx::aron::data
A convenience header to include all aron files (full include, not forward declared)
Definition aron_conversions.cpp:4
armarx::aron::read
void read(ReaderT &aron_r, typename ReaderT::InputType &input, Eigen::Matrix< EigenT, rows, cols, options > &ret)
Definition eigen.h:23
armarx::aron::write
void write(WriterT &aron_w, const Eigen::Matrix< EigenT, rows, cols, options > &input, typename WriterT::ReturnType &ret, const armarx::aron::Path &aron_p=armarx::aron::Path())
Definition eigen.h:138
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::aron::TypeName::Get
static const char * Get()
Definition TypeName.h:14