Go to the documentation of this file.
6 #ifndef __pinocchio_multibody_joint_revolute_unaligned_hpp__
7 #define __pinocchio_multibody_joint_revolute_unaligned_hpp__
20 template<
typename Scalar,
int Options = context::Options>
24 template<
typename Scalar,
int Options>
30 template<
typename Scalar,
int Options,
typename MotionDerived>
36 template<
typename _Scalar,
int _Options>
44 typedef Eigen::Matrix<Scalar, 3, 1, Options>
Vector3;
45 typedef Eigen::Matrix<Scalar, 6, 1, Options>
Vector6;
46 typedef Eigen::Matrix<Scalar, 4, 4, Options>
Matrix4;
47 typedef Eigen::Matrix<Scalar, 6, 6, Options>
Matrix6;
65 template<
typename _Scalar,
int _Options>
66 struct MotionRevoluteUnalignedTpl : MotionBase<MotionRevoluteUnalignedTpl<_Scalar, _Options>>
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 template<
typename Vector3Like,
typename OtherScalar>
82 inline PlainReturnType
plain()
const
84 return PlainReturnType(PlainReturnType::Vector3::Zero(),
m_axis *
m_w);
87 template<
typename OtherScalar>
93 template<
typename MotionDerived>
99 template<
typename Derived>
106 template<
typename S2,
int O2,
typename D2>
110 v.angular().noalias() =
m_w *
m.rotation() *
m_axis;
113 v.linear().noalias() =
m.translation().cross(
v.angular());
116 template<
typename S2,
int O2>
124 template<
typename S2,
int O2,
typename D2>
130 v3_tmp.noalias() =
m_axis.cross(
m.translation());
132 v.linear().noalias() =
m.rotation().transpose() * v3_tmp;
135 v.angular().noalias() =
m.rotation().transpose() *
m_axis;
139 template<
typename S2,
int O2>
147 template<
typename M1,
typename M2>
159 template<
typename M1>
197 template<
typename S1,
int O1,
typename MotionDerived>
198 inline typename MotionDerived::MotionPlain
201 typename MotionDerived::MotionPlain
res(m2);
206 template<
typename MotionDerived,
typename S2,
int O2>
207 inline typename MotionDerived::MotionPlain
210 return m2.motionAction(m1);
213 template<
typename Scalar,
int Options>
216 template<
typename _Scalar,
int _Options>
238 typedef Eigen::Matrix<Scalar, 3, 1, Options>
Vector3;
243 template<
typename Scalar,
int Options>
249 template<
typename Scalar,
int Options,
typename MotionDerived>
257 template<
typename Scalar,
int Options,
typename ForceDerived>
262 typedef Eigen::Matrix<
271 template<
typename Scalar,
int Options,
typename ForceSet>
277 Eigen::Transpose<const Vector3>,
278 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>
::type ReturnType;
281 template<
typename _Scalar,
int _Options>
285 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
299 template<
typename Vector3Like>
305 template<
typename Vector1Like>
306 JointMotion
__mult__(
const Eigen::MatrixBase<Vector1Like> &
v)
const
308 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
309 return JointMotion(
m_axis,
v[0]);
312 template<
typename S1,
int O1>
321 res.template segment<3>(ANGULAR).noalias() =
m.rotation() *
m_axis;
322 res.template segment<3>(LINEAR).noalias() =
323 m.translation().cross(
res.template segment<3>(ANGULAR));
327 template<
typename S1,
int O1>
335 res.template segment<3>(ANGULAR).noalias() =
m.rotation().transpose() *
m_axis;
336 res.template segment<3>(LINEAR).noalias() =
337 -
m.rotation().transpose() *
m.translation().cross(
m_axis);
355 template<
typename ForceDerived>
367 template<
typename ForceSet>
372 ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
374 return ref.
axis().transpose() * F.template middleRows<3>(ANGULAR);
392 S.template segment<3>(LINEAR).setZero();
393 S.template segment<3>(ANGULAR) =
m_axis;
397 template<
typename MotionDerived>
401 const typename MotionDerived::ConstLinearType
v =
m.linear();
402 const typename MotionDerived::ConstAngularType
w =
m.angular();
405 res.template segment<3>(LINEAR).noalias() =
v.cross(
m_axis);
406 res.template segment<3>(ANGULAR).noalias() =
w.cross(
m_axis);
430 template<
typename S1,
int O1,
typename S2,
int O2>
439 template<
typename S1,
int O1,
typename S2,
int O2>
454 res.template segment<3>(Inertia::LINEAR) = -
m *
c.cross(cru.
axis());
455 res.template segment<3>(Inertia::ANGULAR).noalias() = I * cru.
axis();
456 res.template segment<3>(Inertia::ANGULAR) +=
457 c.cross(
res.template segment<3>(Inertia::LINEAR));
464 template<
typename M6Like,
typename Scalar,
int Options>
466 Eigen::MatrixBase<M6Like>,
480 template<
typename M6Like,
typename Scalar,
int Options>
482 Eigen::MatrixBase<M6Like>,
491 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
492 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.
axis();
497 template<
typename Scalar,
int Options>
500 template<
typename _Scalar,
int _Options>
521 typedef Eigen::Matrix<Scalar, 6, NV, Options>
U_t;
522 typedef Eigen::Matrix<Scalar, NV, NV, Options>
D_t;
523 typedef Eigen::Matrix<Scalar, 6, NV, Options>
UD_t;
531 template<
typename _Scalar,
int _Options>
538 template<
typename _Scalar,
int _Options>
545 template<
typename _Scalar,
int _Options>
547 :
public JointDataBase<JointDataRevoluteUnalignedTpl<_Scalar, _Options>>
549 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
569 :
joint_q(ConfigVector_t::Zero())
570 ,
joint_v(TangentVector_t::Zero())
571 ,
M(Transformation_t::Identity())
576 ,
UDinv(UD_t::Zero())
581 template<
typename Vector3Like>
583 :
joint_q(ConfigVector_t::Zero())
584 ,
joint_v(TangentVector_t::Zero())
585 ,
M(Transformation_t::Identity())
590 ,
UDinv(UD_t::Zero())
597 return std::string(
"JointDataRevoluteUnaligned");
607 template<
typename _Scalar,
int _Options>
608 struct JointModelRevoluteUnalignedTpl
609 :
public JointModelBase<JointModelRevoluteUnalignedTpl<_Scalar, _Options>>
611 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
614 typedef Eigen::Matrix<Scalar, 3, 1, _Options>
Vector3;
634 template<
typename Vector3Like>
638 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
644 return JointDataDerived(
axis);
663 template<
typename ConfigVector>
664 void calc(JointDataDerived &
data,
const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
671 template<
typename TangentVector>
673 calc(JointDataDerived &
data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
679 template<
typename ConfigVector,
typename TangentVector>
681 JointDataDerived &
data,
682 const typename Eigen::MatrixBase<ConfigVector> &
qs,
683 const typename Eigen::MatrixBase<TangentVector> & vs)
const
690 template<
typename VectorLike,
typename Matrix6Like>
692 JointDataDerived &
data,
693 const Eigen::MatrixBase<VectorLike> & armature,
694 const Eigen::MatrixBase<Matrix6Like> & I,
695 const bool update_I)
const
697 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) *
axis;
699 Scalar(1) / (
axis.dot(
data.U.template segment<3>(Motion::ANGULAR)) + armature[0]);
708 return std::string(
"JointModelRevoluteUnaligned");
716 template<
typename NewScalar>
720 ReturnType
res(
axis.template cast<NewScalar>());
735 #include <boost/type_traits.hpp>
739 template<
typename Scalar,
int Options>
741 :
public integral_constant<bool, true>
745 template<
typename Scalar,
int Options>
747 :
public integral_constant<bool, true>
751 template<
typename Scalar,
int Options>
753 :
public integral_constant<bool, true>
757 template<
typename Scalar,
int Options>
759 :
public integral_constant<bool, true>
764 #endif // ifndef __pinocchio_multibody_joint_revolute_unaligned_hpp__
ConstraintForceOp< JointMotionSubspaceRevoluteUnalignedTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
MotionTpl< Scalar, Options > ReturnType
SE3GroupAction< JointMotionSubspaceRevoluteUnalignedTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
Eigen::Matrix< Scalar, NV, NV, Options > D_t
JointMotionSubspaceRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
Forward declaration of the multiplication operation return type. Should be overloaded,...
const std::vector< bool > hasConfigurationLimitInTangent() const
PlainReturnType plain() const
JointModelRevoluteUnalignedTpl()
#define PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(D1, D2)
Macro giving access to the return type of the dot product operation.
SE3Tpl< Scalar, Options > Transformation_t
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.
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
void calc(JointDataDerived &data, const Blank, const typename Eigen::MatrixBase< TangentVector > &vs) const
MotionRevoluteUnalignedTpl< Scalar, Options > Motion_t
const typedef DenseBase ConstMatrixReturnType
MotionRevoluteUnalignedTpl< context::Scalar > MotionRevoluteUnaligned
Eigen::internal::remove_const< M6LikeCols >::type M6LikeColsNonConst
Constraint::Vector3 Vector3
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...
InertiaTpl< S1, O1 > Inertia
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
std::string shortname() const
JointDataRevoluteUnalignedTpl< Scalar, Options > JointDataDerived
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void addTo(MotionDense< MotionDerived > &v) const
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Eigen::Matrix< typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3, typename ForceDense< ForceDerived >::ConstAngularType), 1, 1, Options > ReturnType
const typedef Vector3 ConstLinearType
MotionRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis, const OtherScalar &w)
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
MotionTpl< Scalar, Options > MotionPlain
SizeDepType< 3 >::ColsReturn< M6Like >::ConstType M6LikeCols
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
JointMotionSubspaceRevoluteUnalignedTpl< Scalar, Options > Constraint_t
traits< JointMotionSubspaceRevoluteUnalignedTpl >::Vector3 Vector3
MotionDerived::MotionPlain operator+(const MotionHelicalUnalignedTpl< S1, O1 > &m1, const MotionDense< MotionDerived > &m2)
static std::string classname()
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
ReturnTypeNotDefined ReturnType
void setIndexes(JointIndex id, int q, int v)
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
Return type of the ation of a Motion onto an object of type D.
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
TransposeConst transpose() const
ConstLinearType linear() const
bool isEqual(const JointModelRevoluteUnalignedTpl &other) const
Matrix4 HomogeneousMatrixType
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
JointDataDerived createData() const
JointModelRevoluteUnalignedTpl< Scalar, Options > JointModelDerived
Symmetric3Tpl< Scalar, Options > Symmetric3
JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
MotionZeroTpl< Scalar, Options > Bias_t
Eigen::Matrix< Scalar, 1, 1, Options > JointForce
ReturnTypeNotDefined ReturnType
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
traits< JointMotionSubspaceRevoluteUnalignedTpl< Scalar, Options > >::Vector3 Vector3
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
static ReturnType run(const Inertia &Y, const Constraint &cru)
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
Free-flyer joint in .
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
JointMotionSubspaceRevoluteUnalignedTpl< Scalar, Options > Constraint
MotionTpl< Scalar, Options > ReturnType
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
JointMotionSubspaceRevoluteUnalignedTpl< S2, O2 > Constraint
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR ConfigVector_t joint_q
traits< JointMotionSubspaceRevoluteUnalignedTpl< Scalar, Options > >::Vector3 Vector3
MotionAlgebraAction< JointMotionSubspaceRevoluteUnalignedTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
MotionRevoluteUnalignedTpl< Scalar, Options > JointMotion
JointMotionSubspaceRevoluteUnalignedTpl()
const typedef Vector3 ConstAngularType
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType
const JointMotionSubspaceRevoluteUnalignedTpl & ref
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
static std::string classname()
Eigen::Matrix< S2, 6, 1, O2 > ReturnType
TransposeConst(const JointMotionSubspaceRevoluteUnalignedTpl &ref)
MotionRevoluteUnalignedTpl __mult__(const OtherScalar &alpha) const
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.
std::string shortname() const
JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
MotionPlain motionAction(const MotionDense< M1 > &v) const
const Vector3 & axis() const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
bool comparison_eq(const LhsType &lhs_value, const RhsType &rhs_value)
JointModelRevoluteUnalignedTpl(const Scalar &x, const Scalar &y, const Scalar &z)
Return type of the Constraint::Transpose * ForceSet operation.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
DenseBase MatrixReturnType
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
MotionPlain PlainReturnType
bool isEqual_impl(const MotionRevoluteUnalignedTpl &other) const
MatrixMatrixProduct< Eigen::Transpose< const Vector3 >, typename Eigen::MatrixBase< const ForceSet >::template NRowsBlockXpr< 3 >::Type >::type ReturnType
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &cru)
DenseBase matrix_impl() const
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
MotionRevoluteUnalignedTpl()
Eigen::Matrix< Scalar, 6, 1, Options > DenseBase
EIGEN_STRONG_INLINE MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionHelicalUnalignedTpl< S2, O2 > &m2)
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
bool isEqual(const JointMotionSubspaceRevoluteUnalignedTpl &other) const
Common traits structure to fully define base classes for CRTP.
const Scalar & angularRate() const
ConstraintForceSetOp< JointMotionSubspaceRevoluteUnalignedTpl, ForceSet >::ReturnType operator*(const Eigen::MatrixBase< ForceSet > &F)
Vector3 axis
3d main axis of the joint.
const Vector3 & axis() const
#define PINOCCHIO_EIGEN_REF_TYPE(D)
const std::vector< bool > hasConfigurationLimit() const
Eigen::Matrix< Scalar, 6, NV, Options > U_t
JointDataRevoluteUnalignedTpl()
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl)
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
Eigen::Matrix< Scalar, 1, 1, Options > ReducedSquaredMatrix
JointMotionSubspaceRevoluteUnalignedTpl< Scalar, Options > Constraint
bool isEqual(const JointModelBase< OtherDerived > &) const
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
ConstAngularType angular() const
JointModelRevoluteUnalignedTpl< NewScalar, Options > cast() const
Main pinocchio namespace.
JointModelBase< JointModelRevoluteUnalignedTpl > Base
SE3GroupAction< JointMotionSubspaceRevoluteUnalignedTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
void setTo(MotionDense< Derived > &other) const
const typedef MatrixMatrixProduct< M6LikeColsNonConst, Vector3 >::type ReturnType
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:10