Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/multibody/joint/joint-model-base.hpp	Lines:	102	102	100.0 %
Date:	2024-01-23 21:41:47	Branches:	26	50	52.0 %


//
// 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
  {
    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 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 Matrix6Type>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Type> & I,
                  const bool update_I = false) const
    {
      derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,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 !(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 other.id() == id()
      && other.idx_q() == idx_q()
      && 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()); }

    /// \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(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:

    /// Default constructor: protected.
    ///
    /// Prevent the construction of stand-alone JointModelBase.
    inline JointModelBase()
    : i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}

    /// Copy constructor: protected.
    ///
    /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
    /// objects. Copy is done by calling copy operator.
    inline JointModelBase(const JointModelBase & clone)
    { *this = clone; }

    /// Copy operator: protected.
    ///
    /// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
    /// objects.
    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__



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2019 CNRS INRIA
3			// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#ifndef __pinocchio_multibody_joint_model_base_hpp__
7			#define __pinocchio_multibody_joint_model_base_hpp__
8
9			#include "pinocchio/multibody/joint/joint-base.hpp"
10			#include "pinocchio/multibody/joint/joint-common-operations.hpp"
11
12			#include "pinocchio/math/matrix-block.hpp"
13
14			#include <limits>
15
16			#define PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,TYPENAME) \
17			typedef Eigen::DenseIndex Index; \
18			typedef TYPENAME traits<Joint>::Scalar Scalar; \
19			typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \
20			typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \
21			typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \
22			typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \
23			typedef TYPENAME traits<Joint>::Motion_t Motion_t; \
24			typedef TYPENAME traits<Joint>::Bias_t Bias_t; \
25			typedef TYPENAME traits<Joint>::U_t U_t; \
26			typedef TYPENAME traits<Joint>::D_t D_t; \
27			typedef TYPENAME traits<Joint>::UD_t UD_t; \
28			enum { \
29			Options = traits<Joint>::Options, \
30			NQ = traits<Joint>::NQ, \
31			NV = traits<Joint>::NV \
32			}; \
33			typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \
34			typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t
35
36			#ifdef __clang__
37
38			#define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
39			#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
40
41			#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
42
43			#define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
44			#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
45
46			#else
47
48			#define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
49			#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
50
51			#endif
52
53			#define PINOCCHIO_JOINT_USE_INDEXES(Joint) \
54			typedef JointModelBase<Joint> Base; \
55			using Base::idx_q; \
56			using Base::idx_v
57
58			#define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \
59			template<typename Scalar, int Options, typename NewScalar> \
60			struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
61			{ typedef JointModelTpl<NewScalar,Options> type; }
62
63			namespace pinocchio
64			{
65
66			template<typename Derived>
67			struct JointModelBase
68			{
69			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70
71			typedef typename traits<Derived>::JointDerived JointDerived;
72			PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
73
74		20126	JointModelDerived & derived() { return static_cast<Derived>(this); }
75		26167980	const JointModelDerived & derived() const { return static_cast<const Derived>(this); }
76
77		20162	JointDataDerived createData() const { return derived().createData(); }
78
79		6	const std::vector<bool> hasConfigurationLimit() const
80			{
81		6	return derived().hasConfigurationLimit();
82			}
83
84		6	const std::vector<bool> hasConfigurationLimitInTangent() const
85			{
86		6	return derived().hasConfigurationLimitInTangent();
87			}
88
89			template<typename ConfigVectorType>
90		801132	void calc(JointDataDerived & data,
91			const Eigen::MatrixBase<ConfigVectorType> & qs) const
92			{
93		801132	derived().calc(data,qs.derived());
94		801132	}
95
96			template<typename ConfigVectorType, typename TangentVectorType>
97		89670	void calc(JointDataDerived & data,
98			const Eigen::MatrixBase<ConfigVectorType> & qs,
99			const Eigen::MatrixBase<TangentVectorType> & vs) const
100			{
101		89670	derived().calc(data,qs.derived(),vs.derived());
102		89670	}
103
104			template<typename Matrix6Type>
105		13944	void calc_aba(JointDataDerived & data,
106			const Eigen::MatrixBase<Matrix6Type> & I,
107			const bool update_I = false) const
108			{
109		13944	derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I);
110		13944	}
111
112		27164940	int nv() const { return derived().nv_impl(); }
113		2130791	int nq() const { return derived().nq_impl(); }
114
115			// Default _impl methods are reimplemented by dynamic-size joints.
116		14289268	int nv_impl() const { return NV; }
117		1177577	int nq_impl() const { return NQ; }
118
119		4974134	int idx_q() const { return derived().idx_q_impl(); }
120		6003922	int idx_v() const { return derived().idx_v_impl(); }
121		506419	JointIndex id() const { return derived().id_impl(); }
122
123		3132538	int idx_q_impl() const { return i_q; }
124		3295516	int idx_v_impl() const { return i_v; }
125		504707	JointIndex id_impl() const { return i_id; }
126
127		20124	void setIndexes(JointIndex id, int q, int v)
128		20124	{ derived().setIndexes_impl(id, q, v); }
129
130		19988	void setIndexes_impl(JointIndex id,int q,int v)
131		19988	{ i_id = id, i_q = q; i_v = v; }
132
133		72	void disp(std::ostream & os) const
134			{
135			using namespace std;
136			os
137	✓✗✓✗	144	<< shortname() << endl
138	✓✗✓✗ ✓✗✓✗	72	<< " index: " << id() << endl
139	✓✗✓✗ ✓✗✓✗	72	<< " index q: " << idx_q() << endl
140	✓✗✓✗ ✓✗✓✗	72	<< " index v: " << idx_v() << endl
141	✓✗✓✗ ✓✗✓✗	72	<< " nq: " << nq() << endl
142	✓✗✓✗ ✓✗✓✗	72	<< " nv: " << nv() << endl
143			;
144		72	}
145
146		72	friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
147			{
148		72	joint.disp(os);
149		72	return os;
150			}
151
152		1134	std::string shortname() const { return derived().shortname(); }
153		22	static std::string classname() { return Derived::classname(); }
154
155			template<typename NewScalar>
156		442	typename CastType<NewScalar,Derived>::type cast() const
157		442	{ return derived().template cast<NewScalar>(); }
158
159			template <class OtherDerived>
160		3432	bool operator==(const JointModelBase<OtherDerived> & other) const
161		3432	{ return derived().isEqual(other.derived()); }
162
163			template <class OtherDerived>
164		47	bool operator!=(const JointModelBase<OtherDerived> & other) const
165		47	{ return !(derived() == other.derived()); }
166
167			template <class OtherDerived>
168		4	bool isEqual(const JointModelBase<OtherDerived> &) const
169		4	{ return false; }
170
171		6138	bool isEqual(const JointModelBase<Derived> & other) const
172			{
173		6138	return derived().hasSameIndexes(other.derived());
174			}
175
176			template <class OtherDerived>
177		6178	bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const
178			{
179		6178	return other.id() == id()
180	✓✗	6086	&& other.idx_q() == idx_q()
181	✓✓✓✗	12264	&& other.idx_v() == idx_v();
182			}
183
184			/* Acces to dedicated segment in robot config space. */
185			// Const access
186			template<typename D>
187			typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
188		121262	jointConfigSelector(const Eigen::MatrixBase<D> & a) const
189		121262	{ return derived().jointConfigSelector_impl(a); }
190
191			template<typename D>
192			typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
193		121250	jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
194		121250	{ return SizeDepType<NQ>::segment(a.derived(),idx_q(),nq()); }
195
196			// Non-const access
197			template<typename D>
198			typename SizeDepType<NQ>::template SegmentReturn<D>::Type
199		136458	jointConfigSelector(Eigen::MatrixBase<D> & a) const
200		136458	{ return derived().jointConfigSelector_impl(a); }
201
202			template<typename D>
203			typename SizeDepType<NQ>::template SegmentReturn<D>::Type
204		136452	jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
205		136452	{ return SizeDepType<NQ>::segment(a,idx_q(),nq()); }
206
207			/* Acces to dedicated segment in robot config velocity space. */
208			// Const access
209			template<typename D>
210			typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
211		159610	jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
212		159610	{ return derived().jointVelocitySelector_impl(a.derived()); }
213
214			template<typename D>
215			typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
216		159608	jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
217		159608	{ return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
218
219			// Non-const access
220			template<typename D>
221			typename SizeDepType<NV>::template SegmentReturn<D>::Type
222		173132	jointVelocitySelector(Eigen::MatrixBase<D> & a) const
223		173132	{ return derived().jointVelocitySelector_impl(a.derived()); }
224
225			template<typename D>
226			typename SizeDepType<NV>::template SegmentReturn<D>::Type
227		173132	jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
228		173132	{ return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
229
230			/* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
231			// Const access
232			template<typename D>
233			typename SizeDepType<NV>::template ColsReturn<D>::ConstType
234		770	jointCols(const Eigen::MatrixBase<D>& A) const
235		770	{ return derived().jointCols_impl(A.derived()); }
236
237			template<typename D>
238			typename SizeDepType<NV>::template ColsReturn<D>::ConstType
239		770	jointCols_impl(const Eigen::MatrixBase<D>& A) const
240		770	{ return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
241
242			// Non-const access
243			template<typename D>
244			typename SizeDepType<NV>::template ColsReturn<D>::Type
245		203456	jointCols(Eigen::MatrixBase<D>& A) const
246		203456	{ return derived().jointCols_impl(A.derived()); }
247
248			template<typename D>
249			typename SizeDepType<NV>::template ColsReturn<D>::Type
250		203456	jointCols_impl(Eigen::MatrixBase<D> & A) const
251		203456	{ return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
252
253			/* Acces to dedicated rows in a matrix.*/
254			// Const access
255			template<typename D>
256			typename SizeDepType<NV>::template RowsReturn<D>::ConstType
257		36	jointRows(const Eigen::MatrixBase<D> & A) const
258		36	{ return derived().jointRows_impl(A.derived()); }
259
260			template<typename D>
261			typename SizeDepType<NV>::template RowsReturn<D>::ConstType
262		36	jointRows_impl(const Eigen::MatrixBase<D> & A) const
263		36	{ return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
264
265			// Non-const access
266			template<typename D>
267			typename SizeDepType<NV>::template RowsReturn<D>::Type
268		72	jointRows(Eigen::MatrixBase<D> & A) const
269		72	{ return derived().jointRows_impl(A.derived()); }
270
271			template<typename D>
272			typename SizeDepType<NV>::template RowsReturn<D>::Type
273		72	jointRows_impl(Eigen::MatrixBase<D> & A) const
274		72	{ return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
275
276			/// \brief Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat
277			// Const access
278			template<typename D>
279			typename SizeDepType<NV>::template BlockReturn<D>::ConstType
280			jointBlock(const Eigen::MatrixBase<D> & Mat) const
281			{ return derived().jointBlock_impl(Mat.derived()); }
282
283			template<typename D>
284			typename SizeDepType<NV>::template BlockReturn<D>::ConstType
285			jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
286			{ return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
287
288			// Non-const access
289			template<typename D>
290			typename SizeDepType<NV>::template BlockReturn<D>::Type
291		504	jointBlock(Eigen::MatrixBase<D> & Mat) const
292		504	{ return derived().jointBlock_impl(Mat.derived()); }
293
294			template<typename D>
295			typename SizeDepType<NV>::template BlockReturn<D>::Type
296		504	jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
297		504	{ return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
298
299			protected:
300
301			/// Default constructor: protected.
302			///
303			/// Prevent the construction of stand-alone JointModelBase.
304		40572	inline JointModelBase()
305		40572	: i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}
306
307			/// Copy constructor: protected.
308			///
309			/// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
310			/// objects. Copy is done by calling copy operator.
311		155334	inline JointModelBase(const JointModelBase & clone)
312		155334	{ *this = clone; }
313
314			/// Copy operator: protected.
315			///
316			/// Copy of stand-alone JointModelBase are prevented, but can be used from inhereting
317			/// objects.
318		77964	inline JointModelBase & operator=(const JointModelBase & clone)
319			{
320		77964	i_id = clone.i_id;
321		77964	i_q = clone.i_q;
322		77964	i_v = clone.i_v;
323		77964	return *this;
324			}
325
326			// data
327			JointIndex i_id; // ID of the joint in the multibody list.
328			int i_q; // Index of the joint configuration in the joint configuration vector.
329			int i_v; // Index of the joint velocity in the joint velocity vector.
330
331			}; // struct JointModelBase
332
333			} // namespace pinocchio
334
335			#endif // ifndef __pinocchio_multibody_joint_model_base_hpp__
336