GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 60 64 93.8%
Branches: 49 94 52.1%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__
6 #define __pinocchio_multibody_joint_revolute_unbounded_hpp__
7
8 #include "pinocchio/math/fwd.hpp"
9 #include "pinocchio/math/sincos.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #include "pinocchio/multibody/joint/joint-base.hpp"
12 #include "pinocchio/multibody/joint/joint-revolute.hpp"
13
14 namespace pinocchio
15 {
16
17 template<typename Scalar, int Options, int axis>
18 struct JointRevoluteUnboundedTpl;
19
20 template<typename _Scalar, int _Options, int axis>
21 struct traits<JointRevoluteUnboundedTpl<_Scalar, _Options, axis>>
22 {
23 enum
24 {
25 NQ = 2,
26 NV = 1
27 };
28 typedef _Scalar Scalar;
29 enum
30 {
31 Options = _Options
32 };
33 typedef JointDataRevoluteUnboundedTpl<Scalar, Options, axis> JointDataDerived;
34 typedef JointModelRevoluteUnboundedTpl<Scalar, Options, axis> JointModelDerived;
35 typedef JointMotionSubspaceRevoluteTpl<Scalar, Options, axis> Constraint_t;
36 typedef TransformRevoluteTpl<Scalar, Options, axis> Transformation_t;
37 typedef MotionRevoluteTpl<Scalar, Options, axis> Motion_t;
38 typedef MotionZeroTpl<Scalar, Options> Bias_t;
39
40 // [ABA]
41 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
42 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
43 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
44
45 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
46 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
47
48 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
49 };
50
51 template<typename _Scalar, int _Options, int axis>
52 struct traits<JointDataRevoluteUnboundedTpl<_Scalar, _Options, axis>>
53 {
54 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
55 typedef _Scalar Scalar;
56 };
57
58 template<typename _Scalar, int _Options, int axis>
59 struct traits<JointModelRevoluteUnboundedTpl<_Scalar, _Options, axis>>
60 {
61 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
62 typedef _Scalar Scalar;
63 };
64
65 template<typename _Scalar, int _Options, int axis>
66 struct JointDataRevoluteUnboundedTpl
67 : public JointDataBase<JointDataRevoluteUnboundedTpl<_Scalar, _Options, axis>>
68 {
69 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
71 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
72 2524 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
73
74 ConfigVector_t joint_q;
75 TangentVector_t joint_v;
76
77 Constraint_t S;
78 Transformation_t M;
79 Motion_t v;
80 Bias_t c;
81
82 // [ABA] specific data
83 U_t U;
84 D_t Dinv;
85 UD_t UDinv;
86 D_t StU;
87
88 19062 JointDataRevoluteUnboundedTpl()
89
3/5
✓ Branch 1 taken 9515 times.
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 19062 : joint_q(Scalar(1), Scalar(0))
90
3/5
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 9515 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 19062 , joint_v(TangentVector_t::Zero())
91
3/6
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
 19062 , M((Scalar)0, (Scalar)1)
92
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 19062 , v((Scalar)0)
93
3/5
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 9515 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 19062 , U(U_t::Zero())
94
3/5
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 9515 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 19062 , Dinv(D_t::Zero())
95
3/5
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 9515 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 19062 , UDinv(UD_t::Zero())
96
3/5
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 9515 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 16 times.
✗ Branch 7 not taken.
 57186 , StU(D_t::Zero())
97 {
98 19062 }
99
100 864 static std::string classname()
101 {
102
2/4
✓ Branch 3 taken 432 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 432 times.
✗ Branch 7 not taken.
 1728 return std::string("JointDataRUB") + axisLabel<axis>();
103 }
104 9 std::string shortname() const
105 {
106 18 return classname();
107 }
108
109 }; // struct JointDataRevoluteUnbounded
110
111 template<typename NewScalar, typename Scalar, int Options, int axis>
112 struct CastType<NewScalar, JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
113 {
114 typedef JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> type;
115 };
116
117 template<typename _Scalar, int _Options, int axis>
118 struct JointModelRevoluteUnboundedTpl
119 : public JointModelBase<JointModelRevoluteUnboundedTpl<_Scalar, _Options, axis>>
120 {
121 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
122 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
123 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
124 typedef JointRevoluteTpl<Scalar, _Options, axis> JointDerivedBase;
125
126 typedef JointModelBase<JointModelRevoluteUnboundedTpl> Base;
127 using Base::id;
128 using Base::idx_q;
129 using Base::idx_v;
130 using Base::setIndexes;
131
132 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
133
134 18606 JointDataDerived createData() const
135 {
136 18606 return JointDataDerived();
137 }
138
139 2 const std::vector<bool> hasConfigurationLimit() const
140 {
141
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 return {false, false};
142 }
143
144 2 const std::vector<bool> hasConfigurationLimitInTangent() const
145 {
146
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 return {false};
147 }
148
149 template<typename ConfigVector>
150 36902 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
151 {
152
1/2
✓ Branch 3 taken 18451 times.
✗ Branch 4 not taken.
 36902 data.joint_q = qs.template segment<NQ>(idx_q());
153
154 36902 const Scalar & ca = data.joint_q[0];
155 36902 const Scalar & sa = data.joint_q[1];
156
157 36902 data.M.setValues(sa, ca);
158 36902 }
159
160 template<typename TangentVector>
161 void
162 6 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
163 const
164 {
165 6 data.joint_v[0] = vs[idx_v()];
166 6 data.v.angularRate() = data.joint_v[0];
167 6 }
168
169 template<typename ConfigVector, typename TangentVector>
170 6228 void calc(
171 JointDataDerived & data,
172 const typename Eigen::MatrixBase<ConfigVector> & qs,
173 const typename Eigen::MatrixBase<TangentVector> & vs) const
174 {
175 6228 calc(data, qs.derived());
176
0/4
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 6228 data.joint_v[0] = vs[idx_v()];
177 6228 data.v.angularRate() = data.joint_v[0];
178 6228 }
179
180 template<typename VectorLike, typename Matrix6Like>
181 44 void calc_aba(
182 JointDataDerived & data,
183 const Eigen::MatrixBase<VectorLike> & armature,
184 const Eigen::MatrixBase<Matrix6Like> & I,
185 const bool update_I) const
186 {
187
1/2
✓ Branch 2 taken 22 times.
✗ Branch 3 not taken.
 44 data.U = I.col(Inertia::ANGULAR + axis);
188
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 90 data.Dinv[0] =
189
3/6
✓ 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.
 46 (Scalar)(1) / (I(Inertia::ANGULAR + axis, Inertia::ANGULAR + axis) + armature[0]);
190
2/4
✓ Branch 3 taken 22 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 22 times.
✗ Branch 7 not taken.
 44 data.UDinv.noalias() = data.U * data.Dinv[0];
191
192
2/2
✓ Branch 0 taken 7 times.
✓ Branch 1 taken 15 times.
 44 if (update_I)
193
3/6
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 14 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
194 44 }
195
196 111183 static std::string classname()
197 {
198
2/4
✓ Branch 3 taken 55590 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 55590 times.
✗ Branch 7 not taken.
 222366 return std::string("JointModelRUB") + axisLabel<axis>();
199 }
200 18 std::string shortname() const
201 {
202 55185 return classname();
203 }
204
205 ✗ Vector3 getMotionAxis() const
206 {
207 switch (axis)
208 {
209 case 0:
210 ✗ return Vector3::UnitX();
211 case 1:
212 ✗ return Vector3::UnitY();
213 case 2:
214 ✗ return Vector3::UnitZ();
215 default:
216 assert(false && "must never happen");
217 break;
218 }
219 }
220
221 /// \returns An expression of *this with the Scalar type casted to NewScalar.
222 template<typename NewScalar>
223 16 JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> cast() const
224 {
225 typedef JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> ReturnType;
226 16 ReturnType res;
227 16 res.setIndexes(id(), idx_q(), idx_v());
228 16 return res;
229 }
230
231 }; // struct JointModelRevoluteUnboundedTpl
232
233 struct UnboundedRevoluteAffineTransform
234 {
235 template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
236 4 static void run(
237 const Eigen::MatrixBase<ConfigVectorIn> & q,
238 const Scalar & scaling,
239 const Scalar & offset,
240 const Eigen::MatrixBase<ConfigVectorOut> & dest)
241 {
242 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorIn, 2);
243 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(ConfigVectorOut, 2);
244
245
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 const typename ConfigVectorIn::Scalar & ca = q(0);
246
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 const typename ConfigVectorIn::Scalar & sa = q(1);
247
248
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca);
249 4 const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
250
251 4 ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest);
252
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 4 SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0));
253 4 }
254 };
255
256 template<typename Scalar, int Options, int axis>
257 struct ConfigVectorAffineTransform<JointRevoluteUnboundedTpl<Scalar, Options, axis>>
258 {
259 typedef UnboundedRevoluteAffineTransform Type;
260 };
261
262 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointRUBX;
263 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointDataRUBX;
264 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointModelRUBX;
265
266 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointRUBY;
267 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointDataRUBY;
268 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointModelRUBY;
269
270 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointRUBZ;
271 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointDataRUBZ;
272 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointModelRUBZ;
273
274 } // namespace pinocchio
275
276 #include <boost/type_traits.hpp>
277
278 namespace boost
279 {
280 template<typename Scalar, int Options, int axis>
281 struct has_nothrow_constructor<::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
282 : public integral_constant<bool, true>
283 {
284 };
285
286 template<typename Scalar, int Options, int axis>
287 struct has_nothrow_copy<::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
288 : public integral_constant<bool, true>
289 {
290 };
291
292 template<typename Scalar, int Options, int axis>
293 struct has_nothrow_constructor<::pinocchio::JointDataRevoluteUnboundedTpl<Scalar, Options, axis>>
294 : public integral_constant<bool, true>
295 {
296 };
297
298 template<typename Scalar, int Options, int axis>
299 struct has_nothrow_copy<::pinocchio::JointDataRevoluteUnboundedTpl<Scalar, Options, axis>>
300 : public integral_constant<bool, true>
301 {
302 };
303 } // namespace boost
304
305 #endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__
306