Go to the documentation of this file.
5 #ifndef __pinocchio_multibody_joint_helical_hpp__
6 #define __pinocchio_multibody_joint_helical_hpp__
18 template<
typename Scalar,
int Options,
int axis>
21 template<
typename Scalar,
int Options,
int axis>
27 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
33 template<
typename _Scalar,
int _Options,
int axis>
41 typedef Eigen::Matrix<Scalar, 3, 1, Options>
Vector3;
42 typedef Eigen::Matrix<Scalar, 6, 1, Options>
Vector6;
43 typedef Eigen::Matrix<Scalar, 4, 4, Options>
Matrix4;
44 typedef Eigen::Matrix<Scalar, 6, 6, Options>
Matrix6;
62 template<
typename Scalar,
int Options,
int axis>
65 template<
typename _Scalar,
int _Options,
int _axis>
77 typedef Eigen::Matrix<Scalar, 3, 1, Options>
Vector3;
78 typedef Eigen::Matrix<Scalar, 3, 3, Options>
Matrix3;
83 typedef typename Vector3::ConstantReturnType
LinearRef;
89 template<
typename Scalar,
int Options,
int axis>
95 template<
typename _Scalar,
int _Options,
int axis>
98 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
116 PlainType
res(PlainType::Identity());
122 operator PlainType()
const
127 template<
typename S2,
int O2>
136 res.rotation().col(0) =
m.rotation().col(0);
137 res.rotation().col(1).noalias() =
m_cos *
m.rotation().col(1) +
m_sin *
m.rotation().col(2);
138 res.rotation().col(2).noalias() =
res.rotation().col(0).cross(
res.rotation().col(1));
142 res.rotation().col(2).noalias() =
m_cos *
m.rotation().col(2) +
m_sin *
m.rotation().col(0);
143 res.rotation().col(1) =
m.rotation().col(1);
144 res.rotation().col(0).noalias() =
res.rotation().col(1).cross(
res.rotation().col(2));
148 res.rotation().col(0).noalias() =
m_cos *
m.rotation().col(0) +
m_sin *
m.rotation().col(1);
149 res.rotation().col(1).noalias() =
res.rotation().col(2).cross(
res.rotation().col(0));
150 res.rotation().col(2) =
m.rotation().col(2);
154 assert(
false &&
"must never happen");
158 res.translation() =
m.translation();
190 template<
typename Scalar1,
typename Scalar2,
typename Scalar3>
204 AngularType
m(AngularType::Identity());
223 rot.coeffRef(1, 1) =
m_cos;
224 rot.coeffRef(1, 2) = -
m_sin;
225 rot.coeffRef(2, 1) =
m_sin;
226 rot.coeffRef(2, 2) =
m_cos;
230 rot.coeffRef(0, 0) =
m_cos;
231 rot.coeffRef(0, 2) =
m_sin;
232 rot.coeffRef(2, 0) = -
m_sin;
233 rot.coeffRef(2, 2) =
m_cos;
237 rot.coeffRef(0, 0) =
m_cos;
238 rot.coeffRef(0, 1) = -
m_sin;
239 rot.coeffRef(1, 0) =
m_sin;
240 rot.coeffRef(1, 1) =
m_cos;
244 assert(
false &&
"must never happen");
251 template<
typename _Scalar,
int _Options,
int axis>
252 struct MotionHelicalTpl : MotionBase<MotionHelicalTpl<_Scalar, _Options, axis>>
254 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
272 inline PlainReturnType
plain()
const
277 template<
typename OtherScalar>
283 template<
typename MotionDerived>
286 for (Eigen::DenseIndex k = 0; k < 3; ++k)
293 template<
typename MotionDerived>
297 v.angular()[
axis] += (OtherScalar)
m_w;
298 v.linear()[
axis] += (OtherScalar)
m_v;
301 template<
typename S2,
int O2,
typename D2>
304 v.angular().noalias() =
m.rotation().col(
axis) *
m_w;
305 v.linear().noalias() =
m.translation().cross(
v.angular()) +
m_v * (
m.rotation().col(
axis));
308 template<
typename S2,
int O2>
317 template<
typename S2,
int O2,
typename D2>
322 v.linear().noalias() =
323 m.rotation().transpose() *
v.angular() +
m_v * (
m.rotation().transpose().col(
axis));
326 v.angular().noalias() =
m.rotation().transpose().col(
axis) *
m_w;
329 template<
typename S2,
int O2>
337 template<
typename M1,
typename M2>
348 template<
typename M1>
382 template<
typename S1,
int O1,
int axis,
typename MotionDerived>
383 typename MotionDerived::MotionPlain
386 typename MotionDerived::MotionPlain
res(m2);
391 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
392 EIGEN_STRONG_INLINE
typename MotionDerived::MotionPlain
395 return m2.motionAction(m1);
398 template<
typename Scalar,
int Options,
int axis>
401 template<
typename Scalar,
int Options,
int axis>
407 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
413 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
419 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
422 typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
ReturnType;
425 template<
typename _Scalar,
int _Options,
int axis>
450 template<
class Constra
intDerived>
454 typename Eigen::Matrix<typename ConstraintDerived::Scalar, 1, 1, ConstraintDerived::Options>
458 template<
typename _Scalar,
int _Options,
int axis>
462 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
485 template<
typename Vector1Like>
486 JointMotion
__mult__(
const Eigen::MatrixBase<Vector1Like> &
v)
const
488 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
489 assert(
v.size() == 1);
490 return JointMotion(
v[0],
v[0] *
m_pitch);
493 template<
typename S1,
int O1>
499 res.template segment<3>(LINEAR) =
501 res.template segment<3>(ANGULAR) =
m.rotation().col(
axis);
505 template<
typename S1,
int O1>
512 res.template segment<3>(LINEAR).noalias() =
515 res.template segment<3>(ANGULAR) =
m.rotation().transpose().col(
axis);
533 template<
typename ForceDerived>
541 template<
typename Derived>
545 assert(F.rows() == 6);
570 template<
typename MotionDerived>
580 res.template segment<3>(LINEAR) +=
res.template segment<3>(ANGULAR);
605 template<
typename _Scalar,
int _Options,
int _axis>
611 Eigen::Matrix<_Scalar, 1, 1, _Options>
res;
612 res(0) = 1.0 + S_transpose.
ref.
h() *
S.h();
616 template<
typename _Scalar,
int _Options,
int _axis>
628 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
637 template<
typename S1,
int O1,
typename S2,
int O2>
646 const S2 & m_pitch = constraint.
h();
650 const S1 &
m =
Y.mass(), &
x =
Y.lever()[0], &
y =
Y.lever()[1], &z =
Y.lever()[2];
653 res <<
m * m_pitch, -
m * z,
m *
y, I(0, 0) +
m * (
y *
y + z * z),
654 I(0, 1) -
m *
x *
y +
m * z * m_pitch, I(0, 2) -
m *
x * z -
m *
y * m_pitch;
660 template<
typename S1,
int O1,
typename S2,
int O2>
669 const S2 & m_pitch = constraint.
h();
673 const S1 &
m =
Y.mass(), &
x =
Y.lever()[0], &
y =
Y.lever()[1], &z =
Y.lever()[2];
676 res <<
m * z,
m * m_pitch, -
m *
x, I(1, 0) -
m *
x *
y -
m * z * m_pitch,
677 I(1, 1) +
m * (
x *
x + z * z), I(1, 2) -
m *
y * z +
m *
x * m_pitch;
683 template<
typename S1,
int O1,
typename S2,
int O2>
692 const S2 & m_pitch = constraint.
h();
696 const S1 &
m =
Y.mass(), &
x =
Y.lever()[0], &
y =
Y.lever()[1], &z =
Y.lever()[2];
699 res << -
m *
y,
m *
x,
m * m_pitch, I(2, 0) -
m *
x * z +
m *
y * m_pitch,
700 I(2, 1) -
m *
y * z -
m *
x * m_pitch, I(2, 2) +
m * (
x *
x +
y *
y);
707 template<
typename M6Like,
typename S2,
int O2,
int axis>
716 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
718 Eigen::MatrixBase<M6Like>,
726 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
727 return (
Y.col(Inertia::ANGULAR +
axis) +
Y.col(Inertia::LINEAR +
axis) * constraint.
h());
732 template<
typename _Scalar,
int _Options,
int axis>
753 typedef Eigen::Matrix<Scalar, 6, NV, Options>
U_t;
754 typedef Eigen::Matrix<Scalar, NV, NV, Options>
D_t;
755 typedef Eigen::Matrix<Scalar, 6, NV, Options>
UD_t;
763 template<
typename _Scalar,
int _Options,
int axis>
770 template<
typename _Scalar,
int _Options,
int axis>
777 template<
typename _Scalar,
int _Options,
int axis>
780 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
800 :
joint_q(ConfigVector_t::Zero())
801 ,
joint_v(TangentVector_t::Zero())
807 ,
UDinv(UD_t::Zero())
814 return std::string(
"JointDataH") + axisLabel<axis>();
823 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
829 template<
typename _Scalar,
int _Options,
int axis>
832 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
842 typedef Eigen::Matrix<Scalar, 3, 1, _Options>
Vector3;
846 return JointDataDerived();
868 template<
typename ConfigVector>
869 EIGEN_DONT_INLINE
void
870 calc(JointDataDerived &
data,
const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
879 template<
typename TangentVector>
880 EIGEN_DONT_INLINE
void
881 calc(JointDataDerived &
data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
885 data.v.angularRate() =
data.joint_v[0];
889 template<
typename ConfigVector,
typename TangentVector>
891 JointDataDerived &
data,
892 const typename Eigen::MatrixBase<ConfigVector> &
qs,
893 const typename Eigen::MatrixBase<TangentVector> & vs)
const
898 data.v.angularRate() =
data.joint_v[0];
902 template<
typename VectorLike,
typename Matrix6Like>
904 JointDataDerived &
data,
905 const Eigen::MatrixBase<VectorLike> & armature,
906 const Eigen::MatrixBase<Matrix6Like> & I,
907 const bool update_I)
const
921 return std::string(
"JointModelH") + axisLabel<axis>();
933 return Vector3::UnitX();
935 return Vector3::UnitY();
937 return Vector3::UnitZ();
939 assert(
false &&
"must never happen");
945 template<
typename NewScalar>
972 #include <boost/type_traits.hpp>
976 template<
typename Scalar,
int Options,
int axis>
978 :
public integral_constant<bool, true>
982 template<
typename Scalar,
int Options,
int axis>
984 :
public integral_constant<bool, true>
988 template<
typename Scalar,
int Options,
int axis>
990 :
public integral_constant<bool, true>
994 template<
typename Scalar,
int Options,
int axis>
996 :
public integral_constant<bool, true>
1001 #endif // ifndef __pinocchio_multibody_joint_helical_hpp__
Eigen::Matrix< S2, 6, 1 > ReturnType
AxisAngular::CartesianAxis3 CartesianAxis3Angular
Eigen::Matrix< typename ConstraintDerived::Scalar, 1, 1, ConstraintDerived::Options > ReturnType
const std::vector< bool > hasConfigurationLimitInTangent() const
JointHelicalTpl< context::Scalar, context::Options, 0 > JointHX
JointDataHelicalTpl< context::Scalar, context::Options, 1 > JointDataHY
TransposeConst transpose() const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
MotionTpl< Scalar, Options > ReturnType
SpatialAxis< ANGULAR+axis > AxisAngular
Eigen::Matrix< Scalar, 1, 1 > ReturnType
Forward declaration of the multiplication operation return type. Should be overloaded,...
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > ReturnType
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
AxisLinear::CartesianAxis3 CartesianAxis3Linear
SpatialAxis< ANGULAR+axis > AxisLinear
bool isEqual(const JointMotionSubspaceHelicalTpl &) const
MotionHelicalTpl< Scalar, Options, axis > Motion_t
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
static void cross(const Eigen::MatrixBase< V3_in > &vin, const Eigen::MatrixBase< V3_out > &vout)
JointModelHelicalTpl< context::Scalar, context::Options, 0 > JointModelHX
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 std::string classname()
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...
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
JointModelHelicalTpl< Scalar, Options, axis > JointModelDerived
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
ConstraintForceOp< JointMotionSubspaceHelicalTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
JointMotionSubspaceHelicalTpl(const Scalar &h)
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
static ReturnType run(const Inertia &Y, const Constraint &constraint)
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
JointModelHelicalTpl< context::Scalar, context::Options, 1 > JointModelHY
JointDataHelicalTpl< context::Scalar, context::Options, 2 > JointDataHZ
JointDataHelicalTpl< Scalar, Options, axis > JointDataDerived
ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType
JointModelBase< JointModelHelicalTpl > Base
static std::string classname()
const Scalar & linearRate() const
MotionDerived::MotionPlain operator+(const MotionHelicalUnalignedTpl< S1, O1 > &m1, const MotionDense< MotionDerived > &m2)
ReturnTypeNotDefined ReturnType
MotionZeroTpl< Scalar, Options > Bias_t
void setIndexes(JointIndex id, int q, int v)
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
AxisLinear::CartesianAxis3 CartesianAxis3Linear
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Vector3 getMotionAxis() const
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Return type of the ation of a Motion onto an object of type D.
std::string shortname() const
TridiagonalSymmetricMatrixApplyOnTheLeftReturnType< LhsMatrixType, TridiagonalSymmetricMatrixTpl< S, O > > operator*(const Eigen::MatrixBase< LhsMatrixType > &lhs, const TridiagonalSymmetricMatrixTpl< S, O > &rhs)
std::string shortname() const
ConstLinearType linear() const
JointDataDerived createData() const
InertiaTpl< S1, O1 > Inertia
SpatialAxis< axis+ANGULAR > AxisAngular
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &constraint)
MotionTpl< Scalar, Options > ReturnType
JointMotionSubspaceHelicalTpl< S2, O2, 1 > Constraint
const Scalar & angularRate() const
Symmetric3Tpl< Scalar, Options > Symmetric3
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
MotionHelicalTpl __mult__(const OtherScalar &alpha) const
ReturnTypeNotDefined ReturnType
Matrix4 HomogeneousMatrixType
MotionHelicalTpl< Scalar, Options, axis > JointMotion
MotionHelicalTpl(const Scalar &w, const Scalar &v)
JointHelicalTpl< context::Scalar, context::Options, 2 > JointHZ
const JointMotionSubspaceHelicalTpl & ref
const typedef Vector3 ConstLinearType
InertiaTpl< S1, O1 > Inertia
JointMotionSubspaceHelicalTpl< S2, O2, 0 > Constraint
SE3GroupAction< JointMotionSubspaceHelicalTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MOTION_TYPEDEF_TPL(MotionHelicalTpl)
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointHelicalTpl< _Scalar, _Options, axis > JointDerived
MotionAlgebraAction< JointMotionSubspaceHelicalTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
JointHelicalTpl< context::Scalar, context::Options, 1 > JointHY
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
DenseBase MatrixReturnType
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR ConfigVector_t joint_q
InertiaTpl< S1, O1 > Inertia
const std::vector< bool > hasConfigurationLimit() const
const typedef DenseBase ConstMatrixReturnType
JointModelHelicalTpl< NewScalar, Options, axis > type
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
EIGEN_STRONG_INLINE void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
bool isEqual_impl(const MotionHelicalTpl &other) const
const typedef Vector3 ConstAngularType
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
void addTo(MotionDense< MotionDerived > &v) const
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
JointModelHelicalTpl< context::Scalar, context::Options, 2 > JointModelHZ
void setTo(MotionDense< MotionDerived > &m) const
MotionTpl< Scalar, Options > MotionPlain
TransposeConst(const JointMotionSubspaceHelicalTpl &ref)
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
JointModelHelicalTpl< NewScalar, Options, axis > cast() const
MotionPlain motionAction(const MotionDense< M1 > &v) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
bool comparison_eq(const LhsType &lhs_value, const RhsType &rhs_value)
SpatialAxis< axis+LINEAR > AxisLinear
AxisAngular::CartesianAxis3 CartesianAxis3Angular
JointMotionSubspaceHelicalTpl< S2, O2, 2 > Constraint
Return type of the Constraint::Transpose * ForceSet operation.
Eigen::Matrix< Scalar, 1, 1, Options > JointForce
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
Base class for rigid transformation.
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
Eigen::Matrix< Scalar, 6, 1 > ReturnType
JointMotionSubspaceHelicalTpl< Scalar, Options, axis > Constraint
JointMotionSubspaceHelicalTpl()
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const Blank, const typename Eigen::MatrixBase< TangentVector > &vs) const
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.
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
DenseBase matrix_impl() const
EIGEN_STRONG_INLINE MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionHelicalUnalignedTpl< S2, O2 > &m2)
ConstraintForceSetOp< JointMotionSubspaceHelicalTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
JointHelicalTpl< _Scalar, _Options, axis > JointDerived
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
JointModelHelicalTpl(const Scalar &h)
Common traits structure to fully define base classes for CRTP.
Eigen::Matrix< Scalar, 1, 1, Options > ReducedSquaredMatrix
static ReturnType run(const Inertia &Y, const Constraint &constraint)
Eigen::Matrix< Scalar, 6, 1, Options > DenseBase
Eigen::Matrix< S2, 6, 1, O2 > ReturnType
JointDataHelicalTpl< context::Scalar, context::Options, 0 > JointDataHX
MotionPlain PlainReturnType
#define PINOCCHIO_EIGEN_REF_TYPE(D)
PlainReturnType plain() const
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
TransformHelicalTpl< Scalar, Options, axis > Transformation_t
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
SE3GroupAction< JointMotionSubspaceHelicalTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
Eigen::Matrix< Scalar, NV, NV, Options > D_t
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
Eigen::Matrix< Scalar, 6, NV, Options > U_t
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointHelicalTpl< _Scalar, _Options, axis > JointDerived
static ReturnType run(const Inertia &Y, const Constraint &constraint)
JointMotionSubspaceHelicalTpl< Scalar, Options, axis > Constraint_t
ConstAngularType angular() const
Cast scalar type from type FROM to type TO.
static void alphaCross(const Scalar &s, const Eigen::MatrixBase< V3_in > &vin, const Eigen::MatrixBase< V3_out > &vout)
Main pinocchio namespace.
JointHelicalTpl< _Scalar, _Options, axis > JointDerived
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:10