Merge pull request #825 from jcarpent/topic/joints

Add JointRevoluteUnboundedUnaligned

bindings/python/algorithm/algorithms.hpp

@@ -5,8 +5,8 @@
 #ifndef __pinocchio_python_algorithm_hpp__
 #define __pinocchio_python_algorithm_hpp__
 
-#include <boost/python.hpp>
 #include "pinocchio/bindings/python/fwd.hpp"
+#include <boost/python.hpp>
 
 namespace pinocchio
 {

bindings/python/parsers/python.hpp

 //
-// Copyright (c) 2016 CNRS
+// Copyright (c) 2016-2019 CNRS INRIA
 //
 
-#ifndef __pinocchio_pythonparser_hpp__
-#define __pinocchio_pythonparser_hpp__
-
-#include <boost/python.hpp>
+#ifndef __pinocchio_python_parser_python_hpp__
+#define __pinocchio_python_parser_python_hpp__
 
 #include "pinocchio/multibody/model.hpp"
 
+#include <boost/python.hpp>
+
 namespace pinocchio
 {
   namespace python
@@ -24,10 +24,12 @@ namespace pinocchio
     /// \returns The model constructed by the Python script.
     ///
     // TODO: look inside the context of Python and find an occurence of object Model
-    Model buildModel(const std::string & filename, const std::string & var_name = "model", bool verbose = false);
+    Model buildModel(const std::string & filename,
+                     const std::string & var_name = "model",
+                     bool verbose = false);
     
   } // namespace python
   
 } // namespace pinocchio
 
-#endif // ifndef __pinocchio_pythonparser_hpp__
+#endif // ifndef __pinocchio_python_parser_python_hpp__

bindings/python/utils/list.hpp

@@ -5,6 +5,8 @@
 #ifndef __pinocchio_python_utils_list_hpp__
 #define __pinocchio_python_utils_list_hpp__
 
+#include "pinocchio/bindings/python/fwd.hpp"
+
 #include <boost/python.hpp>
 #include <vector>
 

cmake @ 40207e1a

-Subproject commit 21b0f597d8953cd6d6daed1e377dd6c34fcef64b
+Subproject commit 40207e1a74854fa99a42304837f826f1692d4440

examples/interpolation-SE3.cpp

@@ -21,4 +21,5 @@ int main ()
     aSE3.interpolate(pose_s,pose_g,0.5f, pole_u);
     std::cout << pole_u << std::endl;
     return 0;
-}
\ No newline at end of file
+}
+

examples/overview-SE3.cpp

 #include <iostream>
 #include "pinocchio/multibody/liegroup/liegroup.hpp"
 
-
-
 using namespace pinocchio;
 int main ()
 { 
@@ -28,4 +26,5 @@ int main ()
     std::cout << pose_check << std::endl;
 
     return 0;
-}
\ No newline at end of file
+}
+

src/algorithm/regressor.hxx

@@ -96,9 +96,11 @@ namespace pinocchio
 
   template<typename MotionVelocity, typename MotionAcceleration, typename OutputType>
   inline void
-  bodyRegressor(const MotionDense<MotionVelocity> & v, const MotionDense<MotionAcceleration> & a, const Eigen::MatrixBase<OutputType> & regressor)
+  bodyRegressor(const MotionDense<MotionVelocity> & v,
+                const MotionDense<MotionAcceleration> & a,
+                const Eigen::MatrixBase<OutputType> & regressor)
   {
-    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(OutputType, params, 6, 10);
+    PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(OutputType, regressor, 6, 10);
 
     typedef typename MotionVelocity::Scalar Scalar;
     enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options };

src/container/boost-container-limits.hpp

+//
+// Copyright (c) 2019 INRIA
+//
+
+#ifndef __pinocchio_container_boost_container_limits_hpp__
+#define __pinocchio_container_boost_container_limits_hpp__
+
+#define PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE_DEFAULT 30
+
+#ifndef PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE
+#define PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE_DEFAULT
+#endif
+
+#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
+
+#ifndef BOOST_MPL_LIMIT_LIST_SIZE
+  #define BOOST_MPL_LIMIT_LIST_SIZE PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE
+#endif
+
+#ifndef BOOST_MPL_LIMIT_VECTOR_SIZE
+  #define BOOST_MPL_LIMIT_VECTOR_SIZE PINOCCHIO_BOOST_MPL_LIMIT_CONTAINER_SIZE
+#endif
+
+#endif // ifndef __pinocchio_container_boost_container_limits_hpp__

