5 #ifndef __pinocchio_motion_ref_hpp__
6 #define __pinocchio_motion_ref_hpp__
11 template<
typename Vector6ArgType>
14 typedef typename Vector6ArgType::Scalar Scalar;
15 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
19 Options = Vector6::Options
21 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
22 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
23 typedef Matrix6 ActionMatrixType;
24 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType;
25 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType;
26 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
27 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
30 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
31 typedef DataRefType ToVectorReturnType;
32 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
33 typedef ConstDataRefType ToVectorConstReturnType;
38 template<
typename Vector6ArgType>
44 template<
typename Vector6ArgType,
typename MotionDerived>
52 template<
typename Vector6ArgType,
typename Scalar>
53 struct RHSScalarMultiplication<
MotionRef<Vector6ArgType>, Scalar >
58 template<
typename Vector6ArgType,
typename Scalar>
59 struct LHSScalarMultiplication< MotionRef<Vector6ArgType>, Scalar >
61 typedef typename traits< MotionRef<Vector6ArgType> >::MotionPlain ReturnType;
65 template<
typename Vector6ArgType>
66 class MotionRef :
public MotionDense< MotionRef<Vector6ArgType> >
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 typedef MotionDense<MotionRef> Base;
71 typedef typename traits<MotionRef>::DataRefType DataRefType;
74 using Base::operator=;
79 using Base::__opposite__;
80 using Base::__minus__;
86 MotionRef(
typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
89 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
90 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
91 assert(v_like.size() == 6);
99 ToVectorConstReturnType toVector_impl()
const {
return m_ref; }
100 ToVectorReturnType toVector_impl() {
return m_ref; }
103 ConstAngularType angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
104 ConstLinearType linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
105 AngularType angular_impl() {
return m_ref.template segment<3> (ANGULAR); }
106 LinearType linear_impl() {
return m_ref.template segment<3> (LINEAR); }
108 template<
typename V3>
109 void angular_impl(
const Eigen::MatrixBase<V3> & w)
111 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
115 template<
typename V3>
116 void linear_impl(
const Eigen::MatrixBase<V3> & v)
118 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
123 template<
typename S1,
int O1>
124 MotionPlain __plus__(
const MotionTpl<S1,O1> & v)
const
125 {
return MotionPlain(m_ref+v.toVector()); }
127 template<
typename Vector6Like>
128 MotionPlain __plus__(
const MotionRef<Vector6ArgType> & v)
const
129 {
return MotionPlain(m_ref+v.toVector()); }
131 template<
typename S1,
int O1>
132 MotionPlain __minus__(
const MotionTpl<S1,O1> & v)
const
133 {
return MotionPlain(m_ref-v.toVector()); }
135 template<
typename Vector6Like>
136 MotionPlain __minus__(
const MotionRef<Vector6ArgType> & v)
const
137 {
return MotionPlain(m_ref-v.toVector()); }
139 template<
typename S1,
int O1>
140 MotionRef & __pequ__(
const MotionTpl<S1,O1> & v)
141 { m_ref += v.toVector();
return *
this; }
143 template<
typename Vector6Like>
144 MotionRef & __pequ__(
const MotionRef<Vector6ArgType> & v)
145 { m_ref += v.toVector();
return *
this; }
147 template<
typename S1,
int O1>
148 MotionRef & __mequ__(
const MotionTpl<S1,O1> & v)
149 { m_ref -= v.toVector();
return *
this; }
151 template<
typename Vector6Like>
152 MotionRef & __mequ__(
const MotionRef<Vector6ArgType> & v)
153 { m_ref -= v.toVector();
return *
this; }
155 template<
typename OtherScalar>
156 MotionPlain __mult__(
const OtherScalar & alpha)
const
157 {
return MotionPlain(alpha*m_ref); }
161 inline PlainReturnType plain()
const {
return PlainReturnType(m_ref); }
168 template<
typename Vector6ArgType>
171 typedef typename Vector6ArgType::Scalar Scalar;
172 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
176 Options = Vector6::Options
178 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
179 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
180 typedef Matrix6 ActionMatrixType;
181 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
182 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
183 typedef ConstLinearType LinearType;
184 typedef ConstAngularType AngularType;
187 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
188 typedef ConstDataRefType ToVectorConstReturnType;
189 typedef ConstDataRefType DataRefType;
190 typedef DataRefType ToVectorReturnType;
195 template<
typename Vector6ArgType>
197 :
public MotionDense< MotionRef<const Vector6ArgType> >
200 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
205 using Base::operator=;
209 using Base::__plus__;
210 using Base::__opposite__;
211 using Base::__minus__;
212 using Base::__mult__;
214 MotionRef(
typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
217 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
218 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
219 assert(v_like.size() == 6);
222 ToVectorConstReturnType toVector_impl()
const {
return m_ref; }
225 ConstAngularType angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
226 ConstLinearType linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
229 template<
typename S1,
int O1>
231 {
return MotionPlain(m_ref+v.toVector()); }
233 template<
typename Vector6Like>
235 {
return MotionPlain(m_ref+v.toVector()); }
237 template<
typename S1,
int O1>
239 {
return MotionPlain(m_ref-v.toVector()); }
241 template<
typename Vector6Like>
243 {
return MotionPlain(m_ref-v.toVector()); }
245 template<
typename OtherScalar>
246 MotionPlain __mult__(
const OtherScalar & alpha)
const
247 {
return MotionPlain(alpha*m_ref); }
249 const MotionRef & ref()
const {
return *
this; }
251 inline PlainReturnType plain()
const {
return PlainReturnType(m_ref); }
260 #endif // ifndef __pinocchio_motion_ref_hpp__