[Incremental][C++] Add complete translational joint

CMakeLists.txt

@@ -77,6 +77,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint-spherical-ZYX.hpp
   multibody/joint/joint-prismatic.hpp
   multibody/joint/joint-planar.hpp
+  multibody/joint/joint-translation.hpp
   multibody/joint/joint-free-flyer.hpp
   multibody/joint/joint-variant.hpp
   multibody/joint/joint-generic.hpp

src/multibody/joint/joint-translation.hpp

+#ifndef __se3_joint_translation_hpp__
+#define __se3_joint_translation_hpp__
+
+#include "pinocchio/multibody/joint/joint-base.hpp"
+#include "pinocchio/multibody/constraint.hpp"
+#include "pinocchio/spatial/inertia.hpp"
+#include "pinocchio/spatial/skew.hpp"
+
+namespace se3
+{
+
+  struct JointDataTranslation;
+  struct JointModelTranslation;
+
+  struct JointTranslation
+  {
+    struct BiasZero
+    {
+      operator Motion () const { return Motion::Zero(); }
+    }; // struct BiasZero
+
+    friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
+    friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }
+
+    struct MotionTranslation
+    {
+      MotionTranslation ()                   : v (Motion::Vector3 (NAN, NAN, NAN)) {}
+      MotionTranslation (const Motion::Vector3 & v) : v (v)  {}
+      MotionTranslation (const MotionTranslation & other) : v (other.v)  {}
+      Motion::Vector3 v;
+
+      Motion::Vector3 & operator() () { return v; }
+      const Motion::Vector3 & operator() () const { return v; }
+
+      operator Motion() const
+      {
+        return Motion (v, Motion::Vector3::Zero ());
+      }
+
+      MotionTranslation & operator= (const MotionTranslation & other)
+      {
+        v = other.v;
+        return *this;
+      }
+    }; // struct MotionTranslation
+
+    friend const MotionTranslation operator+ (const MotionTranslation & m, const BiasZero &)
+    { return m; }
+
+    friend Motion operator+ (const MotionTranslation & m1, const Motion & m2)
+    {
+      return Motion (m2.linear () + m1.v, m2.angular ());
+    }
+
+    struct ConstraintTranslationSubspace
+    {
+    public:
+
+      Motion operator* (const MotionTranslation & vj) const
+      { return Motion (vj (), Motion::Vector3::Zero ()); }
+
+      ConstraintTranslationSubspace () {}
+
+      struct ConstraintTranspose
+      {
+        const ConstraintTranslationSubspace & ref;
+        ConstraintTranspose(const ConstraintTranslationSubspace & ref) : ref(ref) {}
+
+        Force::Vector3 operator* (const Force & phi)
+        {
+          return phi.linear ();
+        }
+
+
+        /* [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 &, const Eigen::MatrixBase<D> & F )
+        {
+          assert(F.rows()==6);
+          return  F.template middleRows <3> (Inertia::LINEAR);
+        }
+      }; // struct ConstraintTranspose
+
+      ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }
+
+      operator ConstraintXd () const
+      {
+        Eigen::Matrix<double,6,3> S;
+        S.block <3,3> (Inertia::LINEAR, 0).setIdentity ();
+        S.block <3,3> (Inertia::ANGULAR, 0).setZero ();
+        return ConstraintXd(S);
+      }
+
+      Eigen::Matrix <double,6,3> se3Action (const SE3 & m) const
+      {
+        Eigen::Matrix <double,6,3> M;
+        M.block <3,3> (Motion::LINEAR, 0) = m.rotation ();
+        M.block <3,3> (Motion::ANGULAR, 0).setZero ();
+
+        return M;
+      }
+
+    }; // struct ConstraintTranslationSubspace
+
+    template<typename D>
+    friend Motion operator* (const ConstraintTranslationSubspace & S, const Eigen::MatrixBase<D> & v)
+    {
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
+      return Motion (v, Motion::Vector3::Zero ());
+    }
+
+  }; // struct JointTranslation
+
+  Motion operator^ (const Motion & m1, const JointTranslation::MotionTranslation & m2)
+  {
+    return Motion (m1.angular ().cross (m2.v), Motion::Vector3::Zero ());
+  }
+
+  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
+  Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const JointTranslation::ConstraintTranslationSubspace & S)
+  {
+    Eigen::Matrix <double, 6, 3> M;
+    M.block <3,3> (Inertia::ANGULAR, 0) = alphaSkew(Y.mass (), Y.lever ());
+    //    M.block <3,3> (Inertia::LINEAR, 0) = Y.mass () * Inertia::Matrix3::Identity ();
+    M.block <3,3> (Inertia::LINEAR, 0).setZero ();
+    M.block <3,3> (Inertia::LINEAR, 0).diagonal ().fill (Y.mass ());
+
+    return M;
+  }
+
+  namespace internal
+  {
+    template<>
+    struct ActionReturn<JointTranslation::ConstraintTranslationSubspace >
+    { typedef Eigen::Matrix<double,6,3> Type; };
+  }
+
+
+  template<>
+  struct traits<JointTranslation>
+  {
+    typedef JointDataTranslation JointData;
+    typedef JointModelTranslation JointModel;
+    typedef JointTranslation::ConstraintTranslationSubspace Constraint_t;
+    typedef SE3 Transformation_t;
+    typedef JointTranslation::MotionTranslation Motion_t;
+    typedef JointTranslation::BiasZero Bias_t;
+    typedef Eigen::Matrix<double,6,3> F_t;
+    enum {
+      NQ = 3,
+      NV = 3
+    };
+  };
+  
+  template<> struct traits<JointDataTranslation> { typedef JointTranslation Joint; };
+  template<> struct traits<JointModelTranslation> { typedef JointTranslation Joint; };
+
+  struct JointDataTranslation : public JointDataBase<JointDataTranslation>
+  {
+    typedef JointTranslation Joint;
+    SE3_JOINT_TYPEDEF;
+
+    typedef Eigen::Matrix<double,6,6> Matrix6;
+    typedef Eigen::Matrix<double,3,3> Matrix3;
+    typedef Eigen::Matrix<double,3,1> Vector3;
+
+    Constraint_t S;
+    Transformation_t M;
+    Motion_t v;
+    Bias_t c;
+
+    JointDataTranslation () : M(1) {}
+  };
+
+  struct JointModelTranslation : public JointModelBase<JointModelTranslation>
+  {
+    typedef JointTranslation Joint;
+    SE3_JOINT_TYPEDEF;
+
+    JointData createData() const { return JointData(); }
+
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs) const
+    {
+      data.M.translation (qs.segment<NQ>(idx_q ()));
+    }
+    void calc (JointData & data,
+               const Eigen::VectorXd & qs,
+               const Eigen::VectorXd & vs ) const
+    {
+      data.M.translation (qs.segment<NQ> (idx_q ()));
+      data.v () = vs.segment<NQ> (idx_v ());
+    }
+  };
+  
+} // namespace se3
+
+#endif // ifndef __se3_joint_translation_hpp__

src/multibody/joint/joint-variant.hpp

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

unittest/joints.cpp

@@ -9,6 +9,7 @@
 #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-translation.hpp"
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/algorithm/crba.hpp"
@@ -406,6 +407,205 @@ BOOST_AUTO_TEST_CASE ( test_crba )
 
 BOOST_AUTO_TEST_SUITE_END ()
 
+BOOST_AUTO_TEST_SUITE ( JointTranslation )
+
+BOOST_AUTO_TEST_CASE ( test_kinematics )
+{
+  using namespace se3;
+
+  typedef Motion::Vector3 Vector3;
+  typedef Motion::Vector6 Vector6;
+
+  Motion expected_v_J (Motion::Zero ());
+  Motion expected_c_J (Motion::Zero ());
+
+  SE3 expected_configuration (SE3::Identity ());
+
+  JointDataTranslation joint_data;
+  JointModelTranslation joint_model;
+
+  joint_model.setIndexes (0, 0, 0);
+
+  Vector3 q (Vector3::Zero());
+  Vector3 q_dot (Vector3::Zero());
+
+  // -------
+  q = Vector3 (0., 0., 0.);
+  q_dot = Vector3 (0., 0., 0.);
+
+  joint_model.calc (joint_data, q, q_dot);
+
+  printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+  is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation());
+  is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation ());
+  is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector());
+  is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector());
+
+  // -------
+  q = Vector3 (1., 0., 0.);
+  q_dot = Vector3 (1., 0., 0.);
+
+  joint_model.calc (joint_data, q, q_dot);
+
+  printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+  expected_configuration.translation () << 1, 0, 0;
+
+  expected_v_J.linear () << 1., 0., 0.;
+
+  is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+  is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+  is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+  is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+
+  // -------
+  q = Vector3 (0., 1., 0.);
+  q_dot = Vector3 (0., 1., 0.);
+
+  joint_model.calc (joint_data, q, q_dot);
+
+  printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+  expected_configuration.translation () << 0, 1., 0;
+
+  expected_v_J.linear () << 0., 1., 0.;
+
+  is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+  is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+  is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+  is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+
+  // -------
+  q = Vector3 (0., 0., 1.);
+  q_dot = Vector3 (0., 0., 1.);
+
+  joint_model.calc (joint_data, q, q_dot);
+
+  printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+  expected_configuration.translation () << 0, 0, 1;
+
+  expected_v_J.linear () << 0., 0., 1.;
+
+  is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-12);
+  is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-12);
+  is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-12);
+  is_matrix_absolutely_closed (expected_c_J.toVector (), ((Motion) joint_data.c).toVector(), 1e-12);
+
+  // -------
+  q = Vector3 (1., 1., 1.);
+  q_dot = Vector3 (1., 1., 1.);
+
+  joint_model.calc (joint_data, q, q_dot);
+
+  printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+  expected_configuration.translation () << 1., 1., 1.;
+  expected_v_J.linear () << 1., 1., 1.;
+
+  is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+  is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+  is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+  is_matrix_absolutely_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.);
+
+  joint_model.calc (joint_data, q, q_dot);
+
+  printOutJointData <JointDataTranslation> (q, q_dot, joint_data);
+
+  expected_configuration.translation () = q;
+  expected_v_J.linear () = q_dot;
+
+  is_matrix_absolutely_closed (expected_configuration.rotation (), joint_data.M.rotation(), 1e-10);
+  is_matrix_absolutely_closed (expected_configuration.translation (), joint_data.M.translation (), 1e-10);
+  is_matrix_absolutely_closed (expected_v_J.toVector (), ((Motion) joint_data.v).toVector(), 1e-10);
+  is_matrix_absolutely_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;
+
+  Model model;
+  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
+
+  model.addBody (model.getBodyId("universe"), JointModelTranslation (), SE3::Identity (), inertia, "root");
+
+  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));
+
+  rnea (model, data, q, v, a);
+  Vector3 tau_expected (Vector3::Zero ());
+
+  tau_expected  << 0,    0, 9.81;
+
+  is_matrix_absolutely_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);
+
+  rnea (model, data, q, v, a);
+  tau_expected << 1,     1, 10.81;
+
+  is_matrix_absolutely_closed (tau_expected, data.tau, 1e-12);
+
+  q << 3, 2, 1;
+  v = Eigen::VectorXd::Ones (model.nv);
+  a = Eigen::VectorXd::Ones (model.nv);
+
+  rnea (model, data, q, v, a);
+  tau_expected << 1,     1, 10.81;
+
+  is_matrix_absolutely_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;
+
+  Model model;
+  Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
+
+  model.addBody (model.getBodyId("universe"), JointModelTranslation (), SE3::Identity (), inertia, "root");
+
+  Data data (model);
+
+  Eigen::VectorXd q (Eigen::VectorXd::Zero (model.nq));
+  Eigen::MatrixXd M_expected (model.nv,model.nv);
+
+  crba (model, data, q);
+  M_expected = Matrix3::Identity ();
+
+  is_matrix_absolutely_closed (M_expected, data.M, 1e-14);
+
+  q = Eigen::VectorXd::Ones (model.nq);
+
+  crba (model, data, q);
+
+  is_matrix_absolutely_closed (M_expected, data.M, 1e-12);
+
+  q << 3, 2, 1;
+  
+  crba (model, data, q);
+  
+  is_matrix_absolutely_closed (M_expected, data.M, 1e-10);
+}
+
+BOOST_AUTO_TEST_SUITE_END ()
+
 
 BOOST_AUTO_TEST_SUITE ( JointSpherical )