6 #ifndef __pinocchio_joint_revolute_hpp__ 7 #define __pinocchio_joint_revolute_hpp__ 9 #include "pinocchio/math/sincos.hpp" 10 #include "pinocchio/spatial/inertia.hpp" 11 #include "pinocchio/multibody/constraint.hpp" 12 #include "pinocchio/multibody/joint/joint-base.hpp" 13 #include "pinocchio/spatial/spatial-axis.hpp" 14 #include "pinocchio/utils/axis-label.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
95 : m_sin(sin), m_cos(cos)
100 PlainType
res(PlainType::Identity());
101 _setRotation (res.rotation());
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");
152 template<
typename OtherScalar>
153 void setValues(
const OtherScalar & sin,
const OtherScalar & cos)
154 { m_sin = sin; m_cos = cos; }
156 LinearType
translation()
const {
return LinearType::PlainObject::Zero(3); };
158 AngularType
m(AngularType::Identity(3));
165 return m_cos == other.
m_cos && m_sin == other.
m_sin;
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>
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);
224 inline PlainReturnType
plain()
const {
return Axis() * m_w; }
226 template<
typename OtherScalar>
232 template<
typename MotionDerived>
236 for(Eigen::DenseIndex k = 0; k < 3; ++k)
240 template<
typename MotionDerived>
247 template<
typename S2,
int O2,
typename D2>
254 template<
typename S2,
int O2>
258 se3Action_impl(m,res);
262 template<
typename S2,
int O2,
typename D2>
274 template<
typename S2,
int O2>
278 se3ActionInverse_impl(m,res);
282 template<
typename M1,
typename M2>
287 CartesianAxis3::alphaCross(-m_w,v.
linear(),mout.
linear());
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
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>
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>
486 static inline ReturnType
run(
const Inertia &
Y,
511 template<
typename S1,
int O1,
typename S2,
int O2>
517 static inline ReturnType
run(
const Inertia &
Y,
542 template<
typename S1,
int O1,
typename S2,
int O2>
548 static inline ReturnType
run(
const Inertia &
Y,
574 template<
typename M6Like,
typename S2,
int O2,
int axis>
583 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
588 static inline ReturnType
run(
const Eigen::MatrixBase<M6Like> &
Y,
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 718 calc(data,qs.derived());
720 data.v.angularRate() =
static_cast<Scalar>(vs[
idx_v()]);
723 template<
typename Matrix6Like>
725 const Eigen::MatrixBase<Matrix6Like> & I,
726 const bool update_I)
const 728 data.U = I.col(Inertia::ANGULAR +
axis);
729 data.Dinv[0] =
Scalar(1)/I(Inertia::ANGULAR +
axis,Inertia::ANGULAR +
axis);
730 data.UDinv.noalias() = data.U * data.Dinv[0];
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__ Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
#define PINOCCHIO_SE3_TYPEDEF_TPL(Derived)
const Symmetric3 & inertia() const
std::string shortname() const
JointDataRevoluteTpl< Scalar, Options, axis > JointDataDerived
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
ConstraintRevoluteTpl< S2, O2, 2 > Constraint
void se3Action_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
const ConstraintRevoluteTpl & ref
JointRevoluteTpl< double, 0, 1 > JointRY
JointRevoluteTpl< Scalar, Options, axis > JointDerived
JointModelRevoluteTpl< NewScalar, Options, axis > type
SE3GroupAction< ConstraintRevoluteTpl >::ReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
static std::string classname()
MotionPlain se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m) const
const std::vector< bool > hasConfigurationLimitInTangent() const
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &)
MotionPlain motionAction(const MotionDense< M1 > &v) const
TransformRevoluteTpl< Scalar, Options, axis > Transformation_t
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
JointDataRevoluteTpl< double, 0, 0 > JointDataRX
MotionAlgebraAction< ConstraintRevoluteTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
MotionZeroTpl< Scalar, Options > Bias_t
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...
InertiaTpl< S1, O1 > Inertia
Return type of the Constraint::Transpose * ForceSet operation.
static std::string classname()
Return type of the ation of a Motion onto an object of type D.
JointRevoluteTpl< Scalar, Options, axis > JointDerived
#define MOTION_TYPEDEF_TPL(Derived)
PlainReturnType plain() const
PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(const Eigen
JointModelRevoluteTpl< Scalar, Options, axis > JointModelDerived
Eigen::Matrix< Scalar, 6, NV, Options > U_t
MotionRevoluteTpl< Scalar, Options, axis > JointMotion
const std::vector< bool > hasConfigurationLimit() const
JointRevoluteTpl< double, 0, 2 > JointRZ
InertiaTpl< S1, O1 > Inertia
SE3GroupAction< ConstraintRevoluteTpl >::ReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
const Vector3 & lever() const
void addTo(MotionDense< MotionDerived > &v) const
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...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Axis::CartesianAxis3 CartesianAxis3
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
void se3ActionInverse_impl(const SE3Tpl< S2, O2 > &m, MotionDense< D2 > &v) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteTpl< _Scalar, _Options, axis > JointDerived
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Eigen::Matrix< Scalar, 6, NV, Options > UD_t
static ReturnType run(const Inertia &Y, const Constraint &)
JointDataRevoluteTpl< double, 0, 2 > JointDataRZ
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.
ConstraintForceOp< ConstraintRevoluteTpl, ForceDerived >::ReturnType operator*(const ForceDense< ForceDerived > &f) const
const Vector3 ConstAngularType
Eigen::Matrix< Scalar, 4, 4, Options > Matrix4
MotionDerived::MotionPlain operator+(const MotionPlanarTpl< Scalar, Options > &m1, const MotionDense< MotionDerived > &m2)
ConstAngularType angular() const
MotionAlgebraAction< OtherSpatialType, Derived >::ReturnType cross(const OtherSpatialType &d) const
Eigen::Matrix< S2, 6, 1, O2 > ReturnType
bool isEqual(const ConstraintRevoluteTpl &) const
Eigen::Matrix< Scalar, 6, 6, Options > Matrix6
InertiaTpl< S1, O1 > Inertia
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointRevoluteTpl< _Scalar, _Options, axis > JointDerived
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
JointDataDerived createData() const
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
Eigen::Matrix< Scalar, 3, 1, Options > Vector3
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
Eigen::MatrixBase< ForceSet >::ConstRowXpr ReturnType
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR Constraint_t S
DenseBase MatrixReturnType
std::string shortname() const
JointDataRevoluteTpl< double, 0, 1 > JointDataRY
JointModelRevoluteTpl< NewScalar, Options, axis > cast() const
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
Eigen::Matrix< Scalar, 1, 1, Options > JointForce
ForceDense< ForceDerived >::ConstAngularType::template ConstFixedSegmentReturnType< 1 >::Type ReturnType
JointModelRevoluteTpl< double, 0, 2 > JointModelRZ
DenseBase matrix_impl() const
const Scalar & angularRate() const
MotionTpl< Scalar, Options > MotionPlain
MotionPlain PlainReturnType
EIGEN_STRONG_INLINE void motionAction(const MotionDense< M1 > &v, MotionDense< M2 > &mout) const
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
MotionTpl< Scalar, Options > ReturnType
static ReturnType run(const Inertia &Y, const Constraint &)
MotionPlain se3Action_impl(const SE3Tpl< S2, O2 > &m) const
void setTo(MotionDense< MotionDerived > &m) const
EIGEN_DONT_INLINE void calc(JointDataDerived &data, const typename Eigen::MatrixBase< ConfigVector > &qs) const
Eigen::Matrix< Scalar, NV, 1, Options > TangentVector_t
MotionRevoluteTpl< Scalar, Options, axis > Motion_t
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
SpatialAxis< ANGULAR+axis > Axis
const Vector3 ConstLinearType
Eigen::Matrix< Scalar, 6, 1, Options > DenseBase
SpatialAxis< axis+ANGULAR > Axis
JointMotion __mult__(const Eigen::MatrixBase< Vector1Like > &v) const
Eigen::Matrix< Scalar, 6, 1, Options > ReturnType
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, NQ, 1, Options > ConfigVector_t
ConstraintRevoluteTpl< S2, O2, 0 > Constraint
#define PINOCCHIO_EIGEN_REF_TYPE(D)
ConstAngularRef rotation() const
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
const DenseBase ConstMatrixReturnType
Main pinocchio namespace.
ReturnTypeNotDefined ReturnType
Base class for rigid transformation.
Matrix4 HomogeneousMatrixType
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...
TransposeConst transpose() const
MotionRevoluteTpl(const Eigen::MatrixBase< Vector1Like > &v)
Common traits structure to fully define base classes for CRTP.
Symmetric3Tpl< double, 0 > Symmetric3
ConstraintRevoluteTpl< S2, O2, 1 > Constraint
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
ConstraintRevoluteTpl< Scalar, Options, axis > Constraint
MotionRevoluteTpl __mult__(const OtherScalar &alpha) const
M6Like::ConstColXpr ReturnType
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...
bool isEqual_impl(const MotionRevoluteTpl &other) const
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
MotionDerived::MotionPlain operator^(const MotionDense< MotionDerived > &m1, const MotionPlanarTpl< S2, O2 > &m2)
static ReturnType run(const Inertia &Y, const Constraint &)
JointRevoluteTpl< double, 0, 0 > JointRX
Return type of the Constraint::Transpose * Force operation.
Eigen::Matrix< Scalar, NV, NV, Options > D_t
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
Eigen::Matrix< Scalar, 6, 1, Options > Vector6
MatrixDerived plain(const Eigen::PlainObjectBase< MatrixDerived > &m)
ReturnTypeNotDefined ReturnType
JointModelBase< JointModelRevoluteTpl > Base
ConstraintRevoluteTpl< Scalar, Options, axis > Constraint_t
MotionRevoluteTpl(const Scalar &w)
TransposeConst(const ConstraintRevoluteTpl &ref)
ConstLinearRef translation() const
MotionTpl< Scalar, Options > ReturnType
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)