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 
14 namespace 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::error::AronException
A base class for aron exceptions.
Definition: Exception.h:36
Writer.h
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:13
armarx::aron::aron_p
const T WriterT::ReturnType const armarx::aron::Path & aron_p
Definition: rw.h:24
armarx::aron::TypeName
Definition: TypeName.h:11
Reader.h
armarx::aron::Path
The Path class.
Definition: Path.h:35
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:98
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:23
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::aron::input
ReaderT::InputType & input
Definition: rw.h:12
armarx::aron::requires
requires(!aron::detail::DtoAndBoAreSame< DtoT, BoT >) void toAron(std
Definition: aron_conversions.h:129
TypeName.h
ExpressionException.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:138
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:27
armarx::aron::TypeName::Get
static const char * Get()
Definition: TypeName.h:14