pinocchio  2.2.1-dirty
serialization/joints.hpp
1 //
2 // Copyright (c) 2019 INRIA
3 //
4 
5 #ifndef __pinocchio_serialization_joints_hpp__
6 #define __pinocchio_serialization_joints_hpp__
7 
8 #include "pinocchio/multibody/joint/joints.hpp"
9 #include "pinocchio/multibody/joint/joint-generic.hpp"
10 #include "pinocchio/multibody/joint/joint-composite.hpp"
11 #include "pinocchio/multibody/joint/joint-collection.hpp"
12 
13 #include "pinocchio/serialization/fwd.hpp"
14 #include "pinocchio/serialization/eigen.hpp"
15 #include "pinocchio/serialization/vector.hpp"
16 #include "pinocchio/serialization/aligned-vector.hpp"
17 
18 #include <boost/serialization/split_free.hpp>
19 #include <boost/serialization/variant.hpp>
20 
21 namespace pinocchio
22 {
23  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
24  struct Serialize< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
25  {
26  template<typename Archive>
27  static void run(Archive & ar,
29  {
30  using boost::serialization::make_nvp;
31 
32  ar & make_nvp("m_nq",joint.m_nq);
33  ar & make_nvp("m_nv",joint.m_nv);
34  ar & make_nvp("m_idx_q",joint.m_idx_q);
35  ar & make_nvp("m_nqs",joint.m_nqs);
36  ar & make_nvp("m_idx_v",joint.m_idx_v);
37  ar & make_nvp("m_nvs",joint.m_nvs);
38  ar & make_nvp("njoints",joint.njoints);
39 
40  ar & make_nvp("joints",joint.joints);
41  ar & make_nvp("jointPlacements",joint.jointPlacements);
42  }
43  };
44 }
45 
46 namespace boost
47 {
48  namespace serialization
49  {
50 
51  // For some older version of gcc, we have to rely on an additional namespace
52  // to avoid ambiguous call to boost::serialization::serialize
53  namespace fix
54  {
55  template <class Archive, typename Derived>
56  void serialize(Archive & ar,
58  const unsigned int version)
59  {
60  split_free(ar, joint, version);
61  }
62  }
63 
64  template<class Archive, typename Derived>
65  void save(Archive & ar,
67  const unsigned int /*version*/)
68  {
69  const pinocchio::JointIndex i_id = joint.id();
70  const int i_q = joint.idx_q(), i_v = joint.idx_v();
71 
72  ar & make_nvp("i_id",i_id);
73  ar & make_nvp("i_q",i_q);
74  ar & make_nvp("i_v",i_v);
75  }
76 
77  template<class Archive, typename Derived>
78  void load(Archive & ar,
80  const unsigned int /*version*/)
81  {
82  pinocchio::JointIndex i_id;
83  int i_q, i_v;
84 
85  ar & make_nvp("i_id",i_id);
86  ar & make_nvp("i_q",i_q);
87  ar & make_nvp("i_v",i_v);
88 
89  joint.setIndexes(i_id,i_q,i_v);
90  }
91 
92  template <class Archive, typename Scalar, int Options, int axis>
93  void serialize(Archive & ar,
95  const unsigned int version)
96  {
98 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
99  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
100  }
101 
102  template <class Archive, typename Scalar, int Options, int axis>
103  void serialize(Archive & ar,
105  const unsigned int version)
106  {
108 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
109  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
110  }
111 
112  template <class Archive, typename Scalar, int Options, int axis>
113  void serialize(Archive & ar,
115  const unsigned int version)
116  {
118 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
119  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
120  }
121 
122  template <class Archive, typename Scalar, int Options>
123  void serialize(Archive & ar,
125  const unsigned int version)
126  {
128 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
129  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
130  }
131 
132  template <class Archive, typename Scalar, int Options>
133  void serialize(Archive & ar,
135  const unsigned int version)
136  {
138 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
139  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
140  }
141 
142  template <class Archive, typename Scalar, int Options>
143  void serialize(Archive & ar,
145  const unsigned int version)
146  {
148 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
149  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
150  }
151 
152  template <class Archive, typename Scalar, int Options>
153  void serialize(Archive & ar,
155  const unsigned int version)
156  {
158 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
159  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
160  }
161 
162  template <class Archive, typename Scalar, int Options>
163  void serialize(Archive & ar,
165  const unsigned int version)
166  {
168 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
169  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
170  }
171 
172  template <class Archive, typename Scalar, int Options>
173  void serialize(Archive & ar,
175  const unsigned int version)
176  {
178 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
179  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
180  ar & make_nvp("axis",joint.axis);
181  }
182 
183  template <class Archive, typename Scalar, int Options>
184  void serialize(Archive & ar,
186  const unsigned int version)
187  {
189  // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
190  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
191  ar & make_nvp("axis",joint.axis);
192  }
193 
194  template <class Archive, typename Scalar, int Options>
195  void serialize(Archive & ar,
197  const unsigned int version)
198  {
200 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
201  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
202  ar & make_nvp("axis",joint.axis);
203  }
204 
205  template <class Archive, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
206  void serialize(Archive & ar,
208  const unsigned int version)
209  {
211 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
212  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
213 
215  }
216 
217  template <class Archive, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
218  void serialize(Archive & ar,
220  const unsigned int version)
221  {
223 // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
224  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
225 
226  typedef typename JointCollectionTpl<Scalar,Options>::JointModelVariant JointModelVariant;
227  ar & make_nvp("base_variant",base_object<JointModelVariant>(joint));
228  }
229 
230  template <class Archive, typename JointModel>
231  void serialize(Archive & ar,
233  const unsigned int version)
234  {
235  typedef pinocchio::JointModelMimic<JointModel> JointType;
236  // ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
237  fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
238 
239  ar & make_nvp("jmodel",joint.jmodel());
240  ar & make_nvp("scaling",joint.scaling());
241  ar & make_nvp("offset",joint.offset());
242  }
243 
244  }
245 }
246 
247 #endif // ifndef __pinocchio_serialization_joints_hpp__
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
std::vector< int > m_idx_v
Index in the tangent vector.
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
std::vector< int > m_nqs
Dimension of the segment in the config vector.
Main pinocchio namespace.
Definition: treeview.dox:24
JointModelVector joints
Vector of joints contained in the joint composite.
container::aligned_vector< SE3 > jointPlacements
Vector of joint placements. Those placements correspond to the origin of the joint relatively to thei...
int m_nq
Dimensions of the config and tangent space of the composite joint.
int njoints
Number of joints contained in the JointModelComposite.