Commit 70d02c2d authored by jcarpent's avatar jcarpent
Browse files

[Joint] Clean JointModel

parent 343e98a4
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment