| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
| 3 |
|
|
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
| 4 |
|
|
// |
| 5 |
|
|
|
| 6 |
|
|
#ifndef __pinocchio_spatial_se3_base_hpp__ |
| 7 |
|
|
#define __pinocchio_spatial_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<class Derived> |
| 30 |
|
|
struct SE3Base : NumericalBase<Derived> |
| 31 |
|
|
{ |
| 32 |
|
|
PINOCCHIO_SE3_TYPEDEF_TPL(Derived); |
| 33 |
|
|
|
| 34 |
|
1681914 |
Derived & derived() |
| 35 |
|
|
{ |
| 36 |
|
1681914 |
return *static_cast<Derived *>(this); |
| 37 |
|
|
} |
| 38 |
|
18618519 |
const Derived & derived() const |
| 39 |
|
|
{ |
| 40 |
|
18649073 |
return *static_cast<const Derived *>(this); |
| 41 |
|
|
} |
| 42 |
|
|
|
| 43 |
|
|
Derived & const_cast_derived() const |
| 44 |
|
|
{ |
| 45 |
|
|
return *const_cast<Derived *>(&derived()); |
| 46 |
|
|
} |
| 47 |
|
|
|
| 48 |
|
8308788 |
ConstAngularRef rotation() const |
| 49 |
|
|
{ |
| 50 |
|
8308788 |
return derived().rotation_impl(); |
| 51 |
|
|
} |
| 52 |
|
7643939 |
ConstLinearRef translation() const |
| 53 |
|
|
{ |
| 54 |
|
7685040 |
return derived().translation_impl(); |
| 55 |
|
|
} |
| 56 |
|
1145327 |
AngularRef rotation() |
| 57 |
|
|
{ |
| 58 |
|
1165956 |
return derived().rotation_impl(); |
| 59 |
|
|
} |
| 60 |
|
449726 |
LinearRef translation() |
| 61 |
|
|
{ |
| 62 |
|
449850 |
return derived().translation_impl(); |
| 63 |
|
|
} |
| 64 |
|
36199 |
void rotation(const AngularType & R) |
| 65 |
|
|
{ |
| 66 |
|
36218 |
derived().rotation_impl(R); |
| 67 |
|
36218 |
} |
| 68 |
|
29872 |
void translation(const LinearType & t) |
| 69 |
|
|
{ |
| 70 |
|
29886 |
derived().translation_impl(t); |
| 71 |
|
29886 |
} |
| 72 |
|
|
|
| 73 |
|
74 |
HomogeneousMatrixType toHomogeneousMatrix() const |
| 74 |
|
|
{ |
| 75 |
|
74 |
return derived().toHomogeneousMatrix_impl(); |
| 76 |
|
|
} |
| 77 |
|
4 |
operator HomogeneousMatrixType() const |
| 78 |
|
|
{ |
| 79 |
|
4 |
return toHomogeneousMatrix(); |
| 80 |
|
|
} |
| 81 |
|
|
|
| 82 |
|
|
/** |
| 83 |
|
|
* @brief The action matrix \f$ {}^aX_b \f$ of \f$ {}^aM_b \f$. |
| 84 |
|
|
* |
| 85 |
|
|
* With \f$ {}^aM_b = \left( \begin{array}{cc} R & t \\ 0 & 1 \\ \end{array} \right) \f$, |
| 86 |
|
|
* \f[ |
| 87 |
|
|
* {}^aX_b = \left( \begin{array}{cc} R & \hat{t} R \\ 0 & R \\ \end{array} \right) |
| 88 |
|
|
* \f] |
| 89 |
|
|
* |
| 90 |
|
|
* \cheatsheet \f$ {}^a\nu_c = {}^aX_b {}^b\nu_c \f$ |
| 91 |
|
|
*/ |
| 92 |
|
6140 |
ActionMatrixType toActionMatrix() const |
| 93 |
|
|
{ |
| 94 |
|
6140 |
return derived().toActionMatrix_impl(); |
| 95 |
|
|
} |
| 96 |
|
10 |
operator ActionMatrixType() const |
| 97 |
|
|
{ |
| 98 |
|
10 |
return toActionMatrix(); |
| 99 |
|
|
} |
| 100 |
|
|
|
| 101 |
|
|
template<typename Matrix6Like> |
| 102 |
|
|
void toActionMatrix(const Eigen::MatrixBase<Matrix6Like> & action_matrix) const |
| 103 |
|
|
{ |
| 104 |
|
|
derived().toActionMatrix_impl(action_matrix); |
| 105 |
|
|
} |
| 106 |
|
|
|
| 107 |
|
|
/** |
| 108 |
|
|
* @brief The action matrix \f$ {}^bX_a \f$ of \f$ {}^aM_b \f$. |
| 109 |
|
|
* \sa toActionMatrix() |
| 110 |
|
|
*/ |
| 111 |
|
75 |
ActionMatrixType toActionMatrixInverse() const |
| 112 |
|
|
{ |
| 113 |
|
75 |
return derived().toActionMatrixInverse_impl(); |
| 114 |
|
|
} |
| 115 |
|
|
|
| 116 |
|
|
template<typename Matrix6Like> |
| 117 |
|
✗ |
void toActionMatrixInverse(const Eigen::MatrixBase<Matrix6Like> & action_matrix_inverse) const |
| 118 |
|
|
{ |
| 119 |
|
✗ |
derived().toActionMatrixInverse_impl(action_matrix_inverse.const_cast_derived()); |
| 120 |
|
|
} |
| 121 |
|
|
|
| 122 |
|
268 |
ActionMatrixType toDualActionMatrix() const |
| 123 |
|
|
{ |
| 124 |
|
268 |
return derived().toDualActionMatrix_impl(); |
| 125 |
|
|
} |
| 126 |
|
|
|
| 127 |
|
100038 |
void disp(std::ostream & os) const |
| 128 |
|
|
{ |
| 129 |
|
100038 |
static_cast<const Derived *>(this)->disp_impl(os); |
| 130 |
|
100038 |
} |
| 131 |
|
|
|
| 132 |
|
|
template<typename Matrix6Like> |
| 133 |
|
|
void toDualActionMatrix(const Eigen::MatrixBase<Matrix6Like> & dual_action_matrix) const |
| 134 |
|
|
{ |
| 135 |
|
|
derived().toDualActionMatrix_impl(dual_action_matrix); |
| 136 |
|
|
} |
| 137 |
|
|
|
| 138 |
|
1995129 |
typename SE3GroupAction<Derived>::ReturnType operator*(const Derived & m2) const |
| 139 |
|
|
{ |
| 140 |
|
1995129 |
return derived().__mult__(m2); |
| 141 |
|
|
} |
| 142 |
|
|
|
| 143 |
|
|
/// ay = aXb.act(by) |
| 144 |
|
|
template<typename D> |
| 145 |
|
697434 |
typename SE3GroupAction<D>::ReturnType act(const D & d) const |
| 146 |
|
|
{ |
| 147 |
|
697434 |
return derived().act_impl(d); |
| 148 |
|
|
} |
| 149 |
|
|
|
| 150 |
|
|
/// by = aXb.actInv(ay) |
| 151 |
|
|
template<typename D> |
| 152 |
|
206406 |
typename SE3GroupAction<D>::ReturnType actInv(const D & d) const |
| 153 |
|
|
{ |
| 154 |
|
206406 |
return derived().actInv_impl(d); |
| 155 |
|
|
} |
| 156 |
|
|
|
| 157 |
|
13419 |
bool operator==(const Derived & other) const |
| 158 |
|
|
{ |
| 159 |
|
13419 |
return derived().isEqual(other); |
| 160 |
|
|
} |
| 161 |
|
|
|
| 162 |
|
1 |
bool operator!=(const Derived & other) const |
| 163 |
|
|
{ |
| 164 |
|
1 |
return !(*this == other); |
| 165 |
|
|
} |
| 166 |
|
|
|
| 167 |
|
65282 |
bool isApprox( |
| 168 |
|
|
const Derived & other, |
| 169 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
| 170 |
|
|
{ |
| 171 |
|
65282 |
return derived().isApprox_impl(other, prec); |
| 172 |
|
|
} |
| 173 |
|
|
|
| 174 |
|
100038 |
friend std::ostream & operator<<(std::ostream & os, const SE3Base<Derived> & X) |
| 175 |
|
|
{ |
| 176 |
|
100038 |
X.disp(os); |
| 177 |
|
100038 |
return os; |
| 178 |
|
|
} |
| 179 |
|
|
|
| 180 |
|
|
/// |
| 181 |
|
|
/// \returns true if *this is approximately equal to the identity placement, within the |
| 182 |
|
|
/// precision given by prec. |
| 183 |
|
|
/// |
| 184 |
|
|
bool isIdentity( |
| 185 |
|
|
const typename traits<Derived>::Scalar & prec = |
| 186 |
|
|
Eigen::NumTraits<typename traits<Derived>::Scalar>::dummy_precision()) const |
| 187 |
|
|
{ |
| 188 |
|
|
return derived().isIdentity(prec); |
| 189 |
|
|
} |
| 190 |
|
|
|
| 191 |
|
|
/// |
| 192 |
|
|
/// \returns true if the rotational part of *this is a rotation matrix (normalized columns), |
| 193 |
|
|
/// within the precision given by prec. |
| 194 |
|
|
/// |
| 195 |
|
|
bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
| 196 |
|
|
{ |
| 197 |
|
|
return derived().isNormalized(prec); |
| 198 |
|
|
} |
| 199 |
|
|
|
| 200 |
|
|
/// |
| 201 |
|
|
/// \brief Normalize *this in such a way the rotation part of *this lies on SO(3). |
| 202 |
|
|
/// |
| 203 |
|
|
void normalize() |
| 204 |
|
|
{ |
| 205 |
|
|
derived().normalize(); |
| 206 |
|
|
} |
| 207 |
|
|
|
| 208 |
|
|
/// |
| 209 |
|
|
/// \returns a Normalized version of *this, in such a way the rotation part of the returned |
| 210 |
|
|
/// transformation lies on SO(3). |
| 211 |
|
|
/// |
| 212 |
|
|
PlainType normalized() const |
| 213 |
|
|
{ |
| 214 |
|
|
derived().normalized(); |
| 215 |
|
|
} |
| 216 |
|
|
|
| 217 |
|
|
}; // struct SE3Base |
| 218 |
|
|
|
| 219 |
|
|
} // namespace pinocchio |
| 220 |
|
|
|
| 221 |
|
|
#endif // ifndef __pinocchio_spatial_se3_base_hpp__ |
| 222 |
|
|
|