joint-model-base.hpp

Go to the documentation of this file.

//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
 
#ifndef __pinocchio_multibody_joint_model_base_hpp__
#define __pinocchio_multibody_joint_model_base_hpp__
 
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-common-operations.hpp"
 
#include "pinocchio/math/matrix-block.hpp"
 
#include <limits>
 
#define PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, TYPENAME)                                     \
  typedef Eigen::DenseIndex Index;                                                                 \
  typedef TYPENAME traits<Joint>::Scalar Scalar;                                                   \
  typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived;                               \
  typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived;                             \
  typedef TYPENAME traits<Joint>::Constraint_t Constraint_t;                                       \
  typedef TYPENAME traits<Joint>::Transformation_t Transformation_t;                               \
  typedef TYPENAME traits<Joint>::Motion_t Motion_t;                                               \
  typedef TYPENAME traits<Joint>::Bias_t Bias_t;                                                   \
  typedef TYPENAME traits<Joint>::U_t U_t;                                                         \
  typedef TYPENAME traits<Joint>::D_t D_t;                                                         \
  typedef TYPENAME traits<Joint>::UD_t UD_t;                                                       \
  enum                                                                                             \
  {                                                                                                \
    Options = traits<Joint>::Options,                                                              \
    NQ = traits<Joint>::NQ,                                                                        \
    NV = traits<Joint>::NV                                                                         \
  };                                                                                               \
  typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t;                                   \
  typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t
 
#ifdef __clang__
 
  #define PINOCCHIO_JOINT_TYPEDEF(Joint)                                                           \
    PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)                                                  \
    PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
 
#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
 
  #define PINOCCHIO_JOINT_TYPEDEF(Joint)                                                           \
    PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)                                                  \
    PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
 
#else
 
  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)                                                  \
    PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
 
#endif
 
#define PINOCCHIO_JOINT_USE_INDEXES(Joint)                                                         \
  typedef JointModelBase<Joint> Base;                                                              \
  using Base::idx_q;                                                                               \
  using Base::idx_v
 
#define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl)                                    \
  template<typename Scalar, int Options, typename NewScalar>                                       \
  struct CastType<NewScalar, JointModelTpl<Scalar, Options>>                                       \
  {                                                                                                \
    typedef JointModelTpl<NewScalar, Options> type;                                                \
  }
 
namespace pinocchio
{
 
  template<typename Derived>
  struct JointModelBase : NumericalBase<Derived>
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    typedef typename traits<Derived>::JointDerived JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
 
    JointModelDerived & derived()
    {
      return *static_cast<Derived *>(this);
    }
    const JointModelDerived & derived() const
    {
      return *static_cast<const Derived *>(this);
    }
 
    JointDataDerived createData() const
    {
      return derived().createData();
    }
 
    const std::vector<bool> hasConfigurationLimit() const
    {
      return derived().hasConfigurationLimit();
    }
 
    const std::vector<bool> hasConfigurationLimitInTangent() const
    {
      return derived().hasConfigurationLimitInTangent();
    }
 
    template<typename ConfigVectorType>
    void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
    {
      derived().calc(data, qs.derived());
    }
 
    template<typename TangentVectorType>
    void calc(
      JointDataDerived & data,
      const Blank not_used,
      const Eigen::MatrixBase<TangentVectorType> & vs) const
    {
      derived().calc(data, not_used, vs.derived());
    }
 
    template<typename ConfigVectorType, typename TangentVectorType>
    void calc(
      JointDataDerived & data,
      const Eigen::MatrixBase<ConfigVectorType> & qs,
      const Eigen::MatrixBase<TangentVectorType> & vs) const
    {
      derived().calc(data, qs.derived(), vs.derived());
    }
 
    template<typename VectorLike, typename Matrix6Like>
    void calc_aba(
      JointDataDerived & data,
      const Eigen::MatrixBase<VectorLike> & armature,
      const Eigen::MatrixBase<Matrix6Like> & I,
      const bool update_I = false) const
    {
      derived().calc_aba(
        data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I);
    }
 
    int nv() const
    {
      return derived().nv_impl();
    }
    int nq() const
    {
      return derived().nq_impl();
    }
 
    // Default _impl methods are reimplemented by dynamic-size joints.
    int nv_impl() const
    {
      return NV;
    }
    int nq_impl() const
    {
      return NQ;
    }
 
    int idx_q() const
    {
      return derived().idx_q_impl();
    }
    int idx_v() const
    {
      return derived().idx_v_impl();
    }
    JointIndex id() const
    {
      return derived().id_impl();
    }
 
    int idx_q_impl() const
    {
      return i_q;
    }
    int idx_v_impl() const
    {
      return i_v;
    }
    JointIndex id_impl() const
    {
      return i_id;
    }
 
    void setIndexes(JointIndex id, int q, int v)
    {
      derived().setIndexes_impl(id, q, v);
    }
 
    void setIndexes_impl(JointIndex id, int q, int v)
    {
      i_id = id, i_q = q;
      i_v = v;
    }
 
