5 #ifndef __pinocchio_force_ref_hpp__
6 #define __pinocchio_force_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,4,4,Options> Matrix4;
23 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
24 typedef Matrix6 ActionMatrixType;
25 typedef Matrix4 HomogeneousMatrixType;
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;
31 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType;
32 typedef DataRefType ToVectorReturnType;
33 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
34 typedef ConstDataRefType ToVectorConstReturnType;
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!=;
65 ForceRef(
typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) f_like)
68 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
69 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
70 assert(f_like.size() == 6);
78 ToVectorConstReturnType toVector_impl()
const {
return m_ref; }
79 ToVectorReturnType toVector_impl() {
return m_ref; }
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); }
88 void angular_impl(
const Eigen::MatrixBase<V3> & w)
90 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
95 void linear_impl(
const Eigen::MatrixBase<V3> & v)
97 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3,3);
108 template<
typename Vector6ArgType>
111 typedef typename Vector6ArgType::Scalar Scalar;
112 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6;
116 Options = Vector6::Options
118 typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
119 typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
120 typedef Matrix6 ActionMatrixType;
121 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType;
122 typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType;
123 typedef ConstLinearType LinearType;
124 typedef ConstAngularType AngularType;
127 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType;
128 typedef ConstDataRefType ToVectorConstReturnType;
129 typedef ConstDataRefType DataRefType;
130 typedef DataRefType ToVectorReturnType;
135 template<
typename Vector6ArgType>
137 :
public ForceDense< ForceRef<const Vector6ArgType> >
140 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
145 ForceRef(
typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) f_like)
148 EIGEN_STATIC_ASSERT(Vector6ArgType::ColsAtCompileTime == 1,
149 YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
150 assert(f_like.size() == 6);
153 ToVectorConstReturnType toVector_impl()
const {
return m_ref; }
156 ConstAngularType angular_impl()
const {
return ConstAngularType(m_ref.derived(),ANGULAR); }
157 ConstLinearType linear_impl()
const {
return ConstLinearType(m_ref.derived(),LINEAR); }
159 const ForceRef & ref()
const {
return *
this; }
168 #endif // ifndef __pinocchio_force_ref_hpp__