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,6,6,_Options> Matrix6;
18 typedef Matrix6 ActionMatrixType;
19 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
20 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
21 typedef typename Vector6::template FixedSegmentReturnType<3>::Type LinearType;
22 typedef typename Vector6::template FixedSegmentReturnType<3>::Type AngularType;
23 typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
24 typedef typename Vector6::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
36 template<
typename _Scalar,
int _Options>
40 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 enum { Options = _Options };
45 using Base::operator=;
50 using Base::__opposite__;
51 using Base::__minus__;
57 MotionTpl() : m_data() {}
59 template<
typename V1,
typename V2>
60 MotionTpl(
const Eigen::MatrixBase<V1> & v,
const Eigen::MatrixBase<V2> & w)
62 assert(v.size() == 3);
63 assert(w.size() == 3);
64 linear() = v; angular() = w;
68 explicit MotionTpl(
const Eigen::MatrixBase<V6> & v)
71 EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6);
72 assert(v.size() == 6);
76 explicit MotionTpl(
const MotionTpl<Scalar,O2> & clone)
77 : m_data(clone.toVector())
81 explicit MotionTpl(
const MotionDense<M2> & clone)
82 { linear() = clone.linear(); angular() = clone.angular(); }
85 explicit MotionTpl(
const MotionBase<M2> & clone)
89 static MotionTpl Zero() {
return MotionTpl(Vector6::Zero()); }
90 static MotionTpl Random() {
return MotionTpl(Vector6::Random()); }
92 inline PlainReturnType plain()
const {
return *
this; }
94 ToVectorConstReturnType toVector_impl()
const {
return m_data; }
95 ToVectorReturnType toVector_impl() {
return m_data; }
98 ConstAngularType angular_impl()
const {
return m_data.template segment<3> (ANGULAR); }
99 ConstLinearType linear_impl()
const {
return m_data.template segment<3> (LINEAR); }
100 AngularType angular_impl() {
return m_data.template segment<3> (ANGULAR); }
101 LinearType linear_impl() {
return m_data.template segment<3> (LINEAR); }
103 template<
typename V3>
104 void angular_impl(
const Eigen::MatrixBase<V3> & w)
106 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
109 template<
typename V3>
110 void linear_impl(
const Eigen::MatrixBase<V3> & v)
112 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
118 MotionPlain __plus__(
const MotionTpl<Scalar,O2> & v)
const
119 {
return MotionPlain(m_data+v.toVector()); }
121 template<
typename Vector6ArgType>
122 MotionPlain __plus__(
const MotionRef<Vector6ArgType> & v)
const
123 {
return MotionPlain(m_data+v.toVector()); }
126 MotionPlain __minus__(
const MotionTpl<Scalar,O2> & v)
const
127 {
return MotionPlain(m_data-v.toVector()); }
129 template<
typename Vector6ArgType>
130 MotionPlain __minus__(
const MotionRef<Vector6ArgType> & v)
const
131 {
return MotionPlain(m_data-v.toVector()); }
134 MotionTpl & __pequ__(
const MotionTpl<Scalar,O2> & v)
135 { m_data += v.toVector();
return *
this; }
137 template<
typename Vector6ArgType>
138 MotionTpl & __pequ__(
const MotionRef<Vector6ArgType> & v)
139 { m_data += v.toVector();
return *
this; }
142 MotionTpl & __mequ__(
const MotionTpl<Scalar,O2> & v)
143 { m_data -= v.toVector();
return *
this; }
145 template<
typename Vector6ArgType>
146 MotionTpl & __mequ__(
const MotionRef<Vector6ArgType> & v)
147 { m_data -= v.toVector();
return *
this; }
149 template<
typename OtherScalar>
150 MotionPlain __mult__(
const OtherScalar & alpha)
const
151 {
return MotionPlain(alpha*m_data); }
153 MotionRef<Vector6> ref() {
return MotionRef<Vector6>(m_data); }
156 template<
typename NewScalar>
160 ReturnType res(linear().
template cast<NewScalar>(),
161 angular().
template cast<NewScalar>());
172 #endif // ifndef __pinocchio_motion_tpl_hpp__