GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-revolute-unbounded.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 21 59 35.6%
Branches: 10 50 20.0%
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 4 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 136 JointDataRevoluteUnboundedTpl()
89
1/2
✓ Branch 1 taken 68 times.
✗ Branch 2 not taken.
 136 : joint_q(Scalar(1), Scalar(0))
90
1/2
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
 136 , joint_v(TangentVector_t::Zero())
91 136 , M((Scalar)0, (Scalar)1)
92 136 , v((Scalar)0)
93
1/2
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
 136 , U(U_t::Zero())
94
1/2
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
 136 , Dinv(D_t::Zero())
95
1/2
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
 136 , UDinv(UD_t::Zero())
96
1/2
✓ Branch 4 taken 68 times.
✗ Branch 5 not taken.
 408 , StU(D_t::Zero())
97 {
98 136 }
99
100 240 static std::string classname()
101 {
102
2/4
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
 480 return std::string("JointDataRUB") + axisLabel<axis>();
103 }
104 ✗ std::string shortname() const
105 {
106 ✗ 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 16 JointDataDerived createData() const
135 {
136 16 return JointDataDerived();
137 }
138
139 ✗ const std::vector<bool> hasConfigurationLimit() const
140 {
141 ✗ return {false, false};
142 }
143
144 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
145 {
146 ✗ return {false};
147 }
148
149 template<typename ConfigVector>
150 ✗ void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
151 {
152 ✗ data.joint_q = qs.template segment<NQ>(idx_q());
153
154 ✗ const Scalar & ca = data.joint_q[0];
155 ✗ const Scalar & sa = data.joint_q[1];
156
157 ✗ data.M.setValues(sa, ca);
158 }
159
160 template<typename TangentVector>
161 void
162 ✗ calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
163 const
164 {
165 ✗ data.joint_v[0] = vs[idx_v()];
166 ✗ data.v.angularRate() = data.joint_v[0];
167 }
168
169 template<typename ConfigVector, typename TangentVector>
170 6 void calc(
171 JointDataDerived & data,
172 const typename Eigen::MatrixBase<ConfigVector> & qs,
173 const typename Eigen::MatrixBase<TangentVector> & vs) const
174 {
175 6 calc(data, qs.derived());
176 6 data.joint_v[0] = vs[idx_v()];
177 6 data.v.angularRate() = data.joint_v[0];
178 }
179
180 template<typename VectorLike, typename Matrix6Like>
181 ✗ 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 ✗ data.U = I.col(Inertia::ANGULAR + axis);
188 ✗ data.Dinv[0] =
189 ✗ (Scalar)(1) / (I(Inertia::ANGULAR + axis, Inertia::ANGULAR + axis) + armature[0]);
190 ✗ data.UDinv.noalias() = data.U * data.Dinv[0];
191
192 ✗ if (update_I)
193 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
194 }
195
196 240 static std::string classname()
197 {
198
2/4
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 120 times.
✗ Branch 7 not taken.
 480 return std::string("JointModelRUB") + axisLabel<axis>();
199 }
200 ✗ std::string shortname() const
201 {
202 ✗ 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 ✗ JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> cast() const
224 {
225 typedef JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> ReturnType;
226 ✗ ReturnType res;
227 ✗ res.setIndexes(id(), idx_q(), idx_v());
228 ✗ return res;
229 }
230
231 }; // struct JointModelRevoluteUnboundedTpl
232
233 struct UnboundedRevoluteAffineTransform
234 {
235 template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
236 ✗ 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 ✗ const typename ConfigVectorIn::Scalar & ca = q(0);
246 ✗ const typename ConfigVectorIn::Scalar & sa = q(1);
247
248 ✗ const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca);
249 ✗ const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
250
251 ✗ ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest);
252 ✗ SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0));
253 }
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