[Joint] Clean JointModel

src/multibody/joint/joint.hpp

@@ -52,21 +52,22 @@ namespace se3
     typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
     typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
   };
+  
   template<> struct traits<JointData> { typedef Joint JointDerived; };
   template<> struct traits<JointModel> { typedef Joint JointDerived; };
 
   struct JointData : public JointDataBase<JointData> , JointDataVariant
   {
-    typedef JointDataVariant JointDataBoostVariant;
-
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     typedef Joint JointDerived;
+    
     SE3_JOINT_TYPEDEF;
 
-    JointDataVariant& toVariant() { return *static_cast<JointDataVariant*>(this); }
-    const JointDataVariant& toVariant() const { return *static_cast<const JointDataVariant*>(this); }
+    JointDataVariant & toVariant() { return *static_cast<JointDataVariant*>(this); }
+    const JointDataVariant & toVariant() const { return *static_cast<const JointDataVariant*>(this); }
 
     const Constraint_t      S() const  { return constraint_xd(*this); }
-    const Transformation_t  M() const  { return joint_transform(*this); } // featherstone book, page 78 (sXp)
+    const Transformation_t  M() const  { return joint_transform(*this); }
     const Motion_t          v() const  { return motion(*this); }
     const Bias_t            c() const  { return bias(*this); }
     
@@ -76,24 +77,25 @@ namespace se3
     const D_t               Dinv()  const { return dinv_inertia(*this); }
     const UD_t              UDinv() const { return udinv_inertia(*this); }
 
-    JointData() : JointDataBoostVariant() {}
-    JointData(const JointDataVariant & jdata) : JointDataBoostVariant(jdata) {}
+    JointData() : JointDataVariant() {}
+    JointData(const JointDataVariant & jdata) : JointDataVariant(jdata) {}
 
   };
 
   struct JointModel : public JointModelBase<JointModel> , JointModelVariant
   {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     typedef JointModelVariant JointModelBoostVariant;
-
     typedef Joint JointDerived;
+    
     SE3_JOINT_TYPEDEF;
     SE3_JOINT_USE_INDEXES;
     using Base::id;
     using Base::setIndexes;
     using Base::operator==;
 
-    JointModel() : JointModelBoostVariant() {}
-    JointModel(const JointModelVariant & model_variant) : JointModelBoostVariant(model_variant)
+    JointModel() : JointModelVariant() {}
+    JointModel(const JointModelVariant & model_variant) : JointModelVariant(model_variant)
     {}
     
     JointModelVariant& toVariant() { return *static_cast<JointModelVariant*>(this); }
@@ -103,50 +105,32 @@ namespace se3
 
     void calc(JointData & data,const Eigen::VectorXd & q) const { calc_zero_order(*this,data,q); }
 
-    void calc (JointData & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) const
-    {
-      calc_first_order(*this,data,q,v);
-    }
+    void calc(JointData & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) const
+    { calc_first_order(*this,data,q,v); }
     
     void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
-    {
-      ::se3::calc_aba(*this,data,I,update_I);
-    }
+    { ::se3::calc_aba(*this,data,I,update_I); }
 
     ConfigVector_t integrate_impl(const Eigen::VectorXd & q,const Eigen::VectorXd & v) const
-    {
-      return ::se3::integrate(*this,q,v);
-    }
+    { return ::se3::integrate(*this,q,v); }
 
     ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
-    {
-      return ::se3::interpolate(*this,q0,q1,u);
-    }
+    { return ::se3::interpolate(*this,q0,q1,u); }
 
     ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit) const throw (std::runtime_error)
-    { 
-      return ::se3::randomConfiguration(*this,lower_pos_limit,upper_pos_limit);
-    } 
+    { return ::se3::randomConfiguration(*this,lower_pos_limit,upper_pos_limit); }
 
     TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
-    { 
-      return ::se3::difference(*this,q0,q1);
-    } 
+    { return ::se3::difference(*this,q0,q1); }
 
     double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
-    { 
-      return ::se3::distance(*this,q0,q1);
-    }
+    { return ::se3::distance(*this,q0,q1); }
 
     void normalize_impl(Eigen::VectorXd & q) const
-    {
-      return ::se3::normalize(*this,q);
-    }
+    { return ::se3::normalize(*this,q); }
 
     ConfigVector_t neutralConfiguration_impl() const
-    { 
-      return ::se3::neutralConfiguration(*this);
-    } 
+    { return ::se3::neutralConfiguration(*this); } 
 
     std::string shortname() const { return ::se3::shortname(*this); }
     static std::string classname() { return "JointModel"; }
@@ -162,7 +146,7 @@ namespace se3
     void setIndexes(JointIndex id,int nq,int nv) { ::se3::setIndexes(*this,id, nq, nv); }
   };
   
-  typedef std::vector<JointData> JointDataVector;  
+  typedef std::vector<JointData> JointDataVector;
   typedef std::vector<JointModel> JointModelVector;
 
 } // namespace se3