src/fwd.hpp

@@ -13,6 +13,8 @@ namespace pinocchio {}
 #include "pinocchio/warning.hpp"
 #include "pinocchio/config.hpp"
 
+#include "pinocchio/container/boost-container-limits.hpp"
+
 #ifdef PINOCCHIO_WITH_CPPAD_SUPPORT
 #include "pinocchio/math/cppad.hpp"
 #endif

src/math/rotation.hpp

+//
+// Copyright (c) 2019 CNRS
+//
+
+#ifndef __pinocchio_math_rotation_hpp__
+#define __pinocchio_math_rotation_hpp__
+
+#include "pinocchio/fwd.hpp"
+#include "pinocchio/math/matrix.hpp"
+#include <Eigen/Core>
+
+namespace pinocchio
+{
+  ///
+  /// \brief Computes a rotation matrix from a vector and values of sin and cos
+  ///        orientations values.
+  ///
+  /// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix
+  ///
+  template<typename Vector3, typename Scalar, typename Matrix3>
+  void toRotationMatrix(const Eigen::MatrixBase<Vector3> & axis,
+                        const Scalar & cos_value, const Scalar & sin_value,
+                        const Eigen::MatrixBase<Matrix3> & res)
+  {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
+    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
+    
+    assert(isUnitary(axis) && "The axis is not unitary.");
+    
+    Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res);
+    Vector3 sin_axis  = sin_value * axis;
+    Vector3 cos1_axis = (Scalar(1)-cos_value) * axis;
+    
+    Scalar tmp;
+    tmp = cos1_axis.x() * axis.y();
+    res_.coeffRef(0,1) = tmp - sin_axis.z();
+    res_.coeffRef(1,0) = tmp + sin_axis.z();
+    
+    tmp = cos1_axis.x() * axis.z();
+    res_.coeffRef(0,2) = tmp + sin_axis.y();
+    res_.coeffRef(2,0) = tmp - sin_axis.y();
+    
+    tmp = cos1_axis.y() * axis.z();
+    res_.coeffRef(1,2) = tmp - sin_axis.x();
+    res_.coeffRef(2,1) = tmp + sin_axis.x();
+    
+    res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
+  }
+}
+
+#endif //#ifndef __pinocchio_math_rotation_hpp__
+

src/multibody/joint/fwd.hpp

 //
-// Copyright (c) 2016 CNRS
+// Copyright (c) 2016-2019 CNRS INRIA
 //
 
 #ifndef __pinocchio_joint_fwd_hpp__
@@ -29,6 +29,11 @@ namespace pinocchio
   template<typename Scalar, int Options = 0> struct JointDataRevoluteUnalignedTpl;
   typedef JointDataRevoluteUnalignedTpl<double> JointDataRevoluteUnaligned;
 
+  template<typename Scalar, int Options = 0> struct JointModelRevoluteUnboundedUnalignedTpl;
+  typedef JointModelRevoluteUnboundedUnalignedTpl<double> JointModelRevoluteUnboundedUnaligned;
+  template<typename Scalar, int Options = 0> struct JointDataRevoluteUnboundedUnalignedTpl;
+  typedef JointDataRevoluteUnboundedUnalignedTpl<double> JointDataRevoluteUnboundedUnaligned;
+  
   template<typename Scalar, int Options, int axis> struct JointModelRevoluteUnboundedTpl;
   template<typename Scalar, int Options, int axis> struct JointDataRevoluteUnboundedTpl;
 

src/multibody/joint/joint-collection.hpp

@@ -37,6 +37,9 @@ namespace pinocchio
     typedef JointModelRevoluteUnboundedTpl<Scalar,Options,1> JointModelRUBY;
     typedef JointModelRevoluteUnboundedTpl<Scalar,Options,2> JointModelRUBZ;
     
