1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2019 CNRS INRIA |
3 |
|
|
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#ifndef __pinocchio_joint_free_flyer_hpp__ |
7 |
|
|
#define __pinocchio_joint_free_flyer_hpp__ |
8 |
|
|
|
9 |
|
|
#include "pinocchio/macros.hpp" |
10 |
|
|
#include "pinocchio/spatial/inertia.hpp" |
11 |
|
|
#include "pinocchio/spatial/explog.hpp" |
12 |
|
|
#include "pinocchio/multibody/joint/joint-base.hpp" |
13 |
|
|
#include "pinocchio/multibody/constraint.hpp" |
14 |
|
|
#include "pinocchio/math/fwd.hpp" |
15 |
|
|
#include "pinocchio/math/quaternion.hpp" |
16 |
|
|
|
17 |
|
|
namespace pinocchio |
18 |
|
|
{ |
19 |
|
|
|
20 |
|
|
template<typename Scalar, int Options> struct ConstraintIdentityTpl; |
21 |
|
|
|
22 |
|
|
template<typename _Scalar, int _Options> |
23 |
|
|
struct traits< ConstraintIdentityTpl<_Scalar,_Options> > |
24 |
|
|
{ |
25 |
|
|
typedef _Scalar Scalar; |
26 |
|
|
enum { Options = _Options }; |
27 |
|
|
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6; |
28 |
|
|
enum { |
29 |
|
|
LINEAR = 0, |
30 |
|
|
ANGULAR = 3 |
31 |
|
|
}; |
32 |
|
|
typedef MotionTpl<Scalar,Options> JointMotion; |
33 |
|
|
typedef Eigen::Matrix<Scalar,6,1,Options> JointForce; |
34 |
|
|
typedef Eigen::Matrix<Scalar,6,6,Options> DenseBase; |
35 |
|
|
typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType; |
36 |
|
|
typedef typename Matrix6::IdentityReturnType MatrixReturnType; |
37 |
|
|
}; // traits ConstraintRevolute |
38 |
|
|
|
39 |
|
|
|
40 |
|
|
template<typename _Scalar, int _Options> |
41 |
|
|
struct ConstraintIdentityTpl |
42 |
|
|
: ConstraintBase< ConstraintIdentityTpl<_Scalar,_Options> > |
43 |
|
|
{ |
44 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
45 |
|
|
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl) |
46 |
|
|
|
47 |
|
|
enum { NV = 6 }; |
48 |
|
|
|
49 |
|
|
template<typename Vector6Like> |
50 |
|
|
JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const |
51 |
|
|
{ |
52 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6); |
53 |
|
|
return JointMotion(vj); |
54 |
|
|
} |
55 |
|
|
|
56 |
|
|
template<typename S1, int O1> |
57 |
|
|
typename SE3Tpl<S1,O1>::ActionMatrixType |
58 |
|
840 |
se3Action(const SE3Tpl<S1,O1> & m) const |
59 |
|
|
{ |
60 |
|
840 |
return m.toActionMatrix(); |
61 |
|
|
|
62 |
|
|
} |
63 |
|
|
|
64 |
|
|
template<typename S1, int O1> |
65 |
|
|
typename SE3Tpl<S1,O1>::ActionMatrixType |
66 |
|
13 |
se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
67 |
|
|
{ |
68 |
|
13 |
return m.toActionMatrixInverse(); |
69 |
|
|
} |
70 |
|
|
|
71 |
|
31 |
int nv_impl() const { return NV; } |
72 |
|
|
|
73 |
|
|
struct TransposeConst |
74 |
|
|
{ |
75 |
|
|
template<typename Derived> |
76 |
|
|
typename ForceDense<Derived>::ToVectorConstReturnType |
77 |
|
887 |
operator*(const ForceDense<Derived> & phi) |
78 |
|
887 |
{ return phi.toVector(); } |
79 |
|
|
|
80 |
|
|
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ |
81 |
|
|
template<typename MatrixDerived> |
82 |
|
|
typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived) |
83 |
|
64 |
operator*(const Eigen::MatrixBase<MatrixDerived> & F) |
84 |
|
|
{ |
85 |
|
64 |
return F.derived(); |
86 |
|
|
} |
87 |
|
|
}; |
88 |
|
|
|
89 |
|
951 |
TransposeConst transpose() const { return TransposeConst(); } |
90 |
|
41 |
MatrixReturnType matrix_impl() const { return DenseBase::Identity(); } |
91 |
|
|
|
92 |
|
|
template<typename MotionDerived> |
93 |
|
|
typename MotionDerived::ActionMatrixType |
94 |
|
4 |
motionAction(const MotionBase<MotionDerived> & v) const |
95 |
|
4 |
{ return v.toActionMatrix(); } |
96 |
|
|
|
97 |
|
27 |
bool isEqual(const ConstraintIdentityTpl &) const { return true; } |
98 |
|
|
|
99 |
|
|
}; // struct ConstraintIdentityTpl |
100 |
|
|
|
101 |
|
|
template<typename Scalar, int Options, typename Vector6Like> |
102 |
|
|
MotionRef<const Vector6Like> |
103 |
|
2080 |
operator*(const ConstraintIdentityTpl<Scalar,Options> &, |
104 |
|
|
const Eigen::MatrixBase<Vector6Like> & v) |
105 |
|
|
{ |
106 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6); |
107 |
|
|
// typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion; |
108 |
|
|
typedef MotionRef<const Vector6Like> Motion; |
109 |
✓✗ |
2080 |
return Motion(v.derived()); |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
113 |
|
|
template<typename S1, int O1, typename S2, int O2> |
114 |
|
|
inline typename InertiaTpl<S1,O1>::Matrix6 |
115 |
|
40 |
operator*(const InertiaTpl<S1,O1> & Y, const ConstraintIdentityTpl<S2,O2> &) |
116 |
|
|
{ |
117 |
|
40 |
return Y.matrix(); |
118 |
|
|
} |
119 |
|
|
|
120 |
|
|
/* [ABA] Y*S operator*/ |
121 |
|
|
template<typename Matrix6Like, typename S2, int O2> |
122 |
|
|
inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) |
123 |
|
2 |
operator*(const Eigen::MatrixBase<Matrix6Like> & Y, const ConstraintIdentityTpl<S2,O2> &) |
124 |
|
|
{ |
125 |
|
2 |
return Y.derived(); |
126 |
|
|
} |
127 |
|
|
|
128 |
|
|
template<typename S1, int O1> |
129 |
|
|
struct SE3GroupAction< ConstraintIdentityTpl<S1,O1> > |
130 |
|
|
{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; }; |
131 |
|
|
|
132 |
|
|
template<typename S1, int O1, typename MotionDerived> |
133 |
|
|
struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived > |
134 |
|
|
{ typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; }; |
135 |
|
|
|
136 |
|
|
template<typename Scalar, int Options> struct JointFreeFlyerTpl; |
137 |
|
|
|
138 |
|
|
template<typename _Scalar, int _Options> |
139 |
|
|
struct traits< JointFreeFlyerTpl<_Scalar,_Options> > |
140 |
|
|
{ |
141 |
|
|
enum { |
142 |
|
|
NQ = 7, |
143 |
|
|
NV = 6 |
144 |
|
|
}; |
145 |
|
|
typedef _Scalar Scalar; |
146 |
|
|
enum { Options = _Options }; |
147 |
|
|
typedef JointDataFreeFlyerTpl<Scalar,Options> JointDataDerived; |
148 |
|
|
typedef JointModelFreeFlyerTpl<Scalar,Options> JointModelDerived; |
149 |
|
|
typedef ConstraintIdentityTpl<Scalar,Options> Constraint_t; |
150 |
|
|
typedef SE3Tpl<Scalar,Options> Transformation_t; |
151 |
|
|
typedef MotionTpl<Scalar,Options> Motion_t; |
152 |
|
|
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
153 |
|
|
|
154 |
|
|
// [ABA] |
155 |
|
|
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
156 |
|
|
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
157 |
|
|
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
158 |
|
|
|
159 |
|
|
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
160 |
|
|
|
161 |
|
|
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
162 |
|
|
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
163 |
|
|
}; |
164 |
|
|
|
165 |
|
|
template<typename Scalar, int Options> |
166 |
|
|
struct traits< JointDataFreeFlyerTpl<Scalar,Options> > |
167 |
|
|
{ typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; }; |
168 |
|
|
|
169 |
|
|
template<typename Scalar, int Options> |
170 |
|
|
struct traits< JointModelFreeFlyerTpl<Scalar,Options> > |
171 |
|
|
{ typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; }; |
172 |
|
|
|
173 |
|
|
template<typename _Scalar, int _Options> |
174 |
|
|
struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> > |
175 |
|
|
{ |
176 |
|
4264 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
177 |
|
|
typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived; |
178 |
|
|
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
179 |
|
50694 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
180 |
|
|
|
181 |
|
|
Constraint_t S; |
182 |
|
|
Transformation_t M; |
183 |
|
|
Motion_t v; |
184 |
|
|
Bias_t c; |
185 |
|
|
|
186 |
|
|
// [ABA] specific data |
187 |
|
|
U_t U; |
188 |
|
|
D_t Dinv; |
189 |
|
|
UD_t UDinv; |
190 |
|
|
|
191 |
|
3468 |
JointDataFreeFlyerTpl() |
192 |
|
|
: M(Transformation_t::Identity()) |
193 |
|
|
, v(Motion_t::Zero()) |
194 |
|
|
, U(U_t::Zero()) |
195 |
|
|
, Dinv(D_t::Zero()) |
196 |
✓✗✓✗ ✓✗ |
3468 |
, UDinv(UD_t::Identity()) |
197 |
|
3468 |
{} |
198 |
|
|
|
199 |
✓✗ |
44 |
static std::string classname() { return std::string("JointDataFreeFlyer"); } |
200 |
|
3 |
std::string shortname() const { return classname(); } |
201 |
|
|
|
202 |
|
|
}; // struct JointDataFreeFlyerTpl |
203 |
|
|
|
204 |
|
|
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl); |
205 |
|
|
template<typename _Scalar, int _Options> |
206 |
|
|
struct JointModelFreeFlyerTpl |
207 |
|
|
: public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> > |
208 |
|
|
{ |
209 |
|
4538 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
210 |
|
|
typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived; |
211 |
|
|
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
212 |
|
|
|
213 |
|
|
typedef JointModelBase<JointModelFreeFlyerTpl> Base; |
214 |
|
|
using Base::id; |
215 |
|
|
using Base::idx_q; |
216 |
|
|
using Base::idx_v; |
217 |
|
|
using Base::setIndexes; |
218 |
|
|
|
219 |
|
3435 |
JointDataDerived createData() const { return JointDataDerived(); } |
220 |
|
|
|
221 |
|
2 |
const std::vector<bool> hasConfigurationLimit() const |
222 |
|
|
{ |
223 |
✓✗ |
2 |
return {true, true, true, false, false, false, false}; |
224 |
|
|
} |
225 |
|
|
|
226 |
|
2 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
227 |
|
|
{ |
228 |
✓✗ |
2 |
return {true, true, true, false, false, false}; |
229 |
|
|
} |
230 |
|
|
|
231 |
|
|
template<typename ConfigVectorLike> |
232 |
|
|
inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const |
233 |
|
|
{ |
234 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike); |
235 |
|
|
typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion; |
236 |
|
|
typedef Eigen::Map<const Quaternion> ConstQuaternionMap; |
237 |
|
|
|
238 |
|
|
ConstQuaternionMap quat(q_joint.template tail<4>().data()); |
239 |
|
|
//assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision |
240 |
|
|
assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4); |
241 |
|
|
|
242 |
|
|
M.rotation(quat.matrix()); |
243 |
|
|
M.translation(q_joint.template head<3>()); |
244 |
|
|
} |
245 |
|
|
|
246 |
|
|
template<typename Vector3Derived, typename QuaternionDerived> |
247 |
|
|
EIGEN_DONT_INLINE |
248 |
|
22687 |
void calc(JointDataDerived & data, |
249 |
|
|
const typename Eigen::MatrixBase<Vector3Derived> & trans, |
250 |
|
|
const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const |
251 |
|
|
{ |
252 |
✓✗ |
22687 |
data.M.translation(trans); |
253 |
✓✗ |
22687 |
data.M.rotation(quat.matrix()); |
254 |
|
22687 |
} |
255 |
|
|
|
256 |
|
|
template<typename ConfigVector> |
257 |
|
|
EIGEN_DONT_INLINE |
258 |
|
22678 |
void calc(JointDataDerived & data, |
259 |
|
|
const typename Eigen::PlainObjectBase<ConfigVector> & qs) const |
260 |
|
|
{ |
261 |
|
|
typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion; |
262 |
|
|
typedef Eigen::Map<const Quaternion> ConstQuaternionMap; |
263 |
|
|
|
264 |
✓✗✓✗
|
22678 |
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q()); |
265 |
✓✗✓✗
|
22678 |
ConstQuaternionMap quat(q.template tail<4>().data()); |
266 |
|
|
|
267 |
✓✗✓✗
|
22678 |
calc(data,q.template head<3>(),quat); |
268 |
|
22678 |
} |
269 |
|
|
|
270 |
|
|
template<typename ConfigVector> |
271 |
|
|
EIGEN_DONT_INLINE |
272 |
|
1 |
void calc(JointDataDerived & data, |
273 |
|
|
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
274 |
|
|
{ |
275 |
|
|
typedef typename Eigen::Quaternion<Scalar,Options> Quaternion; |
276 |
|
|
|
277 |
✓✗✓✗
|
1 |
typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q()); |
278 |
✓✗✓✗
|
1 |
const Quaternion quat(q.template tail<4>()); |
279 |
|
|
|
280 |
✓✗✓✗
|
1 |
calc(data,q.template head<3>(),quat); |
281 |
|
1 |
} |
282 |
|
|
|
283 |
|
|
template<typename ConfigVector, typename TangentVector> |
284 |
|
|
EIGEN_DONT_INLINE |
285 |
|
3509 |
void calc(JointDataDerived & data, |
286 |
|
|
const typename Eigen::MatrixBase<ConfigVector> & qs, |
287 |
|
|
const typename Eigen::MatrixBase<TangentVector> & vs) const |
288 |
|
|
{ |
289 |
|
3509 |
calc(data,qs.derived()); |
290 |
|
|
|
291 |
✓✗ |
3509 |
data.v = vs.template segment<NV>(idx_v()); |
292 |
|
3509 |
} |
293 |
|
|
|
294 |
|
|
template<typename Matrix6Like> |
295 |
|
264 |
void calc_aba(JointDataDerived & data, |
296 |
|
|
const Eigen::MatrixBase<Matrix6Like> & I, |
297 |
|
|
const bool update_I) const |
298 |
|
|
{ |
299 |
|
264 |
data.U = I; |
300 |
|
|
|
301 |
|
|
// compute inverse |
302 |
|
|
// data.Dinv.setIdentity(); |
303 |
|
|
// I.llt().solveInPlace(data.Dinv); |
304 |
|
264 |
internal::PerformStYSInversion<Scalar>::run(I,data.Dinv); |
305 |
|
|
|
306 |
✓✓ |
264 |
if (update_I) |
307 |
|
2 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero(); |
308 |
|
264 |
} |
309 |
|
|
|
310 |
✓✗ |
16388 |
static std::string classname() { return std::string("JointModelFreeFlyer"); } |
311 |
|
16346 |
std::string shortname() const { return classname(); } |
312 |
|
|
|
313 |
|
|
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
314 |
|
|
template<typename NewScalar> |
315 |
|
9 |
JointModelFreeFlyerTpl<NewScalar,Options> cast() const |
316 |
|
|
{ |
317 |
|
|
typedef JointModelFreeFlyerTpl<NewScalar,Options> ReturnType; |
318 |
|
9 |
ReturnType res; |
319 |
|
9 |
res.setIndexes(id(),idx_q(),idx_v()); |
320 |
|
9 |
return res; |
321 |
|
|
} |
322 |
|
|
|
323 |
|
|
}; // struct JointModelFreeFlyerTpl |
324 |
|
|
|
325 |
|
|
} // namespace pinocchio |
326 |
|
|
|
327 |
|
|
#include <boost/type_traits.hpp> |
328 |
|
|
|
329 |
|
|
namespace boost |
330 |
|
|
{ |
331 |
|
|
template<typename Scalar, int Options> |
332 |
|
|
struct has_nothrow_constructor< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> > |
333 |
|
|
: public integral_constant<bool,true> {}; |
334 |
|
|
|
335 |
|
|
template<typename Scalar, int Options> |
336 |
|
|
struct has_nothrow_copy< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> > |
337 |
|
|
: public integral_constant<bool,true> {}; |
338 |
|
|
|
339 |
|
|
template<typename Scalar, int Options> |
340 |
|
|
struct has_nothrow_constructor< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> > |
341 |
|
|
: public integral_constant<bool,true> {}; |
342 |
|
|
|
343 |
|
|
template<typename Scalar, int Options> |
344 |
|
|
struct has_nothrow_copy< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> > |
345 |
|
|
: public integral_constant<bool,true> {}; |
346 |
|
|
} |
347 |
|
|
|
348 |
|
|
#endif // ifndef __pinocchio_joint_free_flyer_hpp__ |