pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
joint-universal.hpp
1//
2// Copyright (c) 2023 INRIA
3//
4
5#ifndef __pinocchio_multibody_joint_universal_hpp__
6#define __pinocchio_multibody_joint_universal_hpp__
7
8#include "pinocchio/fwd.hpp"
9#include "pinocchio/multibody/joint/joint-base.hpp"
10#include "pinocchio/multibody/joint/joint-spherical.hpp"
11#include "pinocchio/multibody/joint-motion-subspace.hpp"
12#include "pinocchio/spatial/inertia.hpp"
13#include "pinocchio/utils/check.hpp"
14
15#include "pinocchio/math/matrix.hpp"
16#include "pinocchio/math/rotation.hpp"
17
18namespace pinocchio
19{
20 template<typename Scalar, int Options>
21 struct JointMotionSubspaceUniversalTpl;
22
23 template<typename _Scalar, int _Options>
25 {
26 typedef _Scalar Scalar;
27 enum
28 {
29 Options = _Options
30 };
31 enum
32 {
33 LINEAR = 0,
34 ANGULAR = 3
35 };
36
38 typedef Eigen::Matrix<Scalar, 2, 1, Options> JointForce;
39 typedef Eigen::Matrix<Scalar, 6, 2, Options> DenseBase;
40 typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix;
41
42 typedef DenseBase MatrixReturnType;
43 typedef const DenseBase ConstMatrixReturnType;
44
45 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
46 typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32;
47 typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2;
48
49 typedef Matrix2 StDiagonalMatrixSOperationReturnType;
50 }; // traits JointMotionSubspaceUniversalTpl
51
52 template<typename Scalar, int Options>
54 {
55 typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType;
56 };
57
58 template<typename Scalar, int Options, typename MotionDerived>
60 {
61 typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType;
62 };
63
64 template<typename Scalar, int Options, typename ForceDerived>
66 {
67 typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Vector3 Vector3;
68 typedef Eigen::Matrix<
69 typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(
71 2,
72 1,
73 Options>
74 ReturnType;
75 };
76
77 template<typename Scalar, int Options, typename ForceSet>
79 {
80 typedef typename traits<JointMotionSubspaceUniversalTpl<Scalar, Options>>::Matrix32 Matrix32;
81 typedef typename MatrixMatrixProduct<
82 Eigen::Transpose<const Matrix32>,
83 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>::type ReturnType;
84 };
85
86 template<typename _Scalar, int _Options>
88 : JointMotionSubspaceBase<JointMotionSubspaceUniversalTpl<_Scalar, _Options>>
89 {
91 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceUniversalTpl)
92
93 enum
94 {
95 NV = 2
96 };
97
100
102 {
103 }
104
105 template<typename Matrix32Like>
106 JointMotionSubspaceUniversalTpl(const Eigen::MatrixBase<Matrix32Like> & subspace)
107 : m_S(subspace)
108 {
109 }
110
111 template<typename Vector3Like>
112 JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
113 {
114 // EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
115 return JointMotion(m_S * v);
116 }
117
118 template<typename S1, int O1>
119 typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType
120 se3Action(const SE3Tpl<S1, O1> & m) const
121 {
122 typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType;
123
124 ReturnType res;
125 res.template middleRows<3>(ANGULAR).noalias() = m.rotation() * m_S;
126 cross(
127 m.translation(), res.template middleRows<3>(Motion::ANGULAR),
128 res.template middleRows<3>(LINEAR));
129 return res;
130 }
131
132 template<typename S1, int O1>
133 typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType
134 se3ActionInverse(const SE3Tpl<S1, O1> & m) const
135 {
136 typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType;
137
138 ReturnType res;
139 cross(m.translation(), m_S, res.template middleRows<3>(ANGULAR));
140 res.template middleRows<3>(LINEAR).noalias() =
141 -m.rotation().transpose() * res.template middleRows<3>(ANGULAR);
142
143 // ANGULAR
144 res.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S;
145 return res;
146 }
147
148 int nv_impl() const
149 {
150 return NV;
151 }
152
153 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceUniversalTpl>
154 {
157 : ref(ref)
158 {
159 }
160
161 template<typename ForceDerived>
163 operator*(const ForceDense<ForceDerived> & f) const
164 {
165 return ref.m_S.transpose() * f.angular();
166 }
167
168 /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
169 template<typename ForceSet>
171 operator*(const Eigen::MatrixBase<ForceSet> & F)
172 {
174 ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
175 /* Return ax.T * F[3:end,:] */
176 return ref.m_S.transpose() * F.template middleRows<3>(ANGULAR);
177 }
178 };
179
180 TransposeConst transpose() const
181 {
182 return TransposeConst(*this);
183 }
184
185 /* CRBA joint operators
186 * - ForceSet::Block = ForceSet
187 * - ForceSet operator* (Inertia Y,Constraint S)
188 * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
189 * - SE3::act(ForceSet::Block)
190 */
191 DenseBase matrix_impl() const
192 {
193 DenseBase S;
194 S.template middleRows<3>(LINEAR).setZero();
195 S.template middleRows<3>(ANGULAR) = m_S;
196 return S;
197 }
198
199 template<typename MotionDerived>
200 typename MotionAlgebraAction<JointMotionSubspaceUniversalTpl, MotionDerived>::ReturnType
201 motionAction(const MotionDense<MotionDerived> & m) const
202 {
203 const typename MotionDerived::ConstLinearType v = m.linear();
204 const typename MotionDerived::ConstAngularType w = m.angular();
205
206 DenseBase res;
207 cross(v, m_S, res.template middleRows<3>(LINEAR));
208 cross(w, m_S, res.template middleRows<3>(ANGULAR));
209 return res;
210 }
211
212 const Matrix32 & angularSubspace() const
213 {
214 return m_S;
215 }
216 Matrix32 & angularSubspace()
217 {
218 return m_S;
219 }
220
221 bool isEqual(const JointMotionSubspaceUniversalTpl & other) const
222 {
223 return internal::comparison_eq(m_S, other.m_S);
224 }
225
226 protected:
227 Matrix32 m_S;
228
229 }; // struct JointMotionSubspaceUniversalTpl
230
231 namespace details
232 {
233 template<typename Scalar, int Options>
235 {
238
239 static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint)
240 {
241 return constraint.matrix().transpose() * constraint.matrix();
242 }
243 };
244 } // namespace details
245
246 template<typename S1, int O1, typename S2, int O2>
248 {
249 typedef Eigen::Matrix<S2, 6, 2, O2> ReturnType;
250 };
251
252 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
253 namespace impl
254 {
255 template<typename S1, int O1, typename S2, int O2>
257 {
260 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
261 static inline ReturnType run(const Inertia & Y, const Constraint & cru)
262 {
264 Eigen::Matrix<S1, 6, 3, O1> M;
265 alphaSkew(-Y.mass(), Y.lever(), M.template middleRows<3>(Constraint::LINEAR));
266 M.template middleRows<3>(Constraint::ANGULAR) =
267 (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
268
269 return (M * cru.angularSubspace()).eval();
270 }
271 };
272 } // namespace impl
273
274 template<typename M6Like, typename Scalar, int Options>
276 Eigen::MatrixBase<M6Like>,
277 JointMotionSubspaceUniversalTpl<Scalar, Options>>
278 {
279 typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
280 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
281
283 typedef typename Constraint::Matrix32 Matrix32;
284 typedef const typename MatrixMatrixProduct<M6LikeColsNonConst, Matrix32>::type ReturnType;
285 };
286
287 /* [ABA] operator* (Inertia Y,Constraint S) */
288 namespace impl
289 {
290 template<typename M6Like, typename Scalar, int Options>
292 Eigen::MatrixBase<M6Like>,
293 JointMotionSubspaceUniversalTpl<Scalar, Options>>
294 {
296 typedef
297 typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType;
298
299 static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru)
300 {
302 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.angularSubspace();
303 }
304 };
305 } // namespace impl
306
307 template<typename Scalar, int Options>
309
310 template<typename _Scalar, int _Options>
312 {
313 enum
314 {
315 NQ = 2,
316 NV = 2,
317 NVExtended = 2
318 };
319 typedef _Scalar Scalar;
320 enum
321 {
322 Options = _Options
323 };
330
331 // [ABA]
332 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
333 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
334 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
335
336 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
337 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
338
339 typedef boost::mpl::false_ is_mimicable_t;
340
341 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
342 };
343
344 template<typename _Scalar, int _Options>
350
351 template<typename _Scalar, int _Options>
357
358 template<typename _Scalar, int _Options>
359 struct JointDataUniversalTpl : public JointDataBase<JointDataUniversalTpl<_Scalar, _Options>>
360 {
363 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
364 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
365
366 ConfigVector_t joint_q;
367 TangentVector_t joint_v;
368
369 Transformation_t M;
370 Constraint_t S;
371 Motion_t v;
372 Bias_t c;
373
374 // [ABA] specific data
375 U_t U;
376 D_t Dinv;
377 UD_t UDinv;
378 D_t StU;
379
381 : joint_q(ConfigVector_t::Zero())
382 , joint_v(TangentVector_t::Zero())
383 , M(Transformation_t::Identity())
384 , S(Constraint_t::Matrix32::Zero())
385 , v(Motion_t::Vector3::Zero())
386 , c(Bias_t::Vector3::Zero())
387 , U(U_t::Zero())
388 , Dinv(D_t::Zero())
389 , UDinv(UD_t::Zero())
390 , StU(D_t::Zero())
391 {
392 }
393
394 static std::string classname()
395 {
396 return std::string("JointDataUniversal");
397 }
398 std::string shortname() const
399 {
400 return classname();
401 }
402
403 }; // struct JointDataUniversalTpl
404
405 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelUniversalTpl);
406 template<typename _Scalar, int _Options>
407 struct JointModelUniversalTpl : public JointModelBase<JointModelUniversalTpl<_Scalar, _Options>>
408 {
411 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
412 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
413 typedef Eigen::Matrix<Scalar, 3, 3, _Options> Matrix3;
414
416 using Base::id;
417 using Base::idx_q;
418 using Base::idx_v;
419 using Base::idx_vExtended;
420 using Base::setIndexes;
421
423 {
424 }
425
427 const Scalar & x1,
428 const Scalar & y1,
429 const Scalar & z1,
430 const Scalar & x2,
431 const Scalar & y2,
432 const Scalar & z2)
433 : axis1(x1, y1, z1)
434 , axis2(x2, y2, z2)
435 {
436 assert(isUnitary(axis1) && "First Rotation axis is not unitary");
437 assert(isUnitary(axis2) && "Second Rotation axis is not unitary");
438 assert(check_expression_if_real<Scalar>(axis1.dot(axis2) == 0) && "Axii are not orthogonal");
439 }
440
441 template<typename Vector3Like>
443 const Eigen::MatrixBase<Vector3Like> & axis1_, const Eigen::MatrixBase<Vector3Like> & axis2_)
444 : axis1(axis1_)
445 , axis2(axis2_)
446 {
448 assert(isUnitary(axis1) && "First Rotation axis is not unitary");
449 assert(isUnitary(axis2) && "Second Rotation axis is not unitary");
450 assert(
451 check_expression_if_real<Scalar>(axis1.dot(axis2) == Scalar(0))
452 && "Axii are not orthogonal");
453 }
454
455 JointDataDerived createData() const
456 {
457 return JointDataDerived();
458 }
459
460 const std::vector<bool> hasConfigurationLimit() const
461 {
462 return {true, true};
463 }
464
465 const std::vector<bool> hasConfigurationLimitInTangent() const
466 {
467 return {true, true};
468 }
469
470 using Base::isEqual;
471 bool isEqual(const JointModelUniversalTpl & other) const
472 {
473 return Base::isEqual(other) && internal::comparison_eq(axis1, other.axis1)
474 && internal::comparison_eq(axis2, other.axis2);
475 }
476
477 template<typename ConfigVector>
478 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
479 {
480 data.joint_q = qs.template segment<NQ>(idx_q());
481 Scalar c0, s0;
482 SINCOS(data.joint_q(0), &s0, &c0);
483 Scalar c1, s1;
484 SINCOS(data.joint_q(1), &s1, &c1);
485
486 Matrix3 rot1, rot2;
488 toRotationMatrix(axis2, c1, s1, rot2);
489 data.M.rotation() = rot1 * rot2;
490
491 data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y()
492 + rot2.coeffRef(2, 0) * axis1.z(),
493 axis2.x(),
494 rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y()
495 + rot2.coeffRef(2, 1) * axis1.z(),
496 axis2.y(),
497 rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y()
498 + rot2.coeffRef(2, 2) * axis1.z(),
499 axis2.z();
500 }
501
502 template<typename TangentVector>
503 void
504 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
505 const
506 {
507 data.joint_v = vs.template segment<NV>(idx_v());
508 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
509
510 // TODO: need to be done
511 // #define q_dot data.joint_v
512 // Scalar tmp;
513 // tmp = (-s1+axis2.x()*axis2.x()*s1)*axis1.x() +
514 // (axis2.x()*axis2.y()*s1+axis2.z()*c1)*axis1.y() +
515 // (axis2.x()*axis2.z()*s1-axis2.y()*c1)*axis1.z(); data.c()(0) = tmp *
516 // q_dot(1)*q_dot(0); tmp = (axis2.x()*axis2.y()*s1-axis2.z()*c1)*axis1.x() +
517 // (-s1+axis2.y()*axis2.y()*s1)*axis1.y() +
518 // (axis2.y()*axis2.z()*s1+axis2.x()*c1)*axis1.z(); data.c()(1) = tmp *
519 // q_dot(1)*q_dot(0); tmp = (axis2.z()*axis2.x()*s1+axis2.y()*c1)*axis1.x() +
520 // (axis2.y()*axis2.z()*s1-axis2.x()*c1)*axis1.y() +
521 // (-s1+axis2.z()*axis2.z()*s1)*axis1.z(); data.c()(2) = tmp * q_dot(1)*q_dot(0);
522 // #undef q_dot
523 }
524
525 template<typename ConfigVector, typename TangentVector>
526 void calc(
527 JointDataDerived & data,
528 const typename Eigen::MatrixBase<ConfigVector> & qs,
529 const typename Eigen::MatrixBase<TangentVector> & vs) const
530 {
531 data.joint_q = qs.template segment<NQ>(idx_q());
532
533 Scalar c0, s0;
534 SINCOS(data.joint_q(0), &s0, &c0);
535 Scalar c1, s1;
536 SINCOS(data.joint_q(1), &s1, &c1);
537
538 Matrix3 rot1, rot2;
540 toRotationMatrix(axis2, c1, s1, rot2);
541 data.M.rotation() = rot1 * rot2;
542
543 data.S.angularSubspace() << rot2.coeffRef(0, 0) * axis1.x() + rot2.coeffRef(1, 0) * axis1.y()
544 + rot2.coeffRef(2, 0) * axis1.z(),
545 axis2.x(),
546 rot2.coeffRef(0, 1) * axis1.x() + rot2.coeffRef(1, 1) * axis1.y()
547 + rot2.coeffRef(2, 1) * axis1.z(),
548 axis2.y(),
549 rot2.coeffRef(0, 2) * axis1.x() + rot2.coeffRef(1, 2) * axis1.y()
550 + rot2.coeffRef(2, 2) * axis1.z(),
551 axis2.z();
552
553 data.joint_v = vs.template segment<NV>(idx_v());
554 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
555
556#define q_dot data.joint_v
557 Scalar tmp;
558 tmp = (-s1 + axis2.x() * axis2.x() * s1) * axis1.x()
559 + (axis2.x() * axis2.y() * s1 + axis2.z() * c1) * axis1.y()
560 + (axis2.x() * axis2.z() * s1 - axis2.y() * c1) * axis1.z();
561 data.c()(0) = tmp * q_dot(1) * q_dot(0);
562 tmp = (axis2.x() * axis2.y() * s1 - axis2.z() * c1) * axis1.x()
563 + (-s1 + axis2.y() * axis2.y() * s1) * axis1.y()
564 + (axis2.y() * axis2.z() * s1 + axis2.x() * c1) * axis1.z();
565 data.c()(1) = tmp * q_dot(1) * q_dot(0);
566 tmp = (axis2.z() * axis2.x() * s1 + axis2.y() * c1) * axis1.x()
567 + (axis2.y() * axis2.z() * s1 - axis2.x() * c1) * axis1.y()
568 + (-s1 + axis2.z() * axis2.z() * s1) * axis1.z();
569 data.c()(2) = tmp * q_dot(1) * q_dot(0);
570#undef q_dot
571 }
572
573 template<typename VectorLike, typename Matrix6Like>
574 void calc_aba(
575 JointDataDerived & data,
576 const Eigen::MatrixBase<VectorLike> & armature,
577 const Eigen::MatrixBase<Matrix6Like> & I,
578 const bool update_I) const
579 {
580 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
581 data.StU.noalias() =
582 data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
583 data.StU.diagonal() += armature;
584 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
585
586 data.UDinv.noalias() = data.U * data.Dinv;
587
588 if (update_I)
589 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
590 }
591
592 static std::string classname()
593 {
594 return std::string("JointModelUniversal");
595 }
596 std::string shortname() const
597 {
598 return classname();
599 }
600
602 template<typename NewScalar>
604 {
606 ReturnType res(axis1.template cast<NewScalar>(), axis2.template cast<NewScalar>());
607 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
608 return res;
609 }
610
613 Vector3 axis1;
614 Vector3 axis2;
615 }; // struct JointModelUniversalTpl
616
617} // namespace pinocchio
618
619#include <boost/type_traits.hpp>
620
621namespace boost
622{
623 template<typename Scalar, int Options>
624 struct has_nothrow_constructor<::pinocchio::JointModelUniversalTpl<Scalar, Options>>
625 : public integral_constant<bool, true>
626 {
627 };
628
629 template<typename Scalar, int Options>
630 struct has_nothrow_copy<::pinocchio::JointModelUniversalTpl<Scalar, Options>>
631 : public integral_constant<bool, true>
632 {
633 };
634
635 template<typename Scalar, int Options>
636 struct has_nothrow_constructor<::pinocchio::JointDataUniversalTpl<Scalar, Options>>
637 : public integral_constant<bool, true>
638 {
639 };
640
641 template<typename Scalar, int Options>
642 struct has_nothrow_copy<::pinocchio::JointDataUniversalTpl<Scalar, Options>>
643 : public integral_constant<bool, true>
644 {
645 };
646} // namespace boost
647
648#endif // ifndef __pinocchio_multibody_joint_universal_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 SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition sincos.hpp:27
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.
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition skew.hpp:228
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar....
Definition skew.hpp:134
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
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
JointModelUniversalTpl< NewScalar, Options > cast() const
Vector3 axis1
3d main axii of the joint.
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition binary-op.hpp:15
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72