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 const Scalar & scaling_factor)
76 : m_constraint(constraint)
77 , m_scaling_factor(scaling_factor)
82 m_constraint = other.m_constraint;
83 m_scaling_factor = other.m_scaling_factor;
87 template<
typename VectorLike>
88 JointMotion __mult__(
const Eigen::MatrixBase<VectorLike> & v)
const 90 assert(v.size() ==
nv());
91 JointMotion jm = m_constraint * v;
92 return jm * m_scaling_factor;
95 template<
typename S1,
int O1>
99 SE3ActionReturnType res = m_constraint.se3Action(m);
100 return m_scaling_factor * res;
103 template<
typename S1,
int O1>
107 SE3ActionReturnType res = m_constraint.se3ActionInverse(m);
108 return m_scaling_factor * res;
111 int nv_impl()
const {
return m_constraint.nv(); }
118 template<
typename Derived>
119 typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType
123 typedef typename ConstraintForceOp<ScaledConstraint,Derived>::ReturnType ReturnType;
124 return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * f));
128 template<
typename Derived>
129 typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType
132 typedef typename ConstraintForceSetOp<ScaledConstraint,Derived>::ReturnType ReturnType;
133 return ReturnType(ref.m_scaling_factor * (ref.m_constraint.transpose() * F));
140 DenseBase matrix_impl()
const 142 DenseBase S = m_scaling_factor * m_constraint.matrix();
146 template<
typename MotionDerived>
151 ReturnType res = m_scaling_factor * m_constraint.motionAction(m);
155 inline const Scalar & scaling()
const {
return m_scaling_factor; }
156 inline const Constraint & constraint()
const {
return m_constraint; }
160 Constraint m_constraint;
161 Scalar m_scaling_factor;
164 template<
typename S1,
int O1,
typename _Constra
int>
169 typedef typename Constraint::Scalar Scalar;
173 typedef OriginalReturnType ReturnType;
179 template<
typename S1,
int O1,
typename _Constra
int>
186 static inline ReturnType run(
const Inertia & Y,
187 const Constraint & scaled_constraint)
189 return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
194 template<
typename M6Like,
typename _Constra
int>
198 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(OriginalReturnType) ReturnType;
204 template<
typename M6Like,
typename _Constra
int>
210 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
211 const Constraint & scaled_constraint)
213 return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
222 template<
class Jo
int>
249 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
255 template<
class Jo
int>
259 template<
class Jo
int>
263 template<
class Jo
intData>
267 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
272 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
277 const Scalar & scaling)
278 : jdata_ref(jdata.derived())
280 , S(jdata_ref.S,scaling)
285 jdata_ref = other.jdata_ref;
286 scaling = other.scaling;
287 S = Constraint_t(jdata_ref.S,other.scaling);
291 static std::string classname()
293 return std::string(
"JointDataMimic<") + JointData::classname() + std::string(
">");
298 return std::string(
"JointDataMimic<") + jdata_ref.shortname() + std::string(
">");
302 ConstraintTypeConstRef S_accessor()
const {
return S; }
303 TansformTypeConstRef M_accessor()
const {
return jdata_ref.M; }
304 MotionTypeConstRef v_accessor()
const {
return jdata_ref.v; }
305 BiasTypeConstRef c_accessor()
const {
return jdata_ref.c; }
306 UTypeConstRef U_accessor()
const {
return jdata_ref.U; }
307 UTypeRef U_accessor() {
return jdata_ref.U; }
308 DTypeConstRef Dinv_accessor()
const {
return jdata_ref.Dinv; }
309 UDTypeConstRef UDinv_accessor()
const {
return jdata_ref.UDinv; }
311 template<
class Jo
intModel>
331 template<
typename NewScalar,
typename Jo
intModel>
338 template<
class Jo
intModel>
342 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
346 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
354 using Base::setIndexes;
360 const Scalar & scaling,
361 const Scalar & offset)
362 : m_jmodel_ref(jmodel.derived())
367 Base & base() {
return *
static_cast<Base*
>(
this); }
368 const Base & base()
const {
return *
static_cast<const Base*
>(
this); }
370 inline int nq_impl()
const {
return 0; }
371 inline int nv_impl()
const {
return 0; }
373 inline int idx_q_impl()
const {
return m_jmodel_ref.idx_q(); }
374 inline int idx_v_impl()
const {
return m_jmodel_ref.idx_v(); }
376 void setIndexes_impl(JointIndex
id,
int ,
int )
379 Base::i_q = m_jmodel_ref.idx_q();
380 Base::i_v = m_jmodel_ref.idx_v();
385 return JointDataDerived(m_jmodel_ref.createData(),scaling());
388 template<
typename ConfigVector>
390 void calc(JointDataDerived & jdata,
391 const typename Eigen::MatrixBase<ConfigVector> & qs)
const 395 AffineTransform::run(qs.head(m_jmodel_ref.nq()),
396 m_scaling,m_offset,jdata.q_transform);
397 m_jmodel_ref.calc(jdata.jdata_ref,jdata.q_transform);
400 template<
typename ConfigVector,
typename TangentVector>
402 void calc(JointDataDerived & jdata,
403 const typename Eigen::MatrixBase<ConfigVector> & qs,
404 const typename Eigen::MatrixBase<TangentVector> & vs)
const 408 AffineTransform::run(qs.head(m_jmodel_ref.nq()),
409 m_scaling,m_offset,jdata.q_transform);
410 jdata.v_transform = m_scaling * vs.head(m_jmodel_ref.nv());
411 m_jmodel_ref.calc(jdata.jdata_ref,
416 template<
typename Matrix6Like>
417 void calc_aba(JointDataDerived & data,
418 const Eigen::MatrixBase<Matrix6Like> & I,
419 const bool update_I)
const 421 m_jmodel_ref.calc_aba(data.jdata_ref,
422 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I),
426 static std::string classname()
428 return std::string(
"JointModelMimic<") + JointModel::classname() + std::string(
">");;
433 return std::string(
"JointModelMimic<") + m_jmodel_ref.shortname() + std::string(
">");
437 template<
typename NewScalar>
442 ReturnType res(m_jmodel_ref.template cast<NewScalar>(),
443 (NewScalar)m_scaling,
444 (NewScalar)m_offset);
449 const JointModel & jmodel()
const {
return m_jmodel_ref; }
450 JointModel & jmodel() {
return m_jmodel_ref; }
452 const Scalar & scaling()
const {
return m_scaling; }
453 Scalar & scaling() {
return m_scaling; }
455 const Scalar & offset()
const {
return m_offset; }
456 Scalar & offset() {
return m_offset; }
462 Scalar m_scaling, m_offset;
470 jointConfigSelector_impl(
const Eigen::MatrixBase<D> & a)
const 473 m_jmodel_ref.idx_q(),
480 jointConfigSelector_impl(Eigen::MatrixBase<D> & a)
const 483 m_jmodel_ref.idx_q(),
491 jointVelocitySelector_impl(
const Eigen::MatrixBase<D> & a)
const 494 m_jmodel_ref.idx_v(),
501 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a)
const 504 m_jmodel_ref.idx_v(),
512 jointCols_impl(
const Eigen::MatrixBase<D> & A)
const 515 m_jmodel_ref.idx_v(),
522 jointCols_impl(Eigen::MatrixBase<D> & A)
const 525 m_jmodel_ref.idx_v(),
533 jointRows_impl(
const Eigen::MatrixBase<D> & A)
const 536 m_jmodel_ref.idx_v(),
543 jointRows_impl(Eigen::MatrixBase<D> & A)
const 546 m_jmodel_ref.idx_v(),
557 m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
558 m_jmodel_ref.nv(),m_jmodel_ref.nv());
564 jointBlock_impl(Eigen::MatrixBase<D> & Mat)
const 567 m_jmodel_ref.idx_v(),m_jmodel_ref.idx_v(),
568 m_jmodel_ref.nv(),m_jmodel_ref.nv());
575 #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.
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>
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)
TangentVector_t v_transform
Transform velocity vector.
ConfigVector_t q_transform
Transform configuration vector.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
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)
.