hpp-pinocchio  4.11.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
serialization.hh
Go to the documentation of this file.
1 //
2 // Copyright (c) 2020 CNRS
3 // Author: Joseph Mirabel
4 //
5 // This file is part of hpp-pinocchio
6 // hpp-pinocchio is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-pinocchio is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-pinocchio If not, see
17 // <http://www.gnu.org/licenses/>.
18 
19 #ifndef HPP_PINOCCHIO_SERIALIZATION_HH
20 # define HPP_PINOCCHIO_SERIALIZATION_HH
21 
22 # include <set>
23 # include <type_traits>
24 
25 # include <pinocchio/fwd.hpp>
26 # include <boost/serialization/split_free.hpp>
27 # include <boost/serialization/shared_ptr.hpp>
28 # include <boost/serialization/weak_ptr.hpp>
29 
30 # include <pinocchio/serialization/eigen.hpp>
31 # include <hpp/util/serialization.hh>
32 # include <hpp/pinocchio/fwd.hh>
33 # include <hpp/pinocchio/device.hh>
35 
36 BOOST_SERIALIZATION_SPLIT_FREE(hpp::pinocchio::DevicePtr_t)
37 BOOST_SERIALIZATION_SPLIT_FREE(hpp::pinocchio::DeviceWkPtr_t)
38 
39 namespace boost {
40 namespace serialization {
41 template<class Archive>
42 inline void load (Archive& ar, hpp::pinocchio::DevicePtr_t& d, const unsigned int version)
43 {
44  load<Archive, hpp::pinocchio::Device> (ar, d, version);
45  auto* har = hpp::serialization::cast(&ar);
46  if (d && har && har->contains(d->name()))
47  d = har->template get<hpp::pinocchio::Device>(d->name(), true)->self();
48 }
49 template<class Archive>
50 inline void load (Archive& ar, hpp::pinocchio::DeviceWkPtr_t& d, const unsigned int version)
51 {
52  load<Archive, hpp::pinocchio::Device> (ar, d, version);
53  auto* har = hpp::serialization::cast(&ar);
54  std::string name (d.lock()->name());
55  if (d.lock() && har && har->contains(name))
56  d = har->template get<hpp::pinocchio::Device>(name, true)->self();
57 }
58 template<class Archive>
59 inline void load (Archive& ar, hpp::pinocchio::HumanoidRobotPtr_t& d, const unsigned int version)
60 {
61  load<Archive, hpp::pinocchio::HumanoidRobot> (ar, d, version);
62  auto* har = hpp::serialization::cast(&ar);
63  if (d && har && har->contains(d->name()))
64  d = har->template getChildClass<hpp::pinocchio::Device, hpp::pinocchio::HumanoidRobot>(d->name(), true)->self();
65 }
66 template<class Archive>
67 inline void load (Archive& ar, hpp::pinocchio::HumanoidRobotWkPtr_t& d, const unsigned int version)
68 {
69  load<Archive, hpp::pinocchio::HumanoidRobot> (ar, d, version);
70  auto* har = hpp::serialization::cast(&ar);
71  std::string name (d.lock()->name());
72  if (d.lock() && har && har->contains(name))
73  d = har->template getChildClass<hpp::pinocchio::Device, hpp::pinocchio::HumanoidRobot>(name, true)->self();
74 }
75 
76 template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
77 inline void serialize(Archive & ar, Eigen::Array<_Scalar,_Rows,_Cols,_Options,_MaxRows,_MaxCols> & m, const unsigned int version)
78 {
79  (void) version;
80  Eigen::DenseIndex rows(m.rows()), cols(m.cols());
81  ar & BOOST_SERIALIZATION_NVP(rows);
82  ar & BOOST_SERIALIZATION_NVP(cols);
83  if (!Archive::is_saving::value)
84  m.resize(rows,cols);
85  if(m.size() > 0)
86  ar & make_nvp("data",make_array(m.data(), (size_t)m.size()));
87 }
88 
89 } // namespace serialization
90 } // namespace boost
91 
92 namespace hpp {
93 namespace serialization {
94 namespace remove_duplicate {
95 template<typename Key, typename Compare = std::less<Key> >
96 struct ptr_less : Compare {
97  inline bool operator() (Key const* t1, Key const* t2) const { return Compare::operator() (*t1, *t2); }
98 };
99 
100 template<typename Derived>
102  bool operator() (const Eigen::PlainObjectBase<Derived>& a,
103  const Eigen::PlainObjectBase<Derived>& b) const
104  {
105  if (a.size() < b.size()) return true;
106  if (a.size() > b.size()) return false;
107  for(Eigen::Index i = 0; i < a.size(); ++i) {
108  if (a.derived().data()[i] < b.derived().data()[i]) return true;
109  if (a.derived().data()[i] > b.derived().data()[i]) return false;
110  }
111  return false;
112  }
113 };
114 
115 template<typename Key, typename Compare = std::less<Key> >
116 struct archive {
117  typedef Compare compare_type;
119  std::set<Key const*, ptr_compare_type > datas;
120  int hitcount;
121 
122  archive() : hitcount(0) {}
123 };
124 
126 
127 template<class Archive, typename Key>
128 inline void load_or_save_no_remove_duplicate_check (Archive& ar,
129  const char* name,
130  Key& key,
131  const unsigned int version)
132 {
133  (void) version;
134  Key* value = &key;
135  ar & boost::serialization::make_nvp(name, value);
136  if (!Archive::is_saving::value) key = *value;
137 }
138 
139 template<class Archive, typename Key>
140 inline void save_impl (Archive& ar,
141  const char* name,
142  const Key& key,
143  const unsigned int version)
144 {
145  (void) version;
146  Key const* value = &key;
147  ar << boost::serialization::make_nvp(name, value);
148 }
149 
150 template<class Archive, typename Key, typename Compare = std::less<Key>>
151 inline void save_impl (Archive& ar,
152  std::set<Key const*, ptr_less<Key,Compare> >& set,
153  int& hitcount,
154  const char* name,
155  const Key& key,
156  const unsigned int version)
157 {
158  (void) version;
159  if(!Archive::is_saving::value)
160  throw std::logic_error("HPP serialization: cannot load into a const element. This should never happen.");
161  auto result = set.insert(&key);
162  bool inserted = result.second;
163  Key const* k = (inserted ? &key : *result.first);
164  ar & boost::serialization::make_nvp(name, k);
165  if (!inserted) hitcount++;
166 }
167 
168 template<class Archive, typename Key, typename Compare = std::less<Key>>
169 inline void serialize (Archive& ar,
170  std::set<Key const*, ptr_less<Key,Compare> >& set,
171  int& hitcount,
172  const char* name,
173  Key& key,
174  const unsigned int version)
175 {
176  if (Archive::is_saving::value) {
177  save_impl(ar, set, hitcount, name, key, version);
178  } else {
179  load_or_save_no_remove_duplicate_check(ar, name, key, version);
180  }
181 }
182 
183 template <bool is_base>
185 template<typename Archive, typename Key, typename Compare = std::less<Key> >
186 static inline void run (Archive& ar,
187  const char* name,
188  Key& key,
189  const unsigned int version)
190 {
191  archive<Key, Compare>& rda = dynamic_cast<archive<Key, Compare>&>(ar);
192  serialize(ar, rda.datas, rda.hitcount, name, key, version);
193 }
194 template<typename Archive, typename Key, typename Compare = std::less<Key> >
195 static inline void save (Archive& ar,
196  const char* name,
197  const Key& key,
198  const unsigned int version)
199 {
200  archive<Key, Compare>& rda = dynamic_cast<archive<Key, Compare>&>(ar);
201  save_impl(ar, rda.datas, rda.hitcount, name, key, version);
202 }
203 };
204 
205 template <>
206 struct serialiaze_impl<false> {
207 template<typename Archive, typename Key, typename Compare = std::less<Key> >
208 static inline void run (Archive& ar,
209  const char* name,
210  Key& key,
211  const unsigned int version)
212 {
213  if (dynamic_cast<archive<Key, Compare>*>(&ar) != NULL)
214  serialiaze_impl<true>::run<Archive, Key, Compare>(ar, name, key, version);
215  else
216  load_or_save_no_remove_duplicate_check(ar, name, key, version);
217 }
218 template<typename Archive, typename Key, typename Compare = std::less<Key> >
219 static inline void save (Archive& ar,
220  const char* name,
221  const Key& key,
222  const unsigned int version)
223 {
224  if (dynamic_cast<archive<Key, Compare>*>(&ar) != NULL)
225  serialiaze_impl<true>::save<Archive, Key, Compare>(ar, name, key, version);
226  else
227  save_impl(ar, name, key, version);
228 }
229 };
230 
231 template<typename Archive, typename Key, typename Compare = std::less<Key>,
232  bool is_base = std::is_base_of<archive<Key, Compare>, Archive>::value >
233 inline void serialize (Archive& ar,
234  const char* name,
235  Key& key,
236  const unsigned int version)
237 {
238  serialiaze_impl<is_base>::template run<Archive, Key, Compare>(ar, name, key, version);
239 }
240 
241 template<typename Archive, typename Key, typename Compare = std::less<Key>,
242  bool is_base = std::is_base_of<archive<Key, Compare>, Archive>::value >
243 inline void save (Archive& ar,
244  const char* name,
245  const Key& key,
246  const unsigned int version)
247 {
248  serialiaze_impl<is_base>::template save<Archive, Key, Compare>(ar, name, key, version);
249 }
250 
251 template<typename Archive>
252 inline void serialize_vector (Archive& ar,
253  const char* name,
255  const unsigned int version)
256 {
257  serialize<Archive, ::hpp::pinocchio::vector_t, vector_archive::compare_type>
258  (ar, name, key, version);
259 }
260 
261 template<typename Archive>
262 inline void save_vector (Archive& ar,
263  const char* name,
265  const unsigned int version)
266 {
267  save<Archive, ::hpp::pinocchio::vector_t, vector_archive::compare_type>
268  (ar, name, key, version);
269 }
270 
271 } // namespace remove_duplicate
272 } // namespace pinocchio
273 } // namespace hpp
274 
275 #endif // HPP_PINOCCHIO_SERIALIZATION_HH
void save_impl(Archive &ar, std::set< Key const *, ptr_less< Key, Compare > > &set, int &hitcount, const char *name, const Key &key, const unsigned int version)
Definition: serialization.hh:151
Definition: serialization.hh:25
ptr_less< Key, Compare > ptr_compare_type
Definition: serialization.hh:118
void serialize_vector(Archive &ar, const char *name, ::hpp::pinocchio::vector_t &key, const unsigned int version)
Definition: serialization.hh:252
Utility functions.
Definition: body.hh:30
std::set< Key const *, ptr_compare_type > datas
Definition: serialization.hh:119
static void save(Archive &ar, const char *name, const Key &key, const unsigned int version)
Definition: serialization.hh:195
void serialize(Archive &ar, hpp::pinocchio::liegroup::VectorSpaceOperation< Size, rot > &lg, const unsigned int version)
Definition: serialization.hh:29
Definition: serialization.hh:116
static void run(Archive &ar, const char *name, Key &key, const unsigned int version)
Definition: serialization.hh:208
Compare compare_type
Definition: serialization.hh:117
shared_ptr< Device > DevicePtr_t
Definition: fwd.hh:106
static void run(Archive &ar, const char *name, Key &key, const unsigned int version)
Definition: serialization.hh:186
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition: fwd.hh:75
int hitcount
Definition: serialization.hh:120
void save(Archive &ar, const char *name, const Key &key, const unsigned int version)
Definition: serialization.hh:243
shared_ptr< HumanoidRobot > HumanoidRobotPtr_t
Definition: fwd.hh:109
void save_vector(Archive &ar, const char *name, const ::hpp::pinocchio::vector_t &key, const unsigned int version)
Definition: serialization.hh:262
static void save(Archive &ar, const char *name, const Key &key, const unsigned int version)
Definition: serialization.hh:219
Definition: serialization.hh:96
archive<::hpp::pinocchio::vector_t, eigen_compare<::hpp::pinocchio::vector_t > > vector_archive
Definition: serialization.hh:125
void load_or_save_no_remove_duplicate_check(Archive &ar, const char *name, Key &key, const unsigned int version)
Definition: serialization.hh:128
void load(Archive &ar, hpp::pinocchio::DevicePtr_t &d, const unsigned int version)
Definition: serialization.hh:42
archive()
Definition: serialization.hh:122