5 #ifndef __pinocchio_multibody_joint_mimic_hpp__ 6 #define __pinocchio_multibody_joint_mimic_hpp__ 8 #include "pinocchio/macros.hpp" 9 #include "pinocchio/multibody/joint/joint-base.hpp" 16 template<
class Constra
int>
32 template<
class Constra
int>
36 template<
class Constra
int,
typename MotionDerived>
40 template<
class Constra
int,
typename ForceDerived>
47 typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options>
ReturnType;
50 template<
class Constra
int,
typename ForceSet>
56 typedef Eigen::Matrix<Scalar,Constraint::NV,ForceSet::ColsAtCompileTime,Constraint::Options | Eigen::RowMajor>
ReturnType;
59 template<
class Constra
int>
63 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 : m_scaling_factor(scaling_factor)
79 const Scalar & scaling_factor)
80 : m_constraint(constraint)
81 , m_scaling_factor(scaling_factor)
85 : m_constraint(other.m_constraint)
86 , m_scaling_factor(other.m_scaling_factor)
96 template<
typename VectorLike>
97 JointMotion
__mult__(
const Eigen::MatrixBase<VectorLike> &
v)
const 99 assert(v.size() ==
nv());
100 JointMotion jm = m_constraint *
v;
101 return jm * m_scaling_factor;
104 template<
typename S1,
int O1>
108 SE3ActionReturnType
res = m_constraint.se3Action(m);
109 return m_scaling_factor * res;
112 template<
typename S1,
int O1>
116 SE3ActionReturnType
res = m_constraint.se3ActionInverse(m);
117 return m_scaling_factor * res;
120 int nv_impl()
const {
return m_constraint.nv(); }
127 template<
typename Derived>
137 template<
typename Derived>
151 DenseBase S = m_scaling_factor * m_constraint.matrix();
155 template<
typename MotionDerived>
160 ReturnType
res = m_scaling_factor * m_constraint.motionAction(m);
167 inline const Constraint &
constraint()
const {
return m_constraint; }
182 template<
typename S1,
int O1,
typename _Constra
int>
197 template<
typename S1,
int O1,
typename _Constra
int>
204 static inline ReturnType
run(
const Inertia & Y,
205 const Constraint & scaled_constraint)
212 template<
typename M6Like,
typename _Constra
int>
222 template<
typename M6Like,
typename _Constra
int>
228 static inline ReturnType
run(
const Eigen::MatrixBase<M6Like> & Y,
229 const Constraint & scaled_constraint)
231 return scaled_constraint.
scaling() * (Y.derived() * scaled_constraint.
constraint());
240 template<
class Jo
int>
273 template<
class Jo
int>
277 template<
class Jo
int>
281 template<
class Jo
intData>
285 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
294 , m_q_transform(ConfigVector_t::Zero())
295 , m_v_transform(TangentVector_t::Zero())
304 : m_jdata_ref(jdata.derived())
306 , S(m_jdata_ref.S,scaling)
315 S = Constraint_t(m_jdata_ref.S,other.
m_scaling);
337 return std::string(
"JointDataMimic<") + m_jdata_ref.shortname() + std::string(
">");
344 TansformTypeConstRef
M_accessor()
const {
return m_jdata_ref.M; }
347 MotionTypeConstRef
v_accessor()
const {
return m_jdata_ref.v; }
350 BiasTypeConstRef
c_accessor()
const {
return m_jdata_ref.c; }
362 template<
class Jo
intModel>
394 template<
typename NewScalar,
typename Jo
intModel>
401 template<
class Jo
intModel>
405 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
425 : m_jmodel_ref(jmodel.derived())
430 Base &
base() {
return *
static_cast<Base*
>(
this); }
431 const Base &
base()
const {
return *
static_cast<const Base*
>(
this); }
436 inline int idx_q_impl()
const {
return m_jmodel_ref.idx_q(); }
437 inline int idx_v_impl()
const {
return m_jmodel_ref.idx_v(); }
442 Base::i_q = m_jmodel_ref.idx_q();
443 Base::i_v = m_jmodel_ref.idx_v();
448 return JointDataDerived(m_jmodel_ref.createData(),scaling());
451 template<
typename ConfigVector>
453 void calc(JointDataDerived & jdata,
454 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 458 AffineTransform::run(qs.head(m_jmodel_ref.nq()),
459 m_scaling,m_offset,jdata.m_q_transform);
460 m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform);
463 template<
typename ConfigVector,
typename TangentVector>
465 void calc(JointDataDerived & jdata,
466 const typename Eigen::MatrixBase<ConfigVector> & qs,
467 const typename Eigen::MatrixBase<TangentVector> & vs)
const 471 AffineTransform::run(qs.head(m_jmodel_ref.nq()),
472 m_scaling,m_offset,jdata.m_q_transform);
473 jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
474 m_jmodel_ref.calc(jdata.m_jdata_ref,
476 jdata.m_v_transform);
479 template<
typename Matrix6Like>
481 const Eigen::MatrixBase<Matrix6Like> & I,
482 const bool update_I)
const 485 m_jmodel_ref.calc_aba(data.m_jdata_ref,
497 return std::string(
"JointModelMimic<") + m_jmodel_ref.shortname() + std::string(
">");
501 template<
typename NewScalar>
506 ReturnType
res(m_jmodel_ref.template cast<NewScalar>(),
507 (NewScalar)m_scaling,
508 (NewScalar)m_offset);
537 m_jmodel_ref.
idx_q(),
547 m_jmodel_ref.
idx_q(),
558 m_jmodel_ref.
idx_v(),
568 m_jmodel_ref.
idx_v(),
579 m_jmodel_ref.
idx_v(),
589 m_jmodel_ref.
idx_v(),
600 m_jmodel_ref.
idx_v(),
610 m_jmodel_ref.
idx_v(),
622 m_jmodel_ref.
nv(),m_jmodel_ref.
nv());
632 m_jmodel_ref.
nv(),m_jmodel_ref.
nv());
639 #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__ JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
UDTypeConstRef UDinv_accessor() const
traits< Joint >::Motion_t Motion_t
std::string shortname() const
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
static std::string classname()
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef traits< JointModelMimic >::JointDerived JointDerived
JointModelBase< JointModelMimic > Base
const Base & base() const
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...
Eigen::Matrix< Scalar, Constraint::NV, ForceSet::ColsAtCompileTime, Constraint::Options|Eigen::RowMajor > ReturnType
ConfigVector_t m_q_transform
Transform configuration vector.
SE3GroupAction< Constraint >::ReturnType SE3ActionReturnType
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...
traits< Joint >::UD_t UD_t
Return type of the Constraint::Transpose * ForceSet operation.
const Scalar & scaling() const
TangentVector_t m_v_transform
Transform velocity vector.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
traits< Joint >::Scalar Scalar
Return type of the ation of a Motion onto an object of type D.
Constraint::Scalar Scalar
SizeDepType< NV >::template BlockReturn< D >::Type jointBlock_impl(Eigen::MatrixBase< D > &Mat) const
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
ConstraintForceSetOp< Constraint, ForceSet >::ReturnType OriginalReturnType
ConfigVector_t & jointConfiguration()
SE3ActionReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
JointModelMimic< JointModelNewType > type
Constraint::Scalar Scalar
TangentVector_t & jointVelocity()
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
TansformTypeConstRef M_accessor() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef traits< JointDataMimic >::JointDerived JointDerived
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
traits< Constraint >::ConstMatrixReturnType ConstMatrixReturnType
ScaledConstraint< _Constraint > Constraint
JointMotion __mult__(const Eigen::MatrixBase< VectorLike > &v) const
void setIndexes_impl(JointIndex id, int, int)
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock_impl(const Eigen::MatrixBase< D > &Mat) const
Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat...
CastType< NewScalar, JointModelMimic >::type cast() const
SizeDepType< NQ >::template SegmentReturn< D >::Type jointConfigSelector_impl(Eigen::MatrixBase< D > &a) const
ScaledConstraint< typename traits< Joint >::Constraint_t > Constraint_t
CastType< NewScalar, JointModel >::type JointModelNewType
SizeDepType< NV >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
JointMimic< typename traits< Joint >::JointDerived > JointDerived
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef traits< Joint >::ConfigVector_t ConfigVector_t
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
BiasTypeConstRef c_accessor() const
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
ConstraintForceOp< Constraint, ForceDerived >::ReturnType OriginalReturnType
SizeDepType< NV >::template RowsReturn< D >::ConstType jointRows_impl(const Eigen::MatrixBase< D > &A) const
JointDataMimic(const JointDataMimic &other)
UDTypeRef UDinv_accessor()
JointModelMimic< JointModelBase > JointModelDerived
static BlockReturn< D >::ConstType block(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index row_id, typename Eigen::DenseBase< D >::Index col_id, typename Eigen::DenseBase< D >::Index row_size_block=NV, typename Eigen::DenseBase< D >::Index col_size_block=NV)
MultiplicationOp< Inertia, _Constraint >::ReturnType OriginalReturnType
DTypeConstRef Dinv_accessor() const
const TangentVector_t & jointVelocity() const
const Scalar & offset() const
MotionTypeConstRef v_accessor() const
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
const Scalar & scaling() const
static ReturnType run(const Inertia &Y, const Constraint &scaled_constraint)
traits< Joint >::JointModelDerived JointModelBase
ConstraintTypeRef S_accessor()
ScaledConstraint(const ScaledConstraint &other)
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
Constraint & constraint()
InertiaTpl< S1, O1 > Inertia
traits< Joint >::Bias_t Bias_t
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
JointMimic< typename traits< Joint >::JointDerived > JointDerived
ScaledConstraint & operator=(const ScaledConstraint &other)
const Scalar & scaling() const
traits< Joint >::JointDataDerived JointDataBase
MotionTypeRef v_accessor()
Eigen::Matrix< Scalar, IdealReturnType::RowsAtCompileTime, IdealReturnType::ColsAtCompileTime, Constraint::Options > ReturnType
UTypeConstRef U_accessor() const
JointDataMimic(const JointDataBase< JointData > &jdata, const Scalar &scaling)
traits< Constraint >::JointForce JointForce
ScaledConstraint(const Constraint &constraint, const Scalar &scaling_factor)
ConstraintForceSetOp< ScaledConstraint, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
static std::string classname()
MultiplicationOp< Inertia, _Constraint >::ReturnType OriginalReturnType
JointDataMimic< JointDataBase > JointDataDerived
SizeDepType< NV >::template RowsReturn< D >::Type jointRows_impl(Eigen::MatrixBase< D > &A) const
SizeDepType< NV >::template SegmentReturn< D >::Type jointVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
OriginalReturnType ReturnType
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &scaled_constraint)
static ColsReturn< D >::ConstType middleCols(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
TransposeConst transpose() const
Main pinocchio namespace.
std::string shortname() const
ReturnTypeNotDefined ReturnType
JointDataMimic & operator=(const JointDataMimic &other)
ScalarMatrixProduct< Scalar, OriginalReturnType >::type IdealReturnType
static std::string classname()
traits< Constraint >::JointMotion JointMotion
traits< Constraint >::MatrixReturnType MatrixReturnType
DenseBase matrix_impl() const
traits< Joint >::TangentVector_t TangentVector_t
PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorType) integrate(const ModelTpl< Scalar
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
ScaledConstraint< _Constraint > Constraint
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...
ScaledConstraint(const Scalar &scaling_factor)
const ScaledConstraint & ref
Common traits structure to fully define base classes for CRTP.
ScalarMatrixProduct< Scalar, OriginalReturnType >::type IdealReturnType
traits< Joint >::Transformation_t Transformation_t
ScaledConstraint< _Constraint > Constraint
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_multiple_op< Scalar >, const Matrix > type
const ConfigVector_t & jointConfiguration() const
const JointModel & jmodel() const
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
MotionAlgebraAction< ScaledConstraint, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
static std::string classname()
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs) const
MotionAlgebraAction< Constraint, MotionDerived >::ReturnType ReturnType
bool isEqual(const JointDataMimic &other) const
static RowsReturn< D >::ConstType middleRows(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
JointModelMimic(const JointModelBase< JointModel > &jmodel, const Scalar &scaling, const Scalar &offset)
const JointData & jdata() const
traits< Constraint >::DenseBase DenseBase
static SegmentReturn< D >::ConstType segment(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
ConstraintForceOp< ScaledConstraint, Derived >::ReturnType operator*(const ForceDense< Derived > &f) const
bool isEqual(const ScaledConstraint &other) const
JointDataDerived createData() const
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
JointDataBase< JointDataMimic< JointData > > Base
TansformTypeRef M_accessor()
SE3ActionReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
Constraint::Scalar Scalar
Return type of the Constraint::Transpose * Force operation.
SizeDepType< NV >::template SegmentReturn< D >::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
ConstraintTypeConstRef S_accessor() const
TransposeConst(const ScaledConstraint &ref)
traits< Constraint >::Scalar Scalar
ReturnTypeNotDefined ReturnType
SizeDepType< NQ >::template SegmentReturn< D >::ConstType jointConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
ConstraintBase< ScaledConstraint > Base
const Constraint & constraint() const
SE3GroupAction< Constraint >::ReturnType ReturnType
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)