pinocchio  3.3.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
matrix.hpp
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_math_matrix_hpp__
6 #define __pinocchio_math_matrix_hpp__
7 
8 #include "pinocchio/macros.hpp"
9 #include "pinocchio/math/fwd.hpp"
10 #include "pinocchio/utils/static-if.hpp"
11 
12 #include <boost/type_traits.hpp>
13 #include <Eigen/Dense>
14 
15 namespace pinocchio
16 {
17 
18  template<typename Derived>
19  inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
20  {
21  return !((m.derived().array() == m.derived().array()).all());
22  }
23 
24  namespace internal
25  {
26  template<
27  typename MatrixLike,
28  bool value = is_floating_point<typename MatrixLike::Scalar>::value>
29  struct isZeroAlgo
30  {
31  typedef typename MatrixLike::Scalar Scalar;
32  typedef typename MatrixLike::RealScalar RealScalar;
33 
34  static bool run(
35  const Eigen::MatrixBase<MatrixLike> & mat,
36  const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
37  {
38  return mat.isZero(prec);
39  }
40  };
41 
42  template<typename MatrixLike>
43  struct isZeroAlgo<MatrixLike, false>
44  {
45  typedef typename MatrixLike::Scalar Scalar;
46  typedef typename MatrixLike::RealScalar RealScalar;
47 
48  static bool run(
49  const Eigen::MatrixBase<MatrixLike> & /*vec*/,
50  const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
51  {
52  PINOCCHIO_UNUSED_VARIABLE(prec);
53  return true;
54  }
55  };
56  } // namespace internal
57 
58  template<typename MatrixLike>
59  inline bool isZero(
60  const Eigen::MatrixBase<MatrixLike> & m,
61  const typename MatrixLike::RealScalar & prec =
62  Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
63  {
64  return internal::isZeroAlgo<MatrixLike>::run(m, prec);
65  }
66 
67  template<typename M1, typename M2>
69  {
70 #if EIGEN_VERSION_AT_LEAST(3, 2, 90)
71  typedef typename Eigen::Product<M1, M2> type;
72 #else
73  typedef typename Eigen::ProductReturnType<M1, M2>::Type type;
74 #endif
75  };
76 
77  template<typename Scalar, typename Matrix>
79  {
80 #if EIGEN_VERSION_AT_LEAST(3, 3, 0)
81  typedef Eigen::CwiseBinaryOp<
82  EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) < Scalar,
83  typename Eigen::internal::traits<Matrix>::Scalar>,
84  const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type,
85  const Matrix > type;
86 #elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
87  typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
88 #else
89  typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix>
90  type;
91 #endif
92  };
93 
94  template<typename Matrix, typename Scalar>
96  {
97 #if EIGEN_VERSION_AT_LEAST(3, 3, 0)
98  typedef Eigen::CwiseBinaryOp<
99  EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_, product), _op) <
100  typename Eigen::internal::traits<Matrix>::Scalar,
101  Scalar>,
102  const Matrix,
103  const typename Eigen::internal::plain_constant_type<Matrix, Scalar>::type > type;
104 #elif EIGEN_VERSION_AT_LEAST(3, 2, 90)
105  typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
106 #else
107  typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix>
108  type;
109 #endif
110  };
111 
112  namespace internal
113  {
114  template<
115  typename MatrixLike,
117  struct isUnitaryAlgo
118  {
119  typedef typename MatrixLike::Scalar Scalar;
120  typedef typename MatrixLike::RealScalar RealScalar;
121 
122  static bool run(
123  const Eigen::MatrixBase<MatrixLike> & mat,
124  const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
125  {
126  return mat.isUnitary(prec);
127  }
128  };
129 
130  template<typename MatrixLike>
131  struct isUnitaryAlgo<MatrixLike, false>
132  {
133  typedef typename MatrixLike::Scalar Scalar;
134  typedef typename MatrixLike::RealScalar RealScalar;
135 
136  static bool run(
137  const Eigen::MatrixBase<MatrixLike> & /*vec*/,
138  const RealScalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
139  {
140  PINOCCHIO_UNUSED_VARIABLE(prec);
141  return true;
142  }
143  };
144  } // namespace internal
145 
154  template<typename MatrixLike>
155  inline bool isUnitary(
156  const Eigen::MatrixBase<MatrixLike> & mat,
157  const typename MatrixLike::RealScalar & prec =
158  Eigen::NumTraits<typename MatrixLike::Scalar>::dummy_precision())
159  {
160  return internal::isUnitaryAlgo<MatrixLike>::run(mat, prec);
161  }
162 
163  namespace internal
164  {
165  template<
166  typename VectorLike,
167  bool value = is_floating_point<typename VectorLike::Scalar>::value>
168  struct isNormalizedAlgo
169  {
170  typedef typename VectorLike::Scalar Scalar;
171  typedef typename VectorLike::RealScalar RealScalar;
172 
173  static bool run(
174  const Eigen::MatrixBase<VectorLike> & vec,
175  const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
176  {
177  return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec;
178  }
179  };
180 
181  template<typename VectorLike>
182  struct isNormalizedAlgo<VectorLike, false>
183  {
184  typedef typename VectorLike::Scalar Scalar;
185  typedef typename VectorLike::RealScalar RealScalar;
186 
187  static bool run(
188  const Eigen::MatrixBase<VectorLike> & /*vec*/,
189  const RealScalar & prec = Eigen::NumTraits<RealScalar>::dummy_precision())
190  {
191  PINOCCHIO_UNUSED_VARIABLE(prec);
192  return true;
193  }
194  };
195  } // namespace internal
196 
205  template<typename VectorLike>
206  inline bool isNormalized(
207  const Eigen::MatrixBase<VectorLike> & vec,
208  const typename VectorLike::RealScalar & prec =
209  Eigen::NumTraits<typename VectorLike::Scalar>::dummy_precision())
210  {
211  EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
212  return internal::isNormalizedAlgo<VectorLike>::run(vec, prec);
213  }
214 
215  namespace internal
216  {
217  template<
218  typename VectorLike,
219  bool value = is_floating_point<typename VectorLike::Scalar>::value>
220  struct normalizeAlgo
221  {
222  static void run(const Eigen::MatrixBase<VectorLike> & vec)
223  {
224  return vec.const_cast_derived().normalize();
225  }
226  };
227 
228  template<typename VectorLike>
229  struct normalizeAlgo<VectorLike, false>
230  {
231  static void run(const Eigen::MatrixBase<VectorLike> & vec)
232  {
233  using namespace internal;
234  typedef typename VectorLike::RealScalar RealScalar;
235  typedef typename VectorLike::Scalar Scalar;
236  const RealScalar z = vec.squaredNorm();
237  const Scalar sqrt_z = if_then_else(GT, z, Scalar(0), math::sqrt(z), Scalar(1));
238  vec.const_cast_derived() /= sqrt_z;
239  }
240  };
241  } // namespace internal
242 
248  template<typename VectorLike>
249  inline void normalize(const Eigen::MatrixBase<VectorLike> & vec)
250  {
251  EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
252  internal::normalizeAlgo<VectorLike>::run(vec.const_cast_derived());
253  }
254 
255  namespace internal
256  {
257  template<typename Scalar>
258  struct CallCorrectMatrixInverseAccordingToScalar
259  {
260  template<typename MatrixIn, typename MatrixOut>
261  static void
262  run(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest)
263  {
264  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
265  dest_.noalias() = m_in.inverse();
266  }
267  };
268 
269  } // namespace internal
270 
271  template<typename MatrixIn, typename MatrixOut>
272  inline void
273  inverse(const Eigen::MatrixBase<MatrixIn> & m_in, const Eigen::MatrixBase<MatrixOut> & dest)
274  {
275  MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest);
276  internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(
277  m_in, dest_);
278  }
279 
280 } // namespace pinocchio
281 
282 #endif // #ifndef __pinocchio_math_matrix_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition: matrix.hpp:155
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.