joint-mimic.hpp

//
// Copyright (c) 2019-2021 INRIA
//

#ifndef __pinocchio_multibody_joint_mimic_hpp__
#define __pinocchio_multibody_joint_mimic_hpp__

#include "pinocchio/multibody/joint/fwd.hpp"
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"

namespace pinocchio
{
 template<typename _Scalar, int _Options, int MaxDim>
 struct ScaledJointMotionSubspaceTpl;

 template<typename _Scalar, int _Options, int _MaxDim>
 struct traits<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>
 {
 enum
 {
 MaxDim = _MaxDim
 };
 typedef JointMotionSubspaceTpl<Eigen::Dynamic, _Scalar, _Options, _MaxDim>
 RefJointMotionSubspace;
 typedef typename traits<RefJointMotionSubspace>::Scalar Scalar;
 enum
 {
 Options = traits<RefJointMotionSubspace>::Options
 };
 enum
 {
 LINEAR = traits<RefJointMotionSubspace>::LINEAR,
 ANGULAR = traits<RefJointMotionSubspace>::ANGULAR
 };
 typedef typename traits<RefJointMotionSubspace>::JointMotion JointMotion;
 typedef typename traits<RefJointMotionSubspace>::JointForce JointForce;
 typedef typename traits<RefJointMotionSubspace>::DenseBase DenseBase;
 typedef typename traits<RefJointMotionSubspace>::MatrixReturnType MatrixReturnType;
 typedef typename traits<RefJointMotionSubspace>::ConstMatrixReturnType ConstMatrixReturnType;
 }; // traits ScaledJointMotionSubspaceTpl

 template<typename _Scalar, int _Options, int _MaxDim>
 struct SE3GroupAction<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>
 {
 typedef typename SE3GroupAction<typename traits<
 ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace>::ReturnType
 ReturnType;
 };

 template<typename _Scalar, int _Options, int _MaxDim, typename MotionDerived>
 struct MotionAlgebraAction<
 ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>,
 MotionDerived>
 {
 typedef typename MotionAlgebraAction<
 typename traits<
 ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::RefJointMotionSubspace,
 MotionDerived>::ReturnType ReturnType;
 };

 template<typename _Scalar, int _Options, int _MaxDim, typename ForceDerived>
 struct ConstraintForceOp<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, ForceDerived>
 {
 typedef
 typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace
 RefJointMotionSubspace;
 typedef
 typename ConstraintForceOp<RefJointMotionSubspace, ForceDerived>::ReturnType RefReturnType;
 typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType;
 };

 template<typename _Scalar, int _Options, int _MaxDim, typename ForceSet>
 struct ConstraintForceSetOp<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>, ForceSet>
 {
 typedef
 typename ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>::RefJointMotionSubspace
 RefJointMotionSubspace;
 typedef
 typename ConstraintForceSetOp<RefJointMotionSubspace, ForceSet>::ReturnType RefReturnType;
 typedef typename ScalarMatrixProduct<_Scalar, RefReturnType>::type ReturnType;
 };

 template<typename _Scalar, int _Options, int _MaxDim>
 struct ScaledJointMotionSubspaceTpl
 : JointMotionSubspaceBase<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>
 {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ScaledJointMotionSubspaceTpl)
 enum
 {
 NV = Eigen::Dynamic,
 MaxDim = _MaxDim
 };
 typedef JointMotionSubspaceBase<ScaledJointMotionSubspaceTpl> Base;
 using Base::nv;

 typedef typename traits<ScaledJointMotionSubspaceTpl<_Scalar, _Options, _MaxDim>>::
 RefJointMotionSubspace RefJointMotionSubspace;
 typedef typename SE3GroupAction<RefJointMotionSubspace>::ReturnType SE3ActionReturnType;

 ScaledJointMotionSubspaceTpl()
 : m_constraint(0)
 , m_scaling_factor(Scalar(1))
 {
 }

