5 #ifndef __pinocchio_motion_ref_hpp__ 6 #define __pinocchio_motion_ref_hpp__ 11 template<
typename Vector6ArgType>
21 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
22 typedef Eigen::Matrix<Scalar,4,4,Options>
Matrix4;
23 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
26 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type
LinearType;
27 typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type
AngularType;
28 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type
ConstLinearType;
29 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type
ConstAngularType;
40 template<
typename Vector6ArgType>
46 template<
typename Vector6ArgType,
typename MotionDerived>
54 template<
typename Vector6ArgType,
typename Scalar>
60 template<
typename Vector6ArgType,
typename Scalar>
67 template<
typename Vector6ArgType>
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
76 using Base::operator=;
81 using Base::__opposite__;
82 using Base::__minus__;
91 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
92 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
93 assert(v_like.size() == 6);
105 ConstAngularType
angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
106 ConstLinearType
linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
107 AngularType
angular_impl() {
return m_ref.template segment<3> (ANGULAR); }
108 LinearType
linear_impl() {
return m_ref.template segment<3> (LINEAR); }
110 template<
typename V3>
113 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
117 template<
typename V3>
120 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
125 template<
typename S1,
int O1>
127 {
return MotionPlain(m_ref+v.
toVector()); }
129 template<
typename Vector6Like>
131 {
return MotionPlain(m_ref+v.
toVector()); }
133 template<
typename S1,
int O1>
135 {
return MotionPlain(m_ref-v.
toVector()); }
137 template<
typename Vector6Like>
139 {
return MotionPlain(m_ref-v.
toVector()); }
141 template<
typename S1,
int O1>
143 { m_ref += v.
toVector();
return *
this; }
145 template<
typename Vector6Like>
147 { m_ref += v.
toVector();
return *
this; }
149 template<
typename S1,
int O1>
151 { m_ref -= v.
toVector();
return *
this; }
153 template<
typename Vector6Like>
155 { m_ref -= v.
toVector();
return *
this; }
157 template<
typename OtherScalar>
158 MotionPlain
__mult__(
const OtherScalar & alpha)
const 159 {
return MotionPlain(alpha*m_ref); }
163 inline PlainReturnType
plain()
const {
return PlainReturnType(m_ref); }
170 template<
typename Vector6ArgType>
180 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
181 typedef Eigen::Matrix<Scalar,4,4,Options>
Matrix4;
182 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
185 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type
ConstLinearType;
186 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type
ConstAngularType;
199 template<
typename Vector6ArgType>
201 :
public MotionDense< MotionRef<const Vector6ArgType> >
204 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
209 using Base::operator=;
213 using Base::__plus__;
214 using Base::__opposite__;
215 using Base::__minus__;
216 using Base::__mult__;
221 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
222 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
223 assert(v_like.size() == 6);
229 ConstAngularType
angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
230 ConstLinearType
linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
233 template<
typename S1,
int O1>
235 {
return MotionPlain(m_ref+v.
toVector()); }
237 template<
typename Vector6Like>
239 {
return MotionPlain(m_ref+v.
toVector()); }
241 template<
typename S1,
int O1>
243 {
return MotionPlain(m_ref-v.
toVector()); }
245 template<
typename Vector6Like>
247 {
return MotionPlain(m_ref-v.
toVector()); }
249 template<
typename OtherScalar>
250 MotionPlain
__mult__(
const OtherScalar & alpha)
const 251 {
return MotionPlain(alpha*m_ref); }
255 inline PlainReturnType
plain()
const {
return PlainReturnType(m_ref); }
264 #endif // ifndef __pinocchio_motion_ref_hpp__ Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstLinearType
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
ConstAngularType AngularType
MotionPlain __mult__(const OtherScalar &alpha) const
Matrix4 HomogeneousMatrixType
ConstDataRefType ToVectorConstReturnType
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstLinearType
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstAngularType
traits< MotionRef< Vector6ArgType > >::MotionPlain ReturnType
MotionPlain __mult__(const OtherScalar &alpha) const
traits< MotionRef< Vector6ArgType > >::MotionPlain ReturnType
void linear_impl(const Eigen::MatrixBase< V3 > &v)
MotionTpl< Scalar, Options > MotionPlain
Return type of the ation of a Motion onto an object of type D.
ConstDataRefType DataRefType
#define MOTION_TYPEDEF_TPL(Derived)
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef MotionDense< MotionRef > Base
ConstAngularType angular_impl() const
ConstDataRefType ToVectorConstReturnType
MotionRef & __mequ__(const MotionRef< Vector6ArgType > &v)
MotionPlain __minus__(const MotionRef< Vector6ArgType > &v) const
MotionRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) v_like)
MotionRef & __pequ__(const MotionRef< Vector6ArgType > &v)
MotionPlain PlainReturnType
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef MotionDense< MotionRef > Base
MotionRef(const MotionRef &other)
Copy constructor from another MotionRef.
MotionRef & __mequ__(const MotionTpl< S1, O1 > &v)
ConstLinearType LinearType
PlainReturnType plain() const
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
MotionRef< const Vector6ArgType > MotionRefType
traits< MotionRef< Vector6ArgType > >::MotionPlain ReturnType
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
ConstLinearType linear_impl() const
pinocchio::traits< MotionRef< Vector6ArgType > >::MotionPlain ReturnType
MotionRef & __pequ__(const MotionTpl< S1, O1 > &v)
ToVectorConstReturnType toVector_impl() const
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstAngularType
DataRefType ToVectorReturnType
ConstLinearType linear_impl() const
MotionTpl< Scalar, Options > MotionPlain
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
MotionPlain __minus__(const MotionRef< Vector6ArgType > &v) const
DataRefType ToVectorReturnType
MotionPlain PlainReturnType
#define PINOCCHIO_EIGEN_REF_TYPE(D)
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
Main pinocchio namespace.
ToVectorConstReturnType toVector() const
void angular_impl(const Eigen::MatrixBase< V3 > &w)
MotionRef< Vector6ArgType > MotionRefType
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
AngularType angular_impl()
MotionPlain __plus__(const MotionRef< Vector6ArgType > &v) const
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
MotionPlain __plus__(const MotionRef< Vector6ArgType > &v) const
const MotionRef & ref() const
Common traits structure to fully define base classes for CRTP.
traits< MotionRef >::DataRefType DataRefType
Matrix4 HomogeneousMatrixType
Vector6ArgType::Scalar Scalar
PlainReturnType plain() const
traits< MotionRef >::DataRefType DataRefType
MotionPlain __minus__(const MotionTpl< S1, O1 > &v) const
Vector6ArgType::template FixedSegmentReturnType< 3 >::Type LinearType
Vector6ArgType::Scalar Scalar
MotionPlain __plus__(const MotionTpl< S1, O1 > &v) const
ToVectorReturnType toVector_impl()
ToVectorConstReturnType toVector_impl() const
ConstAngularType angular_impl() const
MotionRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) v_like)
Default constructor from a 6 dimensional vector.
MotionPlain __plus__(const MotionTpl< S1, O1 > &v) const
Vector6ArgType::template FixedSegmentReturnType< 3 >::Type AngularType
MotionPlain __minus__(const MotionTpl< S1, O1 > &v) const