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 >
65 template<
typename Vector6ArgType>
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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>
125 {
return MotionPlain(m_ref+v.toVector()); }
127 template<
typename Vector6Like>
129 {
return MotionPlain(m_ref+v.toVector()); }
131 template<
typename S1,
int O1>
133 {
return MotionPlain(m_ref-v.toVector()); }
135 template<
typename Vector6Like>
137 {
return MotionPlain(m_ref-v.toVector()); }
139 template<
typename S1,
int O1>
141 { m_ref += v.toVector();
return *
this; }
143 template<
typename Vector6Like>
145 { m_ref += v.toVector();
return *
this; }
147 template<
typename S1,
int O1>
149 { m_ref -= v.toVector();
return *
this; }
151 template<
typename Vector6Like>
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__
Return type of the ation of a Motion onto an object of type D.
MotionRef(const MotionRef &other)
Copy constructor from another MotionRef.
MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
Default constructor from a 6 dimensional vector.
Main pinocchio namespace.
Common traits structure to fully define base classes for CRTP.