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 |