Verified Commit a7da440c authored by Justin Carpentier's avatar Justin Carpentier
Browse files

motion: add plain methods and cast operator to PlainReturnType

parent db123798
......@@ -47,6 +47,7 @@ namespace pinocchio
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -54,7 +55,8 @@ namespace pinocchio
}; // traits MotionPlanarTpl
template<typename _Scalar, int _Options>
struct MotionPlanarTpl : MotionBase< MotionPlanarTpl<_Scalar,_Options> >
struct MotionPlanarTpl
: MotionBase< MotionPlanarTpl<_Scalar,_Options> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MOTION_TYPEDEF_TPL(MotionPlanarTpl);
......@@ -75,11 +77,11 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
}
// operator MotionPlain() const
// {
// return MotionPlain(typename MotionPlain::Vector3(m_x_dot,m_y_dot,Scalar(0)),
// typename MotionPlain::Vector3(Scalar(0),Scalar(0),m_theta_dot));
// }
inline PlainReturnType plain() const
{
return PlainReturnType(typename PlainReturnType::Vector3(m_x_dot,m_y_dot,Scalar(0)),
typename PlainReturnType::Vector3(Scalar(0),Scalar(0),m_theta_dot));
}
template<typename Derived>
void addTo(MotionDense<Derived> & other) const
......
......@@ -47,6 +47,7 @@ namespace pinocchio
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -54,7 +55,8 @@ namespace pinocchio
}; // traits MotionPrismaticUnalignedTpl
template<typename _Scalar, int _Options>
struct MotionPrismaticUnalignedTpl : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
struct MotionPrismaticUnalignedTpl
: MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);
......@@ -67,10 +69,10 @@ namespace pinocchio
: axis(axis), rate(rate)
{ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
operator MotionPlain() const
inline PlainReturnType plain() const
{
return MotionPlain(axis*rate,
MotionPlain::Vector3::Zero());
return PlainReturnType(axis*rate,
PlainReturnType::Vector3::Zero());
}
template<typename OtherScalar>
......
......@@ -46,6 +46,7 @@ namespace pinocchio
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -67,7 +68,7 @@ namespace pinocchio
MotionPrismaticTpl() {}
MotionPrismaticTpl(const Scalar & v) : rate(v) {}
inline operator MotionPlain() const { return Axis() * rate; }
inline PlainReturnType plain() const { return Axis() * rate; }
template<typename OtherScalar>
MotionPrismaticTpl __mult__(const OtherScalar & alpha) const
......
......@@ -46,6 +46,7 @@ namespace pinocchio
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -53,7 +54,8 @@ namespace pinocchio
}; // traits MotionRevoluteUnalignedTpl
template<typename _Scalar, int _Options>
struct MotionRevoluteUnalignedTpl : MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
struct MotionRevoluteUnalignedTpl
: MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl);
......@@ -67,10 +69,10 @@ namespace pinocchio
, w(w)
{}
inline operator MotionPlain() const
inline PlainReturnType plain() const
{
return MotionPlain(MotionPlain::Vector3::Zero(),
axis*w);
return PlainReturnType(PlainReturnType::Vector3::Zero(),
axis*w);
}
template<typename OtherScalar>
......
......@@ -46,6 +46,7 @@ namespace pinocchio
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -190,7 +191,8 @@ namespace pinocchio
};
template<typename _Scalar, int _Options, int axis>
struct MotionRevoluteTpl : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
struct MotionRevoluteTpl
: MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -210,7 +212,7 @@ namespace pinocchio
EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
}
operator MotionPlain() const { return Axis() * w; }
inline PlainReturnType plain() const { return Axis() * w; }
template<typename OtherScalar>
MotionRevoluteTpl __mult__(const OtherScalar & alpha) const
......
......@@ -47,6 +47,7 @@ namespace pinocchio
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -54,7 +55,8 @@ namespace pinocchio
}; // traits MotionSphericalTpl
template<typename _Scalar, int _Options>
struct MotionSphericalTpl : MotionBase< MotionSphericalTpl<_Scalar,_Options> >
struct MotionSphericalTpl
: MotionBase< MotionSphericalTpl<_Scalar,_Options> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -70,10 +72,10 @@ namespace pinocchio
Vector3 & operator() () { return w; }
const Vector3 & operator() () const { return w; }
// operator MotionPlain() const
// {
// return MotionPlain(MotionPlain::Vector3::Zero(), w);
// }
inline PlainReturnType plain() const
{
return PlainReturnType(PlainReturnType::Vector3::Zero(), w);
}
template<typename MotionDerived>
void addTo(MotionDense<MotionDerived> & other) const
......
......@@ -46,6 +46,7 @@ namespace pinocchio
typedef const Vector3 ConstLinearType;
typedef Matrix6 ActionMatrixType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3
......@@ -53,7 +54,8 @@ namespace pinocchio
}; // traits MotionTranslationTpl
template<typename _Scalar, int _Options>
struct MotionTranslationTpl : MotionBase< MotionTranslationTpl<_Scalar,_Options> >
struct MotionTranslationTpl
: MotionBase< MotionTranslationTpl<_Scalar,_Options> >
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -73,10 +75,10 @@ namespace pinocchio
Vector3 & operator()() { return rate; }
const Vector3 & operator()() const { return rate; }
// operator MotionPlain() const
// {
// return MotionPlain(rate,MotionPlain::Vector3::Zero());
// }
inline PlainReturnType plain() const
{
return PlainReturnType(rate,PlainReturnType::Vector3::Zero());
}
MotionTranslationTpl & operator=(const MotionTranslationTpl & other)
{
......
......@@ -31,6 +31,9 @@ namespace pinocchio
void linear(const Eigen::MatrixBase<V3Like> & v)
{ derived().linear_impl(v.derived()); }
operator PlainReturnType() const { return derived().plain(); }
PlainReturnType plain() const { return derived().plain(); }
ToVectorConstReturnType toVector() const { return derived().toVector_impl(); }
ToVectorReturnType toVector() { return derived().toVector_impl(); }
operator Vector6() const { return toVector(); }
......
......@@ -26,6 +26,7 @@ namespace pinocchio
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
typedef MotionTpl<Scalar,Options> MotionPlain;
typedef MotionPlain PlainReturnType;
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
typedef DataRefType ToVectorReturnType;
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
......@@ -157,6 +158,8 @@ namespace pinocchio
{ return MotionPlain(alpha*m_ref); }
MotionRef & ref() { return *this; }
inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
protected:
DataRefType m_ref;
......@@ -218,6 +221,8 @@ namespace pinocchio
{ return MotionPlain(alpha*m_ref); }
const MotionRef & ref() const { return *this; }
inline PlainReturnType plain() const { return PlainReturnType(m_ref); }
protected:
DataRefType m_ref;
......
......@@ -23,6 +23,7 @@ namespace pinocchio
typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
typedef MotionTpl<Scalar,_Options> MotionPlain;
typedef const MotionPlain & PlainReturnType;
enum {
LINEAR = 0,
ANGULAR = 3,
......@@ -88,6 +89,8 @@ namespace pinocchio
static MotionTpl Zero() { return MotionTpl(Vector6::Zero()); }
static MotionTpl Random() { return MotionTpl(Vector6::Random()); }
inline PlainReturnType plain() const { return *this; }
ToVectorConstReturnType toVector_impl() const { return m_data; }
ToVectorReturnType toVector_impl() { return m_data; }
......
//
// Copyright (c) 2015-2017 CNRS
// Copyright (c) 2015-2019 CNRS, INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
......@@ -42,15 +42,18 @@ namespace pinocchio
typedef Vector3 LinearType;
typedef const Vector3 ConstLinearType;
typedef Motion MotionPlain;
typedef MotionPlain PlainReturnType;
}; // traits BiasZeroTpl
template<typename Scalar, int Options>
struct BiasZeroTpl : public MotionBase< BiasZeroTpl<Scalar,Options> >
struct BiasZeroTpl
: public MotionBase< BiasZeroTpl<Scalar,Options> >
{
typedef typename traits<BiasZeroTpl>::MotionPlain MotionPlain;
typedef typename traits<BiasZeroTpl>::PlainReturnType PlainReturnType;
// operator MotionPlain () const { return MotionPlain::Zero(); }
static PlainReturnType plain() { return MotionPlain::Zero(); }
template<typename D2>
static bool isEqual_impl(const MotionDense<D2> & other)
......
......@@ -23,6 +23,7 @@ typedef TYPENAME traits<Derived>::ConstAngularType ConstAngularType; \
typedef TYPENAME traits<Derived>::ConstLinearType ConstLinearType; \
typedef TYPENAME traits<Derived>::ActionMatrixType ActionMatrixType; \
typedef TYPENAME traits<Derived>::MotionPlain MotionPlain; \
typedef TYPENAME traits<Derived>::PlainReturnType PlainReturnType; \
enum { \
LINEAR = traits<Derived>::LINEAR, \
ANGULAR = traits<Derived>::ANGULAR \
......
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