 explicit ScaledJointMotionSubspaceTpl(const Scalar & scaling_factor)
 : m_constraint(0)
 , m_scaling_factor(scaling_factor)
 {
 }

 template<typename ConstraintTpl>
 ScaledJointMotionSubspaceTpl(const ConstraintTpl & constraint, const Scalar & scaling_factor)
 : m_constraint(constraint)
 , m_scaling_factor(scaling_factor)
 {
 }

 ScaledJointMotionSubspaceTpl(const ScaledJointMotionSubspaceTpl & other)
 : m_constraint(other.m_constraint)
 , m_scaling_factor(other.m_scaling_factor)
 {
 }

 ScaledJointMotionSubspaceTpl & operator=(const ScaledJointMotionSubspaceTpl & other)
 {
 m_constraint = other.m_constraint;
 m_scaling_factor = other.m_scaling_factor;
 return *this;
 }

 template<typename VectorLike>
 JointMotion __mult__(const Eigen::MatrixBase<VectorLike> & v) const
 {

 assert(v.size() == nv());
 JointMotion jm = m_constraint * v;
 return m_scaling_factor * jm;
 }

 template<typename S1, int O1>
 SE3ActionReturnType se3Action(const SE3Tpl<S1, O1> & m) const
 {
 return m_scaling_factor * m_constraint.se3Action(m);
 }

 template<typename S1, int O1>
 SE3ActionReturnType se3ActionInverse(const SE3Tpl<S1, O1> & m) const
 {
 return m_scaling_factor * m_constraint.se3ActionInverse(m);
 }

 int nv_impl() const
 {
 return m_constraint.nv();
 }

 struct TransposeConst
 {
 const ScaledJointMotionSubspaceTpl & ref;
 explicit TransposeConst(const ScaledJointMotionSubspaceTpl & ref)
 : ref(ref)
 {
 }

 template<typename Derived>
 // typename ConstraintForceOp<ScaledJointMotionSubspaceTpl, Derived>::ReturnType
 JointForce operator*(const ForceDense<Derived> & f) const
 {
 return ref.m_scaling_factor * (ref.m_constraint.transpose() * f);
 }

 template<typename Derived>
 typename ConstraintForceSetOp<ScaledJointMotionSubspaceTpl, Derived>::ReturnType
 operator*(const Eigen::MatrixBase<Derived> & F) const
 {
 return ref.m_scaling_factor * (ref.m_constraint.transpose() * F);
 }

 }; // struct TransposeConst

 TransposeConst transpose() const
 {
 return TransposeConst(*this);
 }

 const DenseBase & matrix_impl() const
 {
 S = m_scaling_factor * m_constraint.matrix_impl();
 return S;
 }

 DenseBase & matrix_impl()
 {
 S = m_scaling_factor * m_constraint.matrix_impl();
 return S;
 }

 template<typename MotionDerived>
 typename MotionAlgebraAction<ScaledJointMotionSubspaceTpl, MotionDerived>::ReturnType
 motionAction(const MotionDense<MotionDerived> & m) const
 {
 return m_scaling_factor * m_constraint.motionAction(m);
 }

 inline const Scalar & scaling() const
 {
 return m_scaling_factor;
 }
 inline Scalar & scaling()
 {
 return m_scaling_factor;
 }

 inline const RefJointMotionSubspace & constraint() const
 {
 return m_constraint.derived();
 }
 inline RefJointMotionSubspace & constraint()
 {
 return m_constraint.derived();
 }

 bool isEqual(const ScaledJointMotionSubspaceTpl & other) const
 {
 return m_constraint == other.m_constraint && m_scaling_factor == other.m_scaling_factor;
 }

