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