+    // Joint Revolute Unbounded Unaligned
+    typedef JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModelRevoluteUnboundedUnaligned;
+    
     // Joint Prismatic
     typedef JointModelPrismaticTpl<Scalar,Options,0> JointModelPX;
     typedef JointModelPrismaticTpl<Scalar,Options,1> JointModelPY;
@@ -74,6 +77,7 @@ namespace pinocchio
     , JointModelPrismaticUnaligned
     , JointModelTranslation
     , JointModelRUBX, JointModelRUBY, JointModelRUBZ
+    , JointModelRevoluteUnboundedUnaligned
     , boost::recursive_wrapper<JointModelComposite>
     > JointModelVariant;
    
@@ -90,6 +94,9 @@ namespace pinocchio
     // Joint Revolute Unaligned
     typedef JointDataRevoluteUnalignedTpl<Scalar,Options> JointDataRevoluteUnaligned;
     
+    // Joint Revolute Unaligned
+    typedef JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDataRevoluteUnboundedUnaligned;
+    
     // Joint Revolute UBounded
     typedef JointDataRevoluteUnboundedTpl<Scalar,Options,0> JointDataRUBX;
     typedef JointDataRevoluteUnboundedTpl<Scalar,Options,1> JointDataRUBY;
@@ -132,6 +139,7 @@ namespace pinocchio
     , JointDataPrismaticUnaligned
     , JointDataTranslation
     , JointDataRUBX, JointDataRUBY, JointDataRUBZ
+    , JointDataRevoluteUnboundedUnaligned
     , boost::recursive_wrapper<JointDataComposite>
     > JointDataVariant;
 

src/multibody/joint/joint-prismatic-unaligned.hpp

@@ -198,7 +198,7 @@ namespace pinocchio
   template<typename Scalar, int Options, typename ForceDerived>
   struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceDerived>
   {
-    typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
+    typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
     typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
   };
   

src/multibody/joint/joint-revolute-unbounded-unaligned.hpp

