pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
eigen.hpp
1//
2// Copyright (c) 2017-2020 CNRS INRIA
3//
4
5#ifndef __pinocchio_serialization_eigen_matrix_hpp__
6#define __pinocchio_serialization_eigen_matrix_hpp__
7
8#include <Eigen/Dense>
9#include "pinocchio/math/tensor.hpp"
10
11#include <boost/serialization/split_free.hpp>
12#include <boost/serialization/vector.hpp>
13#include <boost/serialization/array.hpp>
14
15// If hpp-fcl < 3.0.0 The GCC Eigen/Boost.Serialization workaround
16// is already defined.
17// If we don't link against hpp-fcl or hpp-fcl >= 3.0.0 then we must define
18// the workaround.
19#if defined PINOCCHIO_WITH_HPP_FCL
20 #include <hpp/fcl/config.hh>
21 #if !HPP_FCL_VERSION_AT_LEAST(3, 0, 0) // hpp-fcl < 3.0.0
22 #define HPP_FCL_SKIP_EIGEN_BOOST_SERIALIZATION
23 #include <hpp/fcl/serialization/eigen.h>
24 #else // hpp-fcl >= 3.0.0
25 // Workaround a bug in GCC >= 7 and C++17.
26 // ref. https://gitlab.com/libeigen/eigen/-/issues/1676
27 #ifdef __GNUC__
28 #if __GNUC__ >= 7 && __cplusplus >= 201703L
29namespace boost
30{
31 namespace serialization
32 {
33 struct U;
34 }
35} // namespace boost
36namespace Eigen
37{
38 namespace internal
39 {
40 template<>
41 struct traits<boost::serialization::U>
42 {
43 enum
44 {
45 Flags = 0
46 };
47 };
48 } // namespace internal
49} // namespace Eigen
50 #endif
51 #endif
52 #endif
53#else // !PINOCCHIO_WITH_HPP_FCL
54 // Workaround a bug in GCC >= 7 and C++17.
55 // ref. https://gitlab.com/libeigen/eigen/-/issues/1676
56 #ifdef __GNUC__
57 #if __GNUC__ >= 7 && __cplusplus >= 201703L
58namespace boost
59{
60 namespace serialization
61 {
62 struct U;
63 }
64} // namespace boost
65namespace Eigen
66{
67 namespace internal
68 {
69 template<>
70 struct traits<boost::serialization::U>
71 {
72 enum
73 {
74 Flags = 0
75 };
76 };
77 } // namespace internal
78} // namespace Eigen
79 #endif
80 #endif
81#endif
82
83// Similar workaround but for MSVC when C++17 is enabled.
84// TODO Find _MSC_VER range.
85#if (defined(_MSVC_LANG) && _MSVC_LANG >= 201703)
86namespace boost
87{
88 namespace archive
89 {
90 class binary_iarchive;
91 class xml_iarchive;
92 class text_iarchive;
93 } // namespace archive
94} // namespace boost
95namespace Eigen
96{
97 namespace internal
98 {
99 template<>
100 struct traits<boost::archive::binary_iarchive>
101 {
102 enum
103 {
104 Flags = 0
105 };
106 };
107 template<>
108 struct traits<boost::archive::xml_iarchive>
109 {
110 enum
111 {
112 Flags = 0
113 };
114 };
115 template<>
116 struct traits<boost::archive::text_iarchive>
117 {
118 enum
119 {
120 Flags = 0
121 };
122 };
123 } // namespace internal
124} // namespace Eigen
125#endif // MSVC with C++17
126
127namespace boost
128{
129 namespace serialization
130 {
131
132 template<
133 class Archive,
134 typename Scalar,
135 int Rows,
136 int Cols,
137 int Options,
138 int MaxRows,
139 int MaxCols>
140 void save(
141 Archive & ar,
142 const Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> & m,
143 const unsigned int /*version*/)
144 {
145 Eigen::DenseIndex rows(m.rows()), cols(m.cols());
146 if (Rows == Eigen::Dynamic)
147 ar & BOOST_SERIALIZATION_NVP(rows);
148 if (Cols == Eigen::Dynamic)
149 ar & BOOST_SERIALIZATION_NVP(cols);
150 ar & make_nvp("data", make_array(m.data(), (size_t)m.size()));
151 }
152
153 template<
154 class Archive,
155 typename Scalar,
156 int Rows,
157 int Cols,
158 int Options,
159 int MaxRows,
160 int MaxCols>
161 void load(
162 Archive & ar,
163 Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> & m,
164 const unsigned int /*version*/)
165 {
166 Eigen::DenseIndex rows = Rows, cols = Cols;
167 if (Rows == Eigen::Dynamic)
168 ar >> BOOST_SERIALIZATION_NVP(rows);
169 if (Cols == Eigen::Dynamic)
170 ar >> BOOST_SERIALIZATION_NVP(cols);
171 m.resize(rows, cols);
172 ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
173 }
174
175 template<
176 class Archive,
177 typename Scalar,
178 int Rows,
179 int Cols,
180 int Options,
181 int MaxRows,
182 int MaxCols>
183 void serialize(
184 Archive & ar,
185 Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> & m,
186 const unsigned int version)
187 {
188 split_free(ar, m, version);
189 }
190
191 template<class Archive, typename PlainObjectBase, int MapOptions, typename StrideType>
192 void save(
193 Archive & ar,
194 const Eigen::Map<PlainObjectBase, MapOptions, StrideType> & m,
195 const unsigned int /*version*/)
196 {
197 Eigen::DenseIndex rows(m.rows()), cols(m.cols());
198 if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
199 ar & BOOST_SERIALIZATION_NVP(rows);
200 if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
201 ar & BOOST_SERIALIZATION_NVP(cols);
202 ar & make_nvp("data", make_array(m.data(), (size_t)m.size()));
203 }
204
205 template<class Archive, typename PlainObjectBase, int MapOptions, typename StrideType>
206 void load(
207 Archive & ar,
208 Eigen::Map<PlainObjectBase, MapOptions, StrideType> & m,
209 const unsigned int /*version*/)
210 {
211 Eigen::DenseIndex rows = PlainObjectBase::RowsAtCompileTime,
212 cols = PlainObjectBase::ColsAtCompileTime;
213 if (PlainObjectBase::RowsAtCompileTime == Eigen::Dynamic)
214 ar >> BOOST_SERIALIZATION_NVP(rows);
215 if (PlainObjectBase::ColsAtCompileTime == Eigen::Dynamic)
216 ar >> BOOST_SERIALIZATION_NVP(cols);
217 m.resize(rows, cols);
218 ar >> make_nvp("data", make_array(m.data(), (size_t)m.size()));
219 }
220
221 template<class Archive, typename PlainObjectBase, int MapOptions, typename StrideType>
222 void serialize(
223 Archive & ar,
224 Eigen::Map<PlainObjectBase, MapOptions, StrideType> & m,
225 const unsigned int version)
226 {
227 split_free(ar, m, version);
228 }
229
230#if !defined(PINOCCHIO_WITH_EIGEN_TENSOR_MODULE) \
231 && ((__cplusplus <= 199711L && EIGEN_COMP_MSVC < 1900) || defined(__CUDACC__) || defined(EIGEN_AVOID_STL_ARRAY))
232 template<class Archive, typename _IndexType, std::size_t _NumIndices>
233 void save(
234 Archive & ar, const Eigen::array<_IndexType, _NumIndices> & a, const unsigned int /*version*/)
235 {
236 ar & make_nvp("array", make_array(&a.front(), _NumIndices));
237 }
238
239 template<class Archive, typename _IndexType, std::size_t _NumIndices>
240 void
241 load(Archive & ar, Eigen::array<_IndexType, _NumIndices> & a, const unsigned int /*version*/)
242 {
243 ar >> make_nvp("array", make_array(&a.front(), _NumIndices));
244 }
245
246 template<class Archive, typename _IndexType, std::size_t _NumIndices>
247 void
248 serialize(Archive & ar, Eigen::array<_IndexType, _NumIndices> & a, const unsigned int version)
249 {
250 split_free(ar, a, version);
251 }
252#else
253 template<class Archive, class T, std::size_t N>
254 void save(Archive & ar, const std::array<T, N> & a, const unsigned int version)
255 {
256 typedef std::array<T, N> Array;
257 serialize(ar, const_cast<Array &>(a), version);
258 }
259
260 template<class Archive, class T, std::size_t N>
261 void load(Archive & ar, std::array<T, N> & a, const unsigned int version)
262 {
263 serialize(ar, a, version);
264 }
265#endif
266
267#ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
268
269 template<class Archive, typename _IndexType, int _NumIndices>
270 void save(
271 Archive & ar, const Eigen::DSizes<_IndexType, _NumIndices> & ds, const unsigned int version)
272 {
273 save(ar, static_cast<const Eigen::array<_IndexType, _NumIndices> &>(ds), version);
274 }
275
276 template<class Archive, typename _IndexType, int _NumIndices>
277 void load(Archive & ar, Eigen::DSizes<_IndexType, _NumIndices> & ds, const unsigned int version)
278 {
279 load(ar, static_cast<Eigen::array<_IndexType, _NumIndices> &>(ds), version);
280 }
281
282 template<class Archive, typename _IndexType, int _NumIndices>
283 void
284 serialize(Archive & ar, Eigen::DSizes<_IndexType, _NumIndices> & ds, const unsigned int version)
285 {
286 split_free(ar, static_cast<Eigen::array<_IndexType, _NumIndices> &>(ds), version);
287 }
288
289#endif
290
291 template<class Archive, typename _Scalar, int _NumIndices, int _Options, typename _IndexType>
292 void save(
293 Archive & ar,
294 const ::pinocchio::Tensor<_Scalar, _NumIndices, _Options, _IndexType> & t,
295 const unsigned int /*version*/)
296 {
297 typedef ::pinocchio::Tensor<_Scalar, _NumIndices, _Options, _IndexType> Tensor;
298 const typename Tensor::Dimensions & dimensions = t.dimensions();
299
300 ar & BOOST_SERIALIZATION_NVP(dimensions);
301 ar & make_nvp("data", make_array(t.data(), (size_t)t.size()));
302 }
303
304 template<class Archive, typename _Scalar, int _NumIndices, int _Options, typename _IndexType>
305 void load(
306 Archive & ar,
307 ::pinocchio::Tensor<_Scalar, _NumIndices, _Options, _IndexType> & t,
308 const unsigned int /*version*/)
309 {
310 typedef ::pinocchio::Tensor<_Scalar, _NumIndices, _Options, _IndexType> Tensor;
311 typename Tensor::Dimensions dimensions;
312
313 ar >> BOOST_SERIALIZATION_NVP(dimensions);
314 t.resize(dimensions);
315
316 ar >> make_nvp("data", make_array(t.data(), (size_t)t.size()));
317 }
318
319 template<class Archive, typename _Scalar, int _NumIndices, int _Options, typename _IndexType>
320 void serialize(
321 Archive & ar,
322 ::pinocchio::Tensor<_Scalar, _NumIndices, _Options, _IndexType> & t,
323 const unsigned int version)
324 {
325 split_free(ar, t, version);
326 }
327
328 } // namespace serialization
329} // namespace boost
330
331#endif // ifndef __pinocchio_serialization_eigen_matrix_hpp__