[Major] Add spherical euler joint and its unittest

CMakeLists.txt

@@ -71,6 +71,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint-base.hpp
   multibody/joint/joint-revolute.hpp
   multibody/joint/joint-spherical.hpp
+  multibody/joint/joint-spherical-ZYX.hpp
   multibody/joint/joint-prismatic.hpp
   multibody/joint/joint-free-flyer.hpp
   multibody/joint/joint-variant.hpp

src/multibody/joint.hpp

@@ -4,6 +4,7 @@
 #include "pinocchio/multibody/joint/joint-base.hpp"
 #include "pinocchio/multibody/joint/joint-revolute.hpp"
 #include "pinocchio/multibody/joint/joint-spherical.hpp"
+#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
 #include "pinocchio/multibody/joint/joint-prismatic.hpp"
 #include "pinocchio/multibody/joint/joint-free-flyer.hpp"
 #include "pinocchio/multibody/joint/joint-variant.hpp"

src/multibody/joint/joint-spherical-ZYX.hpp

+#ifndef __se3_joint_spherical_ZYX_hpp__
+#define __se3_joint_spherical_ZYX_hpp__
+
+#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/constraint.hpp"
+#include "pinocchio/math/sincos.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/skew.hpp"
+
+namespace se3
+{
+
+  struct JointDataSphericalZYX;
+  struct JointModelSphericalZYX;
+
+  struct JointSphericalZYX
+  {
+    struct BiasSpherical
+    {
+      Motion::Vector3 c_J;
+
+      BiasSpherical ()  {c_J.fill (NAN);}
+      //BiasSpherical (const Motion::Vector3 & c_J) c_J (c_J) {}
+
+      operator Motion () const { return Motion (Motion::Vector3::Zero (), c_J); }
+
+      Motion::Vector3 & operator() () { return c_J; }
+      const Motion::Vector3 & operator() () const { return c_J; }
+
+
+    }; // struct BiasSpherical
+
+    friend const Motion operator+ (const Motion & v, const BiasSpherical & c) { return Motion (v.linear (), v.angular () + c ()); }
+    friend const Motion operator+ (const BiasSpherical & c, const Motion & v) { return Motion (v.linear (), v.angular () + c ()); }
+
+    struct MotionSpherical
+    {
+      MotionSpherical ()                       {w.fill(NAN);}
+      MotionSpherical (const Motion::Vector3 & w) : w (w)  {}
+      Motion::Vector3 w;
+
+      Motion::Vector3 & operator() () { return w; }
+      const Motion::Vector3 & operator() () const { return w; }
+
+      operator Motion() const
+      {
+        return Motion (Motion::Vector3::Zero (), w);
+      }
+    }; // struct MotionSpherical
+
+    friend const MotionSpherical operator+ (const MotionSpherical & m, const BiasSpherical & c)
+    { return MotionSpherical (m.w + c.c_J); }
+
+    friend Motion operator+ (const MotionSpherical & m1, const Motion & m2)
+    {
+      return Motion( m2.linear(),m2.angular() + m1.w);
+    }
+
+    struct ConstraintRotationalSubspace
+    {
+    public:
+      typedef Motion::Scalar Scalar;
+      typedef Eigen::Matrix <Motion::Scalar,3,3> Matrix3;
+
+    public:
+      Matrix3 S_minimal;
+
+      Motion operator* (const MotionSpherical & vj) const
+      { return Motion (Motion::Vector3::Zero (), S_minimal * vj ()); }
+
+      ConstraintRotationalSubspace () : S_minimal () { S_minimal.fill (NAN); }
+      ConstraintRotationalSubspace (const Matrix3 & subspace) : S_minimal (subspace) {}
+
+      Matrix3 & operator() () { return S_minimal; }
+      const Matrix3 & operator() () const { return S_minimal; }
+
+      Matrix3 &  matrix () { return S_minimal; }
+      const Matrix3 & matrix () const { return S_minimal; }
+
+      struct ConstraintTranspose
+      {
+        const ConstraintRotationalSubspace & ref;
+        ConstraintTranspose(const ConstraintRotationalSubspace & ref) : ref(ref) {}
+
+        Force::Vector3 operator* (const Force & phi)
+        {
+          return ref.S_minimal.transpose () * phi.angular ();
+        }
+
+
+        /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
+        template<typename D>
+        friend typename Eigen::Matrix <typename Eigen::MatrixBase<D>::Scalar, 3, -1>
+        operator*( const ConstraintTranspose & constraint_transpose, const Eigen::MatrixBase<D> & F )
+        {
+          assert(F.rows()==6);
+          return constraint_transpose.ref.S_minimal.transpose () * F.template middleRows <3> (Inertia::ANGULAR);
+        }
+      }; // struct ConstraintTranspose
+
+      ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
+
+      operator ConstraintXd () const
+      {
+        Eigen::Matrix<Scalar,6,3> S;
+        S.block <3,3> (Inertia::LINEAR, 0).setZero ();
+        S.block <3,3> (Inertia::ANGULAR, 0) = S_minimal;
+        return ConstraintXd(S);
+      }
+
+      Eigen::Matrix <Scalar,6,3> se3Action (const SE3 & m) const
+      {
+        Eigen::Matrix <Scalar,6,3> X_subspace;
+        X_subspace.block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) * m.rotation ();
+        X_subspace.block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
+
+        return X_subspace * S_minimal;
+      }
+
+    }; // struct ConstraintRotationalSubspace
+
+    template<typename D>
+    friend Motion operator* (const ConstraintRotationalSubspace & S, const Eigen::MatrixBase<D> & v)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+      return Motion (Motion::Vector3::Zero (), S () * v);
+    }
+
+  }; // struct JointSphericalZYX
+
+  Motion operator^ (const Motion & m1, const JointSphericalZYX::MotionSpherical & m2)
+  {
+//    const Motion::Matrix3 m2_cross (skew (Motion::Vector3 (-m2.w)));
+//    return Motion(m2_cross * m1.linear (), m2_cross * m1.angular ());
+    return Motion(m1.linear ().cross (m2.w), m1.angular ().cross (m2.w));
+  }
+
+  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
+  Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointSphericalZYX::ConstraintRotationalSubspace & S)
+  {
+    Eigen::Matrix <Inertia::Scalar, 6, 3> M;
+//    M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
+    M.block <3,3> (Inertia::LINEAR, 0) = alphaSkew ( -Y.mass (),  Y.lever ());
+    M.block <3,3> (Inertia::ANGULAR, 0) = (Inertia::Matrix3)(Y.inertia () - typename Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ()));
+    return M * S.matrix ();
+  }
+
+  namespace internal
+  {
+    template<>
+    struct ActionReturn<JointSphericalZYX::ConstraintRotationalSubspace >
+    { typedef Eigen::Matrix<double,6,3> Type; };
+  }
+
+  template<>
+  struct traits<JointSphericalZYX>
+  {
+    typedef JointDataSphericalZYX JointData;
+    typedef JointModelSphericalZYX JointModel;
+    typedef JointSphericalZYX::ConstraintRotationalSubspace Constraint_t;
+    typedef SE3 Transformation_t;
+    typedef JointSphericalZYX::MotionSpherical Motion_t;
+    typedef JointSphericalZYX::BiasSpherical Bias_t;
+    typedef Eigen::Matrix<double,6,3> F_t;
+    enum {
+      NQ = 3,
+      NV = 3
+    };
+  };
+  template<> struct traits<JointDataSphericalZYX> { typedef JointSphericalZYX Joint; };
+  template<> struct traits<JointModelSphericalZYX> { typedef JointSphericalZYX Joint; };
+
+  struct JointDataSphericalZYX : public JointDataBase<JointDataSphericalZYX>
+  {
+    typedef JointSphericalZYX Joint;
+    SE3_JOINT_TYPEDEF;
+
+    typedef Motion::Scalar Scalar;
+
+    typedef Eigen::Matrix<Scalar,6,6> Matrix6;
+    typedef Eigen::Matrix<Scalar,3,3> Matrix3;
+    typedef Eigen::Matrix<Scalar,3,1> Vector3;
+    
+    Constraint_t S;
+    Transformation_t M;
+    Motion_t v;
+    Bias_t c;
+
+    JointDataSphericalZYX () : M(1)
+    {
+      M.translation (Transformation_t::Vector3::Zero ());
+    }
+  };
+
+  struct JointModelSphericalZYX : public JointModelBase<JointModelSphericalZYX>
+  {
+    typedef JointSphericalZYX Joint;
+    SE3_JOINT_TYPEDEF;
+
+    JointData createData() const { return JointData(); }
+
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs) const
+    {
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ>(idx_q ());
+
+      double c0,s0; SINCOS (q(0), &s0, &c0);
+      double c1,s1; SINCOS (q(1), &s1, &c1);
+      double c2,s2; SINCOS (q(2), &s2, &c2);
+
+      data.M.rotation () << c0 * c1,
+                c0 * s1 * s2 - s0 * c2,
+                c0 * s1 * c2 + s0 * s2,
+                s0 * c1,
+                s0 * s1 * s2 + c0 * c2,
+                s0 * s1 * c2 - c0 * s2,
+                -s1,
+                c1 * s2,
+                c1 * c2;
+
+      data.S.matrix () <<  -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
+    }
+
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs,
+               const Eigen::VectorXd & vs ) const
+    {
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
+      Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NQ> (idx_v ());
+
+      double c0,s0; SINCOS (q(0), &s0, &c0);
+      double c1,s1; SINCOS (q(1), &s1, &c1);
+      double c2,s2; SINCOS (q(2), &s2, &c2);
+
+      data.M.rotation () << c0 * c1,
+                c0 * s1 * s2 - s0 * c2,
+                c0 * s1 * c2 + s0 * s2,
+                s0 * c1,
+                s0 * s1 * s2 + c0 * c2,
+                s0 * s1 * c2 - c0 * s2,
+                -s1,
+                c1 * s2,
+                c1 * c2;
+
+
+      data.S.matrix () <<  -s1, 0., 1., c1 * s2, c2, 0, c1 * c2, -s2, 0;
+
+      data.v () = data.S.matrix () * q_dot;
+
+      data.c ()(0) = -c1 * q_dot (0) * q_dot (1);
+      data.c ()(1) = -s1 * s2 * q_dot (0) * q_dot (1) + c1 * c2 * q_dot (0) * q_dot (2) - s2 * q_dot (1) * q_dot (2);
+      data.c ()(2) = -s1 * c2 * q_dot (0) * q_dot (1) - c1 * s2 * q_dot (0) * q_dot (2) - c2 * q_dot (1) * q_dot (2);
+    }
+  };
+
+} // namespace se3
+
+#endif // ifndef __se3_joint_spherical_ZYX_hpp__
\ No newline at end of file

