Go to the documentation of this file.
6 #ifndef __pinocchio_joint_prismatic_unaligned_hpp__
7 #define __pinocchio_joint_prismatic_unaligned_hpp__
22 template<
typename Scalar,
int Options>
28 template<
typename Scalar,
int Options,
typename MotionDerived>
34 template<
typename _Scalar,
int _Options>
39 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
40 typedef Eigen::Matrix<Scalar,6,1,Options>
Vector6;
41 typedef Eigen::Matrix<Scalar,4,4,Options>
Matrix4;
42 typedef Eigen::Matrix<Scalar,6,6,Options>
Matrix6;
59 template<
typename _Scalar,
int _Options>
60 struct MotionPrismaticUnalignedTpl
61 : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 template<
typename Vector3Like,
typename S2>
72 { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
74 inline PlainReturnType
plain()
const
77 PlainReturnType::Vector3::Zero());
80 template<
typename OtherScalar>
86 template<
typename Derived>
92 template<
typename Derived>
99 template<
typename S2,
int O2,
typename D2>
102 v.linear().noalias() =
m_v * (
m.rotation() *
m_axis);
103 v.angular().setZero();
106 template<
typename S2,
int O2>
114 template<
typename S2,
int O2,
typename D2>
118 v.linear().noalias() =
m_v * (
m.rotation().transpose() *
m_axis);
121 v.angular().setZero();
124 template<
typename S2,
int O2>
132 template<
typename M1,
typename M2>
143 template<
typename M1>
153 return m_axis == other.m_axis &&
m_v == other.m_v;
168 template<
typename Scalar,
int Options,
typename MotionDerived>
169 inline typename MotionDerived::MotionPlain
172 typedef typename MotionDerived::MotionPlain ReturnType;
173 return ReturnType(m1.linearRate() * m1.axis() + m2.
linear(), m2.
angular());
176 template<
typename MotionDerived,
typename S2,
int O2>
177 inline typename MotionDerived::MotionPlain
181 return m2.motionAction(m1);
186 template<
typename _Scalar,
int _Options>
201 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
204 template<
typename Scalar,
int Options>
208 template<
typename Scalar,
int Options,
typename MotionDerived>
212 template<
typename Scalar,
int Options,
typename ForceDerived>
219 template<
typename Scalar,
int Options,
typename ForceSet>
224 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
228 template<
typename _Scalar,
int _Options>
230 :
ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
232 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
241 template<
typename Vector3Like>
244 { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
246 template<
typename Vector1Like>
247 JointMotion
__mult__(
const Eigen::MatrixBase<Vector1Like> &
v)
const
249 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
250 return JointMotion(
m_axis,
v[0]);
253 template<
typename S1,
int O1>
259 v.linear().noalias() =
m.rotation()*
m_axis;
260 v.angular().setZero();
264 template<
typename S1,
int O1>
270 v.linear().noalias() =
m.rotation().transpose()*
m_axis;
271 v.angular().setZero();
282 template<
typename ForceDerived>
293 template<
typename ForceSet>
297 EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
299 return ref.
axis().transpose() * F.template middleRows<3>(LINEAR);
315 S.template segment<3>(LINEAR) =
m_axis;
316 S.template segment<3>(ANGULAR).setZero();
320 template<
typename MotionDerived>
324 res.template segment<3>(LINEAR).noalias() =
v.angular().cross(
m_axis);
325 res.template segment<3>(ANGULAR).setZero();
344 template<
typename S1,
int O1,
typename S2,
int O2>
353 template<
typename S1,
int O1,
typename S2,
int O2>
365 const S1 &
m =
Y.mass();
366 const typename Inertia::Vector3 &
c =
Y.lever();
368 res.template segment<3>(Constraint::LINEAR).noalias() =
m*cpu.
axis();
369 res.template segment<3>(Constraint::ANGULAR).noalias() =
c.cross(
res.template segment<3>(Constraint::LINEAR));
376 template<
typename M6Like,
typename Scalar,
int Options>
390 template<
typename M6Like,
typename Scalar,
int Options>
398 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
399 return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.
axis();
406 template<
typename _Scalar,
int _Options>
423 typedef Eigen::Matrix<Scalar,6,NV,Options>
U_t;
424 typedef Eigen::Matrix<Scalar,NV,NV,Options>
D_t;
425 typedef Eigen::Matrix<Scalar,6,NV,Options>
UD_t;
433 template<
typename Scalar,
int Options>
437 template<
typename _Scalar,
int _Options>
439 :
public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
441 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
457 :
M(Transformation_t::Vector3::Zero())
458 ,
S(Constraint_t::Vector3::Zero())
459 ,
v(Constraint_t::Vector3::Zero(),(
Scalar)0)
462 ,
UDinv(UD_t::Zero())
465 template<
typename Vector3Like>
473 static std::string
classname() {
return std::string(
"JointDataPrismaticUnaligned"); }
478 template<
typename Scalar,
int Options>
483 template<
typename _Scalar,
int _Options>
485 :
public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
487 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
497 typedef Eigen::Matrix<Scalar,3,1,_Options>
Vector3;
506 assert(
isUnitary(
axis) &&
"Translation axis is not unitary");
509 template<
typename Vector3Like>
513 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
514 assert(
isUnitary(
axis) &&
"Translation axis is not unitary");
535 template<
typename ConfigVector>
537 const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
542 data.M.translation().noalias() =
axis *
q;
545 template<
typename ConfigVector,
typename TangentVector>
547 const typename Eigen::MatrixBase<ConfigVector> &
qs,
548 const typename Eigen::MatrixBase<TangentVector> & vs)
const
553 const S2 &
v = vs[
idx_v()];
554 data.v.linearRate() =
v;
557 template<
typename Matrix6Like>
558 void calc_aba(JointDataDerived &
data,
const Eigen::MatrixBase<Matrix6Like> & I,
const bool update_I)
const
560 data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) *
axis;
568 static std::string
classname() {
return std::string(
"JointModelPrismaticUnaligned"); }
572 template<
typename NewScalar>
576 ReturnType
res(
axis.template cast<NewScalar>());
591 #include <boost/type_traits.hpp>
595 template<
typename Scalar,
int Options>
597 :
public integral_constant<bool,true> {};
599 template<
typename Scalar,
int Options>
601 :
public integral_constant<bool,true> {};
603 template<
typename Scalar,
int Options>
605 :
public integral_constant<bool,true> {};
607 template<
typename Scalar,
int Options>
609 :
public integral_constant<bool,true> {};
613 #endif // ifndef __pinocchio_joint_prismatic_unaligned_hpp__
MotionPrismaticUnalignedTpl< Scalar, Options > JointMotion
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
ConstLinearType linear() const
Return the linear part of the force vector.
Forward declaration of the multiplication operation return type. Should be overloaded,...
ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
const typedef DenseBase ConstMatrixReturnType
std::string shortname() 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, 3, 1, _Options > Vector3
JointModelPrismaticUnalignedTpl()
MotionTpl< Scalar, Options > ReturnType
MotionPlain motionAction(const MotionDense< M1 > &v) const
std::string shortname() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointPrismaticUnalignedTpl< _Scalar, _Options > JointDerived
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...
ConstraintPrismaticUnalignedTpl()
ConstraintPrismaticUnalignedTpl< Scalar, Options > Constraint_t
MotionTpl< Scalar, Options > MotionPlain
const std::vector< bool > hasConfigurationLimitInTangent() const
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...
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Transformation_t M
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
JointModelPrismaticUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
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
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
JointModelPrismaticUnalignedTpl(const Scalar &x, const Scalar &y, const Scalar &z)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointPrismaticUnalignedTpl< _Scalar, _Options > JointDerived
const Vector3 & axis() const
ConstraintPrismaticUnalignedTpl< Scalar, Options > Constraint
ReturnTypeNotDefined ReturnType
void setIndexes(JointIndex id, int q, int v)
const Scalar & linearRate() const
MotionPrismaticUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis, const S2 &v)
const std::vector< bool > hasConfigurationLimit() const
Return type of the ation of a Motion onto an object of type D.
ConstraintForceSetOp< ConstraintPrismaticUnalignedTpl, ForceSet >::ReturnType operator*(const Eigen::MatrixBase< ForceSet > &F)
void addTo(MotionDense< Derived > &other) const
InertiaTpl< S1, O1 > Inertia
Eigen::internal::remove_const< M6LikeCols >::type M6LikeColsNonConst
MotionPrismaticUnalignedTpl __mult__(const OtherScalar &alpha) const
PlainReturnType plain() const
void setTo(MotionDense< Derived > &other) const
ConstLinearType linear() const
ConstraintForceOp< ConstraintPrismaticUnalignedTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
static std::string classname()
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
MotionPrismaticUnalignedTpl()
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
ReturnTypeNotDefined ReturnType
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &cru)
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl)
JointPrismaticUnalignedTpl< Scalar, Options > JointDerived
JointDataPrismaticUnalignedTpl< Scalar, Options > JointDataDerived
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
DenseBase matrix_impl() const
Eigen::Matrix< S2, 6, 1, O2 > ReturnType
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
static std::string classname()
TransposeConst(const ConstraintPrismaticUnalignedTpl &ref)
TransposeConst transpose() const
MotionDerived::MotionPlain operator+(const MotionPlanarTpl< Scalar, Options > &m1, const MotionDense< MotionDerived > &m2)
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
JointDataDerived createData() const
SE3GroupAction< ConstraintPrismaticUnalignedTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
DenseBase motionAction(const MotionDense< MotionDerived > &v) const
traits< ConstraintPrismaticUnalignedTpl >::Vector3 Vector3
Constraint::Vector3 Vector3
Matrix4 HomogeneousMatrixType
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Eigen::Matrix< Scalar, 6, 1, Options > DenseBase
const ConstraintPrismaticUnalignedTpl & ref
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase< Vector3Like > &axis)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
TransformTranslationTpl< Scalar, Options > Transformation_t
Return type of the Constraint::Transpose * ForceSet operation.
Eigen::Matrix< Scalar, NV, NV, Options > D_t
traits< ConstraintPrismaticUnalignedTpl< Scalar, Options > >::Vector3 Vector3
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
bool isEqual(const JointModelPrismaticUnalignedTpl &other) const
MotionPrismaticUnalignedTpl< double > MotionPrismaticUnaligned
const typedef MatrixMatrixProduct< M6LikeColsNonConst, Vector3 >::type ReturnType
JointModelBase< JointModelPrismaticUnalignedTpl > Base
Eigen::Matrix< typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3, typename ForceDense< ForceDerived >::ConstAngularType), 1, 1, Options > ReturnType
ConstraintPrismaticUnalignedTpl< S2, O2 > Constraint
JointPrismaticUnalignedTpl< Scalar, Options > JointDerived
MotionTpl< Scalar, Options > ReturnType
traits< ConstraintPrismaticUnalignedTpl< Scalar, Options > >::Vector3 Vector3
MotionPlain PlainReturnType
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
Eigen::Matrix< Scalar, 6, NV, Options > U_t
bool isEqual(const ConstraintPrismaticUnalignedTpl &other) const
Vector3 axis
3d main axis of the joint.
bool isEqual_impl(const MotionPrismaticUnalignedTpl &other) const
JointModelPrismaticUnalignedTpl< NewScalar, Options > cast() const
Common traits structure to fully define base classes for CRTP.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl)
SizeDepType< 3 >::ColsReturn< M6Like >::ConstType M6LikeCols
SE3GroupAction< ConstraintPrismaticUnalignedTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
MotionZeroTpl< Scalar, Options > Bias_t
#define PINOCCHIO_EIGEN_REF_TYPE(D)
const Vector3 & axis() const
const typedef Vector3 ConstLinearType
DenseBase MatrixReturnType
MotionPrismaticUnalignedTpl< Scalar, Options > Motion_t
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
static ReturnType run(const Inertia &Y, const Constraint &cpu)
ConstraintPrismaticUnalignedTpl< Scalar, Options > Constraint
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
JointDataPrismaticUnalignedTpl()
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
bool isEqual(const JointModelBase< OtherDerived > &) const
ConstAngularType angular() const
Main pinocchio namespace.
void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Eigen::Matrix< Scalar, 1, 1, Options > JointForce
const typedef Vector3 ConstAngularType
JointModelPrismaticUnalignedTpl< Scalar, Options > JointModelDerived
pinocchio
Author(s):
autogenerated on Tue Feb 13 2024 03:43:59