Go to the documentation of this file.
6 #ifndef __pinocchio_joint_revolute_hpp__
7 #define __pinocchio_joint_revolute_hpp__
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>
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;
60 template<
typename _Scalar,
int _Options,
int _axis>
71 typedef Eigen::Matrix<Scalar,3,1,Options>
Vector3;
72 typedef Eigen::Matrix<Scalar,3,3,Options>
Matrix3;
77 typedef typename Vector3::ConstantReturnType
LinearRef;
83 template<
typename Scalar,
int Options,
int axis>
87 template<
typename _Scalar,
int _Options,
int axis>
90 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
100 PlainType
res(PlainType::Identity());
105 operator PlainType()
const {
return plain(); }
107 template<
typename S2,
int O2>
117 res.rotation().col(0) =
m.rotation().col(0);
118 res.rotation().col(1).noalias() =
m_cos *
m.rotation().col(1) +
m_sin *
m.rotation().col(2);
119 res.rotation().col(2).noalias() =
res.rotation().col(0).cross(
res.rotation().col(1));
124 res.rotation().col(2).noalias() =
m_cos *
m.rotation().col(2) +
m_sin *
m.rotation().col(0);
125 res.rotation().col(1) =
m.rotation().col(1);
126 res.rotation().col(0).noalias() =
res.rotation().col(1).cross(
res.rotation().col(2));
131 res.rotation().col(0).noalias() =
m_cos *
m.rotation().col(0) +
m_sin *
m.rotation().col(1);
132 res.rotation().col(1).noalias() =
res.rotation().col(2).cross(
res.rotation().col(0));
133 res.rotation().col(2) =
m.rotation().col(2);
138 assert(
false &&
"must nerver happened");
142 res.translation() =
m.translation();
152 template<
typename OtherScalar>
156 LinearType
translation()
const {
return LinearType::PlainObject::Zero(3); };
158 AngularType
m(AngularType::Identity(3));
177 rot.coeffRef(1,1) =
m_cos; rot.coeffRef(1,2) = -
m_sin;
178 rot.coeffRef(2,1) =
m_sin; rot.coeffRef(2,2) =
m_cos;
183 rot.coeffRef(0,0) =
m_cos; rot.coeffRef(0,2) =
m_sin;
184 rot.coeffRef(2,0) = -
m_sin; rot.coeffRef(2,2) =
m_cos;
189 rot.coeffRef(0,0) =
m_cos; rot.coeffRef(0,1) = -
m_sin;
190 rot.coeffRef(1,0) =
m_sin; rot.coeffRef(1,1) =
m_cos;
195 assert(
false &&
"must nerver happened");
202 template<
typename _Scalar,
int _Options,
int axis>
203 struct MotionRevoluteTpl
204 : MotionBase< MotionRevoluteTpl<_Scalar,_Options,axis> >
206 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
216 template<
typename Vector1Like>
220 using namespace Eigen;
221 EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
226 template<
typename OtherScalar>
232 template<
typename MotionDerived>
235 m.linear().setZero();
236 for(Eigen::DenseIndex k = 0; k < 3; ++k)
240 template<
typename MotionDerived>
244 v.angular()[
axis] += (OtherScalar)
m_w;
247 template<
typename S2,
int O2,
typename D2>
250 v.angular().noalias() =
m.rotation().col(
axis) *
m_w;
251 v.linear().noalias() =
m.translation().cross(
v.angular());
254 template<
typename S2,
int O2>
262 template<
typename S2,
int O2,
typename D2>
268 v.linear().noalias() =
m.rotation().transpose() *
v.angular();
271 v.angular().noalias() =
m.rotation().transpose().col(
axis) *
m_w;
274 template<
typename S2,
int O2>
282 template<
typename M1,
typename M2>
293 template<
typename M1>
306 return m_w == other.m_w;
314 template<
typename S1,
int O1,
int axis,
typename MotionDerived>
315 typename MotionDerived::MotionPlain
319 typename MotionDerived::MotionPlain
res(m2);
324 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
326 typename MotionDerived::MotionPlain
329 return m2.motionAction(m1);
334 template<
typename Scalar,
int Options,
int axis>
338 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
342 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
346 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
348 {
typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr
ReturnType; };
350 template<
typename _Scalar,
int _Options,
int axis>
366 template<
typename _Scalar,
int _Options,
int axis>
370 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
379 template<
typename Vector1Like>
380 JointMotion
__mult__(
const Eigen::MatrixBase<Vector1Like> &
v)
const
381 {
return JointMotion(
v[0]); }
383 template<
typename S1,
int O1>
389 res.template segment<3>(LINEAR) =
m.translation().cross(
m.rotation().col(
axis));
390 res.template segment<3>(ANGULAR) =
m.rotation().col(
axis);
394 template<
typename S1,
int O1>
402 res.template segment<3>(ANGULAR) =
m.rotation().transpose().col(
axis);
413 template<
typename ForceDerived>
419 template<
typename Derived>
424 return F.row(ANGULAR +
axis);
444 template<
typename MotionDerived>
459 template<
typename _Scalar,
int _Options,
int _axis>
471 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
480 template<
typename S1,
int O1,
typename S2,
int O2>
511 template<
typename S1,
int O1,
typename S2,
int O2>
542 template<
typename S1,
int O1,
typename S2,
int O2>
574 template<
typename M6Like,
typename S2,
int O2,
int axis>
583 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
591 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
592 return Y.col(Inertia::ANGULAR +
axis);
597 template<
typename _Scalar,
int _Options,
int axis>
614 typedef Eigen::Matrix<Scalar,6,NV,Options>
U_t;
615 typedef Eigen::Matrix<Scalar,NV,NV,Options>
D_t;
616 typedef Eigen::Matrix<Scalar,6,NV,Options>
UD_t;
624 template<
typename Scalar,
int Options,
int axis>
628 template<
typename Scalar,
int Options,
int axis>
632 template<
typename _Scalar,
int _Options,
int axis>
635 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
655 , UDinv(UD_t::Zero())
660 return std::string(
"JointDataR") + axisLabel<axis>();
666 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
672 template<
typename _Scalar,
int _Options,
int axis>
674 :
public JointModelBase< JointModelRevoluteTpl<_Scalar,_Options,axis> >
676 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
686 JointDataDerived
createData()
const {
return JointDataDerived(); }
700 template<
typename ConfigVector>
703 const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
707 const OtherScalar &
q =
qs[
idx_q()];
708 OtherScalar ca,sa;
SINCOS(
q,&sa,&ca);
709 data.M.setValues(sa,ca);
712 template<
typename ConfigVector,
typename TangentVector>
715 const typename Eigen::MatrixBase<ConfigVector> &
qs,
716 const typename Eigen::MatrixBase<TangentVector> & vs)
const
723 template<
typename Matrix6Like>
725 const Eigen::MatrixBase<Matrix6Like> & I,
726 const bool update_I)
const
728 data.U = I.col(Inertia::ANGULAR +
axis);
738 return std::string(
"JointModelR") + axisLabel<axis>();
743 template<
typename NewScalar>
768 #include <boost/type_traits.hpp>
772 template<
typename Scalar,
int Options,
int axis>
774 :
public integral_constant<bool,true> {};
776 template<
typename Scalar,
int Options,
int axis>
778 :
public integral_constant<bool,true> {};
780 template<
typename Scalar,
int Options,
int axis>
782 :
public integral_constant<bool,true> {};
784 template<
typename Scalar,
int Options,
int axis>
786 :
public integral_constant<bool,true> {};
789 #endif // ifndef __pinocchio_joint_revolute_hpp__
SE3GroupAction< ConstraintRevoluteTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
TransposeConst(const ConstraintRevoluteTpl &ref)
MotionRevoluteTpl< Scalar, Options, axis > JointMotion
Forward declaration of the multiplication operation return type. Should be overloaded,...
MotionZeroTpl< Scalar, Options > Bias_t
JointDataRevoluteTpl< double, 0, 0 > JointDataRX
InertiaTpl< S1, O1 > Inertia
JointModelBase< JointModelRevoluteTpl > Base
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
ConstraintRevoluteTpl< S2, O2, 0 > Constraint
JointDataRevoluteTpl< Scalar, Options, axis > JointDataDerived
static std::string classname()
JointRevoluteTpl< Scalar, Options, axis > JointDerived
MotionRevoluteTpl __mult__(const OtherScalar &alpha) const
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
const std::vector< bool > hasConfigurationLimit() const
static void cross(const Eigen::MatrixBase< V3_in > &vin, const Eigen::MatrixBase< V3_out > &vout)
Eigen::Matrix< S2, 6, 1, O2 > ReturnType
SE3GroupAction< ConstraintRevoluteTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
TransformRevoluteTpl< Scalar, Options, axis > Transformation_t
static std::string classname()
const std::vector< bool > hasConfigurationLimitInTangent() const
PlainReturnType plain() const
Return type of the Constraint::Transpose * Force operation.
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
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...
MotionAlgebraAction< ConstraintRevoluteTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
ConstraintRevoluteTpl< S2, O2, 1 > Constraint
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
ConstraintForceSetOp< ConstraintRevoluteTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain.
JointDataRevoluteTpl< double, 0, 2 > JointDataRZ
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
Eigen::MatrixBase< ForceSet >::ConstRowXpr ReturnType
JointRevoluteTpl< Scalar, Options, axis > JointDerived
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteTpl< _Scalar, _Options, axis > JointDerived
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
JointModelRevoluteTpl< Scalar, Options, axis > JointModelDerived
Eigen::Matrix< Scalar, 6, NV, Options > U_t
JointDataRevoluteTpl< double, 0, 1 > JointDataRY
const Scalar & angularRate() const
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
EIGEN_STRONG_INLINE void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
Matrix4 HomogeneousMatrixType
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)
ReturnTypeNotDefined ReturnType
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteTpl< _Scalar, _Options, axis > JointDerived
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
ConstraintForceOp< ConstraintRevoluteTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
std::string shortname() const
JointDataDerived createData() const
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
M6Like::ConstColXpr ReturnType
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Return type of the ation of a Motion onto an object of type D.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW MOTION_TYPEDEF_TPL(MotionRevoluteTpl)
MotionRevoluteTpl(const Eigen::MatrixBase< Vector1Like > &v)
ConstLinearType linear() const
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Constraint_t S
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
InertiaTpl< S1, O1 > Inertia
DenseBase MatrixReturnType
bool isEqual(const ConstraintRevoluteTpl &) const
ReturnTypeNotDefined ReturnType
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
Axis::CartesianAxis3 CartesianAxis3
DenseBase matrix_impl() const
InertiaTpl< S1, O1 > Inertia
MotionPlain motionAction(const MotionDense< M1 > &v) const
const typedef DenseBase ConstMatrixReturnType
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
MotionPlain PlainReturnType
static ReturnType run(const Inertia &Y, const Constraint &)
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
ForceDense< ForceDerived >::ConstAngularType::template ConstFixedSegmentReturnType< 1 >::Type ReturnType
MotionRevoluteTpl< Scalar, Options, axis > Motion_t
MotionDerived::MotionPlain operator+(const MotionPlanarTpl< Scalar, Options > &m1, const MotionDense< MotionDerived > &m2)
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...
JointModelRevoluteTpl< NewScalar, Options, axis > cast() const
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
Symmetric3Tpl< double, 0 > Symmetric3
MotionRevoluteTpl(const Scalar &w)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
JointModelRevoluteTpl< NewScalar, Options, axis > type
bool isEqual_impl(const MotionRevoluteTpl &other) const
ConstraintRevoluteTpl< Scalar, Options, axis > Constraint
Eigen::Matrix< Scalar, 1, 1, Options > JointForce
SpatialAxis< ANGULAR+axis > Axis
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Eigen::Matrix< Scalar, 6, 1, Options > DenseBase
TransposeConst transpose() const
Return type of the Constraint::Transpose * ForceSet operation.
MotionTpl< Scalar, Options > ReturnType
Base class for rigid transformation.
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
const typedef Vector3 ConstLinearType
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &)
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
JointRevoluteTpl< double, 0, 1 > JointRY
const typedef Vector3 ConstAngularType
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
Common traits structure to fully define base classes for CRTP.
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
void setTo(MotionDense< MotionDerived > &m) const
MotionTpl< Scalar, Options > ReturnType
#define PINOCCHIO_EIGEN_REF_TYPE(D)
Eigen::Matrix< Scalar, NV, NV, Options > D_t
JointRevoluteTpl< double, 0, 2 > JointRZ
std::string shortname() const
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
static ReturnType run(const Inertia &Y, const Constraint &)
MotionTpl< Scalar, Options > MotionPlain
ConstraintRevoluteTpl< Scalar, Options, axis > Constraint_t
static ReturnType run(const Inertia &Y, const Constraint &)
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
ConstraintRevoluteTpl< S2, O2, 2 > Constraint
ConstAngularType angular() const
Return the angular part of the force vector.
JointRevoluteTpl< double, 0, 0 > JointRX
void addTo(MotionDense< MotionDerived > &v) const
ConstAngularType angular() const
static void alphaCross(const Scalar &s, const Eigen::MatrixBase< V3_in > &vin, const Eigen::MatrixBase< V3_out > &vout)
Main pinocchio namespace.
const ConstraintRevoluteTpl & ref
SpatialAxis< axis+ANGULAR > Axis
pinocchio
Author(s):
autogenerated on Tue Feb 13 2024 03:43:59