+//
+// Copyright (c) 2019 INRIA
+//
+
+#ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__
+#define __pinocchio_joint_revolute_unbounded_unaligned_hpp__
+
+#include "pinocchio/fwd.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/math/rotation.hpp"
+#include "pinocchio/math/matrix.hpp"
+
+#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
+
+namespace pinocchio
+{
+
+  template<typename Scalar, int Options = 0> struct JointRevoluteUnboundedUnalignedTpl;
+  
+  template<typename _Scalar, int _Options>
+  struct traits< JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
+  {
+    enum {
+      NQ = 2,
+      NV = 1
+    };
+    typedef _Scalar Scalar;
+    enum { Options = _Options };
+    
+    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
+    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
+    
+    typedef JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDataDerived;
+    typedef JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModelDerived;
+    typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t;
+    typedef SE3Tpl<Scalar,Options> Transformation_t;
+    typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t;
+    typedef BiasZeroTpl<Scalar,Options> Bias_t;
+    typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
+    
+    // [ABA]
+    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
+    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
+    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
+    
+    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
+  };
+
+  template<typename Scalar, int Options>
+  struct traits< JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+  { typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; };
+  
+  template<typename Scalar, int Options>
+  struct traits <JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+  { typedef JointRevoluteUnboundedUnalignedTpl<Scalar,Options> JointDerived; };
+
+  template<typename _Scalar, int _Options>
+  struct JointDataRevoluteUnboundedUnalignedTpl
+  : public JointDataBase< JointDataRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
+  {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
+    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
+    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
+    
+    Transformation_t M;
+    Constraint_t S;
+    Motion_t v;
+    Bias_t c;
+    
+    // [ABA] specific data
+    U_t U;
+    D_t Dinv;
+    UD_t UDinv;
+    
+    JointDataRevoluteUnboundedUnalignedTpl() {}
+    
+    template<typename Vector3Like>
+    JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
+    : M(1)
+    , S(axis)
+    , v(axis,(Scalar)NAN)
+    , U(), Dinv(), UDinv()
+    {}
+    
+    static std::string classname() { return std::string("JointDataRevoluteUnboundedUnalignedTpl"); }
+    std::string shortname() const { return classname(); }
+    
+  }; // struct JointDataRevoluteUnboundedUnalignedTpl
+
+  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
+  
+  template<typename _Scalar, int _Options>
+  struct JointModelRevoluteUnboundedUnalignedTpl
+  : public JointModelBase< JointModelRevoluteUnboundedUnalignedTpl<_Scalar,_Options> >
+  {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
+    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
+    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
+    
+    typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base;
+    using Base::id;
+    using Base::idx_q;
+    using Base::idx_v;
+    using Base::setIndexes;
+    
+    JointModelRevoluteUnboundedUnalignedTpl() {}
+    
+    JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x,
+                                            const Scalar & y,
+                                            const Scalar & z)
+    : axis(x,y,z)
+    {
+      axis.normalize();
+      assert(isUnitary(axis) && "Rotation axis is not unitary");
+    }
+    
+    template<typename Vector3Like>
+    JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
+    : axis(axis)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
+      assert(isUnitary(axis) && "Rotation axis is not unitary");
+    }
+
+    JointDataDerived createData() const { return JointDataDerived(axis); }
+    
+    using Base::isEqual;
+    bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
+    {
+      return Base::isEqual(other) && axis == other.axis;
+    }
+    
+    template<typename ConfigVector>
+    void calc(JointDataDerived & data,
+              const typename Eigen::MatrixBase<ConfigVector> & qs) const
+    {
+      typedef typename ConfigVector::Scalar OtherScalar;
+      typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type
+      & q = qs.template segment<NQ>(idx_q());
+      
+      const OtherScalar & ca = q(0);
+      const OtherScalar & sa = q(1);
+      
+      toRotationMatrix(axis,ca,sa,data.M.rotation());
+    }
+
+    template<typename ConfigVector, typename TangentVector>
+    void calc(JointDataDerived & data,
+              const typename Eigen::MatrixBase<ConfigVector> & qs,
+              const typename Eigen::MatrixBase<TangentVector> & vs) const
+    {
+      calc(data,qs.derived());
+      data.v.w = (Scalar)vs[idx_v()];
+    }
+    
+    template<typename Matrix6Like>
+    void calc_aba(JointDataDerived & data,
+                  const Eigen::MatrixBase<Matrix6Like> & I,
+                  const bool update_I) const
+    {
+      data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
+      data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
+      data.UDinv.noalias() = data.U * data.Dinv;
+      
+      if (update_I)
+        PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
+    }
+    
+    static std::string classname() { return std::string("JointModelRevoluteUnboundedUnaligned"); }
+    std::string shortname() const { return classname(); }
+    
+    /// \returns An expression of *this with the Scalar type casted to NewScalar.
+    template<typename NewScalar>
+    JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> cast() const
+    {
+      typedef JointModelRevoluteUnboundedUnalignedTpl<NewScalar,Options> ReturnType;
+      ReturnType res(axis.template cast<NewScalar>());
+      res.setIndexes(id(),idx_q(),idx_v());
+      return res;
+    }
+
+    // data
+    ///
+    /// \brief 3d main axis of the joint.
+    ///
+    Vector3 axis;
+  }; // struct JointModelRevoluteUnboundedUnalignedTpl
+
+} //namespace pinocchio
+
+#include <boost/type_traits.hpp>
+
+namespace boost
+{
+  template<typename Scalar, int Options>
+  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+  : public integral_constant<bool,true> {};
+  
+  template<typename Scalar, int Options>
+  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+  : public integral_constant<bool,true> {};
+  
+  template<typename Scalar, int Options>
+  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+  : public integral_constant<bool,true> {};
+  
+  template<typename Scalar, int Options>
+  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+  : public integral_constant<bool,true> {};
+}
+
+
+#endif // ifndef __pinocchio_joint_revolute_unbounded_unaligned_hpp__

src/multibody/joint/joints.hpp

@@ -8,10 +8,11 @@
 #include "pinocchio/multibody/joint/joint-free-flyer.hpp"
 #include "pinocchio/multibody/joint/joint-planar.hpp"
 #include "pinocchio/multibody/joint/joint-prismatic.hpp"
