| Directory: | ./ |
|---|---|
| File: | include/pinocchio/spatial/force-ref.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 22 | 22 | 100.0% |
| Branches: | 2 | 4 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2017-2019 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_spatial_force_ref_hpp__ | ||
| 6 | #define __pinocchio_spatial_force_ref_hpp__ | ||
| 7 | |||
| 8 | namespace pinocchio | ||
| 9 | { | ||
| 10 | |||
| 11 | template<typename Vector6ArgType> | ||
| 12 | struct traits<ForceRef<Vector6ArgType>> | ||
| 13 | { | ||
| 14 | typedef typename Vector6ArgType::Scalar Scalar; | ||
| 15 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6; | ||
| 16 | enum | ||
| 17 | { | ||
| 18 | LINEAR = 0, | ||
| 19 | ANGULAR = 3, | ||
| 20 | Options = Vector6::Options | ||
| 21 | }; | ||
| 22 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 23 | typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; | ||
| 24 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
| 25 | typedef Matrix6 ActionMatrixType; | ||
| 26 | typedef Matrix4 HomogeneousMatrixType; | ||
| 27 | typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type LinearType; | ||
| 28 | typedef typename Vector6ArgType::template FixedSegmentReturnType<3>::Type AngularType; | ||
| 29 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType; | ||
| 30 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType; | ||
| 31 | typedef ForceTpl<Scalar, Options> ForcePlain; | ||
| 32 | typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) DataRefType; | ||
| 33 | typedef DataRefType ToVectorReturnType; | ||
| 34 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType; | ||
| 35 | typedef ConstDataRefType ToVectorConstReturnType; | ||
| 36 | typedef ForceRef<Vector6ArgType> ForceRefType; | ||
| 37 | |||
| 38 | }; // traits ForceRef | ||
| 39 | |||
| 40 | template<typename Vector6ArgType> | ||
| 41 | struct SE3GroupAction<ForceRef<Vector6ArgType>> | ||
| 42 | { | ||
| 43 | typedef typename traits<ForceRef<Vector6ArgType>>::ForcePlain ReturnType; | ||
| 44 | }; | ||
| 45 | |||
| 46 | template<typename Vector6ArgType, typename MotionDerived> | ||
| 47 | struct MotionAlgebraAction<ForceRef<Vector6ArgType>, MotionDerived> | ||
| 48 | { | ||
| 49 | typedef typename traits<ForceRef<Vector6ArgType>>::ForcePlain ReturnType; | ||
| 50 | }; | ||
| 51 | |||
| 52 | template<typename Vector6ArgType> | ||
| 53 | class ForceRef : public ForceDense<ForceRef<Vector6ArgType>> | ||
| 54 | { | ||
| 55 | public: | ||
| 56 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 57 | typedef ForceDense<ForceRef> Base; | ||
| 58 | typedef typename traits<ForceRef>::DataRefType DataRefType; | ||
| 59 | FORCE_TYPEDEF_TPL(ForceRef); | ||
| 60 | |||
| 61 | using Base::operator=; | ||
| 62 | using Base::operator==; | ||
| 63 | using Base::operator!=; | ||
| 64 | |||
| 65 | /// \brief Default constructor from a 6 dimensional vector. | ||
| 66 | 234101 | ForceRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) f_like) | |
| 67 | 234101 | : m_ref(f_like) | |
| 68 | { | ||
| 69 | EIGEN_STATIC_ASSERT( | ||
| 70 | Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX); | ||
| 71 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 118227 times.
|
234101 | assert(f_like.size() == 6); |
| 72 | 234101 | } | |
| 73 | |||
| 74 | /// \brief Copy constructor from another ForceRef. | ||
| 75 | ForceRef(const ForceRef & other) | ||
| 76 | : m_ref(other.m_ref) | ||
| 77 | { | ||
| 78 | } | ||
| 79 | |||
| 80 | ToVectorConstReturnType toVector_impl() const | ||
| 81 | { | ||
| 82 | return m_ref; | ||
| 83 | } | ||
| 84 | ToVectorReturnType toVector_impl() | ||
| 85 | { | ||
| 86 | return m_ref; | ||
| 87 | } | ||
| 88 | |||
| 89 | // Getters | ||
| 90 | 2124 | ConstAngularType angular_impl() const | |
| 91 | { | ||
| 92 | 2124 | return ConstAngularType(m_ref.derived(), ANGULAR); | |
| 93 | } | ||
| 94 | 3034 | ConstLinearType linear_impl() const | |
| 95 | { | ||
| 96 | 3034 | return ConstLinearType(m_ref.derived(), LINEAR); | |
| 97 | } | ||
| 98 | 390962 | AngularType angular_impl() | |
| 99 | { | ||
| 100 | 390962 | return m_ref.template segment<3>(ANGULAR); | |
| 101 | } | ||
| 102 | 195267 | LinearType linear_impl() | |
| 103 | { | ||
| 104 | 389475 | return m_ref.template segment<3>(LINEAR); | |
| 105 | } | ||
| 106 | |||
| 107 | template<typename V3> | ||
| 108 | void angular_impl(const Eigen::MatrixBase<V3> & w) | ||
| 109 | { | ||
| 110 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
| 111 | angular_impl() = w; | ||
| 112 | } | ||
| 113 | |||
| 114 | template<typename V3> | ||
| 115 | void linear_impl(const Eigen::MatrixBase<V3> & v) | ||
| 116 | { | ||
| 117 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(V3, 3); | ||
| 118 | linear_impl() = v; | ||
| 119 | } | ||
| 120 | |||
| 121 | 2 | ForceRef & ref() | |
| 122 | { | ||
| 123 | 2 | return *this; | |
| 124 | } | ||
| 125 | |||
| 126 | protected: | ||
| 127 | DataRefType m_ref; | ||
| 128 | |||
| 129 | }; // class ForceRef<Vector6Like> | ||
| 130 | |||
| 131 | template<typename Vector6ArgType> | ||
| 132 | struct traits<ForceRef<const Vector6ArgType>> | ||
| 133 | { | ||
| 134 | typedef typename Vector6ArgType::Scalar Scalar; | ||
| 135 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6ArgType) Vector6; | ||
| 136 | enum | ||
| 137 | { | ||
| 138 | LINEAR = 0, | ||
| 139 | ANGULAR = 3, | ||
| 140 | Options = Vector6::Options | ||
| 141 | }; | ||
| 142 | typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; | ||
| 143 | typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; | ||
| 144 | typedef Matrix6 ActionMatrixType; | ||
| 145 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstLinearType; | ||
| 146 | typedef typename Vector6ArgType::template ConstFixedSegmentReturnType<3>::Type ConstAngularType; | ||
| 147 | typedef ConstLinearType LinearType; | ||
| 148 | typedef ConstAngularType AngularType; | ||
| 149 | typedef ForceTpl<Scalar, Options> ForcePlain; | ||
| 150 | typedef ForcePlain PlainReturnType; | ||
| 151 | typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) ConstDataRefType; | ||
| 152 | typedef ConstDataRefType ToVectorConstReturnType; | ||
| 153 | typedef ConstDataRefType DataRefType; | ||
| 154 | typedef DataRefType ToVectorReturnType; | ||
| 155 | typedef ForceRef<const Vector6ArgType> ForceRefType; | ||
| 156 | |||
| 157 | }; // traits ForceRef<const Vector6ArgType> | ||
| 158 | |||
| 159 | template<typename Vector6ArgType> | ||
| 160 | class ForceRef<const Vector6ArgType> : public ForceDense<ForceRef<const Vector6ArgType>> | ||
| 161 | { | ||
| 162 | public: | ||
| 163 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 164 | typedef ForceDense<ForceRef> Base; | ||
| 165 | typedef typename traits<ForceRef>::DataRefType DataRefType; | ||
| 166 | FORCE_TYPEDEF_TPL(ForceRef); | ||
| 167 | |||
| 168 | 5007 | ForceRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) f_like) | |
| 169 | 5007 | : m_ref(f_like) | |
| 170 | { | ||
| 171 | EIGEN_STATIC_ASSERT( | ||
| 172 | Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX); | ||
| 173 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 5007 times.
|
5007 | assert(f_like.size() == 6); |
| 174 | 5007 | } | |
| 175 | |||
| 176 | ToVectorConstReturnType toVector_impl() const | ||
| 177 | { | ||
| 178 | return m_ref; | ||
| 179 | } | ||
| 180 | |||
| 181 | // Getters | ||
| 182 | 5007 | ConstAngularType angular_impl() const | |
| 183 | { | ||
| 184 | 5007 | return ConstAngularType(m_ref.derived(), ANGULAR); | |
| 185 | } | ||
| 186 | 5147 | ConstLinearType linear_impl() const | |
| 187 | { | ||
| 188 | 5147 | return ConstLinearType(m_ref.derived(), LINEAR); | |
| 189 | } | ||
| 190 | |||
| 191 | const ForceRef & ref() const | ||
| 192 | { | ||
| 193 | return *this; | ||
| 194 | } | ||
| 195 | |||
| 196 | protected: | ||
| 197 | DataRefType m_ref; | ||
| 198 | |||
| 199 | }; // class ForceRef<Vector6Like> | ||
| 200 | |||
| 201 | } // namespace pinocchio | ||
| 202 | |||
| 203 | #endif // ifndef __pinocchio_spatial_force_ref_hpp__ | ||
| 204 |