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__