5 #ifndef __pinocchio_force_ref_hpp__ 6 #define __pinocchio_force_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;
39 template<
typename Vector6ArgType>
45 template<
typename Vector6ArgType,
typename MotionDerived>
51 template<
typename Vector6ArgType>
55 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 using Base::operator=;
61 using Base::operator==;
62 using Base::operator!=;
68 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
69 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
70 assert(f_like.size() == 6);
82 ConstAngularType
angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
83 ConstLinearType
linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
84 AngularType
angular_impl() {
return m_ref.template segment<3> (ANGULAR); }
85 LinearType
linear_impl() {
return m_ref.template segment<3> (LINEAR); }
90 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
97 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
108 template<
typename Vector6ArgType>
118 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
119 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
121 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type
ConstLinearType;
122 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type
ConstAngularType;
135 template<
typename Vector6ArgType>
137 :
public ForceDense< ForceRef<const Vector6ArgType> >
140 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
148 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
149 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
150 assert(f_like.size() == 6);
156 ConstAngularType
angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
157 ConstLinearType
linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
168 #endif // ifndef __pinocchio_force_ref_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
DataRefType ToVectorReturnType
traits< ForceRef< Vector6ArgType > >::ForcePlain ReturnType
traits< ForceRef >::DataRefType DataRefType
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstLinearType
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Vector6ArgType::Scalar Scalar
const ForceRef & ref() const
Return type of the ation of a Motion onto an object of type D.
ConstDataRefType ToVectorConstReturnType
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
ConstDataRefType ToVectorConstReturnType
ConstLinearType linear_impl() const
ConstAngularType angular_impl() const
ConstLinearType LinearType
ForceRef< Vector6ArgType > ForceRefType
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef ForceDense< ForceRef > Base
ForcePlain PlainReturnType
ForceRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) f_like)
ForceRef< const Vector6ArgType > ForceRefType
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
ToVectorConstReturnType toVector_impl() const
ToVectorReturnType toVector_impl()
#define FORCE_TYPEDEF_TPL(Derived)
Matrix4 HomogeneousMatrixType
ForceTpl< Scalar, Options > ForcePlain
ConstDataRefType DataRefType
Vector6ArgType::Scalar Scalar
void linear_impl(const Eigen::MatrixBase< V3 > &v)
ConstAngularType angular_impl() const
ForceTpl< Scalar, Options > ForcePlain
#define PINOCCHIO_EIGEN_REF_TYPE(D)
Main pinocchio namespace.
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstLinearType
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...
ConstAngularType AngularType
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstAngularType
ConstLinearType linear_impl() const
Common traits structure to fully define base classes for CRTP.
DataRefType ToVectorReturnType
AngularType angular_impl()
traits< ForceRef >::DataRefType DataRefType
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
ToVectorConstReturnType toVector_impl() const
Vector6ArgType::template ConstFixedSegmentReturnType< 3 >::Type ConstAngularType
traits< ForceRef< Vector6ArgType > >::ForcePlain ReturnType
void angular_impl(const Eigen::MatrixBase< V3 > &w)
Vector6ArgType::template FixedSegmentReturnType< 3 >::Type LinearType
ForceRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) f_like)
Default constructor from a 6 dimensional vector.
ForceRef(const ForceRef &other)
Copy constructor from another ForceRef.
Vector6ArgType::template FixedSegmentReturnType< 3 >::Type AngularType
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef ForceDense< ForceRef > Base
Eigen::Matrix< Scalar, 3, 1, Options > Vector3