GCC Code Coverage Report
 Directory: ./ Exec Total Coverage File: include/pinocchio/spatial/se3-base.hpp Lines: 34 36 94.4 % Date: 2024-04-26 13:14:21 Branches: 0 0 - %

 Line Branch Exec Source 1 // 2 // Copyright (c) 2015-2020 CNRS INRIA 3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. 4 // 5 6 #ifndef __pinocchio_se3_base_hpp__ 7 #define __pinocchio_se3_base_hpp__ 8 9 namespace pinocchio 10 { 11  /** \brief Base class for rigid transformation. 12  * 13  * The rigid transform aMb can be seen in two ways: 14  * 15  * - given a point p expressed in frame B by its coordinate vector \f$^bp \f$, \f$^aM_b \f$ 16  * computes its coordinates in frame A by \f$^ap = {}^aM_b {}^bp \f$. 17  * - \f$^aM_b \f$ displaces a solid S centered at frame A into the solid centered in 18  * B. In particular, the origin of A is displaced at the origin of B: 19  * \f$^aM_b {}^aA = {}^aB \f$. 20 21  * The rigid displacement is stored as a rotation matrix and translation vector by: 22  * \f$^aM_b x = {}^aR_b x + {}^aAB \f$ 23  * where \f$^aAB\f$ is the vector from origin A to origin B expressed in coordinates A. 24  * 25  * \cheatsheet \f${}^aM_c = {}^aM_b {}^bM_c \f$ 26  * 27  * \ingroup pinocchio_spatial 28  */ 29  template 30  struct SE3Base 31  { 32  PINOCCHIO_SE3_TYPEDEF_TPL(Derived); 33 34 717611  Derived & derived() { return *static_cast(this); } 35 8872374  const Derived& derived() const { return *static_cast(this); } 36 37 3770010  ConstAngularRef rotation() const { return derived().rotation_impl(); } 38 3296472  ConstLinearRef translation() const { return derived().translation_impl(); } 39 568481  AngularRef rotation() { return derived().rotation_impl(); } 40 97551  LinearRef translation() { return derived().translation_impl(); } 41 29023  void rotation(const AngularType & R) { derived().rotation_impl(R); } 42 22556  void translation(const LinearType & t) { derived().translation_impl(t); } 43 44 5  HomogeneousMatrixType toHomogeneousMatrix() const 45  { 46 5  return derived().toHomogeneousMatrix_impl(); 47  } 48 4  operator HomogeneousMatrixType() const { return toHomogeneousMatrix(); } 49 50  /** 51  * @brief The action matrix \f${}^aX_b \f$ of \f${}^aM_b \f$. 52  * 53  * With \f${}^aM_b = \left( \begin{array}{cc} R & t \\ 0 & 1 \\ \end{array} \right) \f$, 54  * \f[ 55  * {}^aX_b = \left( \begin{array}{cc} R & \hat{t} R \\ 0 & R \\ \end{array} \right) 56  * \f] 57  * 58  * \cheatsheet \f${}^a\nu_c = {}^aX_b {}^b\nu_c \f$ 59  */ 60 3853  ActionMatrixType toActionMatrix() const 61  { 62 3853  return derived().toActionMatrix_impl(); 63  } 64 10  operator ActionMatrixType() const { return toActionMatrix(); } 65 66  /** 67  * @brief The action matrix \f${}^bX_a \f$ of \f${}^aM_b \f$. 68  * \sa toActionMatrix() 69  */ 70 15  ActionMatrixType toActionMatrixInverse() const 71  { 72 15  return derived().toActionMatrixInverse_impl(); 73  } 74 75 88  ActionMatrixType toDualActionMatrix() const 76 88  { return derived().toDualActionMatrix_impl(); } 77 78 4  void disp(std::ostream & os) const 79  { 80 4  static_cast(this)->disp_impl(os); 81 4  } 82 83  typename SE3GroupAction::ReturnType 84 1413723  operator*(const Derived & m2) const 85 1413723  { return derived().__mult__(m2); } 86 87  /// ay = aXb.act(by) 88  template 89  typename SE3GroupAction::ReturnType 90 287243  act(const D & d) const 91  { 92 287243  return derived().act_impl(d); 93  } 94 95  /// by = aXb.actInv(ay) 96  template typename SE3GroupAction::ReturnType 97 172179  actInv(const D & d) const 98  { 99 172179  return derived().actInv_impl(d); 100  } 101 102 13365  bool operator==(const Derived & other) const 103 13365  { return derived().isEqual(other); } 104 105  bool operator!=(const Derived & other) const 106  { return !(*this == other); } 107 108 55775  bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits::dummy_precision()) const 109  { 110 55775  return derived().isApprox_impl(other, prec); 111  } 112 113 4  friend std::ostream & operator <<(std::ostream & os,const SE3Base & X) 114  { 115 4  X.disp(os); 116 4  return os; 117  } 118 119  /// 120  /// \returns true if *this is approximately equal to the identity placement, within the precision given by prec. 121  /// 122  bool isIdentity(const typename traits::Scalar & prec = Eigen::NumTraits::Scalar>::dummy_precision()) const 123  { 124  return derived().isIdentity(prec); 125  } 126 127  /// 128  /// \returns true if the rotational part of *this is a rotation matrix (normalized columns), within the precision given by prec. 129  /// 130  bool isNormalized(const Scalar & prec = Eigen::NumTraits::dummy_precision()) const 131  { 132  return derived().isNormalized(prec); 133  } 134 135  /// 136  /// \brief Normalize *this in such a way the rotation part of *this lies on SO(3). 137  /// 138  void normalize() 139  { 140  derived().normalize(); 141  } 142 143  /// 144  /// \returns a Normalized version of *this, in such a way the rotation part of the returned transformation lies on SO(3). 145  /// 146  PlainType normalized() const 147  { 148  derived().normalized(); 149  } 150 151  }; // struct SE3Base 152 153 } // namespace pinocchio 154 155 #endif // ifndef __pinocchio_se3_base_hpp__

 Generated by: GCOVR (Version 4.2)