From 70d02c2ddb9d0c484c6332313d467c681d115d63 Mon Sep 17 00:00:00 2001
From: jcarpent <jcarpent@laas.fr>
Date: Tue, 9 Aug 2016 13:21:14 +0200
Subject: [PATCH] [Joint] Clean JointModel
---
src/multibody/joint/joint.hpp | 62 +++++++++++++----------------------
1 file changed, 23 insertions(+), 39 deletions(-)
diff --git a/src/multibody/joint/joint.hpp b/src/multibody/joint/joint.hpp
index d56d9e98f..03892cfea 100644
--- a/src/multibody/joint/joint.hpp
+++ b/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
--
GitLab