-#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
 #include "pinocchio/multibody/joint/joint-prismatic-unaligned.hpp"
 #include "pinocchio/multibody/joint/joint-revolute.hpp"
 #include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp"
+#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
+#include "pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp"
 #include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
 #include "pinocchio/multibody/joint/joint-spherical.hpp"
 #include "pinocchio/multibody/joint/joint-translation.hpp"

src/multibody/liegroup/liegroup.hpp

 //
-// Copyright (c) 2016-2018 CNRS
+// Copyright (c) 2016-2019 CNRS INRIA
 //
 
 #ifndef __pinocchio_lie_group_hpp__
@@ -57,6 +57,12 @@ namespace pinocchio
     typedef SpecialOrthogonalOperationTpl<2,Scalar,Options> type;
   };
   
+  template<typename Scalar, int Options>
+  struct LieGroupMap::operation<JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
+  {
+    typedef SpecialOrthogonalOperationTpl<2,Scalar,Options> type;
+  };
+  
 }
 
 #endif // ifndef __pinocchio_lie_group_hpp__

src/parsers/urdf/model.hxx

@@ -355,13 +355,13 @@ namespace pinocchio
                   typename SE3::Vector3 joint_axis((Scalar)joint->axis.x,(Scalar)joint->axis.y,(Scalar)joint->axis.z);
                   joint_info << " unaligned along (" << joint_axis.transpose() << ")";
 
-                  typedef typename JointCollection::JointModelRevoluteUnaligned::ConfigVector_t ConfigVector_t;
+                  typedef typename JointCollection::JointModelRevoluteUnboundedUnaligned::ConfigVector_t ConfigVector_t;
 
                   const Scalar infty = std::numeric_limits<Scalar>::infinity();
                   ConfigVector_t lower_position(ConfigVector_t::Constant(-infty));
                   ConfigVector_t upper_position(ConfigVector_t::Constant(infty));
 
-                  addJointAndBody(model, typename JointCollection::JointModelRevoluteUnaligned(joint_axis.normalized()),
+                  addJointAndBody(model, typename JointCollection::JointModelRevoluteUnboundedUnaligned(joint_axis.normalized()),
                                   parentFrameId,jointPlacement,joint->name,
                                   Y,link->name,
                                   max_effort,max_velocity,

src/serialization/fwd.hpp

@@ -5,6 +5,7 @@
 #ifndef __pinocchio_serialization_fwd_hpp__
 #define __pinocchio_serialization_fwd_hpp__
 
+#include "pinocchio/fwd.hpp"
 #include <boost/serialization/nvp.hpp>
 
 #define BOOST_SERIALIZATION_MAKE_NVP(member) boost::serialization::make_nvp(##member,member)

src/serialization/joints.hpp

@@ -180,6 +180,17 @@ namespace boost
       ar & make_nvp("axis",joint.axis);
     }
     
+    template <class Archive, typename Scalar, int Options>
+    void serialize(Archive & ar,
+                   pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> & joint,
+                   const unsigned int version)
+    {
+      typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointType;
+      //      ar & make_nvp("base_class",base_object< pinocchio::JointModelBase<JointType> >(joint));
+      fix::serialize(ar,*static_cast<pinocchio::JointModelBase<JointType> *>(&joint),version);
+      ar & make_nvp("axis",joint.axis);
+    }
+    
     template <class Archive, typename Scalar, int Options>
     void serialize(Archive & ar,
                    pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> & joint,

unittest/CMakeLists.txt

@@ -41,6 +41,7 @@ ENDMACRO(ADD_PINOCCHIO_UNIT_TEST)
 ADD_PINOCCHIO_UNIT_TEST(eigen-basic-op eigen3)
 ADD_PINOCCHIO_UNIT_TEST(sincos eigen3)
 ADD_PINOCCHIO_UNIT_TEST(quaternion eigen3)
+ADD_PINOCCHIO_UNIT_TEST(rotation eigen3)
 
 # Pinocchio features
 ADD_PINOCCHIO_UNIT_TEST(tspatial eigen3)