serialization: add serialization of JointConstraint

src/serialization/joints-constraint.hpp

+//
+// Copyright (c) 2019 INRIA
+//
+
+#ifndef __pinocchio_serialization_joints_constraint_hpp__
+#define __pinocchio_serialization_joints_constraint_hpp__
+
+#include "pinocchio/serialization/fwd.hpp"
+
+#include <boost/serialization/split_free.hpp>
+#include <boost/serialization/vector.hpp>
+
+namespace boost
+{
+  namespace serialization
+  {
+    
+    template <class Archive, typename Scalar, int Options, int axis>
+    void serialize(Archive & /*ar*/,
+                   pinocchio::ConstraintRevoluteTpl<Scalar,Options,axis> & /*S*/,
+                   const unsigned int /*version*/)
+    {}
+  
+    template <class Archive, typename Scalar, int Options, int axis>
+    void serialize(Archive & /*ar*/,
+                   pinocchio::ConstraintPrismaticTpl<Scalar,Options,axis> & /*S*/,
+                   const unsigned int /*version*/)
+    {}
+  
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & /*ar*/,
+                   pinocchio::ConstraintSphericalTpl<Scalar,Options> & /*S*/,
+                   const unsigned int /*version*/)
+    {}
+  
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & /*ar*/,
+                   pinocchio::ConstraintTranslationTpl<Scalar,Options> & /*S*/,
+                   const unsigned int /*version*/)
+    {}
+  
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & /*ar*/,
+                   pinocchio::ConstraintIdentityTpl<Scalar,Options> & /*S*/,
+                   const unsigned int /*version*/)
+    {}
+  
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & ar,
+                   pinocchio::ConstraintRevoluteUnalignedTpl<Scalar,Options> & S,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("axis",S.axis());
+    }
+  
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & ar,
+                   pinocchio::ConstraintPrismaticUnalignedTpl<Scalar,Options> & S,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("axis",S.axis());
+    }
+  
+    template <class Archive, int Dim, typename Scalar, int Options>
+    void serialize(Archive & ar,
+                   pinocchio::ConstraintTpl<Dim,Scalar,Options> & S,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("matrix",S.matrix());
+    }
+  
+    template <class Archive, class Constraint>
+    void serialize(Archive & ar,
+                   pinocchio::ScaledConstraint<Constraint> & S,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("scaling",S.scaling());
+      ar & make_nvp("constraint",S.constraint());
+    }
+  
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & /*ar*/,
+                   pinocchio::ConstraintPlanarTpl<Scalar,Options> & /*S*/,
+                   const unsigned int /*version*/)
+    {}
+  
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & ar,
+                   pinocchio::ConstraintSphericalZYXTpl<Scalar,Options> & S,
+                   const unsigned int /*version*/)
+    {
+      ar & make_nvp("angularSubspace",S.angularSubspace());
+    }
+    
+  }
+}
+
+#endif // ifndef __pinocchio_serialization_joints_constraint_hpp__
+

src/serialization/joints.hpp

@@ -20,6 +20,7 @@
 
 #include "pinocchio/serialization/joints-transform.hpp"
 #include "pinocchio/serialization/joints-motion.hpp"
+#include "pinocchio/serialization/joints-constraint.hpp"
 #include "pinocchio/serialization/joints-model.hpp"
 
 #endif // ifndef __pinocchio_serialization_joints_hpp__

unittest/serialization.cpp

@@ -316,6 +316,7 @@ struct TestJointTransform
   {
     typedef typename JointModel::JointDerived JointDerived;
     typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
+    typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint;
     typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointData;
     typedef pinocchio::JointDataBase<JointData> JointDataBase;
     JointModel jmodel = init<JointModel>::run();
@@ -334,6 +335,9 @@ struct TestJointTransform
     jmodel.calc(jdata,q_random);
     Transform & m = jdata_base.M();
     test(m);
+    
+    Constraint & S = jdata_base.S();
+    test(S);
   }
   
   template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
@@ -348,6 +352,7 @@ struct TestJointTransform
     typedef pinocchio::JointModelMimic<JointModel> JointModelMimic;
     typedef typename JointModelMimic::JointDerived JointDerived;
     typedef typename pinocchio::traits<JointDerived>::Transformation_t Transform;
+    typedef typename pinocchio::traits<JointDerived>::Constraint_t Constraint;
     typedef typename pinocchio::traits<JointDerived>::JointDataDerived JointDataMimic;
     typedef pinocchio::JointDataBase<JointDataMimic> JointDataBase;
     JointModelMimic jmodel_mimic = init<JointModelMimic>::run();
@@ -367,6 +372,9 @@ struct TestJointTransform
     jmodel_mimic.calc(jdata_mimic,q_random);
     Transform & m = jdata_mimic_base.M();
     test(m);
+    
+    Constraint & S = jdata_mimic_base.S();
+    test(S);
   }
   
   template<typename Transform>