GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 68 73 93.2%
Branches: 73 158 46.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4
5 #ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__
6 #define __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__
7
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/spatial/inertia.hpp"
10 #include "pinocchio/math/rotation.hpp"
11 #include "pinocchio/math/matrix.hpp"
12
13 #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
14
15 namespace pinocchio
16 {
17
18 template<typename Scalar, int Options = 0>
19 struct JointRevoluteUnboundedUnalignedTpl;
20
21 template<typename _Scalar, int _Options>
22 struct traits<JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
23 {
24 enum
25 {
26 NQ = 2,
27 NV = 1
28 };
29 typedef _Scalar Scalar;
30 enum
31 {
32 Options = _Options
33 };
34
35 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
36 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
37
38 typedef JointDataRevoluteUnboundedUnalignedTpl<Scalar, Options> JointDataDerived;
39 typedef JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options> JointModelDerived;
40 typedef JointMotionSubspaceRevoluteUnalignedTpl<Scalar, Options> Constraint_t;
41 typedef SE3Tpl<Scalar, Options> Transformation_t;
42 typedef MotionRevoluteUnalignedTpl<Scalar, Options> Motion_t;
43 typedef MotionZeroTpl<Scalar, Options> Bias_t;
44 typedef Eigen::Matrix<Scalar, 6, NV, Options> F_t;
45
46 // [ABA]
47 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
48 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
49 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
50
51 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
52 };
53
54 template<typename _Scalar, int _Options>
55 struct traits<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
56 {
57 typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived;
58 typedef _Scalar Scalar;
59 };
60
61 template<typename _Scalar, int _Options>
62 struct traits<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
63 {
64 typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived;
65 typedef _Scalar Scalar;
66 };
67
68 template<typename _Scalar, int _Options>
69 struct JointDataRevoluteUnboundedUnalignedTpl
70 : public JointDataBase<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
71 {
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived;
74 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
75 837 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
76
77 ConfigVector_t joint_q;
78 TangentVector_t joint_v;
79
80 Transformation_t M;
81 Constraint_t S;
82 Motion_t v;
83 Bias_t c;
84
85 // [ABA] specific data
86 U_t U;
87 D_t Dinv;
88 UD_t UDinv;
89 D_t StU;
90
91 76 JointDataRevoluteUnboundedUnalignedTpl()
92
3/5
✓ Branch 1 taken 72 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 76 : joint_q(Scalar(1), Scalar(0))
93
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 76 , joint_v(TangentVector_t::Zero())
94
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 76 , M(Transformation_t::Identity())
95
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 76 , S(Constraint_t::Vector3::Zero())
96
3/6
✓ Branch 1 taken 76 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 76 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 76 , v(Constraint_t::Vector3::Zero(), (Scalar)0)
97
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 76 , U(U_t::Zero())
98
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 76 , Dinv(D_t::Zero())
99
3/5
✓ Branch 1 taken 4 times.
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 76 , UDinv(UD_t::Zero())
100
3/5
✓ Branch 2 taken 4 times.
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 152 , StU(D_t::Zero())
101 {
102 76 }
103
104 template<typename Vector3Like>
105 16 JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
106
3/5
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 16 : joint_q(Scalar(1), Scalar(0))
107
3/5
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 16 , joint_v(TangentVector_t::Zero())
108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 16 , M(Transformation_t::Identity())
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 16 , S(axis)
110
2/4
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 16 , v(axis, (Scalar)0)
111
3/5
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 16 , U(U_t::Zero())
112
3/5
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 16 , Dinv(D_t::Zero())
113
3/5
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 16 , UDinv(UD_t::Zero())
114
3/5
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 32 , StU(D_t::Zero())
115 {
116 16 }
117
118 144 static std::string classname()
119 {
120
1/2
✓ Branch 2 taken 144 times.
✗ Branch 3 not taken.
 144 return std::string("JointDataRevoluteUnboundedUnalignedTpl");
121 }
122 3 std::string shortname() const
123 {
124 3 return classname();
125 }
126
127 }; // struct JointDataRevoluteUnboundedUnalignedTpl
128
129 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
130
131 template<typename _Scalar, int _Options>
132 struct JointModelRevoluteUnboundedUnalignedTpl
133 : public JointModelBase<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
134 {
135 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
136 typedef JointRevoluteUnboundedUnalignedTpl<_Scalar, _Options> JointDerived;
137 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
138 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
139
140 typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base;
141 using Base::id;
142 using Base::idx_q;
143 using Base::idx_v;
144 using Base::setIndexes;
145
146 92 JointModelRevoluteUnboundedUnalignedTpl()
147
1/2
✓ Branch 3 taken 92 times.
✗ Branch 4 not taken.
 92 : axis(Vector3::UnitX())
148 {
149 92 }
150
151 JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x, const Scalar & y, const Scalar & z)
152 : axis(x, y, z)
153 {
154 normalize(axis);
155 assert(isUnitary(axis) && "Rotation axis is not unitary");
156 }
157
158 template<typename Vector3Like>
159 33 JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
160 33 : axis(axis)
161 {
162 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
163
5/11
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 33 assert(isUnitary(axis) && "Rotation axis is not unitary");
164 33 }
165
166 16 JointDataDerived createData() const
167 {
168 16 return JointDataDerived(axis);
169 }
170
171 ✗ const std::vector<bool> hasConfigurationLimit() const
172 {
173 ✗ return {false, false};
174 }
175
176 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
177 {
178 ✗ return {false};
179 }
180
181 using Base::isEqual;
182 15 bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
183 {
184
3/4
✓ Branch 1 taken 13 times.
✓ Branch 2 taken 2 times.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 15 return Base::isEqual(other) && internal::comparison_eq(axis, other.axis);
185 }
186
187 template<typename ConfigVector>
188 32 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
189 {
190
1/2
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
 32 data.joint_q = qs.template segment<NQ>(idx_q());
191
192 32 const Scalar & ca = data.joint_q(0);
193 32 const Scalar & sa = data.joint_q(1);
194
195 32 toRotationMatrix(axis, ca, sa, data.M.rotation());
196 32 }
197
198 template<typename TangentVector>
199 void
200 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
201 const
202 {
203 1 data.joint_v[0] = vs[idx_v()];
204 1 data.v.angularRate() = data.joint_v[0];
205 1 }
206
207 template<typename ConfigVector, typename TangentVector>
208 20 void calc(
209 JointDataDerived & data,
210 const typename Eigen::MatrixBase<ConfigVector> & qs,
211 const typename Eigen::MatrixBase<TangentVector> & vs) const
212 {
213 20 calc(data, qs.derived());
214
0/4
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 20 data.joint_v[0] = vs[idx_v()];
215 20 data.v.angularRate() = data.joint_v[0];
216 20 }
217
218 template<typename VectorLike, typename Matrix6Like>
219 5 void calc_aba(
220 JointDataDerived & data,
221 const Eigen::MatrixBase<VectorLike> & armature,
222 const Eigen::MatrixBase<Matrix6Like> & I,
223 const bool update_I) const
224 {
225
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
226
0/10
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 10 data.Dinv[0] =
227
3/8
✗ Branch 1 not taken.
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5 times.
✗ Branch 9 not taken.
 5 Scalar(1) / (axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + armature[0]);
228
2/4
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 data.UDinv.noalias() = data.U * data.Dinv;
229
230
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 5 times.
 5 if (update_I)
231 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
232 5 }
233
234 166 static std::string classname()
235 {
236
1/2
✓ Branch 2 taken 166 times.
✗ Branch 3 not taken.
 166 return std::string("JointModelRevoluteUnboundedUnaligned");
237 }
238 24 std::string shortname() const
239 {
240 24 return classname();
241 }
242
243 /// \returns An expression of *this with the Scalar type casted to NewScalar.
244 template<typename NewScalar>
245 5 JointModelRevoluteUnboundedUnalignedTpl<NewScalar, Options> cast() const
246 {
247 typedef JointModelRevoluteUnboundedUnalignedTpl<NewScalar, Options> ReturnType;
248
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 5 ReturnType res(axis.template cast<NewScalar>());
249
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 5 res.setIndexes(id(), idx_q(), idx_v());
250 5 return res;
251 }
252
253 // data
254 ///
255 /// \brief axis of rotation of the joint.
256 ///
257 Vector3 axis;
258 }; // struct JointModelRevoluteUnboundedUnalignedTpl
259
260 } // namespace pinocchio
261
262 #include <boost/type_traits.hpp>
263
264 namespace boost
265 {
266 template<typename Scalar, int Options>
267 struct has_nothrow_constructor<
268 ::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>>
269 : public integral_constant<bool, true>
270 {
271 };
272
273 template<typename Scalar, int Options>
274 struct has_nothrow_copy<::pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar, Options>>
275 : public integral_constant<bool, true>
276 {
277 };
278
279 template<typename Scalar, int Options>
280 struct has_nothrow_constructor<
281 ::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar, Options>>
282 : public integral_constant<bool, true>
283 {
284 };
285
286 template<typename Scalar, int Options>
287 struct has_nothrow_copy<::pinocchio::JointDataRevoluteUnboundedUnalignedTpl<Scalar, Options>>
288 : public integral_constant<bool, true>
289 {
290 };
291 } // namespace boost
292
293 #endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_unaligned_hpp__
294