Go to the documentation of this file.
6 #ifndef __pinocchio_joint_revolute_unaligned_hpp__
7 #define __pinocchio_joint_revolute_unaligned_hpp__
21 template<
typename Scalar,
int Options>
27 template<
typename Scalar,
int Options,
typename MotionDerived>
33 template<
typename _Scalar,
int _Options>
38 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
39 typedef Eigen::Matrix<Scalar,6,1,Options>
Vector6;
40 typedef Eigen::Matrix<Scalar,4,4,Options>
Matrix4;
41 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
58 template<
typename _Scalar,
int _Options>
59 struct MotionRevoluteUnalignedTpl
60 : MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 template<
typename Vector3Like,
typename OtherScalar>
69 const OtherScalar &
w)
74 inline PlainReturnType
plain()
const
76 return PlainReturnType(PlainReturnType::Vector3::Zero(),
80 template<
typename OtherScalar>
86 template<
typename MotionDerived>
92 template<
typename Derived>
99 template<
typename S2,
int O2,
typename D2>
103 v.angular().noalias() =
m_w *
m.rotation() *
m_axis;
106 v.linear().noalias() =
m.translation().cross(
v.angular());
109 template<
typename S2,
int O2>
117 template<
typename S2,
int O2,
typename D2>
123 v3_tmp.noalias() =
m_axis.cross(
m.translation());
125 v.linear().noalias() =
m.rotation().transpose() * v3_tmp;
128 v.angular().noalias() =
m.rotation().transpose() *
m_axis;
132 template<
typename S2,
int O2>
140 template<
typename M1,
typename M2>
152 template<
typename M1>
162 return m_axis == other.m_axis &&
m_w == other.m_w;
177 template<
typename S1,
int O1,
typename MotionDerived>
178 inline typename MotionDerived::MotionPlain
182 typename MotionDerived::MotionPlain
res(m2);
187 template<
typename MotionDerived,
typename S2,
int O2>
188 inline typename MotionDerived::MotionPlain
192 return m2.motionAction(m1);
197 template<
typename _Scalar,
int _Options>
212 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
215 template<
typename Scalar,
int Options>
219 template<
typename Scalar,
int Options,
typename MotionDerived>
223 template<
typename Scalar,
int Options,
typename ForceDerived>
230 template<
typename Scalar,
int Options,
typename ForceSet>
235 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
239 template<
typename _Scalar,
int _Options>
241 :
ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
243 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
252 template<
typename Vector3Like>
257 template<
typename Vector1Like>
258 JointMotion
__mult__(
const Eigen::MatrixBase<Vector1Like> &
v)
const
260 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
261 return JointMotion(
m_axis,
v[0]);
264 template<
typename S1,
int O1>
272 res.template segment<3>(ANGULAR).noalias() =
m.rotation() *
m_axis;
273 res.template segment<3>(LINEAR).noalias() =
m.translation().cross(
res.template segment<3>(ANGULAR));
277 template<
typename S1,
int O1>
284 res.template segment<3>(ANGULAR).noalias() =
m.rotation().transpose() *
m_axis;
285 res.template segment<3>(LINEAR).noalias() = -
m.rotation().transpose() *
m.translation().cross(
m_axis);
296 template<
typename ForceDerived>
307 template<
typename ForceSet>
311 EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
313 return ref.
axis().transpose() * F.template middleRows<3>(ANGULAR);
329 S.template segment<3>(LINEAR).setZero();
330 S.template segment<3>(ANGULAR) =
m_axis;
334 template<
typename MotionDerived>
338 const typename MotionDerived::ConstLinearType
v =
m.linear();
339 const typename MotionDerived::ConstAngularType
w =
m.angular();
342 res.template segment<3>(LINEAR).noalias() =
v.cross(
m_axis);
343 res.template segment<3>(ANGULAR).noalias() =
w.cross(
m_axis);
362 template<
typename S1,
int O1,
typename S2,
int O2>
371 template<
typename S1,
int O1,
typename S2,
int O2>
384 const typename Inertia::Vector3 &
c =
Y.lever();
387 res.template segment<3>(Inertia::LINEAR) = -
m*
c.cross(cru.
axis());
388 res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.
axis();
389 res.template segment<3>(Inertia::ANGULAR) +=
c.cross(
res.template segment<3>(Inertia::LINEAR));
396 template<
typename M6Like,
typename Scalar,
int Options>
410 template<
typename M6Like,
typename Scalar,
int Options>
419 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
420 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.
axis();
427 template<
typename _Scalar,
int _Options>
444 typedef Eigen::Matrix<Scalar,6,NV,Options>
U_t;
445 typedef Eigen::Matrix<Scalar,NV,NV,Options>
D_t;
446 typedef Eigen::Matrix<Scalar,6,NV,Options>
UD_t;
455 template<
typename Scalar,
int Options>
459 template<
typename Scalar,
int Options>
463 template<
typename _Scalar,
int _Options>
465 :
public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> >
467 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
483 :
M(Transformation_t::Identity())
484 ,
S(Constraint_t::Vector3::Zero())
485 ,
v(Constraint_t::Vector3::Zero(),(
Scalar)0)
488 ,
UDinv(UD_t::Zero())
491 template<
typename Vector3Like>
493 :
M(Transformation_t::Identity())
498 ,
UDinv(UD_t::Zero())
501 static std::string
classname() {
return std::string(
"JointDataRevoluteUnaligned"); }
507 template<
typename _Scalar,
int _Options>
508 struct JointModelRevoluteUnalignedTpl
509 :
public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> >
511 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
514 typedef Eigen::Matrix<Scalar,3,1,_Options>
Vector3;
533 template<
typename Vector3Like>
537 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
559 template<
typename ConfigVector>
561 const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
564 typedef Eigen::AngleAxis<Scalar>
AngleAxis;
566 const OtherScalar &
q =
qs[
idx_q()];
571 template<
typename ConfigVector,
typename TangentVector>
573 const typename Eigen::MatrixBase<ConfigVector> &
qs,
574 const typename Eigen::MatrixBase<TangentVector> & vs)
const
581 template<
typename Matrix6Like>
582 void calc_aba(JointDataDerived &
data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const
584 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) *
axis;
592 static std::string
classname() {
return std::string(
"JointModelRevoluteUnaligned"); }
596 template<
typename NewScalar>
600 ReturnType
res(
axis.template cast<NewScalar>());
615 #include <boost/type_traits.hpp>
619 template<
typename Scalar,
int Options>
621 :
public integral_constant<bool,true> {};
623 template<
typename Scalar,
int Options>
625 :
public integral_constant<bool,true> {};
627 template<
typename Scalar,
int Options>
629 :
public integral_constant<bool,true> {};
631 template<
typename Scalar,
int Options>
633 :
public integral_constant<bool,true> {};
637 #endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__
JointRevoluteUnalignedTpl< Scalar, Options > JointDerived
const Vector3 & axis() const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
Forward declaration of the multiplication operation return type. Should be overloaded,...
MotionZeroTpl< Scalar, Options > Bias_t
const std::vector< bool > hasConfigurationLimitInTangent() const
PlainReturnType plain() const
JointModelRevoluteUnalignedTpl()
DenseBase matrix_impl() const
#define PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(D1, D2)
Macro giving access to the return type of the dot product operation.
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
ConstraintRevoluteUnalignedTpl< Scalar, Options > Constraint_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...
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
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...
std::string shortname() const
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
void addTo(MotionDense< MotionDerived > &v) const
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
const typedef Vector3 ConstLinearType
const typedef DenseBase ConstMatrixReturnType
MotionRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis, const OtherScalar &w)
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
MotionTpl< Scalar, Options > ReturnType
Eigen::Matrix< Scalar, 6, 1, Options > DenseBase
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Transformation_t M
ConstraintForceOp< ConstraintRevoluteUnalignedTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
static std::string classname()
JointModelRevoluteUnalignedTpl< NewScalar, Options > cast() const
Eigen::Matrix< typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3, typename ForceDense< ForceDerived >::ConstAngularType), 1, 1, Options > ReturnType
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
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
SE3GroupAction< ConstraintRevoluteUnalignedTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
Eigen::Matrix< Scalar, 6, NV, Options > U_t
ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
const ConstraintRevoluteUnalignedTpl & ref
Return type of the ation of a Motion onto an object of type D.
ConstraintRevoluteUnalignedTpl< S2, O2 > Constraint
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &cru)
ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl, ForceSet >::ReturnType operator*(const Eigen::MatrixBase< ForceSet > &F)
ConstLinearType linear() const
bool isEqual(const JointModelRevoluteUnalignedTpl &other) const
Matrix4 HomogeneousMatrixType
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
JointDataDerived createData() const
static ReturnType run(const Inertia &Y, const Constraint &cru)
Eigen::Matrix< Scalar, 1, 1, Options > JointForce
ReturnTypeNotDefined ReturnType
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
MotionTpl< Scalar, Options > ReturnType
DenseBase MatrixReturnType
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
Eigen::AngleAxis< FCL_REAL > AngleAxis
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
MotionAlgebraAction< ConstraintRevoluteUnalignedTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
ConstraintRevoluteUnalignedTpl< Scalar, Options > Constraint
JointModelRevoluteUnalignedTpl< Scalar, Options > JointModelDerived
MatrixMatrixProduct< Eigen::Transpose< const Vector3 >, typename Eigen::MatrixBase< const ForceSet >::template NRowsBlockXpr< 3 >::Type >::type ReturnType
SE3Tpl< Scalar, Options > Transformation_t
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
MotionDerived::MotionPlain operator+(const MotionPlanarTpl< Scalar, Options > &m1, const MotionDense< MotionDerived > &m2)
const typedef Vector3 ConstAngularType
Eigen::Matrix< S2, 6, 1, O2 > ReturnType
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
static std::string classname()
Symmetric3Tpl< double, 0 > Symmetric3
MotionRevoluteUnalignedTpl __mult__(const OtherScalar &alpha) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
traits< ConstraintRevoluteUnalignedTpl< Scalar, Options > >::Vector3 Vector3
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
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
MotionPlain motionAction(const MotionDense< M1 > &v) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
JointModelRevoluteUnalignedTpl(const Scalar &x, const Scalar &y, const Scalar &z)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
Return type of the Constraint::Transpose * ForceSet operation.
MotionTpl< Scalar, Options > MotionPlain
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
MotionPlain PlainReturnType
bool isEqual_impl(const MotionRevoluteUnalignedTpl &other) const
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
ConstraintRevoluteUnalignedTpl()
TransposeConst(const ConstraintRevoluteUnalignedTpl &ref)
MotionRevoluteUnalignedTpl< double > MotionRevoluteUnaligned
MotionRevoluteUnalignedTpl< Scalar, Options > Motion_t
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
MotionRevoluteUnalignedTpl()
traits< ConstraintRevoluteUnalignedTpl >::Vector3 Vector3
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
Eigen::internal::remove_const< M6LikeCols >::type M6LikeColsNonConst
const typedef MatrixMatrixProduct< M6LikeColsNonConst, Vector3 >::type ReturnType
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
Common traits structure to fully define base classes for CRTP.
const Scalar & angularRate() const
ConstraintRevoluteUnalignedTpl< Scalar, Options > Constraint
SizeDepType< 3 >::ColsReturn< M6Like >::ConstType M6LikeCols
Vector3 axis
3d main axis of the joint.
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
const Vector3 & axis() const
#define PINOCCHIO_EIGEN_REF_TYPE(D)
MotionRevoluteUnalignedTpl< Scalar, Options > JointMotion
const std::vector< bool > hasConfigurationLimit() const
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
JointRevoluteUnalignedTpl< Scalar, Options > JointDerived
JointDataRevoluteUnalignedTpl< Scalar, Options > JointDataDerived
JointDataRevoluteUnalignedTpl()
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl)
Constraint::Vector3 Vector3
traits< ConstraintRevoluteUnalignedTpl< Scalar, Options > >::Vector3 Vector3
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
InertiaTpl< S1, O1 > Inertia
TransposeConst transpose() const
ConstAngularType angular() const
Return the angular part of the force vector.
bool isEqual(const ConstraintRevoluteUnalignedTpl &other) const
bool isEqual(const JointModelBase< OtherDerived > &) const
ConstAngularType angular() const
Eigen::Matrix< Scalar, NV, NV, Options > D_t
Main pinocchio namespace.
JointModelBase< JointModelRevoluteUnalignedTpl > Base
void setTo(MotionDense< Derived > &other) const
SE3GroupAction< ConstraintRevoluteUnalignedTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
pinocchio
Author(s):
autogenerated on Tue Feb 13 2024 03:43:59