Verified Commit 36a4f076 authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[Joint] Add cast operator to JointModelTpl

parent fff6b626
......@@ -183,7 +183,20 @@ namespace se3
*/
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline std::string shortname(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
/**
* @brief Visit a JointModelTpl<Scalar,...> to cast it into JointModelTpl<NewScalar,...>
*
* @tparam NewScalar new scalar type of of the JointModelTpl
*
* @param[in] jmodel The joint model to cast.
*
* @return A new JointModelTpl<NewScalar,...> casted from JointModelTpl<Scalar,...>.
*/
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type
cast_joint(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel);
//
// Visitors on JointDatas
......
......@@ -314,6 +314,28 @@ namespace se3
template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
inline std::string shortname(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel)
{ return JointShortnameVisitor::run(jmodel);}
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct JointCastVisitor
: fusion::JointVisitorBase< JointCastVisitor<NewScalar,Scalar,Options,JointCollectionTpl>, typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type >
{
typedef fusion::NoArg ArgsType;
typedef typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type ReturnType;
template<typename JointModelDerived>
static ReturnType algo(const JointModelBase<JointModelDerived> & jmodel)
{ return ReturnType(jmodel.template cast<NewScalar>()); }
};
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
typename CastType< NewScalar,JointModelTpl<Scalar,Options,JointCollectionTpl> >::type
cast_joint(const JointModelTpl<Scalar,Options,JointCollectionTpl> & jmodel)
{
typedef JointCastVisitor<NewScalar,Scalar,Options,JointCollectionTpl> Algo;
return Algo::run(jmodel);
}
//
// Visitors on JointDatas
......
......@@ -113,6 +113,12 @@ namespace se3
}
};
template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
struct CastType< NewScalar, JointModelTpl<Scalar,Options,JointCollectionTpl> >
{
typedef JointModelTpl<NewScalar,Options,JointCollectionTpl> type;
};
template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
struct JointModelTpl
......@@ -133,6 +139,7 @@ namespace se3
using Base::id;
using Base::setIndexes;
using Base::operator==;
using Base::operator!=;
JointModelTpl() : JointModelVariant() {}
......@@ -156,6 +163,13 @@ namespace se3
JointDataDerived createData()
{ return ::se3::createData<JointCollection>(*this); }
using Base::isEqual;
bool isEqual(const JointModelTpl & other) const
{
return Base::isEqual(other)
&& toVariant() == other.toVariant();
}
template<typename ConfigVector>
void calc(JointDataDerived & data,
const Eigen::MatrixBase<ConfigVector> & q) const
......@@ -183,6 +197,13 @@ namespace se3
void setIndexes(JointIndex id, int nq, int nv)
{ ::se3::setIndexes(*this,id, nq, nv); }
/// \returns An expression of *this with the Scalar type casted to NewScalar.
template<typename NewScalar>
JointModelTpl<NewScalar,Options,JointCollectionTpl> cast() const
{
return cast_joint<NewScalar,Scalar,Options,JointCollectionTpl>(*this);
}
};
typedef container::aligned_vector<JointData> JointDataVector;
......
Supports Markdown
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