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  std::stringstream ss;
30  ss << "Received wrong dimensions for matrix member. Dimensions are " << shape.at(0)
31  << "," << shape.at(1) << " but should be " << ret.rows() << "/" << ret.cols();
32 
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  ARMARX_CHECK_AND_THROW(
40  ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
41  ::armarx::aron::error::AronException(__PRETTY_FUNCTION__, ss.str()));
42 
43  std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
44  }
45 
46  template <class ReaderT, class EigenT, int cols, int options>
47  requires data::isReader<ReaderT>
48 
49  void
50  read(ReaderT& aron_r,
51  typename ReaderT::InputType& input,
52  Eigen::Matrix<EigenT, Eigen::Dynamic, cols, options>& ret)
53  {
54  std::string typeAsString;
55  std::vector<int> shape;
56  std::vector<unsigned char> data;
57  aron_r.readNDArray(input, shape, typeAsString, data);
58 
59  ret.resize(shape.at(0));
60 
61  ARMARX_CHECK_AND_THROW(
62  ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
63  ::armarx::aron::error::AronException(
64  __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
65  ARMARX_CHECK_AND_THROW(
66  typeAsString == TypeName<EigenT>::Get(),
67  ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
68  "Received wrong typename",
69  typeAsString,
70  TypeName<EigenT>::Get()));
71  std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
72  }
73 
74  template <class ReaderT, class EigenT, int rows, int options>
75  requires data::isReader<ReaderT>
76 
77  void
78  read(ReaderT& aron_r,
79  typename ReaderT::InputType& input,
80  Eigen::Matrix<EigenT, rows, Eigen::Dynamic, options>& ret)
81  {
82  std::string typeAsString;
83  std::vector<int> shape;
84  std::vector<unsigned char> data;
85  aron_r.readNDArray(input, shape, typeAsString, data);
86 
87  ret.resize(shape.at(1));
88 
89  ARMARX_CHECK_AND_THROW(
90  ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
91  ::armarx::aron::error::AronException(
92  __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
93  ARMARX_CHECK_AND_THROW(
94  typeAsString == TypeName<EigenT>::Get(),
95  ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
96  "Received wrong typename",
97  typeAsString,
98  TypeName<EigenT>::Get()));
99  std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
100  }
101 
102  template <class ReaderT, class EigenT, int options>
103  requires data::isReader<ReaderT>
104 
105  void
106  read(ReaderT& aron_r,
107  typename ReaderT::InputType& input,
108  Eigen::Matrix<EigenT, Eigen::Dynamic, Eigen::Dynamic, options>& ret)
109  {
110  std::string typeAsString;
111  std::vector<int> shape;
112  std::vector<unsigned char> data;
113  aron_r.readNDArray(input, shape, typeAsString, data);
114 
115  ret.resize(shape.at(0), shape.at(1));
116 
117  ARMARX_CHECK_AND_THROW(
118  ret.rows() == shape.at(0) and ret.cols() == shape.at(1),
119  ::armarx::aron::error::AronException(
120  __PRETTY_FUNCTION__, "Received wrong dimensions for member 'pose'."));
121  ARMARX_CHECK_AND_THROW(
122  typeAsString == TypeName<EigenT>::Get(),
123  ::armarx::aron::error::ValueNotValidException(__PRETTY_FUNCTION__,
124  "Received wrong typename",
125  typeAsString,
126  TypeName<EigenT>::Get()));
127  std::memcpy(reinterpret_cast<unsigned char*>(ret.data()), data.data(), data.size());
128  }
129 
130  template <class WriterT, class EigenT, int rows, int cols, int options>
131  requires data::isWriter<WriterT>
132 
133  void
134  write(WriterT& aron_w,
135  const Eigen::Matrix<EigenT, rows, cols, options>& input,
136  typename WriterT::ReturnType& ret,
137  const armarx::aron::Path& aron_p = armarx::aron::Path())
138  {
139  ret = aron_w.writeNDArray(
140  {static_cast<int>(input.rows()), static_cast<int>(input.cols()), sizeof(EigenT)},
141  TypeName<EigenT>::Get(),
142  reinterpret_cast<const unsigned char*>(input.data()),
143  aron_p);
144  }
145 
146  template <class ReaderT, class EigenT>
147  requires data::isReader<ReaderT>
148 
149  void
150  read(ReaderT& aron_r, typename ReaderT::InputType& input, Eigen::Quaternion<EigenT>& ret)
151  {
152  std::string typeAsString;
153  std::vector<int> shape;
154  std::vector<unsigned char> data;
155  aron_r.readNDArray(input, shape, typeAsString, data);
156  std::memcpy(
157  reinterpret_cast<unsigned char*>(ret.coeffs().data()), data.data(), data.size());
158  }
159 
160  template <class WriterT, class EigenT>
161  requires data::isWriter<WriterT>
162 
163  void
164  write(WriterT& aron_w,
165  const Eigen::Quaternion<EigenT>& input,
166  typename WriterT::ReturnType& ret,
167  const armarx::aron::Path& aron_p = armarx::aron::Path())
168  {
169  ret = aron_w.writeNDArray({1, 4, sizeof(EigenT)},
170  TypeName<EigenT>::Get(),
171  reinterpret_cast<const unsigned char*>(input.coeffs().data()),
172  aron_p);
173  }
174  } // namespace aron
175 
176 } // 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
armarx::aron::requires
requires(!aron::detail::DtoAndBoAreSame< DtoT, BoT >) void toAron(std
Definition: aron_conversions.h:127
TypeName.h
Exception.h
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:134
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