    void disp(std::ostream & os) const
    {
      using namespace std;
      os << shortname() << endl
         << "  index: " << id() << endl
         << "  index q: " << idx_q() << endl
         << "  index v: " << idx_v() << endl
         << "  nq: " << nq() << endl
         << "  nv: " << nv() << endl;
    }
 
    friend std::ostream & operator<<(std::ostream & os, const JointModelBase<Derived> & joint)
    {
      joint.disp(os);
      return os;
    }
 
    std::string shortname() const
    {
      return derived().shortname();
    }
    static std::string classname()
    {
      return Derived::classname();
    }
 
    template<typename NewScalar>
    typename CastType<NewScalar, Derived>::type cast() const
    {
      return derived().template cast<NewScalar>();
    }
 
    template<class OtherDerived>
    bool operator==(const JointModelBase<OtherDerived> & other) const
    {
      return derived().isEqual(other.derived());
    }
 
    template<class OtherDerived>
    bool operator!=(const JointModelBase<OtherDerived> & other) const
    {
      return !(internal::comparison_eq(derived(), other.derived()));
    }
 
    template<class OtherDerived>
    bool isEqual(const JointModelBase<OtherDerived> &) const
    {
      return false;
    }
 
    bool isEqual(const JointModelBase<Derived> & other) const
    {
      return derived().hasSameIndexes(other.derived());
    }
 
    template<class OtherDerived>
    bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const
    {
      return internal::comparison_eq(other.id(), id())
             && internal::comparison_eq(other.idx_q(), idx_q())
             && internal::comparison_eq(other.idx_v(), idx_v());
    }
 
    /* Acces to dedicated segment in robot config space.  */
    // Const access
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
    jointConfigSelector(const Eigen::MatrixBase<D> & a) const
    {
      return derived().jointConfigSelector_impl(a);
    }
 
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
    jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NQ>::segment(a.derived(), idx_q(), nq());
    }
 
    // Non-const access
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
    jointConfigSelector(Eigen::MatrixBase<D> & a) const
    {
      return derived().jointConfigSelector_impl(a);
    }
 
    template<typename D>
    typename SizeDepType<NQ>::template SegmentReturn<D>::Type
    jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NQ>::segment(a, idx_q(), nq());
    }
 
    /* Acces to dedicated segment in robot config velocity space.  */
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
    jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
    {
      return derived().jointVelocitySelector_impl(a.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
    jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NV>::segment(a.derived(), idx_v(), nv());
    }
 
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::Type
    jointVelocitySelector(Eigen::MatrixBase<D> & a) const
    {
      return derived().jointVelocitySelector_impl(a.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template SegmentReturn<D>::Type
    jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
    {
      return SizeDepType<NV>::segment(a.derived(), idx_v(), nv());
    }
 
    /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::ConstType
    jointCols(const Eigen::MatrixBase<D> & A) const
    {
      return derived().jointCols_impl(A.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::ConstType
    jointCols_impl(const Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv());
    }
 
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
    {
      return derived().jointCols_impl(A.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template ColsReturn<D>::Type
    jointCols_impl(Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv());
    }
 
    /* Acces to dedicated rows in a matrix.*/
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template RowsReturn<D>::ConstType
    jointRows(const Eigen::MatrixBase<D> & A) const
    {
      return derived().jointRows_impl(A.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template RowsReturn<D>::ConstType
    jointRows_impl(const Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv());
    }
 
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template RowsReturn<D>::Type jointRows(Eigen::MatrixBase<D> & A) const
    {
      return derived().jointRows_impl(A.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template RowsReturn<D>::Type
    jointRows_impl(Eigen::MatrixBase<D> & A) const
    {
      return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv());
    }
 
    // Const access
    template<typename D>
    typename SizeDepType<NV>::template BlockReturn<D>::ConstType
    jointBlock(const Eigen::MatrixBase<D> & Mat) const
    {
      return derived().jointBlock_impl(Mat.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template BlockReturn<D>::ConstType
    jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
    {
      return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
    }
 
    // Non-const access
    template<typename D>
    typename SizeDepType<NV>::template BlockReturn<D>::Type
    jointBlock(Eigen::MatrixBase<D> & Mat) const
    {
      return derived().jointBlock_impl(Mat.derived());
    }
 
    template<typename D>
    typename SizeDepType<NV>::template BlockReturn<D>::Type
    jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
    {
      return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
    }
 
  protected:
    inline JointModelBase()
    : i_id(std::numeric_limits<JointIndex>::max())
    , i_q(-1)
    , i_v(-1)
    {
    }
 
    inline JointModelBase(const JointModelBase & clone)
    {
      *this = clone;
    }
 
    inline JointModelBase & operator=(const JointModelBase & clone)
    {
      i_id = clone.i_id;
      i_q = clone.i_q;
      i_v = clone.i_v;
      return *this;
    }
 
    // data
    JointIndex i_id; // ID of the joint in the multibody list.
    int i_q;         // Index of the joint configuration in the joint configuration vector.
    int i_v;         // Index of the joint velocity in the joint velocity vector.
 
  }; // struct JointModelBase
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_multibody_joint_model_base_hpp__