GCC Code Coverage Report

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