hpp-pinocchio 6.0.0
Wrapping of the kinematic/dynamic chain Pinocchio for HPP.
Loading...
Searching...
No Matches
serialization.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2020 CNRS
3// Author: Joseph Mirabel
4//
5
6// Redistribution and use in source and binary forms, with or without
7// modification, are permitted provided that the following conditions are
8// met:
9//
10// 1. Redistributions of source code must retain the above copyright
11// notice, this list of conditions and the following disclaimer.
12//
13// 2. Redistributions in binary form must reproduce the above copyright
14// notice, this list of conditions and the following disclaimer in the
15// documentation and/or other materials provided with the distribution.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
18// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
20// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
21// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
22// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
23// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
25// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
26// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
28// DAMAGE.
29
30#ifndef HPP_PINOCCHIO_SERIALIZATION_HH
31#define HPP_PINOCCHIO_SERIALIZATION_HH
32
33#include <boost/serialization/shared_ptr.hpp>
34#include <boost/serialization/split_free.hpp>
35#include <boost/serialization/weak_ptr.hpp>
37#include <hpp/pinocchio/fwd.hh>
39#include <hpp/util/serialization.hh>
40#include <pinocchio/fwd.hpp>
41#include <pinocchio/serialization/eigen.hpp>
42#include <set>
43#include <type_traits>
44
45BOOST_SERIALIZATION_SPLIT_FREE(hpp::pinocchio::DevicePtr_t)
46BOOST_SERIALIZATION_SPLIT_FREE(hpp::pinocchio::DeviceWkPtr_t)
47
48namespace boost {
49namespace serialization {
50template <class Archive>
51inline void load(Archive& ar, hpp::pinocchio::DevicePtr_t& d,
52 const unsigned int version) {
53 load<Archive, hpp::pinocchio::Device>(ar, d, version);
54 auto* har = hpp::serialization::cast(&ar);
55 if (d && har && har->contains(d->name()))
56 d = har->template get<hpp::pinocchio::Device>(d->name(), true)->self();
57}
58template <class Archive>
59inline void load(Archive& ar, hpp::pinocchio::DeviceWkPtr_t& d,
60 const unsigned int version) {
61 load<Archive, hpp::pinocchio::Device>(ar, d, version);
62 auto* har = hpp::serialization::cast(&ar);
63 std::string name(d.lock()->name());
64 if (d.lock() && har && har->contains(name))
65 d = har->template get<hpp::pinocchio::Device>(name, true)->self();
66}
67template <class Archive>
68inline void load(Archive& ar, hpp::pinocchio::HumanoidRobotPtr_t& d,
69 const unsigned int version) {
70 load<Archive, hpp::pinocchio::HumanoidRobot>(ar, d, version);
71 auto* har = hpp::serialization::cast(&ar);
72 if (d && har && har->contains(d->name()))
73 d = har->template getChildClass<hpp::pinocchio::Device,
75 true)
76 ->self();
77}
78template <class Archive>
79inline void load(Archive& ar, hpp::pinocchio::HumanoidRobotWkPtr_t& d,
80 const unsigned int version) {
81 load<Archive, hpp::pinocchio::HumanoidRobot>(ar, d, version);
82 auto* har = hpp::serialization::cast(&ar);
83 std::string name(d.lock()->name());
84 if (d.lock() && har && har->contains(name))
85 d = har->template getChildClass<hpp::pinocchio::Device,
87 ->self();
88}
89
90template <class Archive, typename _Scalar, int _Rows, int _Cols, int _Options,
91 int _MaxRows, int _MaxCols>
92inline void serialize(
93 Archive& ar,
94 Eigen::Array<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& m,
95 const unsigned int version) {
96 (void)version;
97 Eigen::DenseIndex rows(m.rows()), cols(m.cols());
98 ar& BOOST_SERIALIZATION_NVP(rows);
99 ar& BOOST_SERIALIZATION_NVP(cols);
100 if (!Archive::is_saving::value) m.resize(rows, cols);
101 if (m.size() > 0)
102 ar& make_nvp("data", make_array(m.data(), (size_t)m.size()));
103}
104
105} // namespace serialization
106} // namespace boost
107
108namespace hpp {
109namespace serialization {
110namespace remove_duplicate {
111template <typename Key, typename Compare = std::less<Key>>
112struct ptr_less : Compare {
113 inline bool operator()(Key const* t1, Key const* t2) const {
114 return Compare::operator()(*t1, *t2);
115 }
116};
117
118template <typename Derived>
120 bool operator()(const Eigen::PlainObjectBase<Derived>& a,
121 const Eigen::PlainObjectBase<Derived>& b) const {
122 if (a.size() < b.size()) return true;
123 if (a.size() > b.size()) return false;
124 for (Eigen::Index i = 0; i < a.size(); ++i) {
125 if (a.derived().data()[i] < b.derived().data()[i]) return true;
126 if (a.derived().data()[i] > b.derived().data()[i]) return false;
127 }
128 return false;
129 }
130};
131
132template <typename Key, typename Compare = std::less<Key>>
133struct archive {
134 typedef Compare compare_type;
136 std::set<Key const*, ptr_compare_type> datas;
138
140};
141
142typedef archive<::hpp::pinocchio::vector_t,
143 eigen_compare<::hpp::pinocchio::vector_t>>
145
146template <class Archive, typename Key>
148 const char* name, Key& key,
149 const unsigned int version) {
150 (void)version;
151 Key* value = &key;
152 ar& boost::serialization::make_nvp(name, value);
153 if (!Archive::is_saving::value) key = *value;
154}
155
156template <class Archive, typename Key>
157inline void save_impl(Archive& ar, const char* name, const Key& key,
158 const unsigned int version) {
159 (void)version;
160 Key const* value = &key;
161 ar << boost::serialization::make_nvp(name, value);
162}
163
164template <class Archive, typename Key, typename Compare = std::less<Key>>
165inline void save_impl(Archive& ar,
166 std::set<Key const*, ptr_less<Key, Compare>>& set,
167 int& hitcount, const char* name, const Key& key,
168 const unsigned int version) {
169 (void)version;
170 if (!Archive::is_saving::value)
171 throw std::logic_error(
172 "HPP serialization: cannot load into a const element. This should "
173 "never happen.");
174 auto result = set.insert(&key);
175 bool inserted = result.second;
176 Key const* k = (inserted ? &key : *result.first);
177 ar& boost::serialization::make_nvp(name, k);
178 if (!inserted) hitcount++;
179}
180
181template <class Archive, typename Key, typename Compare = std::less<Key>>
182inline void serialize(Archive& ar,
183 std::set<Key const*, ptr_less<Key, Compare>>& set,
184 int& hitcount, const char* name, Key& key,
185 const unsigned int version) {
186 if (Archive::is_saving::value) {
187 save_impl(ar, set, hitcount, name, key, version);
188 } else {
189 load_or_save_no_remove_duplicate_check(ar, name, key, version);
190 }
191}
192
193template <bool is_base>
195 template <typename Archive, typename Key, typename Compare = std::less<Key>>
196 static inline void run(Archive& ar, const char* name, Key& key,
197 const unsigned int version) {
198 archive<Key, Compare>& rda = dynamic_cast<archive<Key, Compare>&>(ar);
199 serialize(ar, rda.datas, rda.hitcount, name, key, version);
200 }
201 template <typename Archive, typename Key, typename Compare = std::less<Key>>
202 static inline void save(Archive& ar, const char* name, const Key& key,
203 const unsigned int version) {
204 archive<Key, Compare>& rda = dynamic_cast<archive<Key, Compare>&>(ar);
205 save_impl(ar, rda.datas, rda.hitcount, name, key, version);
206 }
207};
208
209template <>
210struct serialiaze_impl<false> {
211 template <typename Archive, typename Key, typename Compare = std::less<Key>>
212 static inline void run(Archive& ar, const char* name, Key& key,
213 const unsigned int version) {
214 if (dynamic_cast<archive<Key, Compare>*>(&ar) != NULL)
216 else
217 load_or_save_no_remove_duplicate_check(ar, name, key, version);
218 }
219 template <typename Archive, typename Key, typename Compare = std::less<Key>>
220 static inline void save(Archive& ar, const char* name, const Key& key,
221 const unsigned int version) {
222 if (dynamic_cast<archive<Key, Compare>*>(&ar) != NULL)
224 version);
225 else
226 save_impl(ar, name, key, version);
227 }
228};
229
230template <typename Archive, typename Key, typename Compare = std::less<Key>,
231 bool is_base = std::is_base_of<archive<Key, Compare>, Archive>::value>
232inline void serialize(Archive& ar, const char* name, Key& key,
233 const unsigned int version) {
234 serialiaze_impl<is_base>::template run<Archive, Key, Compare>(ar, name, key,
235 version);
236}
237
238template <typename Archive, typename Key, typename Compare = std::less<Key>,
239 bool is_base = std::is_base_of<archive<Key, Compare>, Archive>::value>
240inline void save(Archive& ar, const char* name, const Key& key,
241 const unsigned int version) {
242 serialiaze_impl<is_base>::template save<Archive, Key, Compare>(ar, name, key,
243 version);
244}
245
246template <typename Archive>
247inline void serialize_vector(Archive& ar, const char* name,
249 const unsigned int version) {
250 serialize<Archive, ::hpp::pinocchio::vector_t, vector_archive::compare_type>(
251 ar, name, key, version);
252}
253
254template <typename Archive>
255inline void save_vector(Archive& ar, const char* name,
256 const ::hpp::pinocchio::vector_t& key,
257 const unsigned int version) {
258 save<Archive, ::hpp::pinocchio::vector_t, vector_archive::compare_type>(
259 ar, name, key, version);
260}
261
262} // namespace remove_duplicate
263} // namespace serialization
264} // namespace hpp
265
266#endif // HPP_PINOCCHIO_SERIALIZATION_HH
Robot with geometric and dynamic pinocchio.
Definition device.hh:60
Humanoid robot.
Definition humanoid-robot.hh:44
void serialize(Archive &ar, hpp::pinocchio::liegroup::VectorSpaceOperation< Size, rot > &lg, const unsigned int version)
Definition serialization.hh:40
void load(Archive &ar, hpp::pinocchio::DevicePtr_t &d, const unsigned int version)
Definition serialization.hh:51
Definition serialization.hh:36
shared_ptr< Device > DevicePtr_t
Definition fwd.hh:118
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > vector_t
Definition fwd.hh:88
shared_ptr< HumanoidRobot > HumanoidRobotPtr_t
Definition fwd.hh:121
archive<::hpp::pinocchio::vector_t, eigen_compare<::hpp::pinocchio::vector_t > > vector_archive
Definition serialization.hh:144
void save_impl(Archive &ar, const char *name, const Key &key, const unsigned int version)
Definition serialization.hh:157
void save(Archive &ar, const char *name, const Key &key, const unsigned int version)
Definition serialization.hh:240
void save_vector(Archive &ar, const char *name, const ::hpp::pinocchio::vector_t &key, const unsigned int version)
Definition serialization.hh:255
void serialize(Archive &ar, std::set< Key const *, ptr_less< Key, Compare > > &set, int &hitcount, const char *name, Key &key, const unsigned int version)
Definition serialization.hh:182
void serialize_vector(Archive &ar, const char *name, ::hpp::pinocchio::vector_t &key, const unsigned int version)
Definition serialization.hh:247
void load_or_save_no_remove_duplicate_check(Archive &ar, const char *name, Key &key, const unsigned int version)
Definition serialization.hh:147
Utility functions.
Definition body.hh:39
Definition serialization.hh:133
int hitcount
Definition serialization.hh:137
Compare compare_type
Definition serialization.hh:134
archive()
Definition serialization.hh:139
std::set< Key const *, ptr_compare_type > datas
Definition serialization.hh:136
ptr_less< Key, Compare > ptr_compare_type
Definition serialization.hh:135
bool operator()(const Eigen::PlainObjectBase< Derived > &a, const Eigen::PlainObjectBase< Derived > &b) const
Definition serialization.hh:120
Definition serialization.hh:112
bool operator()(Key const *t1, Key const *t2) const
Definition serialization.hh:113
static void run(Archive &ar, const char *name, Key &key, const unsigned int version)
Definition serialization.hh:212
static void save(Archive &ar, const char *name, const Key &key, const unsigned int version)
Definition serialization.hh:220
static void save(Archive &ar, const char *name, const Key &key, const unsigned int version)
Definition serialization.hh:202
static void run(Archive &ar, const char *name, Key &key, const unsigned int version)
Definition serialization.hh:196