5 #ifndef __pinocchio_joint_mimic_hpp__ 6 #define __pinocchio_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)
86 m_constraint = other.m_constraint;
87 m_scaling_factor = other.m_scaling_factor;
91 template<
typename VectorLike>
92 JointMotion __mult__(
const Eigen::MatrixBase<VectorLike> & v)
const 94 assert(v.size() ==
nv());
95 JointMotion jm = m_constraint * v;
96 return jm * m_scaling_factor;
99 template<
typename S1,
int O1>
103 SE3ActionReturnType res = m_constraint.se3Action(m);
104 return m_scaling_factor * res;
107 template<
typename S1,
int O1>
111 SE3ActionReturnType res = m_constraint.se3ActionInverse(m);
112 return m_scaling_factor * res;
115 int nv_impl()
const {
return m_constraint.nv(); }
122 template<
typename Derived>
123 typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
127 typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
128 return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
132 template<
typename Derived>
133 typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
136 typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
137 return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
144 DenseBase matrix_impl()
const 146 DenseBase S = m_scaling_factor * m_constraint.matrix();
150 template<
typename MotionDerived>
155 ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
159 inline const Scalar & scaling()
const {
return m_scaling_factor; }
160 inline Scalar & scaling() {
return m_scaling_factor; }
162 inline const Constraint & constraint()
const {
return m_constraint; }
163 inline Constraint & constraint() {
return m_constraint; }
167 return m_constraint == other.m_constraint
168 && m_scaling_factor == other.m_scaling_factor;
173 Constraint m_constraint;
174 Scalar m_scaling_factor;
177 template<
typename S1,
int O1,
typename _Constra
int>
182 typedef typename Constraint::Scalar Scalar;
186 typedef OriginalReturnType ReturnType;
192 template<
typename S1,
int O1,
typename _Constra
int>
199 static inline ReturnType run(
const Inertia & Y,
200 const Constraint & scaled_constraint)
202 return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
207 template<
typename M6Like,
typename _Constra
int>
211 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType;
217 template<
typename M6Like,
typename _Constra
int>
223 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
224 const Constraint & scaled_constraint)
226 return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
235 template<
class Jo
int>
262 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
268 template<
class Jo
int>
272 template<
class Jo
int>
276 template<
class Jo
intData>
280 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
285 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
288 : m_scaling((Scalar)0)
289 , m_q_transform(ConfigVector_t::Zero())
290 , m_v_transform(TangentVector_t::Zero())
295 const Scalar & scaling)
296 : m_jdata_ref(jdata.derived())
298 , S(m_jdata_ref.S,scaling)
303 m_jdata_ref = other.m_jdata_ref;
304 m_scaling = other.m_scaling;
307 S = Constraint_t(m_jdata_ref.S,other.m_scaling);
313 return Base::isEqual(other)
314 && m_jdata_ref == other.m_jdata_ref
315 && m_scaling == other.m_scaling
321 static std::string classname()
323 return std::string(
"JointDataMimic<") + JointData::classname() + std::string(
">");
328 return std::string(
"JointDataMimic<") + m_jdata_ref.shortname() + std::string(
">");
332 ConstraintTypeConstRef S_accessor()
const {
return S; }
333 ConstraintTypeRef S_accessor() {
return S; }
335 TansformTypeConstRef M_accessor()
const {
return m_jdata_ref.M; }
336 TansformTypeRef M_accessor() {
return m_jdata_ref.M; }
338 MotionTypeConstRef v_accessor()
const {
return m_jdata_ref.v; }
339 MotionTypeRef v_accessor() {
return m_jdata_ref.v; }
341 BiasTypeConstRef c_accessor()
const {
return m_jdata_ref.c; }
342 BiasTypeRef c_accessor() {
return m_jdata_ref.c; }
344 UTypeConstRef U_accessor()
const {
return m_jdata_ref.U; }
345 UTypeRef U_accessor() {
return m_jdata_ref.U; }
347 DTypeConstRef Dinv_accessor()
const {
return m_jdata_ref.Dinv; }
348 DTypeRef Dinv_accessor() {
return m_jdata_ref.Dinv; }
350 UDTypeConstRef UDinv_accessor()
const {
return m_jdata_ref.UDinv; }
351 UDTypeRef UDinv_accessor() {
return m_jdata_ref.UDinv; }
353 template<
class Jo
intModel>
356 const JointData & jdata()
const {
return m_jdata_ref; }
357 JointData & jdata() {
return m_jdata_ref; }
359 const Scalar & scaling()
const {
return m_scaling; }
360 Scalar & scaling() {
return m_scaling; }
362 ConfigVector_t & jointConfiguration() {
return m_q_transform; }
363 const ConfigVector_t & jointConfiguration()
const {
return m_q_transform; }
365 TangentVector_t & jointVelocity() {
return m_v_transform; }
366 const TangentVector_t & jointVelocity()
const {
return m_v_transform; }
385 template<
typename NewScalar,
typename Jo
intModel>
392 template<
class Jo
intModel>
396 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
400 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
408 using Base::setIndexes;
414 const Scalar & scaling,
415 const Scalar & offset)
416 : m_jmodel_ref(jmodel.derived())
421 Base & base() {
return *
static_cast<Base*
>(
this); }
422 const Base & base()
const {
return *
static_cast<const Base*
>(
this); }
424 inline int nq_impl()
const {
return 0; }
425 inline int nv_impl()
const {
return 0; }
427 inline int idx_q_impl()
const {
return m_jmodel_ref.idx_q(); }
428 inline int idx_v_impl()
const {
return m_jmodel_ref.idx_v(); }
430 void setIndexes_impl(JointIndex
id,
int ,
int )
433 Base::i_q = m_jmodel_ref.idx_q();
434 Base::i_v = m_jmodel_ref.idx_v();
439 return JointDataDerived(m_jmodel_ref.createData(),scaling());
442 template<
typename ConfigVector>
444 void calc(JointDataDerived & jdata,
445 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 449 AffineTransform::run(qs.head(m_jmodel_ref.nq()),
450 m_scaling,m_offset,jdata.m_q_transform);
451 m_jmodel_ref.calc(jdata.m_jdata_ref,jdata.m_q_transform);
454 template<
typename ConfigVector,
typename TangentVector>
456 void calc(JointDataDerived & jdata,
457 const typename Eigen::MatrixBase<ConfigVector> & qs,
458 const typename Eigen::MatrixBase<TangentVector> & vs)
const 462 AffineTransform::run(qs.head(m_jmodel_ref.nq()),
463 m_scaling,m_offset,jdata.m_q_transform);
464 jdata.m_v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
465 m_jmodel_ref.calc(jdata.m_jdata_ref,
467 jdata.m_v_transform);
470 template<
typename Matrix6Like>
471 void calc_aba(JointDataDerived & data,
472 const Eigen::MatrixBase<Matrix6Like> & I,
473 const bool update_I)
const 476 m_jmodel_ref.calc_aba(data.m_jdata_ref,
477 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
481 static std::string classname()
483 return std::string(
"JointModelMimic<") + JointModel::classname() + std::string(
">");;
488 return std::string(
"JointModelMimic<") + m_jmodel_ref.shortname() + std::string(
">");
492 template<
typename NewScalar>
497 ReturnType res(m_jmodel_ref.template cast<NewScalar>(),
498 (NewScalar)m_scaling,
499 (NewScalar)m_offset);
504 const JointModel & jmodel()
const {
return m_jmodel_ref; }
505 JointModel & jmodel() {
return m_jmodel_ref; }
507 const Scalar & scaling()
const {
return m_scaling; }
508 Scalar & scaling() {
return m_scaling; }
510 const Scalar & offset()
const {
return m_offset; }
511 Scalar & offset() {
return m_offset; }
517 Scalar m_scaling, m_offset;
525 jointConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const 528 m_jmodel_ref.idx_q(),
535 jointConfigSelector_impl(Eigen::MatrixBase<D> & a)
const 538 m_jmodel_ref.idx_q(),
546 jointVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const 549 m_jmodel_ref.idx_v(),
556 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const 559 m_jmodel_ref.idx_v(),
567 jointCols_impl(
const Eigen::MatrixBase<D> & A)
const 570 m_jmodel_ref.idx_v(),
577 jointCols_impl(Eigen::MatrixBase<D> & A)
const 580 m_jmodel_ref.idx_v(),
588 jointRows_impl(
const Eigen::MatrixBase<D> & A)
const 591 m_jmodel_ref.idx_v(),
598 jointRows_impl(Eigen::MatrixBase<D> & A)
const 601 m_jmodel_ref.idx_v(),
612 m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
613 m_jmodel_ref.nv(),m_jmodel_ref.nv());
619 jointBlock_impl(Eigen::MatrixBase<D> & Mat)
const 622 m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
623 m_jmodel_ref.nv(),m_jmodel_ref.nv());
630 #endif // ifndef __pinocchio_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.
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...
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...
Source from #include <cppad/example/cppad_eigen.hpp>
CastType< NewScalar, JointModelMimic >::type cast() const
TangentVector_t m_v_transform
Transform velocity vector.
ConstraintForceSetOp< ScaledConstraint, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
Main pinocchio namespace.
Common traits structure to fully define base classes for CRTP.
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)
.