6 #ifndef __pinocchio_joint_revolute_unaligned_hpp__ 7 #define __pinocchio_joint_revolute_unaligned_hpp__ 9 #include "pinocchio/fwd.hpp" 10 #include "pinocchio/multibody/joint/joint-base.hpp" 11 #include "pinocchio/multibody/constraint.hpp" 12 #include "pinocchio/spatial/inertia.hpp" 13 #include "pinocchio/math/matrix.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>
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>
96 other.
angular().noalias() = m_axis*m_w;
99 template<
typename S2,
int O2,
typename D2>
109 template<
typename S2,
int O2>
113 se3Action_impl(m,res);
117 template<
typename S2,
int O2,
typename D2>
132 template<
typename S2,
int O2>
136 se3ActionInverse_impl(m,res);
140 template<
typename M1,
typename M2>
152 template<
typename M1>
162 return m_axis == other.
m_axis && m_w == other.
m_w;
168 const Vector3 &
axis()
const {
return m_axis; }
169 Vector3 &
axis() {
return m_axis; }
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
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>
227 typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,
Options>
ReturnType;
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);
348 const Vector3 &
axis()
const {
return m_axis; }
349 Vector3 &
axis() {
return m_axis; }
353 return m_axis == other.
m_axis;
362 template<
typename S1,
int O1,
typename S2,
int O2>
371 template<
typename S1,
int O1,
typename S2,
int O2>
377 static inline ReturnType
run(
const Inertia &
Y,
378 const Constraint & cru)
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>
416 static inline ReturnType
run(
const Eigen::MatrixBase<M6Like> &
Y,
417 const Constraint & cru)
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>
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);
538 assert(
isUnitary(axis) &&
"Rotation axis is not unitary");
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 576 calc(data,qs.derived());
578 data.v.angularRate() =
static_cast<Scalar>(vs[
idx_v()]);
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;
585 data.Dinv[0] = (
Scalar)(1)/
axis.dot(data.U.template segment<3>(Motion::ANGULAR));
586 data.UDinv.noalias() = data.U * data.Dinv;
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__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
MotionTpl< Scalar, Options > ReturnType
const Symmetric3 & inertia() const
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
PlainReturnType plain() const
ConstraintForceOp< ConstraintRevoluteUnalignedTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
traits< ConstraintRevoluteUnalignedTpl< Scalar, Options > >::Vector3 Vector3
InertiaTpl< S1, O1 > Inertia
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
JointDataRevoluteUnalignedTpl()
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...
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
Return type of the Constraint::Transpose * ForceSet operation.
ConstraintRevoluteUnalignedTpl< Scalar, Options > Constraint_t
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
MatrixMatrixProduct< Eigen::Transpose< const Vector3 >, typename Eigen::MatrixBase< const ForceSet >::template NRowsBlockXpr< 3 >::Type >::type ReturnType
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
Return type of the ation of a Motion onto an object of type D.
SE3GroupAction< ConstraintRevoluteUnalignedTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
#define MOTION_TYPEDEF_TPL(Derived)
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
void addTo(MotionDense< MotionDerived > &v) const
MotionPlain motionAction(const MotionDense< M1 > &v) const
std::string shortname() const
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
const Vector3 & lever() const
MotionRevoluteUnalignedTpl()
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &cru)
JointModelRevoluteUnalignedTpl()
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Eigen::Matrix< Scalar, 6, 1, Options > DenseBase
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
SE3GroupAction< ConstraintRevoluteUnalignedTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
bool isEqual(const JointModelRevoluteUnalignedTpl &other) const
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
JointRevoluteUnalignedTpl< Scalar, Options > JointDerived
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
Matrix4 HomogeneousMatrixType
Eigen::Matrix< Scalar, NV, NV, Options > D_t
static std::string classname()
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
bool isEqual(const ConstraintRevoluteUnalignedTpl &other) const
MotionRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis, const OtherScalar &w)
ConstLinearType linear() const
ConstAngularType angular() const
Return the angular part of the force vector.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
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.
MotionRevoluteUnalignedTpl< Scalar, Options > JointMotion
MotionDerived::MotionPlain operator+(const MotionPlanarTpl< Scalar, Options > &m1, const MotionDense< MotionDerived > &m2)
Eigen::internal::remove_const< M6LikeCols >::type M6LikeColsNonConst
ConstAngularType angular() const
Vector3 axis
3d main axis of the joint.
virtual bool isEqual(const CollisionGeometry &other) const=0
const Vector3 ConstLinearType
traits< ConstraintRevoluteUnalignedTpl< Scalar, Options > >::Vector3 Vector3
const std::vector< bool > hasConfigurationLimit() const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
ConstraintRevoluteUnalignedTpl< Scalar, Options > Constraint
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
const Vector3 ConstAngularType
JointModelBase< JointModelRevoluteUnalignedTpl > Base
Constraint::Vector3 Vector3
void setTo(MotionDense< Derived > &other) const
static ReturnType run(const Inertia &Y, const Constraint &cru)
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
ConstraintRevoluteUnalignedTpl< Scalar, Options > Constraint
ConstraintRevoluteUnalignedTpl< S2, O2 > Constraint
MotionRevoluteUnalignedTpl __mult__(const OtherScalar &alpha) const
MotionAlgebraAction< ConstraintRevoluteUnalignedTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
MotionRevoluteUnalignedTpl< double > MotionRevoluteUnaligned
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
const DenseBase ConstMatrixReturnType
MotionPlain PlainReturnType
DenseBase MatrixReturnType
#define PINOCCHIO_EIGEN_REF_TYPE(D)
ConstraintRevoluteUnalignedTpl()
JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Transformation_t M
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
ConstAngularRef rotation() const
bool isEqual_impl(const MotionRevoluteUnalignedTpl &other) const
JointModelRevoluteUnalignedTpl(const Scalar &x, const Scalar &y, const Scalar &z)
Main pinocchio namespace.
ReturnTypeNotDefined ReturnType
Eigen::AngleAxis< FCL_REAL > AngleAxis
Eigen::Matrix< typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3, typename ForceDense< ForceDerived >::ConstAngularType), 1, 1, Options > ReturnType
const Vector3 & axis() const
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
MotionTpl< Scalar, Options > MotionPlain
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
std::string shortname() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
void setZero(std::vector< MatType, Eigen::aligned_allocator< MatType > > &Ms)
Eigen::Matrix< Scalar, 6, NV, Options > U_t
Common traits structure to fully define base classes for CRTP.
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
Symmetric3Tpl< double, 0 > Symmetric3
Eigen::Matrix< S2, 6, 1, O2 > ReturnType
static std::string classname()
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Eigen::Matrix< Scalar, 1, 1, Options > JointForce
MotionTpl< Scalar, Options > ReturnType
JointModelRevoluteUnalignedTpl< NewScalar, Options > cast() 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.
JointDataDerived createData() const
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
DenseBase matrix_impl() const
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
const MatrixMatrixProduct< M6LikeColsNonConst, Vector3 >::type ReturnType
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl, ForceSet >::ReturnType operator*(const Eigen::MatrixBase< ForceSet > &F)
Eigen::ProductReturnType< M1, M2 >::Type type
traits< ConstraintRevoluteUnalignedTpl >::Vector3 Vector3
JointModelRevoluteUnalignedTpl< Scalar, Options > JointModelDerived
static void motionAction(const MotionDense< MotionDerived > &v, const Eigen::MatrixBase< Mat > &iF, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion on a set of forces, represented by a 6xN matrix whose each column represent a spat...
JointDataRevoluteUnalignedTpl< Scalar, Options > JointDataDerived
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
TransposeConst(const ConstraintRevoluteUnalignedTpl &ref)
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteUnalignedTpl< _Scalar, _Options > JointDerived
SE3Tpl< Scalar, Options > Transformation_t
MotionZeroTpl< Scalar, Options > Bias_t
Return type of the Constraint::Transpose * Force operation.
JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
MotionRevoluteUnalignedTpl< Scalar, Options > Motion_t
ReturnTypeNotDefined ReturnType
ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
SizeDepType< 3 >::ColsReturn< M6Like >::ConstType M6LikeCols
const Scalar & angularRate() const
TransposeConst transpose() const
const Vector3 & axis() const
const std::vector< bool > hasConfigurationLimitInTangent() const
JointRevoluteUnalignedTpl< Scalar, Options > JointDerived
ConstLinearRef translation() const
const ConstraintRevoluteUnalignedTpl & ref
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)