pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
rotation.hpp
1 //
2 // Copyright (c) 2019-2020 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_math_rotation_hpp__
6 #define __pinocchio_math_rotation_hpp__
7 
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/math/matrix.hpp"
10 
11 #include <Eigen/Core>
12 #include <Eigen/Geometry>
13 #include <Eigen/SVD>
14 
15 namespace pinocchio
16 {
23  template<typename Vector3, typename Scalar, typename Matrix3>
24  void toRotationMatrix(const Eigen::MatrixBase<Vector3> & axis,
25  const Scalar & cos_value, const Scalar & sin_value,
26  const Eigen::MatrixBase<Matrix3> & res)
27  {
28  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
29  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
30 
31  assert(isUnitary(axis) && "The axis is not unitary.");
32 
33  Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res);
34  Vector3 sin_axis = sin_value * axis;
35  Vector3 cos1_axis = (Scalar(1)-cos_value) * axis;
36 
37  Scalar tmp;
38  tmp = cos1_axis.x() * axis.y();
39  res_.coeffRef(0,1) = tmp - sin_axis.z();
40  res_.coeffRef(1,0) = tmp + sin_axis.z();
41 
42  tmp = cos1_axis.x() * axis.z();
43  res_.coeffRef(0,2) = tmp + sin_axis.y();
44  res_.coeffRef(2,0) = tmp - sin_axis.y();
45 
46  tmp = cos1_axis.y() * axis.z();
47  res_.coeffRef(1,2) = tmp - sin_axis.x();
48  res_.coeffRef(2,1) = tmp + sin_axis.x();
49 
50  res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
51  }
52 
58  template<typename Matrix3>
59  void normalizeRotation(const Eigen::MatrixBase<Matrix3> & rot)
60  {
61  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
62  Matrix3 & rot_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,rot);
63 
64  typedef typename Matrix3::Scalar Scalar;
65  enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options };
66  typedef Eigen::Quaternion<Scalar,Options> Quaternion;
67  Quaternion quat(rot); quat.normalize();
68  rot_ = quat.toRotationMatrix();
69  }
70 
78  template<typename Matrix3>
79  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
80  orthogonalProjection(const Eigen::MatrixBase<Matrix3> & mat)
81  {
82  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
83  typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) ReturnType;
84 
85  typedef Eigen::JacobiSVD<Matrix3> SVD;
86  const SVD svd(mat,Eigen::ComputeFullU | Eigen::ComputeFullV);
87 
88  ReturnType res;
89  res.template leftCols<2>().noalias() = svd.matrixU() * svd.matrixV().transpose().template leftCols<2>();
90  res.col(2).noalias() = res.col(0).cross(res.col(1));
91  return res;
92  }
93 }
94 
95 #endif //#ifndef __pinocchio_math_rotation_hpp__
96 
void normalizeRotation(const Eigen::MatrixBase< Matrix3 > &rot)
Orthogonormalization procedure for a rotation matrix (closed enough to SO(3)).
Definition: rotation.hpp:59
Definition: casadi.hpp:47
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:100
Matrix3 orthogonalProjection(const Eigen::MatrixBase< Matrix3 > &mat)
Orthogonal projection of a matrix on the SO(3) manifold.
Definition: rotation.hpp:80
Main pinocchio namespace.
Definition: treeview.dox:24
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:24