6 #ifndef __pinocchio_motion_tpl_hpp__
7 #define __pinocchio_motion_tpl_hpp__
11 template<
typename _Scalar,
int _Options>
14 typedef _Scalar Scalar;
15 typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
16 typedef Eigen::Matrix<Scalar,6,1,_Options> Vector6;
17 typedef Eigen::Matrix<Scalar,4,4,_Options> Matrix4;
18 typedef Eigen::Matrix<Scalar,6,6,_Options> Matrix6;
19 typedef Matrix6 ActionMatrixType;
20 typedef Matrix4 HomogeneousMatrixType;
21 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
22 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
23 typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
24 typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
25 typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
26 typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
38 template<
typename _Scalar,
int _Options>
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45 enum { Options = _Options };
47 using Base::operator=;
52 using Base::__opposite__;
53 using Base::__minus__;
59 MotionTpl() : m_data() {}
61 template<
typename V1,
typename V2>
62 MotionTpl(
const Eigen::MatrixBase<V1> & v,
const Eigen::MatrixBase<V2> & w)
64 assert(v.size() == 3);
65 assert(w.size() == 3);
66 linear() = v; angular() = w;
70 explicit MotionTpl(
const Eigen::MatrixBase<V6> & v)
73 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
74 assert(v.size() == 6);
78 explicit MotionTpl(
const MotionTpl<Scalar,O2> & clone)
79 : m_data(clone.toVector())
83 template <
typename M2,
84 typename std::enable_if<
85 !std::is_convertible<MotionDense<M2>, MotionTpl>::value,
87 explicit MotionTpl(
const MotionDense<M2> & clone)
88 { linear() = clone.linear(); angular() = clone.angular(); }
94 template <
typename M2,
95 typename std::enable_if<
96 !std::is_convertible<MotionBase<M2>, MotionTpl>::value,
98 explicit MotionTpl(
const MotionBase<M2> & clone)
102 static MotionTpl Zero() {
return MotionTpl(Vector6::Zero()); }
103 static MotionTpl Random() {
return MotionTpl(Vector6::Random()); }
105 inline PlainReturnType plain()
const {
return *
this; }
107 ToVectorConstReturnType toVector_impl()
const {
return m_data; }
108 ToVectorReturnType toVector_impl() {
return m_data; }
111 ConstAngularType angular_impl()
const {
return m_data.template segment<3> (ANGULAR); }
112 ConstLinearType linear_impl()
const {
return m_data.template segment<3> (LINEAR); }
113 AngularType angular_impl() {
return m_data.template segment<3> (ANGULAR); }
114 LinearType linear_impl() {
return m_data.template segment<3> (LINEAR); }
116 template<
typename V3>
117 void angular_impl(
const Eigen::MatrixBase<V3> & w)
119 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
122 template<
typename V3>
123 void linear_impl(
const Eigen::MatrixBase<V3> & v)
125 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
131 MotionPlain __plus__(
const MotionTpl<Scalar,O2> & v)
const
132 {
return MotionPlain(m_data+v.toVector()); }
134 template<
typename Vector6ArgType>
135 MotionPlain __plus__(
const MotionRef<Vector6ArgType> & v)
const
136 {
return MotionPlain(m_data+v.toVector()); }
139 MotionPlain __minus__(
const MotionTpl<Scalar,O2> & v)
const
140 {
return MotionPlain(m_data-v.toVector()); }
142 template<
typename Vector6ArgType>
143 MotionPlain __minus__(
const MotionRef<Vector6ArgType> & v)
const
144 {
return MotionPlain(m_data-v.toVector()); }
147 MotionTpl & __pequ__(
const MotionTpl<Scalar,O2> & v)
148 { m_data += v.toVector();
return *
this; }
150 template<
typename Vector6ArgType>
151 MotionTpl & __pequ__(
const MotionRef<Vector6ArgType> & v)
152 { m_data += v.toVector();
return *
this; }
155 MotionTpl & __mequ__(
const MotionTpl<Scalar,O2> & v)
156 { m_data -= v.toVector();
return *
this; }
158 template<
typename Vector6ArgType>
159 MotionTpl & __mequ__(
const MotionRef<Vector6ArgType> & v)
160 { m_data -= v.toVector();
return *
this; }
162 template<
typename OtherScalar>
163 MotionPlain __mult__(
const OtherScalar & alpha)
const
164 {
return MotionPlain(alpha*m_data); }
166 MotionRef<Vector6> ref() {
return MotionRef<Vector6>(m_data); }
169 template<
typename NewScalar>
173 ReturnType res(linear().
template cast<NewScalar>(),
174 angular().
template cast<NewScalar>());
185 #endif // ifndef __pinocchio_motion_tpl_hpp__