Program Listing for File motion-tpl.hpp
↰ Return to documentation for file (include/pinocchio/spatial/motion-tpl.hpp
)
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_spatial_motion_tpl_hpp__
#define __pinocchio_spatial_motion_tpl_hpp__
namespace pinocchio
{
template<typename _Scalar, int _Options>
struct traits<MotionTpl<_Scalar, _Options>>
{
typedef _Scalar Scalar;
typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
typedef Eigen::Matrix<Scalar, 6, 1, _Options> Vector6;
typedef Eigen::Matrix<Scalar, 4, 4, _Options> Matrix4;
typedef Eigen::Matrix<Scalar, 6, 6, _Options> Matrix6;
typedef Matrix6 ActionMatrixType;
typedef Matrix4 HomogeneousMatrixType;
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
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,
Options = _Options
};
typedef MotionRef<Vector6> MotionRefType;
}; // traits MotionTpl
template<typename _Scalar, int _Options>
class MotionTpl : public MotionDense<MotionTpl<_Scalar, _Options>>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef MotionDense<MotionTpl> Base;
MOTION_TYPEDEF_TPL(MotionTpl);
enum
{
Options = _Options
};
using Base::operator=;
using Base::angular;
using Base::linear;
using Base::__mequ__;
using Base::__minus__;
using Base::__mult__;
using Base::__opposite__;
using Base::__pequ__;
using Base::__plus__;
// Constructors
MotionTpl()
{
}
template<typename V1, typename V2>
MotionTpl(const Eigen::MatrixBase<V1> & v, const Eigen::MatrixBase<V2> & w)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V1);
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V2);
linear() = v;
angular() = w;
}
template<typename V6>
explicit MotionTpl(const Eigen::MatrixBase<V6> & v)
: m_data(v)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
}
MotionTpl(const MotionTpl & other)
{
*this = other;
}
template<typename S2, int O2>
explicit MotionTpl(const MotionTpl<S2, O2> & other)
{
*this = other.template cast<Scalar>();
}
template<int O2>
explicit MotionTpl(const MotionTpl<Scalar, O2> & clone)
: m_data(clone.toVector())
{
}
// Same explanation as converting constructor from MotionBase
template<
typename M2,
typename std::enable_if<!std::is_convertible<MotionDense<M2>, MotionTpl>::value, bool>::type =
true>
explicit MotionTpl(const MotionDense<M2> & clone)
{
linear() = clone.linear();
angular() = clone.angular();
}
// MotionBase implement a conversion function to PlainReturnType.
// Usually, PlainReturnType is defined as MotionTpl.
// In this case, this converting constructor is redundant and
// create a warning with -Wconversion
template<
typename M2,
typename std::enable_if<!std::is_convertible<MotionBase<M2>, MotionTpl>::value, bool>::type =
true>
explicit MotionTpl(const MotionBase<M2> & clone)
{
*this = clone;
}
MotionTpl & operator=(const MotionTpl & clone) // Copy assignment operator
{
m_data = clone.toVector();
return *this;
}
// initializers
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;
}
// Getters
ConstAngularType angular_impl() const
{
return m_data.template segment<3>(ANGULAR);
}
ConstLinearType linear_impl() const
{
return m_data.template segment<3>(LINEAR);
}
AngularType angular_impl()
{
return m_data.template segment<3>(ANGULAR);
}
LinearType linear_impl()
{
return m_data.template segment<3>(LINEAR);
}
template<typename V3>
void angular_impl(const Eigen::MatrixBase<V3> & w)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
angular_impl() = w;
}
template<typename V3>
void linear_impl(const Eigen::MatrixBase<V3> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3);
linear_impl() = v;
}
// Specific operators for MotionTpl and MotionRef
template<int O2>
MotionPlain __plus__(const MotionTpl<Scalar, O2> & v) const
{
return MotionPlain(m_data + v.toVector());
}
template<typename Vector6ArgType>
MotionPlain __plus__(const MotionRef<Vector6ArgType> & v) const
{
return MotionPlain(m_data + v.toVector());
}
template<int O2>
MotionPlain __minus__(const MotionTpl<Scalar, O2> & v) const
{
return MotionPlain(m_data - v.toVector());
}
template<typename Vector6ArgType>
MotionPlain __minus__(const MotionRef<Vector6ArgType> & v) const
{
return MotionPlain(m_data - v.toVector());
}
template<int O2>
MotionTpl & __pequ__(const MotionTpl<Scalar, O2> & v)
{
m_data += v.toVector();
return *this;
}
template<typename Vector6ArgType>
MotionTpl & __pequ__(const MotionRef<Vector6ArgType> & v)
{
m_data += v.toVector();
return *this;
}
template<int O2>
MotionTpl & __mequ__(const MotionTpl<Scalar, O2> & v)
{
m_data -= v.toVector();
return *this;
}
template<typename Vector6ArgType>
MotionTpl & __mequ__(const MotionRef<Vector6ArgType> & v)
{
m_data -= v.toVector();
return *this;
}
template<typename OtherScalar>
MotionPlain __mult__(const OtherScalar & alpha) const
{
return MotionPlain(alpha * m_data);
}
MotionRef<Vector6> ref()
{
return MotionRef<Vector6>(m_data);
}
template<typename NewScalar>
MotionTpl<NewScalar, Options> cast() const
{
typedef MotionTpl<NewScalar, Options> ReturnType;
ReturnType res(linear().template cast<NewScalar>(), angular().template cast<NewScalar>());
return res;
}
protected:
Vector6 m_data;
}; // class MotionTpl
} // namespace pinocchio
#endif // ifndef __pinocchio_spatial_motion_tpl_hpp__