Go to the documentation of this file.
5 #ifndef __pinocchio_multibody_joint_mimic_hpp__
6 #define __pinocchio_multibody_joint_mimic_hpp__
16 template<
typename _Scalar,
int _Options,
int MaxDim>
19 template<
typename _Scalar,
int _Options,
int _MaxDim>
45 template<
typename _Scalar,
int _Options,
int _MaxDim>
53 template<
typename _Scalar,
int _Options,
int _MaxDim,
typename MotionDerived>
64 template<
typename _Scalar,
int _Options,
int _MaxDim,
typename ForceDerived>
75 template<
typename _Scalar,
int _Options,
int _MaxDim,
typename ForceSet>
86 template<
typename _Scalar,
int _Options,
int _MaxDim>
90 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
116 template<
typename Constra
intTpl>
136 template<
typename VectorLike>
137 JointMotion
__mult__(
const Eigen::MatrixBase<VectorLike> &
v)
const
140 assert(
v.size() ==
nv());
145 template<
typename S1,
int O1>
151 template<
typename S1,
int O1>
170 template<
typename Derived>
178 template<
typename Derived>
204 template<
typename MotionDerived>
240 template<
typename S1,
int O1,
typename S2,
int O2,
int MD2>
247 typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2>
ReturnType;
253 template<
typename S1,
int O1,
typename S2,
int O2,
int MD2>
267 template<
typename M6Like,
typename S2,
int O2,
int MD2>
271 typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2>
ReturnType;
277 template<
typename M6Like,
typename S2,
int O2,
int MD2>
285 run(
const Eigen::MatrixBase<M6Like> & Y,
const Constraint & scaled_constraint)
292 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
294 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
296 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
299 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
323 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
U_t;
324 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>
D_t;
325 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
UD_t;
353 template<
typename _Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
360 template<
typename _Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
367 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
369 :
public JointDataBase<JointDataMimicTpl<_Scalar, _Options, JointCollectionTpl>>
371 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
407 S = Constraint_t(other.
S);
421 return std::string(
"JointDataMimic");
520 void disp(std::ostream & os)
const
544 template<
typename S,
int O>
class JointCollectionTpl>
550 template<
typename _Scalar,
int _Options,
template<
typename S,
int O>
class JointCollectionTpl>
552 :
public JointModelBase<JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>>
554 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
584 template<
typename Jo
intModel>
591 template<
typename Jo
intModelMimicking,
typename Jo
intModelMimicked>
603 assert(jmodel_mimicking.
nq() == jmodel_mimicked.
nq());
604 assert(jmodel_mimicking.
nv() == jmodel_mimicked.
nv());
608 jmodel_mimicked.
id(), jmodel_mimicked.
idx_q(), jmodel_mimicked.
idx_v(),
614 return *
static_cast<Base *
>(
this);
618 return *
static_cast<const Base *
>(
this);
643 "Mimic joint index is lower than its directing joint. Should never happen");
647 Base::i_vExtended = vExtended;
672 return JointDataDerived(
686 template<
typename ConfigVector>
687 EIGEN_DONT_INLINE
void
688 calc(JointDataDerived & jdata,
const typename Eigen::MatrixBase<ConfigVector> &
qs)
const
696 template<
typename ConfigVector,
typename TangentVector>
698 JointDataDerived & jdata,
699 const typename Eigen::MatrixBase<ConfigVector> &
qs,
700 const typename Eigen::MatrixBase<TangentVector> & vs)
const
706 jdata.joint_v_transformed =
m_scaling * jdata.joint_v;
709 jdata.m_jdata_mimicking, jdata.joint_q_transformed, jdata.joint_v_transformed);
712 template<
typename VectorLike,
typename Matrix6Like>
715 const Eigen::MatrixBase<VectorLike> &,
716 const Eigen::MatrixBase<Matrix6Like> &,
721 &&
"Joint Mimic is not supported for aba yet. Remove it from your model if you want to use "
727 return std::string(
"JointModelMimic");
736 template<
typename NewScalar>
744 res.setIndexes(
id(), Base::i_q, Base::i_v, Base::i_vExtended);
870 void disp(std::ostream & os)
const
875 os <<
" Mimic scaling: " <<
m_scaling << std::endl;
876 os <<
" Mimic offset: " <<
m_offset << std::endl;
883 #include <boost/type_traits.hpp>
887 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
888 struct has_nothrow_constructor<
890 :
public integral_constant<bool, true>
894 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
896 :
public integral_constant<bool, true>
900 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
901 struct has_nothrow_constructor<
903 :
public integral_constant<bool, true>
907 template<
typename Scalar,
int Options,
template<
typename S,
int O>
class JointCollectionTpl>
909 :
public integral_constant<bool, true>
914 #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__
MotionAlgebraAction< ScaledJointMotionSubspaceTpl, MotionDerived >::ReturnType motionAction(const MotionDense< MotionDerived > &m) const
ConfigVectorTypeConstRef joint_q_accessor() const
UD_t UDinv_accessor() const
ConstraintForceSetOp< ScaledJointMotionSubspaceTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (RefConstraint::Transpose S, ForceSet::Block)
Forward declaration of the multiplication operation return type. Should be overloaded,...
const DenseBase & matrix_impl() const
MotionTpl< Scalar, Options > Motion_t
MotionTpl< Scalar, Options > Motion
JointMimicTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
const std::vector< bool > hasConfigurationLimitInTangent() const
InertiaTpl< Scalar, Options > Inertia
CastType< NewScalar, JointModelMimicTpl >::type cast() const
ScaledJointMotionSubspaceTpl & operator=(const ScaledJointMotionSubspaceTpl &other)
SizeDepType< NQ >::template SegmentReturn< D >::Type JointMappedVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
const std::vector< bool > hasConfigurationLimitInTangent() const
#define PINOCCHIO_THROW(condition, exception_type, message)
Generic macro to throw an exception in Pinocchio if the condition is not met with a given input messa...
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...
const ConfigVector_t & q_transformed() const
void disp(std::ostream &os) const
SizeDepType< NV >::template RowsReturn< D >::ConstType jointRows_impl(const Eigen::MatrixBase< D > &A) const
JointModelMimicTpl< NewScalar, Options, JointCollectionTpl > type
RefJointData m_jdata_mimicking
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 model tangent space correspond...
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
static ColsReturn< D >::ConstType middleCols(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::RefJointMotionSubspace RefJointMotionSubspace
TangentVector_t joint_v_transformed
Transform velocity vector.
JointMimicTpl< _Scalar, Options, JointCollectionTpl > JointDerived
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain.
RefJointData::JointDataVariant RefJointDataVariant
Motion_t MotionTypeConstRef
ConfigVector_t & q_transformed()
const Scalar & offset() const
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
RefJointMotionSubspace & constraint()
JointDataMimicTpl & operator=(const JointDataMimicTpl &other)
TransposeConst(const ScaledJointMotionSubspaceTpl &ref)
bool isEqual(const JointDataMimicTpl &other) const
ConstraintForceSetOp< RefJointMotionSubspace, ForceSet >::ReturnType RefReturnType
ScalarMatrixProduct< _Scalar, RefReturnType >::type ReturnType
void setIndexes_impl(JointIndex id, int, int, int vExtended)
Eigen::Matrix< S2, 6, Eigen::Dynamic, O2, 6, MD2 > ReturnType
std::string shortname() const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointModelBase< JointModelMimicTpl > Base
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
JointDataMimicTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
JointDataTpl< _Scalar, _Options, JointCollectionTpl > RefJointData
const std::vector< bool > hasConfigurationLimit() const
ReturnTypeNotDefined ReturnType
ConstraintTypeConstRef S_accessor() const
Bias_t c_accessor() const
void disp(std::ostream &os) const
SE3ActionReturnType se3Action(const SE3Tpl< S1, O1 > &m) const
SE3Tpl< Scalar, Options > Transformation_t
TangentVector_t joint_v
original velocity vector
Return type of the ation of a Motion onto an object of type D.
const Scalar & scaling() const
SizeDepType< NV >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
traits< RefJointMotionSubspace >::DenseBase DenseBase
MotionAlgebraAction< typename traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::RefJointMotionSubspace, MotionDerived >::ReturnType ReturnType
JointCollection::JointDataVariant JointDataVariant
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > Constraint
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs, const typename Eigen::MatrixBase< TangentVector > &vs) const
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > UD_t
TangentVector_t & TangentVectorTypeRef
JointModelMimicTpl(const JointModelBase< JointModelMimicking > &jmodel_mimicking, const JointModelBase< JointModelMimicked > &jmodel_mimicked, const Scalar &scaling, const Scalar &offset)
ConfigVectorTypeRef joint_q_accessor()
JointCollectionTpl< Scalar, Options > JointCollection
TangentVector_t & v_transformed()
SE3GroupAction< typename traits< ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim > >::RefJointMotionSubspace >::ReturnType ReturnType
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointDataBase< JointDataMimicTpl > Base
ScaledJointMotionSubspaceTpl(const ScaledJointMotionSubspaceTpl &other)
void setIndexes(JointIndex id, int nq, int nv)
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > Constraint
InertiaTpl< S1, O1 > Inertia
JointMotionSubspaceTpl< Eigen::Dynamic, _Scalar, _Options, _MaxDim > RefJointMotionSubspace
Transformation_t M_accessor() const
ReturnTypeNotDefined ReturnType
const typedef TangentVector_t TangentVectorTypeConstRef
const Base & base() const
static std::string classname()
RefJointMotionSubspace m_constraint
TangentVectorTypeConstRef joint_v_accessor() const
ConstraintTypeRef S_accessor()
boost::mpl::false_ is_mimicable_t
Constraint_t & ConstraintTypeRef
JointForce operator*(const ForceDense< Derived > &f) const
int nvExtended_impl() const
const RefJointMotionSubspace & constraint() const
JointDataDerived createData() const
void configVectorAffineTransform(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, const Eigen::MatrixBase< ConfigVectorIn > &qIn, const Scalar &scaling, const Scalar &offset, const Eigen::MatrixBase< ConfigVectorOut > &qOut)
Apply the correct affine transform, on a joint configuration, depending on the joint type.
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > MotionSubspace
JointModel m_jmodel_mimicking
JointModelMimicTpl(const JointModelBase< JointModel > &jmodel, const Scalar &scaling, const Scalar &offset)
traits< RefJointMotionSubspace >::ConstMatrixReturnType ConstMatrixReturnType
const JointModel & jmodel() const
SE3Tpl< Scalar, Options > SE3
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived)
ConfigVector_t joint_q
original configuration vector
Transformation_t TansformTypeRef
MotionTpl< Scalar, Options > Bias_t
static RowsReturn< D >::ConstType middleRows(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
ScaledJointMotionSubspaceTpl< S2, O2, MD2 > Constraint
SizeDepType< NQ >::template SegmentReturn< D >::Type JointMappedConfigSelector_impl(Eigen::MatrixBase< D > &a) const
Eigen::Matrix< S2, 6, Eigen::Dynamic, O2, 6, MD2 > ReturnType
static SegmentReturn< D >::ConstType segment(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size=NV)
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived)
int idx_vExtended() const
traits< RefJointMotionSubspace >::MatrixReturnType MatrixReturnType
const typedef ConfigVector_t & ConfigVectorTypeConstRef
static ReturnType run(const Eigen::MatrixBase< M6Like > &Y, const Constraint &scaled_constraint)
std::string shortname() const
Constraint::Scalar Scalar
ScaledJointMotionSubspaceTpl(const Scalar &scaling_factor)
static std::string classname()
const typedef Constraint_t & ConstraintTypeConstRef
traits< RefJointMotionSubspace >::JointForce JointForce
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
ScaledJointMotionSubspaceTpl()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
int idx_vExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model extended tangent ...
SizeDepType< NQ >::template SegmentReturn< D >::ConstType JointMappedConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
SizeDepType< NV >::template RowsReturn< D >::Type jointRows_impl(Eigen::MatrixBase< D > &A) const
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v, int vExtended)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
Return type of the Constraint::Transpose * ForceSet operation.
ScaledJointMotionSubspaceTpl< Scalar, Options, MaxNVMimicked > Constraint_t
SizeDepType< NQ >::template SegmentReturn< D >::ConstType JointMappedVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock_impl(const Eigen::MatrixBase< D > &Mat) const
JointMotionSubspaceBase< ScaledJointMotionSubspaceTpl > Base
DenseBase & matrix_impl()
JointCollectionTpl< Scalar, Options > JointCollection
Motion_t v_accessor() const
JointMimicTpl< _Scalar, Options, JointCollectionTpl > JointDerived
const ScaledJointMotionSubspaceTpl & ref
ScalarMatrixProduct< _Scalar, RefReturnType >::type ReturnType
SizeDepType< NV >::template BlockReturn< D >::Type jointBlock_impl(Eigen::MatrixBase< D > &Mat) const
SE3GroupAction< RefJointMotionSubspace >::ReturnType SE3ActionReturnType
MultiplicationOp< Eigen::MatrixBase< M6Like >, Constraint >::ReturnType ReturnType
traits< RefJointMotionSubspace >::Scalar Scalar
TransposeConst transpose() const
const Scalar & scaling() const
SE3ActionReturnType se3ActionInverse(const SE3Tpl< S1, O1 > &m) const
const std::vector< bool > hasConfigurationLimit() const
bool isEqual(const ScaledJointMotionSubspaceTpl &other) const
#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED)
ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >::RefJointMotionSubspace RefJointMotionSubspace
ConfigVector_t & ConfigVectorTypeRef
MultiplicationOp< Inertia, Constraint >::ReturnType ReturnType
const TangentVector_t & v_transformed() const
Transformation_t TansformTypeConstRef
void setMimicIndexes(JointIndex id, int q, int v, int vExtended)
Specific way for mimic joints to set the mimicked q,v indexes. Used for manipulating tree (e....
ScaledJointMotionSubspaceTpl(const ConstraintTpl &constraint, const Scalar &scaling_factor)
Common traits structure to fully define base classes for CRTP.
JointDataMimicTpl(const RefJointData &jdata, const Scalar &scaling, const int &nq, const int &nv)
ConfigVector_t joint_q_transformed
Transformed configuration vector.
void calc_aba(JointDataDerived &, const Eigen::MatrixBase< VectorLike > &, const Eigen::MatrixBase< Matrix6Like > &, const bool) const
D_t Dinv_accessor() const
EIGEN_DONT_INLINE void calc(JointDataDerived &jdata, const typename Eigen::MatrixBase< ConfigVector > &qs) const
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)
ConstraintForceOp< RefJointMotionSubspace, ForceDerived >::ReturnType RefReturnType
JointMotion __mult__(const Eigen::MatrixBase< VectorLike > &v) const
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
std::string shortname() const
const RefJointData & jdata() const
JointMimicTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > U_t
traits< RefJointMotionSubspace >::JointMotion JointMotion
JointModelMimicTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
virtual bool isEqual(const CollisionGeometry &other) const=0
Transformation_t M() const
JointDataDerived createData() const
TangentVectorTypeRef joint_v_accessor()
static ReturnType run(const Inertia &Y, const Constraint &scaled_constraint)
ScaledJointMotionSubspaceTpl< _Scalar, _Options, _MaxDim >::RefJointMotionSubspace RefJointMotionSubspace
Cast scalar type from type FROM to type TO.
std::string shortname() const
Main pinocchio namespace.
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
pinocchio
Author(s):
autogenerated on Fri Mar 7 2025 03:41:56