Verified Commit b19fcb6f authored by jcarpent's avatar jcarpent Committed by Justin Carpentier
Browse files

[Spatial] Templatization of BiasZero + renaming in BiasZeroTpl

This templatization is mandatory for JointCompositeTpl
parent a88f1f65
......@@ -158,7 +158,7 @@ namespace se3
typedef ConstraintIdentityTpl<Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionTpl<Scalar,Options> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -329,7 +329,7 @@ namespace se3
typedef ConstraintPlanarTpl<Scalar,Options> Constraint_t;
typedef SE3 Transformation_t;
typedef MotionPlanarTpl<Scalar,Options> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -305,7 +305,7 @@ namespace se3
typedef ConstraintPrismaticUnaligned<Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionPrismaticUnaligned<Scalar,Options> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -355,7 +355,7 @@ namespace se3
typedef ConstraintPrismatic<Scalar,Options,axis> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionPrismatic<Scalar,Options,axis> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -313,7 +313,7 @@ namespace se3
typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -43,7 +43,7 @@ namespace se3
typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
typedef SE3 Transformation_t;
typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -399,7 +399,7 @@ namespace se3
typedef ConstraintRevoluteTpl<Scalar,Options,axis> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionRevoluteTpl<Scalar,Options,axis> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -252,7 +252,7 @@ namespace se3
typedef ConstraintSphericalTpl<Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionSpherical<Scalar,Options> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -274,7 +274,7 @@ namespace se3
typedef ConstraintTranslationTpl<Scalar,Options> Constraint_t;
typedef SE3Tpl<Scalar,Options> Transformation_t;
typedef MotionTranslationTpl<Scalar,Options> Motion_t;
typedef BiasZero Bias_t;
typedef BiasZeroTpl<Scalar,Options> Bias_t;
typedef Eigen::Matrix<Scalar,6,NV,Options> F_t;
// [ABA]
......
......@@ -30,7 +30,7 @@ namespace se3
template<typename Derived> class MotionDense;
template<typename Vector6ArgType> class MotionRef;
template<typename _Scalar, int _Options=0> class MotionTpl;
struct BiasZero;
template<typename Scalar, int Options=0> struct BiasZeroTpl;
template<typename Derived> class ForceBase;
template<typename Derived> class ForceDense;
......
......@@ -25,44 +25,48 @@ namespace se3
namespace internal
{
template<>
struct SE3GroupAction<BiasZero>
template<typename Scalar, int Options>
struct SE3GroupAction< BiasZeroTpl<Scalar,Options> >
{
typedef BiasZero ReturnType;
typedef BiasZeroTpl<Scalar,Options> ReturnType;
};
template<typename MotionDerived>
struct MotionAlgebraAction<BiasZero, MotionDerived>
template<typename Scalar, int Options, typename MotionDerived>
struct MotionAlgebraAction< BiasZeroTpl<Scalar,Options>, MotionDerived>
{
typedef BiasZero ReturnType;
typedef BiasZeroTpl<Scalar,Options> ReturnType;
};
}
template<>
struct traits<BiasZero>
template<typename _Scalar, int _Options>
struct traits< BiasZeroTpl<_Scalar,_Options> >
{
typedef double Scalar;
typedef Eigen::Matrix<double,3,1,0> Vector3;
typedef Eigen::Matrix<double,6,1,0> Vector6;
typedef Eigen::Matrix<double,3,3,0> Matrix3;
typedef Eigen::Matrix<double,6,6,0> Matrix6;
typedef EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
enum {
Options = _Options,
LINEAR = 0,
ANGULAR = 3
};
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef typename EIGEN_REF_CONSTTYPE(Vector6) ToVectorConstReturnType;
typedef typename EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef Matrix6 ActionMatrixType;
typedef Vector3 AngularType;
typedef const Vector3 ConstAngularType;
typedef Vector3 LinearType;
typedef const Vector3 ConstLinearType;
typedef Motion MotionPlain;
enum {
LINEAR = 0,
ANGULAR = 3
};
}; // traits BiasZero
}; // traits BiasZeroTpl
struct BiasZero : public MotionBase<BiasZero>
template<typename Scalar, int Options>
struct BiasZeroTpl : public MotionBase< BiasZeroTpl<Scalar,Options> >
{
typedef traits<BiasZero>::MotionPlain MotionPlain;
typedef typename traits<BiasZeroTpl>::MotionPlain MotionPlain;
operator MotionPlain () const { return MotionPlain::Zero(); }
template<typename D2>
......@@ -73,9 +77,9 @@ namespace se3
static void addTo(const MotionDense<D2> &) {}
template<typename M1>
BiasZero motionAction(const MotionBase<M1> &) const
BiasZeroTpl motionAction(const MotionBase<M1> &) const
{
return BiasZero();
return BiasZeroTpl();
}
template<typename S2, int O2, typename D2>
......@@ -85,9 +89,9 @@ namespace se3
}
template<typename S2, int O2>
BiasZero se3Action_impl(const SE3Tpl<S2,O2> &) const
BiasZeroTpl se3Action_impl(const SE3Tpl<S2,O2> &) const
{
return BiasZero();
return BiasZeroTpl();
}
template<typename S2, int O2, typename D2>
......@@ -97,20 +101,20 @@ namespace se3
}
template<typename S2, int O2>
BiasZero se3ActionInverse_impl(const SE3Tpl<S2,O2> &) const
BiasZeroTpl se3ActionInverse_impl(const SE3Tpl<S2,O2> &) const
{
return BiasZero();
return BiasZeroTpl();
}
}; // struct BiasZero
}; // struct BiasZeroTpl
template<typename M1>
template<typename M1, typename Scalar, int Options>
inline const M1 & operator+(const MotionBase<M1> & v,
const BiasZero&)
const BiasZeroTpl<Scalar,Options> &)
{ return v.derived(); }
template<typename M1>
inline const M1 & operator+(const BiasZero&,
template<typename Scalar, int Options, typename M1>
inline const M1 & operator+(const BiasZeroTpl<Scalar,Options> &,
const MotionBase<M1> & v)
{ return v.derived(); }
......
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