GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/force-ref.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 11 20 55.0%
Branches: 1 4 25.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 200566 ForceRef(typename PINOCCHIO_EIGEN_REF_TYPE(Vector6ArgType) f_like)
67 200566 : 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 100283 times.
 200566 assert(f_like.size() == 6);
72 }
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 1848 ConstAngularType angular_impl() const
91 {
92 1848 return ConstAngularType(m_ref.derived(), ANGULAR);
93 }
94 2500 ConstLinearType linear_impl() const
95 {
96 2500 return ConstLinearType(m_ref.derived(), LINEAR);
97 }
98 343576 AngularType angular_impl()
99 {
100 343576 return m_ref.template segment<3>(ANGULAR);
101 }
102 154482 LinearType linear_impl()
103 {
104 326206 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 ✗ ForceRef & ref()
122 {
123 ✗ 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 ✗ ForceRef(typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6ArgType) f_like)
169 ✗ : m_ref(f_like)
170 {
171 EIGEN_STATIC_ASSERT(
172 Vector6ArgType::ColsAtCompileTime == 1, YOU_TRIED_CALLING_A_VECTOR_METHOD_ON_A_MATRIX);
173 ✗ assert(f_like.size() == 6);
174 }
175
176 ToVectorConstReturnType toVector_impl() const
177 {
178 return m_ref;
179 }
180
181 // Getters
182 ✗ ConstAngularType angular_impl() const
183 {
184 ✗ return ConstAngularType(m_ref.derived(), ANGULAR);
185 }
186 ✗ ConstLinearType linear_impl() const
187 {
188 ✗ 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