pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
joint-revolute-unbounded-unaligned.hpp
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
15namespace pinocchio
16{
17
18 template<typename Scalar, int Options = 0>
20
21 template<typename _Scalar, int _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
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>
63
64 template<typename _Scalar, int _Options>
70
71 template<typename _Scalar, int _Options>
73 : public JointDataBase<JointDataRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
74 {
77 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
78 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
95 : joint_q(Scalar(1), Scalar(0))
96 , joint_v(TangentVector_t::Zero())
97 , M(Transformation_t::Identity())
98 , S(Constraint_t::Vector3::Zero())
99 , v(Constraint_t::Vector3::Zero(), (Scalar)0)
100 , U(U_t::Zero())
101 , Dinv(D_t::Zero())
102 , UDinv(UD_t::Zero())
103 , StU(D_t::Zero())
104 {
105 }
106
107 template<typename Vector3Like>
108 JointDataRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
109 : joint_q(Scalar(1), Scalar(0))
110 , joint_v(TangentVector_t::Zero())
111 , M(Transformation_t::Identity())
112 , S(axis)
113 , v(axis, (Scalar)0)
114 , U(U_t::Zero())
115 , Dinv(D_t::Zero())
116 , UDinv(UD_t::Zero())
117 , StU(D_t::Zero())
118 {
119 }
120
121 static std::string classname()
122 {
123 return std::string("JointDataRevoluteUnboundedUnalignedTpl");
124 }
125 std::string shortname() const
126 {
127 return classname();
128 }
129
130 }; // struct JointDataRevoluteUnboundedUnalignedTpl
131
132 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnboundedUnalignedTpl);
133
134 template<typename _Scalar, int _Options>
136 : public JointModelBase<JointModelRevoluteUnboundedUnalignedTpl<_Scalar, _Options>>
137 {
140 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
141 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
142
144 using Base::id;
145 using Base::idx_q;
146 using Base::idx_v;
147 using Base::idx_vExtended;
148 using Base::setIndexes;
149
151 : axis(Vector3::UnitX())
152 {
153 }
154
155 JointModelRevoluteUnboundedUnalignedTpl(const Scalar & x, const Scalar & y, const Scalar & z)
156 : axis(x, y, z)
157 {
159 assert(isUnitary(axis) && "Rotation axis is not unitary");
160 }
161
162 template<typename Vector3Like>
163 JointModelRevoluteUnboundedUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
164 : axis(axis)
165 {
167 assert(isUnitary(axis) && "Rotation axis is not unitary");
168 }
169
170 JointDataDerived createData() const
171 {
172 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 bool isEqual(const JointModelRevoluteUnboundedUnalignedTpl & other) const
187 {
188 return Base::isEqual(other) && internal::comparison_eq(axis, other.axis);
189 }
190
191 template<typename ConfigVector>
192 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
193 {
194 data.joint_q = qs.template segment<NQ>(idx_q());
195
196 const Scalar & ca = data.joint_q(0);
197 const Scalar & sa = data.joint_q(1);
198
199 toRotationMatrix(axis, ca, sa, data.M.rotation());
200 }
201
202 template<typename TangentVector>
203 void
204 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
205 const
206 {
207 data.joint_v[0] = vs[idx_v()];
208 data.v.angularRate() = data.joint_v[0];
209 }
210
211 template<typename ConfigVector, typename TangentVector>
212 void calc(
213 JointDataDerived & data,
214 const typename Eigen::MatrixBase<ConfigVector> & qs,
215 const typename Eigen::MatrixBase<TangentVector> & vs) const
216 {
217 calc(data, qs.derived());
218 data.joint_v[0] = vs[idx_v()];
219 data.v.angularRate() = data.joint_v[0];
220 }
221
222 template<typename VectorLike, typename Matrix6Like>
223 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 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
230 data.Dinv[0] =
231 Scalar(1) / (axis.dot(data.U.template segment<3>(Motion::ANGULAR)) + armature[0]);
232 data.UDinv.noalias() = data.U * data.Dinv;
233
234 if (update_I)
235 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
236 }
237
238 static std::string classname()
239 {
240 return std::string("JointModelRevoluteUnboundedUnaligned");
241 }
242 std::string shortname() const
243 {
244 return classname();
245 }
246
248 template<typename NewScalar>
250 {
252 ReturnType res(axis.template cast<NewScalar>());
253 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
254 return res;
255 }
256
257 // data
261 Vector3 axis;
262 }; // struct JointModelRevoluteUnboundedUnalignedTpl
263
264 template<typename Scalar, int Options>
269} // namespace pinocchio
270
271#include <boost/type_traits.hpp>
272
273namespace boost
274{
275 template<typename Scalar, int Options>
276 struct has_nothrow_constructor<
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<
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__
Main pinocchio namespace.
Definition treeview.dox:11
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition rotation.hpp:26
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition matrix.hpp:155
Blank type.
Definition fwd.hpp:77
Assign the correct configuration vector space affine transformation according to the joint type....
JointModelRevoluteUnboundedUnalignedTpl< NewScalar, Options > cast() const
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72