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>
34 {
typedef typename SE3GroupAction<Constraint>::ReturnType ReturnType; };
36 template<
class Constra
int,
typename MotionDerived>
38 {
typedef typename MotionAlgebraAction<Constraint,MotionDerived>::ReturnType ReturnType; };
40 template<
class Constra
int,
typename ForceDerived>
43 typedef typename Constraint::Scalar Scalar;
44 typedef typename ConstraintForceOp<Constraint,ForceDerived>::ReturnType OriginalReturnType;
46 typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
47 typedef Eigen::Matrix<Scalar,IdealReturnType::RowsAtCompileTime,IdealReturnType::ColsAtCompileTime,Constraint::Options> ReturnType;
50 template<
class Constra
int,
typename ForceSet>
53 typedef typename Constraint::Scalar Scalar;
54 typedef typename ConstraintForceSetOp<Constraint,ForceSet>::ReturnType OriginalReturnType;
55 typedef typename ScalarMatrixProduct<Scalar,OriginalReturnType>::type IdealReturnType;
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
66 enum { NV = Constraint::NV };
70 typedef typename SE3GroupAction<Constraint>::ReturnType SE3ActionReturnType;
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)
91 m_constraint = other.m_constraint;
92 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>
128 typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
132 typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
133 return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
137 template<
typename Derived>
138 typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
141 typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
142 return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
149 DenseBase matrix_impl()
const 151 DenseBase S = m_scaling_factor * m_constraint.matrix();
155 template<
typename MotionDerived>
160 ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
164 inline const Scalar & scaling()
const {
return m_scaling_factor; }
165 inline Scalar & scaling() {
return m_scaling_factor; }
167 inline const Constraint & constraint()
const {
return m_constraint; }
168 inline Constraint & constraint() {
return m_constraint; }
172 return m_constraint == other.m_constraint
173 && m_scaling_factor == other.m_scaling_factor;
178 Constraint m_constraint;
179 Scalar m_scaling_factor;
182 template<
typename S1,
int O1,
typename _Constra
int>
187 typedef typename Constraint::Scalar Scalar;
191 typedef OriginalReturnType ReturnType;
197 template<
typename S1,
int O1,
typename _Constra
int>
204 static inline ReturnType run(
const Inertia & Y,
205 const Constraint & scaled_constraint)
207 return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
212 template<
typename M6Like,
typename _Constra
int>
216 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType;
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>
267 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
273 template<
class Jo
int>
277 template<
class Jo
int>
281 template<
class Jo
intData>
285 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
290 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
293 : m_scaling((Scalar)0)
294 , m_q_transform(ConfigVector_t::Zero())
295 , m_v_transform(TangentVector_t::Zero())
303 const Scalar & scaling)
304 : m_jdata_ref(jdata.derived())
306 , S(m_jdata_ref.S,scaling)
311 m_jdata_ref = other.m_jdata_ref;
312 m_scaling = other.m_scaling;
315 S = Constraint_t(m_jdata_ref.S,other.m_scaling);
322 return Base::isEqual(other)
323 && m_jdata_ref == other.m_jdata_ref
324 && m_scaling == other.m_scaling
330 static std::string classname()
332 return std::string(
"JointDataMimic<") + JointData::classname() + std::string(
">");
337 return std::string(
"JointDataMimic<") + m_jdata_ref.shortname() + std::string(
">");
341 ConstraintTypeConstRef S_accessor()
const {
return S; }
342 ConstraintTypeRef S_accessor() {
return S; }
344 TansformTypeConstRef M_accessor()
const {
return m_jdata_ref.M; }
345 TansformTypeRef M_accessor() {
return m_jdata_ref.M; }
347 MotionTypeConstRef v_accessor()
const {
return m_jdata_ref.v; }
348 MotionTypeRef v_accessor() {
return m_jdata_ref.v; }
350 BiasTypeConstRef c_accessor()
const {
return m_jdata_ref.c; }
351 BiasTypeRef c_accessor() {
return m_jdata_ref.c; }
353 UTypeConstRef U_accessor()
const {
return m_jdata_ref.U; }
354 UTypeRef U_accessor() {
return m_jdata_ref.U; }
356 DTypeConstRef Dinv_accessor()
const {
return m_jdata_ref.Dinv; }
357 DTypeRef Dinv_accessor() {
return m_jdata_ref.Dinv; }
359 UDTypeConstRef UDinv_accessor()
const {
return m_jdata_ref.UDinv; }
360 UDTypeRef UDinv_accessor() {
return m_jdata_ref.UDinv; }
362 template<
class Jo
intModel>
365 const JointData & jdata()
const {
return m_jdata_ref; }
366 JointData & jdata() {
return m_jdata_ref; }
368 const Scalar & scaling()
const {
return m_scaling; }
369 Scalar & scaling() {
return m_scaling; }
371 ConfigVector_t & jointConfiguration() {
return m_q_transform; }
372 const ConfigVector_t & jointConfiguration()
const {
return m_q_transform; }
374 TangentVector_t & jointVelocity() {
return m_v_transform; }
375 const TangentVector_t & jointVelocity()
const {
return m_v_transform; }
394 template<
typename NewScalar,
typename Jo
intModel>
401 template<
class Jo
intModel>
405 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
409 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
417 using Base::setIndexes;
423 const Scalar & scaling,
424 const Scalar & offset)
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); }
433 inline int nq_impl()
const {
return 0; }
434 inline int nv_impl()
const {
return 0; }
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(); }
439 void setIndexes_impl(JointIndex
id,
int ,
int )
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>
480 void calc_aba(JointDataDerived & data,
481 const Eigen::MatrixBase<Matrix6Like> & I,
482 const bool update_I)
const 485 m_jmodel_ref.calc_aba(data.m_jdata_ref,
486 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
490 static std::string classname()
492 return std::string(
"JointModelMimic<") + JointModel::classname() + std::string(
">");;
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);
513 const JointModel & jmodel()
const {
return m_jmodel_ref; }
514 JointModel & jmodel() {
return m_jmodel_ref; }
516 const Scalar & scaling()
const {
return m_scaling; }
517 Scalar & scaling() {
return m_scaling; }
519 const Scalar & offset()
const {
return m_offset; }
520 Scalar & offset() {
return m_offset; }
526 Scalar m_scaling, m_offset;
534 jointConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const 537 m_jmodel_ref.idx_q(),
544 jointConfigSelector_impl(Eigen::MatrixBase<D> & a)
const 547 m_jmodel_ref.idx_q(),
555 jointVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const 558 m_jmodel_ref.idx_v(),
565 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const 568 m_jmodel_ref.idx_v(),
576 jointCols_impl(
const Eigen::MatrixBase<D> & A)
const 579 m_jmodel_ref.idx_v(),
586 jointCols_impl(Eigen::MatrixBase<D> & A)
const 589 m_jmodel_ref.idx_v(),
597 jointRows_impl(
const Eigen::MatrixBase<D> & A)
const 600 m_jmodel_ref.idx_v(),
607 jointRows_impl(Eigen::MatrixBase<D> & A)
const 610 m_jmodel_ref.idx_v(),
621 m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
622 m_jmodel_ref.nv(),m_jmodel_ref.nv());
628 jointBlock_impl(Eigen::MatrixBase<D> & Mat)
const 631 m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
632 m_jmodel_ref.nv(),m_jmodel_ref.nv());
639 #endif // ifndef __pinocchio_multibody_joint_mimic_hpp__
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.
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
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...
Return type of the Constraint::Transpose * ForceSet operation.
Return type of the ation of a Motion onto an object of type D.
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
bool isEqual(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel_generic, const JointModelBase< JointModelDerived > &jmodel)
Visit a JointModelTpl<Scalar,...> to compare it to JointModelDerived.
TangentVector_t m_v_transform
Transform velocity vector.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
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...
Main pinocchio namespace.
CastType< NewScalar, JointModelMimic >::type cast() const
Common traits structure to fully define base classes for CRTP.
ConstraintForceSetOp< ScaledConstraint, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
ConfigVector_t m_q_transform
Transform configuration vector.
Return type of the Constraint::Transpose * Force operation.
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
.