src/multibody/joint/joint-variant.hpp

@@ -3,13 +3,14 @@
 
 #include "pinocchio/multibody/joint/joint-revolute.hpp"
 #include "pinocchio/multibody/joint/joint-spherical.hpp"
+#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
 #include "pinocchio/multibody/joint/joint-prismatic.hpp"
 #include "pinocchio/multibody/joint/joint-free-flyer.hpp"
 
 namespace se3
 {
-  typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ , JointModelSpherical, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer> JointModelVariant;
-  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ  , JointDataSpherical, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer > JointDataVariant;
+  typedef boost::variant< JointModelRX,JointModelRY,JointModelRZ , JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelFreeFlyer> JointModelVariant;
+  typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ  , JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataFreeFlyer > JointDataVariant;
 
   typedef std::vector<JointModelVariant> JointModelVector;
   typedef std::vector<JointDataVariant> JointDataVector;

unittest/joints.cpp

@@ -6,6 +6,7 @@
 #include "pinocchio/spatial/inertia.hpp"
 #include "pinocchio/multibody/joint/joint-revolute.hpp"
 #include "pinocchio/multibody/joint/joint-spherical.hpp"
+#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
 #include "pinocchio/multibody/joint/joint-prismatic.hpp"
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
@@ -58,226 +59,226 @@ BOOST_AUTO_TEST_SUITE ( JointSphericalZYX )
 
 BOOST_AUTO_TEST_CASE ( test_kinematics )
 {
-  // using namespace se3;
+  using namespace se3;
 
-  // typedef Motion::Vector3 Vector3;
-  // typedef Motion::Vector6 Vector6;
+  typedef Motion::Vector3 Vector3;
+  typedef Motion::Vector6 Vector6;
 
-  // Motion expected_v_J (Motion::Zero ());
-  // Motion expected_c_J (Motion::Zero ());
+  Motion expected_v_J (Motion::Zero ());
+  Motion expected_c_J (Motion::Zero ());
 
-  // SE3 expected_configuration (SE3::Identity ());
+  SE3 expected_configuration (SE3::Identity ());
 
-  // JointDataSphericalZYX joint_data;
-  // JointModelSphericalZYX joint_model;
+  JointDataSphericalZYX joint_data;
+  JointModelSphericalZYX joint_model;
 
-  // joint_model.setIndexes (0, 0, 0);
+  joint_model.setIndexes (0, 0, 0);
 
-  // Vector3 q (Vector3::Zero());
-  // Vector3 q_dot (Vector3::Zero());
+  Vector3 q (Vector3::Zero());
+  Vector3 q_dot (Vector3::Zero());
 
-  // // -------
-  // q = Vector3 (0., 0., 0.);
-  // q_dot = Vector3 (0., 0., 0.);
+  // -------
+  q = Vector3 (0., 0., 0.);
+  q_dot = Vector3 (0., 0., 0.);
 
-  // joint_model.calc (joint_data, q, q_dot);
+  joint_model.calc (joint_data, q, q_dot);
 
-  // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+  printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
 
-  // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation());
-  // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation ());
-  // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
-  // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+  is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation());
+  is_matrix_closed (expected_configuration.translation (), joint_data.M.translation ());
+  is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
+  is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
 
-  // // -------
-  // q = Vector3 (1., 0., 0.);
-  // q_dot = Vector3 (1., 0., 0.);
+  // -------
+  q = Vector3 (1., 0., 0.);
+  q_dot = Vector3 (1., 0., 0.);
 
-  // joint_model.calc (joint_data, q, q_dot);
+  joint_model.calc (joint_data, q, q_dot);
 
-  // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+  printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
 
-  // expected_configuration.rotation ().transpose () <<
-  // 0.54030230586814,  0.8414709848079,               -0,
-  // -0.8414709848079, 0.54030230586814,                0,
-  // 0,                0,               1;
+  expected_configuration.rotation ().transpose () <<
+  0.54030230586814,  0.8414709848079,               -0,
+  -0.8414709848079, 0.54030230586814,                0,
+  0,                0,               1;
 
-  // expected_v_J.angular () << 0., 0., 1.;
+  expected_v_J.angular () << 0., 0., 1.;
 
-  // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
-  // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
-  // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
-  // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+  is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+  is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+  is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+  is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
 
-  // // -------
-  // q = Vector3 (0., 1., 0.);
-  // q_dot = Vector3 (0., 1., 0.);
+  // -------
+  q = Vector3 (0., 1., 0.);
+  q_dot = Vector3 (0., 1., 0.);
 
-  // joint_model.calc (joint_data, q, q_dot);
+  joint_model.calc (joint_data, q, q_dot);
 
-  // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+  printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
 
-  // expected_configuration.rotation ().transpose () <<
-  // 0.54030230586814,                0, -0.8414709848079,
-  // 0,                1,                0,
-  // 0.8414709848079,                0, 0.54030230586814;
+  expected_configuration.rotation ().transpose () <<
+  0.54030230586814,                0, -0.8414709848079,
+  0,                1,                0,
+  0.8414709848079,                0, 0.54030230586814;
 
-  // expected_v_J.angular () << 0., 1., 0.;
+  expected_v_J.angular () << 0., 1., 0.;
 
-  // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
-  // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
-  // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
-  // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+  is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+  is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+  is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+  is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
 
-  // // -------
-  // q = Vector3 (0., 0., 1.);
-  // q_dot = Vector3 (0., 0., 1.);
+  // -------
+  q = Vector3 (0., 0., 1.);
+  q_dot = Vector3 (0., 0., 1.);
 
-  // joint_model.calc (joint_data, q, q_dot);
+  joint_model.calc (joint_data, q, q_dot);
 
-  // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+  printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
 
-  // expected_configuration.rotation ().transpose () <<
-  // 1,                0,               -0,
-  // 0, 0.54030230586814,  0.8414709848079,
-  // 0, -0.8414709848079, 0.54030230586814;
+  expected_configuration.rotation ().transpose () <<
+  1,                0,               -0,
+  0, 0.54030230586814,  0.8414709848079,
+  0, -0.8414709848079, 0.54030230586814;
 
-  // expected_v_J.angular () << 1., 0., 0.;
+  expected_v_J.angular () << 1., 0., 0.;
 
-  // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
-  // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
-  // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
-  // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+  is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+  is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+  is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+  is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
 
-  // // -------
-  // q = Vector3 (1., 1., 1.);
-  // q_dot = Vector3 (1., 1., 1.);
+  // -------
+  q = Vector3 (1., 1., 1.);
+  q_dot = Vector3 (1., 1., 1.);
 
-  // joint_model.calc (joint_data, q, q_dot);
+  joint_model.calc (joint_data, q, q_dot);
 
-  // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+  printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
 
-  // expected_configuration.rotation ().transpose () <<
-  // 0.29192658172643,   0.45464871341284,   -0.8414709848079,
-  // -0.072075012795695,   0.88774981831738,   0.45464871341284,
-  // 0.95372116649051, -0.072075012795695,   0.29192658172643;
+  expected_configuration.rotation ().transpose () <<
+  0.29192658172643,   0.45464871341284,   -0.8414709848079,
+  -0.072075012795695,   0.88774981831738,   0.45464871341284,
+  0.95372116649051, -0.072075012795695,   0.29192658172643;
 
-  // expected_v_J.angular () << 0.1585290151921,  0.99495101928098, -0.54954440308147;
-  // expected_c_J.angular () << -0.54030230586814,   -1.257617821355,  -1.4495997326938;
+  expected_v_J.angular () << 0.1585290151921,  0.99495101928098, -0.54954440308147;
+  expected_c_J.angular () << -0.54030230586814,   -1.257617821355,  -1.4495997326938;
 
-  // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
-  // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
-  // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
-  // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+  is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+  is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+  is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+  is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
 
-  // // -------
-  // q = Vector3 (1., 1.5, 1.9);
-  // q_dot = Vector3 (2., 3., 1.);
+  // -------
+  q = Vector3 (1., 1.5, 1.9);
+  q_dot = Vector3 (2., 3., 1.);
 
-  // joint_model.calc (joint_data, q, q_dot);
+  joint_model.calc (joint_data, q, q_dot);
 
-  // printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
+  printOutJointData <JointDataSphericalZYX> (q, q_dot, joint_data);
 
-  // expected_configuration.rotation ().transpose () <<
-  // 0.03821947317172,  0.059523302749877,  -0.99749498660405,
-  // 0.78204612603915,   0.61961526601658,   0.06693862014091,
-  // 0.62204752922718,   -0.7826454488138, -0.022868599288288;
+  expected_configuration.rotation ().transpose () <<
+  0.03821947317172,  0.059523302749877,  -0.99749498660405,
+  0.78204612603915,   0.61961526601658,   0.06693862014091,
+  0.62204752922718,   -0.7826454488138, -0.022868599288288;
 
-  // expected_v_J.angular () << -0.99498997320811, -0.83599146030869,  -2.8846374616388;
-  // expected_c_J.angular () << -0.42442321000622,  -8.5482150213859,   2.7708697933151;
+  expected_v_J.angular () << -0.99498997320811, -0.83599146030869,  -2.8846374616388;
+  expected_c_J.angular () << -0.42442321000622,  -8.5482150213859,   2.7708697933151;
 
-  // is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
-  // is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
-  // is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
-  // is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
+  is_matrix_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+  is_matrix_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+  is_matrix_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+  is_matrix_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-10);
 }
 
 BOOST_AUTO_TEST_CASE ( test_rnea )
 {
-  // using namespace se3;
-  // typedef Eigen::Matrix <double, 3, 1> Vector3;
-  // typedef Eigen::Matrix <double, 3, 3> Matrix3;
+  using namespace se3;
+  typedef Eigen::Matrix <double, 3, 1> Vector3;
+  typedef Eigen::Matrix <double, 3, 3> Matrix3;
 
-  // Model model;
-  // Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
+  Model model;
+  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
 
-  // model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
+  model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
 
-  // Data data (model);
+  Data data (model);
 
-  // Eigen::VectorXd q = Eigen::VectorXd::Zero (model.nq);
-  // Eigen::VectorXd v = Eigen::VectorXd::Zero (model.nv);
-  // Eigen::VectorXd a = Eigen::VectorXd::Zero (model.nv);
+  Eigen::VectorXd q = Eigen::VectorXd::Zero (model.nq);
+  Eigen::VectorXd v = Eigen::VectorXd::Zero (model.nv);
+  Eigen::VectorXd a = Eigen::VectorXd::Zero (model.nv);
 
-  // rnea (model, data, q, v, a);
-  // Vector3 tau_expected (0., -4.905, 0.);
+  rnea (model, data, q, v, a);
+  Vector3 tau_expected (0., -4.905, 0.);
 
-  // is_matrix_closed (tau_expected, data.tau, 1e-14);
+  is_matrix_closed (tau_expected, data.tau, 1e-14);
 
-  // q = Eigen::VectorXd::Ones (model.nq);
-  // v = Eigen::VectorXd::Ones (model.nv);
-  // a = Eigen::VectorXd::Ones (model.nv);
+  q = Eigen::VectorXd::Ones (model.nq);
+  v = Eigen::VectorXd::Ones (model.nv);
+  a = Eigen::VectorXd::Ones (model.nv);
 
-  // rnea (model, data, q, v, a);
-  // tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;
+  rnea (model, data, q, v, a);
+  tau_expected << -0.53611600195085, -0.74621832606188, -0.38177329067604;
 
-  // is_matrix_closed (tau_expected, data.tau, 1e-12);
+  is_matrix_closed (tau_expected, data.tau, 1e-12);
 
-  // q << 3, 2, 1;
-  // v = Eigen::VectorXd::Ones (model.nv);
-  // a = Eigen::VectorXd::Ones (model.nv);
+  q << 3, 2, 1;
+  v = Eigen::VectorXd::Ones (model.nv);
+  a = Eigen::VectorXd::Ones (model.nv);
 
-  // rnea (model, data, q, v, a);
-  // tau_expected << 0.73934458094049,  2.7804530848031, 0.50684940972146;
+  rnea (model, data, q, v, a);
+  tau_expected << 0.73934458094049,  2.7804530848031, 0.50684940972146;
 
-  // is_matrix_closed (tau_expected, data.tau, 1e-12);
+  is_matrix_closed (tau_expected, data.tau, 1e-12);
 
 }
 
 BOOST_AUTO_TEST_CASE ( test_crba )
 {
-  // using namespace se3;
-  // using namespace std;
-  // typedef Eigen::Matrix <double, 3, 1> Vector3;
-  // typedef Eigen::Matrix <double, 3, 3> Matrix3;
+  using namespace se3;
+  using namespace std;
+  typedef Eigen::Matrix <double, 3, 1> Vector3;
+  typedef Eigen::Matrix <double, 3, 3> Matrix3;
 
-  // Model model;
-  // Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
+  Model model;
+  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
 
-  // model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
+  model.addBody (model.getBodyId("universe"), JointModelSphericalZYX (), SE3::Identity (), inertia, "root");
 
-  // Data data (model);
+  Data data (model);
 
-  // Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
-  // Eigen::MatrixXd M_expected (model.nv,model.nv);
+  Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
+  Eigen::MatrixXd M_expected (model.nv,model.nv);
 
-  // crba (model, data, q);
-  // M_expected <<
-  // 1.25,    0,    0,
-  // 0, 1.25,    0,
-  // 0,    0,    1;
+  crba (model, data, q);
+  M_expected <<
+  1.25,    0,    0,
+  0, 1.25,    0,
+  0,    0,    1;
 
-  // is_matrix_closed (M_expected, data.M, 1e-14);
+  is_matrix_closed (M_expected, data.M, 1e-14);
 
-  // q = Eigen::VectorXd::Ones (model.nq);
+  q = Eigen::VectorXd::Ones (model.nq);
 
-  // crba (model, data, q);
-  // M_expected <<
-  // 1.0729816454316, -5.5511151231258e-17,     -0.8414709848079,
-  // -5.5511151231258e-17,                 1.25,                    0,
-  // -0.8414709848079,                    0,                    1;
+  crba (model, data, q);
+  M_expected <<
+  1.0729816454316, -5.5511151231258e-17,     -0.8414709848079,
+  -5.5511151231258e-17,                 1.25,                    0,
+  -0.8414709848079,                    0,                    1;
 
-  // is_matrix_closed (M_expected, data.M, 1e-12);
+  is_matrix_closed (M_expected, data.M, 1e-12);
 
-  // q << 3, 2, 1;
+  q << 3, 2, 1;
 
-  // crba (model, data, q);
-  // M_expected <<
-  // 1.043294547392, 2.7755575615629e-17,   -0.90929742682568,
-  // 0,                1.25,                   0,
-  // -0.90929742682568,                   0,                  1;
+  crba (model, data, q);
+  M_expected <<
+  1.043294547392, 2.7755575615629e-17,   -0.90929742682568,
+  0,                1.25,                   0,
+  -0.90929742682568,                   0,                  1;
 
-  // is_matrix_closed (M_expected, data.M, 1e-10);
+  is_matrix_closed (M_expected, data.M, 1e-10);
 }
 
 BOOST_AUTO_TEST_SUITE_END ()