pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
tensor.hpp
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #ifndef __pinocchio_math_tensor_hpp__
6 #define __pinocchio_math_tensor_hpp__
7 
8 #include "pinocchio/fwd.hpp"
9 
10 #if !EIGEN_VERSION_AT_LEAST(3,2,90)
11  #define EIGEN_DEVICE_FUNC
12 #endif
13 
14 #ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
15  #include <unsupported/Eigen/CXX11/Tensor>
16 #else
17  #if (__cplusplus <= 199711L && EIGEN_COMP_MSVC < 1900) || defined(__CUDACC__) || defined(EIGEN_AVOID_STL_ARRAY)
18  namespace Eigen {
19  template <typename T, std::size_t n>
20  struct array
21  {
22  EIGEN_DEVICE_FUNC
23  EIGEN_STRONG_INLINE T& operator[] (size_t index)
24  { return values[index]; }
25  EIGEN_DEVICE_FUNC
26  EIGEN_STRONG_INLINE const T& operator[] (size_t index) const
27  { return values[index]; }
28 
29  EIGEN_DEVICE_FUNC
30  EIGEN_STRONG_INLINE T& front() { return values[0]; }
31  EIGEN_DEVICE_FUNC
32  EIGEN_STRONG_INLINE const T& front() const { return values[0]; }
33 
34  EIGEN_DEVICE_FUNC
35  EIGEN_STRONG_INLINE T& back() { return values[n-1]; }
36  EIGEN_DEVICE_FUNC
37  EIGEN_STRONG_INLINE const T& back() const { return values[n-1]; }
38 
39  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
40  static std::size_t size() { return n; }
41 
42  T values[n];
43  };
44 
45  template<class T, std::size_t n>
46  EIGEN_DEVICE_FUNC bool operator==(const array<T,n> & lhs, const array<T,n> & rhs)
47  {
48  for (std::size_t i = 0; i < n; ++i) {
49  if (lhs[i] != rhs[i]) {
50  return false;
51  }
52  }
53  return true;
54  }
55 
56  template<class T, std::size_t n>
57  EIGEN_DEVICE_FUNC bool operator!=(const array<T,n> & lhs, const array<T,n> & rhs)
58  {
59  return !(lhs == rhs);
60  }
61  } // namespace Eigen
62  #else
63  #include <array>
64  namespace Eigen {
65  template <typename T, std::size_t N> using array = std::array<T, N>;
66  } // namespace Eigen
67  #endif
68 #endif
69 
70 namespace pinocchio
71 {
72 
73 #ifndef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
74 
75  // Mimic the Eigen::Tensor module only available for C++11 and more
76  template<typename Scalar_, int NumIndices_, int Options_ = 0, typename IndexType = Eigen::DenseIndex>
77  struct Tensor
78  {
79  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
80 
81  typedef Scalar_ Scalar;
82  enum
83  {
84  Options = Options_,
85  NumIndices = NumIndices_
86  };
87  typedef IndexType Index;
88  typedef Eigen::array<Index,NumIndices_> Dimensions;
89 
90  inline Tensor& base() { return *this; }
91  inline const Tensor& base() const { return *this; }
92 
93  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
94  Dimensions& dimensions() { return m_dimensions; }
95  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
96  const Dimensions& dimensions() const { return m_dimensions; }
97 
98  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rank() const
99  {
100  return NumIndices;
101  }
102 
103  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index dimension(std::size_t n) const
104  {
105  assert(n <= NumIndices && "n is larger than the dimension of the tensor.");
106  return m_dimensions[n];
107  }
108 
109  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index size() const
110  {
111  return m_storage.size();
112  }
113 
114  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar *data()
115  {
116  return m_storage.data();
117  }
118 
119  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar *data() const
120  {
121  return m_storage.data();
122  }
123 
124  EIGEN_DEVICE_FUNC
125  EIGEN_STRONG_INLINE Tensor& setZero()
126  {
127  return setConstant(Scalar(0));
128  }
129 
130  EIGEN_DEVICE_FUNC
131  EIGEN_STRONG_INLINE Tensor& setConstant(const Scalar & val)
132  {
133  m_storage.setConstant(val);
134  return *this;
135  }
136 
137  EIGEN_DEVICE_FUNC
138  EIGEN_STRONG_INLINE Tensor& setRandom()
139  {
140  m_storage.setRandom();
141  return *this;
142  }
143 
144  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor()
145  : m_storage()
146  {
147  }
148 
149  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(const Tensor & other)
150  : m_storage(other.m_storage)
151  , m_dimensions(other.m_dimensions)
152  {
153  }
154 
155  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Tensor(Index dim1)
156  : m_storage(dim1)
157  {
158  m_dimensions[0] = dim1;
159  EIGEN_STATIC_ASSERT(1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
160  }
161 
162  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2)
163  : m_storage(dim1*dim2)
164  {
165  m_dimensions[0] = dim1;
166  m_dimensions[1] = dim2;
167  EIGEN_STATIC_ASSERT(2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
168  }
169 
170  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3)
171  : m_storage(dim1*dim2*dim3)
172  {
173  m_dimensions[0] = dim1;
174  m_dimensions[1] = dim2;
175  m_dimensions[2] = dim3;
176  EIGEN_STATIC_ASSERT(3 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
177  }
178 
179  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3, Index dim4)
180  : m_storage(dim1*dim2*dim3*dim4)
181  {
182  m_dimensions[0] = dim1;
183  m_dimensions[1] = dim2;
184  m_dimensions[2] = dim3;
185  m_dimensions[3] = dim4;
186  EIGEN_STATIC_ASSERT(4 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
187  }
188 
189  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(Index dim1, Index dim2, Index dim3, Index dim4, Index dim5)
190  : m_storage(dim1*dim2*dim3*dim4*dim5)
191  {
192  m_dimensions[0] = dim1;
193  m_dimensions[1] = dim2;
194  m_dimensions[2] = dim3;
195  m_dimensions[3] = dim4;
196  m_dimensions[4] = dim5;
197  EIGEN_STATIC_ASSERT(5 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
198  }
199 
200  EIGEN_DEVICE_FUNC
201  EIGEN_STRONG_INLINE const Scalar& operator()(Index i0) const
202  {
203  EIGEN_STATIC_ASSERT(1 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
204  return m_storage.coeff(i0);
205  }
206 
207  EIGEN_DEVICE_FUNC
208  EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1) const
209  {
210  EIGEN_STATIC_ASSERT(2 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
211  return m_storage.coeff(i0 + i1 * m_dimensions[0]);
212  }
213 
214  EIGEN_DEVICE_FUNC
215  EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2) const
216  {
217  EIGEN_STATIC_ASSERT(3 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
218  return m_storage.coeff(i0 + i1 * m_dimensions[0] + i2 * m_dimensions[1] * m_dimensions[0]);
219  }
220 
221  EIGEN_DEVICE_FUNC
222  EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3) const
223  {
224  EIGEN_STATIC_ASSERT(4 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
225  return coeff(i0 + i1 * m_dimensions[0] + i2 * m_dimensions[1] * m_dimensions[0] + i3 * m_dimensions[2] * m_dimensions[1] * m_dimensions[0]);
226  }
227 
228  EIGEN_DEVICE_FUNC
229  EIGEN_STRONG_INLINE const Scalar& operator()(Index i0, Index i1, Index i2, Index i3, Index i4) const
230  {
231  EIGEN_STATIC_ASSERT(5 == NumIndices, YOU_MADE_A_PROGRAMMING_MISTAKE)
232  return coeff(i0 + i1 * m_dimensions[0] + i2 * m_dimensions[1] * m_dimensions[0] + i3 * m_dimensions[2] * m_dimensions[1] * m_dimensions[0] + i4 * m_dimensions[3] * m_dimensions[2] * m_dimensions[1] * m_dimensions[0]);
233  }
234 
235  EIGEN_DEVICE_FUNC
236  void resize(const Eigen::array<Index,NumIndices> & dimensions)
237  {
238  size_t i;
239  Index size = Index(1);
240  for(i = 0; i < NumIndices; i++)
241  {
242  Eigen::internal::check_rows_cols_for_overflow<Eigen::Dynamic>::run(size, dimensions[i]);
243  size *= dimensions[i];
244  }
245 
246  for(i = 0; i < NumIndices; i++)
247  m_dimensions[i] = dimensions[i];
248 
249  bool size_changed = size != this->size();
250  if(size_changed) m_storage.resize(size);
251 
252 #ifdef EIGEN_INITIALIZE_COEFFS
253  if(size_changed)
254  {
255 #if defined(EIGEN_INITIALIZE_MATRICES_BY_ZERO)
256  m_storage.fill(Scalar(0));
257 #elif defined(EIGEN_INITIALIZE_MATRICES_BY_NAN)
258  m_storage.fill(std::numeric_limits<Scalar>::quiet_NaN());
259 #endif
260  }
261 #endif
262  }
263 
264  EIGEN_DEVICE_FUNC bool operator==(const Tensor & other) const
265  {
266  return m_storage == other.m_storage;
267  }
268 
269  EIGEN_DEVICE_FUNC bool operator!=(const Tensor & other) const
270  {
271  return m_storage != other.m_storage;
272  }
273 
274  protected:
275 
276  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> StorageType;
277  StorageType m_storage;
278 
279  Dimensions m_dimensions;
280 
281  };
282 
283 #else
284 
285  // Use the default Eigen::Tensor module
286  template<typename Scalar_, int NumIndices_, int Options_ = 0, typename IndexType = Eigen::DenseIndex>
287  using Tensor = Eigen::Tensor<Scalar_,NumIndices_,Options_,IndexType>;
288 
289 #endif // ifndef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
290 
291 }
292 
293 #if !EIGEN_VERSION_AT_LEAST(3,2,90)
294  #undef EIGEN_DEVICE_FUNC
295 #endif
296 
297 #endif // ifndef __pinocchio_math_tensor_hpp__
Eigen
Definition: casadi.hpp:47
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24