Go to the documentation of this file.
5 #ifndef __pinocchio_multibody_joint_universal_hpp__
6 #define __pinocchio_multibody_joint_universal_hpp__
20 template<
typename Scalar,
int Options>
23 template<
typename _Scalar,
int _Options>
39 typedef Eigen::Matrix<Scalar, 6, 2, Options>
DenseBase;
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;
52 template<
typename Scalar,
int Options>
58 template<
typename Scalar,
int Options,
typename MotionDerived>
64 template<
typename Scalar,
int Options,
typename ForceDerived>
68 typedef Eigen::Matrix<
77 template<
typename Scalar,
int Options,
typename ForceSet>
82 Eigen::Transpose<const Matrix32>,
83 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>
::type ReturnType;
86 template<
typename _Scalar,
int _Options>
90 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
105 template<
typename Matrix32Like>
111 template<
typename Vector3Like>
112 JointMotion
__mult__(
const Eigen::MatrixBase<Vector3Like> &
v)
const
115 return JointMotion(
m_S *
v);
118 template<
typename S1,
int O1>
125 res.template middleRows<3>(ANGULAR).noalias() =
m.rotation() *
m_S;
127 m.translation(),
res.template middleRows<3>(Motion::ANGULAR),
128 res.template middleRows<3>(LINEAR));
132 template<
typename S1,
int O1>
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);
144 res.template middleRows<3>(ANGULAR).noalias() =
m.rotation().transpose() *
m_S;
161 template<
typename ForceDerived>
165 return ref.
m_S.transpose() *
f.angular();
169 template<
typename ForceSet>
174 ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
176 return ref.
m_S.transpose() * F.template middleRows<3>(ANGULAR);
194 S.template middleRows<3>(LINEAR).setZero();
195 S.template middleRows<3>(ANGULAR) =
m_S;
199 template<
typename MotionDerived>
203 const typename MotionDerived::ConstLinearType
v =
m.linear();
204 const typename MotionDerived::ConstAngularType
w =
m.angular();
233 template<
typename Scalar,
int Options>
241 return constraint.
matrix().transpose() * constraint.
matrix();
246 template<
typename S1,
int O1,
typename S2,
int O2>
255 template<
typename S1,
int O1,
typename S2,
int O2>
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();
274 template<
typename M6Like,
typename Scalar,
int Options>
276 Eigen::MatrixBase<M6Like>,
290 template<
typename M6Like,
typename Scalar,
int Options>
292 Eigen::MatrixBase<M6Like>,
301 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
302 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.
angularSubspace();
307 template<
typename Scalar,
int Options>
310 template<
typename _Scalar,
int _Options>
331 typedef Eigen::Matrix<Scalar, 6, NV, Options>
U_t;
332 typedef Eigen::Matrix<Scalar, NV, NV, Options>
D_t;
333 typedef Eigen::Matrix<Scalar, 6, NV, Options>
UD_t;
341 template<
typename _Scalar,
int _Options>
348 template<
typename _Scalar,
int _Options>
355 template<
typename _Scalar,
int _Options>
358 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
378 :
joint_q(ConfigVector_t::Zero())
379 ,
joint_v(TangentVector_t::Zero())
380 ,
M(Transformation_t::Identity())
381 ,
S(Constraint_t::Matrix32::Zero())
386 ,
UDinv(UD_t::Zero())
393 return std::string(
"JointDataUniversal");
403 template<
typename _Scalar,
int _Options>
404 struct JointModelUniversalTpl :
public JointModelBase<JointModelUniversalTpl<_Scalar, _Options>>
406 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
409 typedef Eigen::Matrix<Scalar, 3, 1, _Options>
Vector3;
410 typedef Eigen::Matrix<Scalar, 3, 3, _Options>
Matrix3;
433 assert(
isUnitary(
axis2) &&
"Second Rotation axis is not unitary");
434 assert(check_expression_if_real<Scalar>(
axis1.dot(
axis2) == 0) &&
"Axii are not orthogonal");
437 template<
typename Vector3Like>
439 const Eigen::MatrixBase<Vector3Like> & axis1_,
const Eigen::MatrixBase<Vector3Like> & axis2_)
443 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
445 assert(
isUnitary(
axis2) &&
"Second Rotation axis is not unitary");
448 &&
"Axii are not orthogonal");
453 return JointDataDerived();
473 template<
typename ConfigVector>
474 void calc(JointDataDerived &
data,
const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
485 data.M.rotation() = rot1 * rot2;
487 data.S.angularSubspace() << rot2.coeffRef(0, 0) *
axis1.x() + rot2.coeffRef(1, 0) *
axis1.y()
488 + rot2.coeffRef(2, 0) *
axis1.z(),
490 rot2.coeffRef(0, 1) *
axis1.x() + rot2.coeffRef(1, 1) *
axis1.y()
491 + rot2.coeffRef(2, 1) *
axis1.z(),
493 rot2.coeffRef(0, 2) *
axis1.x() + rot2.coeffRef(1, 2) *
axis1.y()
494 + rot2.coeffRef(2, 2) *
axis1.z(),
498 template<
typename TangentVector>
500 calc(JointDataDerived &
data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
503 data.joint_v = vs.template segment<NV>(
idx_v());
504 data.v().noalias() =
data.S.angularSubspace() *
data.joint_v;
521 template<
typename ConfigVector,
typename TangentVector>
523 JointDataDerived &
data,
524 const typename Eigen::MatrixBase<ConfigVector> &
qs,
525 const typename Eigen::MatrixBase<TangentVector> & vs)
const
537 data.M.rotation() = rot1 * rot2;
539 data.S.angularSubspace() << rot2.coeffRef(0, 0) *
axis1.x() + rot2.coeffRef(1, 0) *
axis1.y()
540 + rot2.coeffRef(2, 0) *
axis1.z(),
542 rot2.coeffRef(0, 1) *
axis1.x() + rot2.coeffRef(1, 1) *
axis1.y()
543 + rot2.coeffRef(2, 1) *
axis1.z(),
545 rot2.coeffRef(0, 2) *
axis1.x() + rot2.coeffRef(1, 2) *
axis1.y()
546 + rot2.coeffRef(2, 2) *
axis1.z(),
549 data.joint_v = vs.template segment<NV>(
idx_v());
550 data.v().noalias() =
data.S.angularSubspace() *
data.joint_v;
552 #define q_dot data.joint_v
569 template<
typename VectorLike,
typename Matrix6Like>
571 JointDataDerived &
data,
572 const Eigen::MatrixBase<VectorLike> & armature,
573 const Eigen::MatrixBase<Matrix6Like> & I,
574 const bool update_I)
const
576 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) *
data.S.angularSubspace();
578 data.S.angularSubspace().transpose() *
data.U.template middleRows<3>(Motion::ANGULAR);
579 data.StU.diagonal() += armature;
590 return std::string(
"JointModelUniversal");
598 template<
typename NewScalar>
602 ReturnType
res(
axis1.template cast<NewScalar>(),
axis2.template cast<NewScalar>());
615 #include <boost/type_traits.hpp>
619 template<
typename Scalar,
int Options>
621 :
public integral_constant<bool, true>
625 template<
typename Scalar,
int Options>
627 :
public integral_constant<bool, true>
631 template<
typename Scalar,
int Options>
633 :
public integral_constant<bool, true>
637 template<
typename Scalar,
int Options>
639 :
public integral_constant<bool, true>
644 #endif // ifndef __pinocchio_multibody_joint_universal_hpp__
SE3GroupAction< JointMotionSubspaceUniversalTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
Eigen::Matrix< Scalar, 3, 3, Options > ReducedSquaredMatrix
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
TransposeConst transpose() const
Eigen::Matrix< Scalar, NV, NV, Options > D_t
JointModelUniversalTpl< NewScalar, Options > cast() const
Symmetric3Tpl< context::Scalar, context::Options > Symmetric3
Forward declaration of the multiplication operation return type. Should be overloaded,...
MotionSphericalTpl< Scalar, Options > Bias_t
Eigen::Matrix< Scalar, 6, NV, Options > U_t
static std::string classname()
#define PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(D1, D2)
Macro giving access to the return type of the dot product operation.
traits< JointMotionSubspaceUniversalTpl >::Matrix32 Matrix32
JointModelUniversalTpl(const Scalar &x1, const Scalar &y1, const Scalar &z1, const Scalar &x2, const Scalar &y2, const Scalar &z2)
traits< JointMotionSubspaceUniversalTpl >::Vector3 Vector3
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.
Eigen::Matrix< Scalar, 6, 2, Options > ReturnType
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
static ReturnType run(const JointMotionSubspaceBase< Constraint > &constraint)
Return type of the Constraint::Transpose * Force operation.
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
JointDataUniversalTpl< Scalar, Options > JointDataDerived
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR ConfigVector_t joint_q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
traits< Constraint >::StDiagonalMatrixSOperationReturnType ReturnType
Matrix2 StDiagonalMatrixSOperationReturnType
JointDataDerived createData() const
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void calc(JointDataDerived &data, const Blank, const typename Eigen::MatrixBase< TangentVector > &vs) const
DenseBase matrix_impl() const
Eigen::Matrix< Scalar, 6, 2, Options > ReturnType
MatrixMatrixProduct< Eigen::Transpose< const Matrix32 >, typename Eigen::MatrixBase< const ForceSet >::template NRowsBlockXpr< 3 >::Type >::type ReturnType
traits< JointMotionSubspaceUniversalTpl< Scalar, Options > >::Matrix32 Matrix32
JointMotion __mult__(const Eigen::MatrixBase< Vector3Like > &v) const
const std::vector< bool > hasConfigurationLimit() const
JointModelUniversalTpl(const Eigen::MatrixBase< Vector3Like > &axis1_, const Eigen::MatrixBase< Vector3Like > &axis2_)
traits< JointMotionSubspaceUniversalTpl< Scalar, Options > >::Vector3 Vector3
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
Eigen::Matrix< Scalar, 6, 2, Options > DenseBase
Eigen::Matrix< Scalar, 3, 2, Options > Matrix32
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
ReturnTypeNotDefined ReturnType
void setIndexes(JointIndex id, int q, int v)
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Return type of the ation of a Motion onto an object of type D.
JointUniversalTpl< _Scalar, _Options > JointDerived
ConstraintForceSetOp< JointMotionSubspaceUniversalTpl, ForceSet >::ReturnType operator*(const Eigen::MatrixBase< ForceSet > &F)
SE3Tpl< Scalar, Options > Transformation_t
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
DenseBase MatrixReturnType
MotionAlgebraAction< JointMotionSubspaceUniversalTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
bool isEqual(const JointModelUniversalTpl &other) const
MatrixReturnType matrix()
Eigen::internal::remove_const< M6LikeCols >::type M6LikeColsNonConst
const typedef DenseBase ConstMatrixReturnType
std::string shortname() const
JointUniversalTpl< _Scalar, _Options > JointDerived
ReturnTypeNotDefined ReturnType
JointMotionSubspaceUniversalTpl< Scalar, Options > Constraint_t
SizeDepType< 3 >::ColsReturn< M6Like >::ConstType M6LikeCols
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointUniversalTpl< _Scalar, _Options > JointDerived
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
Free-flyer joint in .
const JointMotionSubspaceUniversalTpl & ref
Eigen::Matrix< Scalar, 2, 2, Options > Matrix2
TransposeConst(const JointMotionSubspaceUniversalTpl &ref)
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Eigen::Matrix< typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3, typename ForceDense< ForceDerived >::ConstAngularType), 2, 1, Options > ReturnType
JointMotionSubspaceUniversalTpl< S2, O2 > Constraint
ConstraintForceOp< JointMotionSubspaceUniversalTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
JointMotionSubspaceUniversalTpl(const Eigen::MatrixBase< Matrix32Like > &subspace)
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
MotionSphericalTpl< Scalar, Options > JointMotion
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.
JointMotionSubspaceUniversalTpl< Scalar, Options > Constraint
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
bool comparison_eq(const LhsType &lhs_value, const RhsType &rhs_value)
Return type of the Constraint::Transpose * ForceSet operation.
Constraint::Matrix32 Matrix32
JointMotionSubspaceUniversalTpl< Scalar, Options > Constraint
bool isEqual(const JointMotionSubspaceUniversalTpl &other) const
static std::string classname()
Eigen::Matrix< S2, 6, 2, O2 > ReturnType
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.
static ReturnType run(const Inertia &Y, const Constraint &cru)
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
SE3GroupAction< JointMotionSubspaceUniversalTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
MotionSphericalTpl< Scalar, Options > Motion_t
std::string shortname() const
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
JointMotionSubspaceUniversalTpl()
const Matrix32 & angularSubspace() const
Matrix32 & angularSubspace()
Common traits structure to fully define base classes for CRTP.
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
const typedef MatrixMatrixProduct< M6LikeColsNonConst, Matrix32 >::type ReturnType
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointUniversalTpl< _Scalar, _Options > JointDerived
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....
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
JointModelUniversalTpl< Scalar, Options > JointModelDerived
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
JointModelBase< JointModelUniversalTpl > Base
Eigen::Matrix< Scalar, 2, 1, Options > JointForce
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
const std::vector< bool > hasConfigurationLimitInTangent() const
Eigen::Matrix< Scalar, 3, 3, _Options > Matrix3
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
bool isEqual(const JointModelBase< OtherDerived > &) const
InertiaTpl< S1, O1 > Inertia
JointMotionSubspaceUniversalTpl< Scalar, Options > Constraint
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &cru)
Main pinocchio namespace.
Vector3 axis1
3d main axii of the joint.
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:11