 protected:
 RefJointMotionSubspace m_constraint;
 Scalar m_scaling_factor;
 mutable DenseBase S;
 }; // struct ScaledJointMotionSubspaceTpl

 template<typename S1, int O1, typename S2, int O2, int MD2>
 struct MultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>>
 {
 typedef InertiaTpl<S1, O1> Inertia;
 typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> Constraint;
 typedef typename Constraint::Scalar Scalar;

 typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2> ReturnType;
 };

 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
 namespace impl
 {
 template<typename S1, int O1, typename S2, int O2, int MD2>
 struct LhsMultiplicationOp<InertiaTpl<S1, O1>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>>
 {
 typedef InertiaTpl<S1, O1> Inertia;
 typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> Constraint;
 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;

 static inline ReturnType run(const Inertia & Y, const Constraint & scaled_constraint)
 {
 return scaled_constraint.scaling() * (Y * scaled_constraint.constraint());
 }
 };
 } // namespace impl

 template<typename M6Like, typename S2, int O2, int MD2>
 struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>>
 {
 typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> MotionSubspace;
 typedef Eigen::Matrix<S2, 6, Eigen::Dynamic, O2, 6, MD2> ReturnType;
 };

 /* [ABA] operator* (Inertia Y,Constraint S) */
 namespace impl
 {
 template<typename M6Like, typename S2, int O2, int MD2>
 struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ScaledJointMotionSubspaceTpl<S2, O2, MD2>>
 {
 typedef ScaledJointMotionSubspaceTpl<S2, O2, MD2> Constraint;
 typedef
 typename MultiplicationOp<Eigen::MatrixBase<M6Like>, Constraint>::ReturnType ReturnType;

 static inline ReturnType
 run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & scaled_constraint)
 {
 return scaled_constraint.scaling() * (Y.derived() * scaled_constraint.constraint());
 }
 };
 } // namespace impl

 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct JointMimicTpl;
 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct JointModelMimicTpl;
 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct JointDataMimicTpl;

 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
 struct traits<JointMimicTpl<_Scalar, _Options, JointCollectionTpl>>
 {
 typedef _Scalar Scalar;

 enum
 {
 Options = _Options,
 NQ = Eigen::Dynamic,
 NV = Eigen::Dynamic,
 NVExtended = Eigen::Dynamic,
 MaxNVMimicked = 6
 };

 typedef JointCollectionTpl<Scalar, Options> JointCollection;
 typedef JointDataMimicTpl<Scalar, Options, JointCollectionTpl> JointDataDerived;
 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelDerived;

 typedef ScaledJointMotionSubspaceTpl<Scalar, Options, MaxNVMimicked> Constraint_t;
 typedef SE3Tpl<Scalar, Options> Transformation_t;
 typedef MotionTpl<Scalar, Options> Motion_t;
 typedef MotionTpl<Scalar, Options> Bias_t;

 // [ABA]
 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t;
 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t;
 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t;

 typedef const Constraint_t & ConstraintTypeConstRef;
 typedef Constraint_t & ConstraintTypeRef;
 typedef Transformation_t TansformTypeConstRef;
 typedef Transformation_t TansformTypeRef;
 typedef Motion_t MotionTypeConstRef;
 typedef Motion_t MotionTypeRef;
 typedef Bias_t BiasTypeConstRef;
 typedef Bias_t BiasTypeRef;
 typedef U_t UTypeConstRef;
 typedef U_t UTypeRef;
 typedef D_t DTypeConstRef;
 typedef D_t DTypeRef;
 typedef UD_t UDTypeConstRef;
 typedef UD_t UDTypeRef;

 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t;
 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t;

 typedef const ConfigVector_t & ConfigVectorTypeConstRef;
 typedef ConfigVector_t & ConfigVectorTypeRef;
 typedef const TangentVector_t TangentVectorTypeConstRef;
 typedef TangentVector_t & TangentVectorTypeRef;

 typedef boost::mpl::false_ is_mimicable_t;
 };

 template<typename _Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct traits<JointDataMimicTpl<_Scalar, Options, JointCollectionTpl>>
 {
 typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived;
 typedef _Scalar Scalar;
 };

 template<typename _Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct traits<JointModelMimicTpl<_Scalar, Options, JointCollectionTpl>>
 {
 typedef JointMimicTpl<_Scalar, Options, JointCollectionTpl> JointDerived;
 typedef _Scalar Scalar;
 };

 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
 struct JointDataMimicTpl
 : public JointDataBase<JointDataMimicTpl<_Scalar, _Options, JointCollectionTpl>>
 {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef JointDataBase<JointDataMimicTpl> Base;
 typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived;
 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);

 typedef JointDataTpl<_Scalar, _Options, JointCollectionTpl> RefJointData;
 typedef typename RefJointData::JointDataVariant RefJointDataVariant;

 JointDataMimicTpl()
 : S((Scalar)0)
 {
 joint_q.resize(0, 1);
 joint_q_transformed.resize(0, 1);
 joint_v.resize(0, 1);
 joint_v_transformed.resize(0, 1);
 }

 JointDataMimicTpl(
 const RefJointData & jdata, const Scalar & scaling, const int & nq, const int & nv)
 : m_jdata_mimicking(checkMimic(jdata.derived()))
 , S(m_jdata_mimicking.S(), scaling)
 {
 joint_q.resize(nq, 1);
 joint_q_transformed.resize(nq, 1);
 joint_v.resize(nv, 1);
 joint_v_transformed.resize(nv, 1);
 }

 JointDataMimicTpl(const JointDataMimicTpl & other)
 {
 *this = other;
 }

 JointDataMimicTpl & operator=(const JointDataMimicTpl & other)
 {
 m_jdata_mimicking = other.m_jdata_mimicking;
 joint_q = other.joint_q;
 joint_q_transformed = other.joint_q_transformed;
 joint_v = other.joint_v;
 joint_v_transformed = other.joint_v_transformed;
 S = Constraint_t(other.S);
 return *this;
 }

 using Base::isEqual;
 bool isEqual(const JointDataMimicTpl & other) const
 {
 return Base::isEqual(other) && m_jdata_mimicking == other.m_jdata_mimicking
 && joint_q == other.joint_q && joint_q_transformed == other.joint_q_transformed
 && joint_v == other.joint_v && joint_v_transformed == other.joint_v_transformed;
 }

 static std::string classname()
 {
 return std::string("JointDataMimic");
 }

 std::string shortname() const
 {
 return classname();
 }

 // // Accessors
 ConstraintTypeConstRef S_accessor() const
 {
 return S;
 }
 ConstraintTypeRef S_accessor()
 {
 return S;
 }

 Transformation_t M_accessor() const
 {
 return m_jdata_mimicking.M();
 }

 Motion_t v_accessor() const
 {
 return m_jdata_mimicking.v();
 }

 Bias_t c_accessor() const
 {
 return m_jdata_mimicking.c();
 }

 U_t U_accessor() const
 {
 return m_jdata_mimicking.U();
 }

 D_t Dinv_accessor() const
 {
 return m_jdata_mimicking.Dinv();
 }

 UD_t UDinv_accessor() const
 {
 return m_jdata_mimicking.UDinv();
 }

 D_t StU_accessor() const
 {
 return m_jdata_mimicking.StU();
 }

 friend struct JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>;

 const RefJointData & jdata() const
 {
 return m_jdata_mimicking;
 }
 RefJointData & jdata()
 {
 return m_jdata_mimicking;
 }

 ConfigVectorTypeRef joint_q_accessor()
 {
 return joint_q;
 }
 ConfigVectorTypeConstRef joint_q_accessor() const
 {
 return joint_q;
 }

 ConfigVector_t & q_transformed()
 {
 return joint_q_transformed;
 }
 const ConfigVector_t & q_transformed() const
 {
 return joint_q_transformed;
 }
 TangentVectorTypeRef joint_v_accessor()
 {
 return joint_v;
 }
 TangentVectorTypeConstRef joint_v_accessor() const
 {
 return joint_v;
 }

 TangentVector_t & v_transformed()
 {
 return joint_v_transformed;
 }
 const TangentVector_t & v_transformed() const
 {
 return joint_v_transformed;
 }

 void disp(std::ostream & os) const
 {
 Base::disp(os);
 os << " Mimicking joint data: " << m_jdata_mimicking.shortname() << std::endl;
 }

 RefJointData m_jdata_mimicking;

 ConfigVector_t joint_q;
 ConfigVector_t joint_q_transformed;
 TangentVector_t joint_v;
 TangentVector_t joint_v_transformed;
 // data
 Constraint_t S;
 }; // struct JointDataMimicTpl

 template<
 typename NewScalar,
 typename Scalar,
 int Options,
 template<typename S, int O> class JointCollectionTpl>
 struct CastType<NewScalar, JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>
 {
 typedef JointModelMimicTpl<NewScalar, Options, JointCollectionTpl> type;
 };

 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
 struct JointModelMimicTpl
 : public JointModelBase<JointModelMimicTpl<_Scalar, _Options, JointCollectionTpl>>
 {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef JointModelBase<JointModelMimicTpl> Base;
 typedef JointMimicTpl<_Scalar, _Options, JointCollectionTpl> JointDerived;
 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
 enum
 {
 MaxNVMimicked = traits<JointDerived>::MaxNVMimicked
 };

 typedef JointCollectionTpl<Scalar, Options> JointCollection;
 typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModel;

 typedef SE3Tpl<Scalar, Options> SE3;
 typedef MotionTpl<Scalar, Options> Motion;
 typedef InertiaTpl<Scalar, Options> Inertia;

 using Base::id;
 using Base::idx_q;
 using Base::idx_v;
 using Base::idx_vExtended;
 using Base::nq;
 using Base::nv;
 using Base::nvExtended;
 using Base::setIndexes;

 JointModelMimicTpl()
 {
 }

 template<typename JointModel>
 JointModelMimicTpl(
 const JointModelBase<JointModel> & jmodel, const Scalar & scaling, const Scalar & offset)
 : JointModelMimicTpl(jmodel, jmodel, scaling, offset)
 {
 }

 template<typename JointModelMimicking, typename JointModelMimicked>
 JointModelMimicTpl(
 const JointModelBase<JointModelMimicking> & jmodel_mimicking,
 const JointModelBase<JointModelMimicked> & jmodel_mimicked,
 const Scalar & scaling,
 const Scalar & offset)
 : m_jmodel_mimicking(checkMimic((JointModel)jmodel_mimicking.derived()))
 , m_scaling(scaling)
 , m_offset(offset)
 , m_nqExtended(jmodel_mimicking.nq())
 , m_nvExtended(jmodel_mimicking.nvExtended())
 {
 // Throw in case nq and nv from mimicking and mimicked are different
 PINOCCHIO_CHECK_ARGUMENT_SIZE(
 jmodel_mimicking.nq(), jmodel_mimicked.nq(),
 "Mimicking and mimicked configuration spaces have different sizes");
 PINOCCHIO_CHECK_ARGUMENT_SIZE(
 jmodel_mimicking.nv(), jmodel_mimicked.nv(),
 "Mimicking and mimicked tangent spaces have different sizes");
 PINOCCHIO_CHECK_ARGUMENT_SIZE(
 jmodel_mimicking.nvExtended(), jmodel_mimicked.nvExtended(),
 "Mimicking and mimicked extended tangent spaces have different sizes");

 setMimicIndexes(
 jmodel_mimicked.id(), jmodel_mimicked.idx_q(), jmodel_mimicked.idx_v(),
 jmodel_mimicked.idx_vExtended());
 }

 Base & base()
 {
 return *static_cast<Base *>(this);
 }
 const Base & base() const
 {
 return *static_cast<const Base *>(this);
 }

 inline int nq_impl() const
 {
 return 0;
 }
 inline int nv_impl() const
 {
 return 0;
 }
 inline int nvExtended_impl() const
 {
 return m_nvExtended;
 }

 void setIndexes_impl(JointIndex id, int /*q*/, int /*v*/, int vExtended)
 {
 PINOCCHIO_THROW(
 (id > m_jmodel_mimicking.id()), std::invalid_argument,
 "Mimic joint index is lower than its directing joint. Should never happen");
 Base::i_id = id;
 // Base::i_q = q;
 // Base::i_v = v;
 Base::i_vExtended = vExtended;
 }

 void setMimicIndexes(JointIndex id, int q, int v, int vExtended)
 {
 // Set idx_q, idx_v to zero because only the corresponding subsegment of q,v are passed to the
 // m_jmodel_mimicking, thus, its indexes starts at 0
 m_jmodel_mimicking.setIndexes(id, 0, 0, vExtended);

 // idx_q, idx_v are kept separately to extract the subsegment
 Base::i_q = q;
 Base::i_v = v;
 }

 JointDataDerived createData() const
 {
 return JointDataDerived(
 m_jmodel_mimicking.createData(), scaling(), m_nqExtended, m_nvExtended);
 }

 const std::vector<bool> hasConfigurationLimit() const
 {
 return {};
 }

 const std::vector<bool> hasConfigurationLimitInTangent() const
 {
 return {};
 }

 template<typename ConfigVector>
 PINOCCHIO_DONT_INLINE void
 calc(JointDataDerived & jdata, const typename Eigen::MatrixBase<ConfigVector> & qs) const
 {
 jdata.joint_q = qs.segment(Base::i_q, m_nqExtended);
 configVectorAffineTransform(
 m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed);
 m_jmodel_mimicking.calc(jdata.m_jdata_mimicking, jdata.joint_q_transformed);
 }

 template<typename ConfigVector, typename TangentVector>
 PINOCCHIO_DONT_INLINE void calc(
 JointDataDerived & jdata,
 const typename Eigen::MatrixBase<ConfigVector> & qs,
 const typename Eigen::MatrixBase<TangentVector> & vs) const
 {
 jdata.joint_q = qs.segment(Base::i_q, m_nqExtended);
 jdata.joint_v = vs.segment(Base::i_v, m_nvExtended);
 configVectorAffineTransform(
 m_jmodel_mimicking, jdata.joint_q, m_scaling, m_offset, jdata.joint_q_transformed);
 jdata.joint_v_transformed = m_scaling * jdata.joint_v;

 m_jmodel_mimicking.calc(
 jdata.m_jdata_mimicking, jdata.joint_q_transformed, jdata.joint_v_transformed);
 }

 template<typename VectorLike, typename Matrix6Like>
 void calc_aba(
 JointDataDerived &,
 const Eigen::MatrixBase<VectorLike> &,
 const Eigen::MatrixBase<Matrix6Like> &,
 const bool) const
 {
 assert(
 false
 && "Joint Mimic is not supported for aba yet. Remove it from your model if you want to use "
 "this function");
 }

 static std::string classname()
 {
 return std::string("JointModelMimic");
 }

 std::string shortname() const
 {
 return classname();
 }

 template<typename NewScalar>
 typename CastType<NewScalar, JointModelMimicTpl>::type cast() const
 {
 typedef typename CastType<NewScalar, JointModelMimicTpl>::type ReturnType;
 ReturnType res(
 m_jmodel_mimicking.template cast<NewScalar>(),
 ScalarCast<NewScalar, Scalar>::cast(m_scaling),
 ScalarCast<NewScalar, Scalar>::cast(m_offset));
 res.setIndexes(id(), Base::i_q, Base::i_v, Base::i_vExtended);
 res.setMimicIndexes(m_jmodel_mimicking.id(), Base::i_q, Base::i_v, Base::i_vExtended);
 return res;
 }

 const JointModel & jmodel() const
 {
 return m_jmodel_mimicking;
 }
 JointModel & jmodel()
 {
 return m_jmodel_mimicking;
 }

 const Scalar & scaling() const
 {
 return m_scaling;
 }
 Scalar & scaling()
 {
 return m_scaling;
 }

 const Scalar & offset() const
 {
 return m_offset;
 }
 Scalar & offset()
 {
 return m_offset;
 }

 protected:
 // data
 JointModel m_jmodel_mimicking;
 Scalar m_scaling, m_offset;
 int m_nqExtended, m_nvExtended;

 public:
 /* Acces to dedicated segment in robot config space. */
 // Const access
 template<typename D>
 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
 JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
 {
 return SizeDepType<NQ>::segment(a.derived(), Base::i_q, m_nqExtended);
 }

 // Non-const access
 template<typename D>
 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
 JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const
 {
 return SizeDepType<NQ>::segment(a.derived(), Base::i_q, m_nqExtended);
 }
 /* Acces to dedicated segment in robot tangent space. */
 // Const access
 template<typename D>
 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
 JointMappedVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
 {
 return SizeDepType<NQ>::segment(a.derived(), Base::i_v, m_nvExtended);
 }

 // Non-const access
 template<typename D>
 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
 JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
 {
 return SizeDepType<NQ>::segment(a.derived(), Base::i_v, m_nvExtended);
 }

 /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
 // Const access
 template<typename D>
 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
 jointCols_impl(const Eigen::MatrixBase<D> & A) const
 {
 return SizeDepType<NV>::middleCols(A.derived(), Base::i_v, m_nvExtended);
 }

 // Non-const access
 template<typename D>
 typename SizeDepType<NV>::template ColsReturn<D>::Type
 jointCols_impl(Eigen::MatrixBase<D> & A) const
 {
 return SizeDepType<NV>::middleCols(A.derived(), Base::i_v, m_nvExtended);
 }

 /* Acces to dedicated rows in a matrix.*/
 // Const access
 template<typename D>
 typename SizeDepType<NV>::template RowsReturn<D>::ConstType
 jointRows_impl(const Eigen::MatrixBase<D> & A) const
 {
 return SizeDepType<NV>::middleRows(A.derived(), Base::i_v, m_nvExtended);
 }

 // Non-const access
 template<typename D>
 typename SizeDepType<NV>::template RowsReturn<D>::Type
 jointRows_impl(Eigen::MatrixBase<D> & A) const
 {
 return SizeDepType<NV>::middleRows(A.derived(), Base::i_v, m_nvExtended);
 }

 // /// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the
 // matrix Mat
 // // Const access
 template<typename D>
 typename SizeDepType<NV>::template BlockReturn<D>::ConstType
 jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
 {
 return SizeDepType<NV>::block(
 Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended);
 }

 // Non-const access
 template<typename D>
 typename SizeDepType<NV>::template BlockReturn<D>::Type
 jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
 {
 return SizeDepType<NV>::block(
 Mat.derived(), Base::i_v, Base::i_v, m_nvExtended, m_nvExtended);
 }

 void disp(std::ostream & os) const
 {
 Base::disp(os);
 os << " Mimicking joint type: " << m_jmodel_mimicking.shortname() << std::endl;
 os << " Mimicked joint id: " << m_jmodel_mimicking.id() << std::endl;
 os << " Mimic scaling: " << m_scaling << std::endl;
 os << " Mimic offset: " << m_offset << std::endl;
 }

 }; // struct JointModelMimicTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct has_nothrow_constructor<
 ::pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>
 : public integral_constant<bool, true>
 {
 };

 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct has_nothrow_copy<::pinocchio::JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>
 : public integral_constant<bool, true>
 {
 };

 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct has_nothrow_constructor<
 ::pinocchio::JointDataMimicTpl<Scalar, Options, JointCollectionTpl>>
 : public integral_constant<bool, true>
 {
 };

 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
 struct has_nothrow_copy<::pinocchio::JointDataMimicTpl<Scalar, Options, JointCollectionTpl>>
 : public integral_constant<bool, true>
 {
 };
} // namespace boost

#endif // ifndef __pinocchio_multibody_joint_mimic_hpp__