| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2015-2020 CNRS INRIA |
| 3 |
|
|
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
| 4 |
|
|
// |
| 5 |
|
|
|
| 6 |
|
|
#ifndef __pinocchio_spatial_motion_base_hpp__ |
| 7 |
|
|
#define __pinocchio_spatial_motion_base_hpp__ |
| 8 |
|
|
|
| 9 |
|
|
namespace pinocchio |
| 10 |
|
|
{ |
| 11 |
|
|
|
| 12 |
|
|
template<class Derived> |
| 13 |
|
|
class MotionBase : NumericalBase<Derived> |
| 14 |
|
|
{ |
| 15 |
|
|
public: |
| 16 |
|
|
MOTION_TYPEDEF_TPL(Derived); |
| 17 |
|
|
|
| 18 |
|
4522172 |
Derived & derived() |
| 19 |
|
|
{ |
| 20 |
|
4522172 |
return *static_cast<Derived *>(this); |
| 21 |
|
|
} |
| 22 |
|
5676602 |
const Derived & derived() const |
| 23 |
|
|
{ |
| 24 |
|
10461206 |
return *static_cast<const Derived *>(this); |
| 25 |
|
|
} |
| 26 |
|
|
|
| 27 |
|
|
Derived & const_cast_derived() const |
| 28 |
|
|
{ |
| 29 |
|
|
return *const_cast<Derived *>(&derived()); |
| 30 |
|
|
} |
| 31 |
|
|
|
| 32 |
|
3314970 |
ConstAngularType angular() const |
| 33 |
|
|
{ |
| 34 |
|
3314970 |
return derived().angular_impl(); |
| 35 |
|
|
} |
| 36 |
|
984900 |
ConstLinearType linear() const |
| 37 |
|
|
{ |
| 38 |
|
1962610 |
return derived().linear_impl(); |
| 39 |
|
|
} |
| 40 |
|
869407 |
AngularType angular() |
| 41 |
|
|
{ |
| 42 |
|
1718442 |
return derived().angular_impl(); |
| 43 |
|
|
} |
| 44 |
|
629098 |
LinearType linear() |
| 45 |
|
|
{ |
| 46 |
|
1239255 |
return derived().linear_impl(); |
| 47 |
|
|
} |
| 48 |
|
|
|
| 49 |
|
|
template<typename V3Like> |
| 50 |
|
1 |
void angular(const Eigen::MatrixBase<V3Like> & w) |
| 51 |
|
|
{ |
| 52 |
|
1 |
derived().angular_impl(w.derived()); |
| 53 |
|
1 |
} |
| 54 |
|
|
|
| 55 |
|
|
template<typename V3Like> |
| 56 |
|
6 |
void linear(const Eigen::MatrixBase<V3Like> & v) |
| 57 |
|
|
{ |
| 58 |
|
6 |
derived().linear_impl(v.derived()); |
| 59 |
|
6 |
} |
| 60 |
|
|
|
| 61 |
|
69093 |
operator PlainReturnType() const |
| 62 |
|
|
{ |
| 63 |
1/2
✓ Branch 3 taken 698 times.
✗ Branch 4 not taken.
|
69093 |
return derived().plain(); |
| 64 |
|
|
} |
| 65 |
|
|
PlainReturnType plain() const |
| 66 |
|
|
{ |
| 67 |
|
|
return derived().plain(); |
| 68 |
|
|
} |
| 69 |
|
|
|
| 70 |
|
2259198 |
ToVectorConstReturnType toVector() const |
| 71 |
|
|
{ |
| 72 |
|
2259198 |
return derived().toVector_impl(); |
| 73 |
|
|
} |
| 74 |
|
206732 |
ToVectorReturnType toVector() |
| 75 |
|
|
{ |
| 76 |
|
236731 |
return derived().toVector_impl(); |
| 77 |
|
|
} |
| 78 |
|
4 |
operator Vector6() const |
| 79 |
|
|
{ |
| 80 |
|
4 |
return toVector(); |
| 81 |
|
|
} |
| 82 |
|
|
|
| 83 |
|
164 |
ActionMatrixType toActionMatrix() const |
| 84 |
|
|
{ |
| 85 |
|
164 |
return derived().toActionMatrix_impl(); |
| 86 |
|
|
} |
| 87 |
|
6 |
ActionMatrixType toDualActionMatrix() const |
| 88 |
|
|
{ |
| 89 |
|
6 |
return derived().toDualActionMatrix_impl(); |
| 90 |
|
|
} |
| 91 |
|
|
operator Matrix6() const |
| 92 |
|
|
{ |
| 93 |
|
|
return toActionMatrix(); |
| 94 |
|
|
} |
| 95 |
|
|
|
| 96 |
|
|
/** |
| 97 |
|
|
* @brief The homogeneous representation of the motion vector \f$ \xi \f$. |
| 98 |
|
|
* |
| 99 |
|
|
* With \f$ \hat{\xi} = \left( \begin{array}{cc} \omega & v \\ 0 & 0 \\ \end{array} \right) \f$, |
| 100 |
|
|
* \f[ |
| 101 |
|
|
* {}^a\dot{M}_b = \hat{\xi} {}^aM_b |
| 102 |
|
|
* \f] |
| 103 |
|
|
* |
| 104 |
|
|
* @note This function is provided for completeness, but it is not the best |
| 105 |
|
|
* way to use Motion quantities in terms of sparsity exploitation and |
| 106 |
|
|
* general efficiency. For integration, the recommended way is to use |
| 107 |
|
|
* Motion vectors along with the \ref integrate function. |
| 108 |
|
|
*/ |
| 109 |
|
✗ |
HomogeneousMatrixType toHomogeneousMatrix() const |
| 110 |
|
|
{ |
| 111 |
|
✗ |
return derived().toHomogeneousMatrix_impl(); |
| 112 |
|
|
} |
| 113 |
|
|
|
| 114 |
|
40579 |
void setZero() |
| 115 |
|
|
{ |
| 116 |
|
40579 |
derived().setZero(); |
| 117 |
|
40579 |
} |
| 118 |
|
|
|
| 119 |
|
|
template<typename M2> |
| 120 |
|
8443 |
bool operator==(const MotionBase<M2> & other) const |
| 121 |
|
|
{ |
| 122 |
|
8443 |
return derived().isEqual_impl(other.derived()); |
| 123 |
|
|
} |
| 124 |
|
|
|
| 125 |
|
|
template<typename M2> |
| 126 |
|
1 |
bool operator!=(const MotionBase<M2> & other) const |
| 127 |
|
|
{ |
| 128 |
|
1 |
return !(derived() == other.derived()); |
| 129 |
|
|
} |
| 130 |
|
|
|
| 131 |
|
|
Derived operator-() const |
| 132 |
|
|
{ |
| 133 |
|
|
return derived().__opposite__(); |
| 134 |
|
|
} |
| 135 |
|
1 |
Derived operator+(const MotionBase<Derived> & v) const |
| 136 |
|
|
{ |
| 137 |
|
1 |
return derived().__plus__(v.derived()); |
| 138 |
|
|
} |
| 139 |
|
|
Derived operator-(const MotionBase<Derived> & v) const |
| 140 |
|
|
{ |
| 141 |
|
|
return derived().__minus__(v.derived()); |
| 142 |
|
|
} |
| 143 |
|
|
Derived & operator+=(const MotionBase<Derived> & v) |
| 144 |
|
|
{ |
| 145 |
|
|
return derived().__pequ__(v.derived()); |
| 146 |
|
|
} |
| 147 |
|
|
Derived & operator-=(const MotionBase<Derived> & v) |
| 148 |
|
|
{ |
| 149 |
|
|
return derived().__mequ__(v.derived()); |
| 150 |
|
|
} |
| 151 |
|
|
|
| 152 |
|
|
template<typename OtherScalar> |
| 153 |
|
|
typename internal::RHSScalarMultiplication<Derived, OtherScalar>::ReturnType |
| 154 |
|
45652 |
operator*(const OtherScalar & alpha) const |
| 155 |
|
|
{ |
| 156 |
|
45652 |
return derived().__mult__(alpha); |
| 157 |
|
|
} |
| 158 |
|
|
|
| 159 |
|
|
template<typename OtherScalar> |
| 160 |
|
4 |
Derived operator/(const OtherScalar & alpha) const |
| 161 |
|
|
{ |
| 162 |
|
4 |
return derived().__div__(alpha); |
| 163 |
|
|
} |
| 164 |
|
|
|
| 165 |
|
|
template<typename OtherSpatialType> |
| 166 |
|
|
typename MotionAlgebraAction<OtherSpatialType, Derived>::ReturnType |
| 167 |
|
282673 |
cross(const OtherSpatialType & d) const |
| 168 |
|
|
{ |
| 169 |
|
282673 |
return derived().cross_impl(d); |
| 170 |
|
|
} |
| 171 |
|
|
|
| 172 |
|
4948 |
bool isApprox( |
| 173 |
|
|
const Derived & other, |
| 174 |
|
|
const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
| 175 |
|
|
{ |
| 176 |
|
4948 |
return derived().isApprox_impl(other, prec); |
| 177 |
|
|
} |
| 178 |
|
|
|
| 179 |
|
101 |
bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const |
| 180 |
|
|
{ |
| 181 |
|
101 |
return derived().isZero_impl(prec); |
| 182 |
|
|
} |
| 183 |
|
|
|
| 184 |
|
|
template<typename S2, int O2> |
| 185 |
|
247494 |
typename SE3GroupAction<Derived>::ReturnType se3Action(const SE3Tpl<S2, O2> & m) const |
| 186 |
|
|
{ |
| 187 |
|
247494 |
return derived().se3Action_impl(m); |
| 188 |
|
|
} |
| 189 |
|
|
|
| 190 |
|
|
template<typename S2, int O2> |
| 191 |
|
166042 |
typename SE3GroupAction<Derived>::ReturnType se3ActionInverse(const SE3Tpl<S2, O2> & m) const |
| 192 |
|
|
{ |
| 193 |
|
166042 |
return derived().se3ActionInverse_impl(m); |
| 194 |
|
|
} |
| 195 |
|
|
|
| 196 |
|
|
template<typename ForceDerived> |
| 197 |
|
|
Scalar dot(const ForceDense<ForceDerived> & f) const |
| 198 |
|
|
{ |
| 199 |
|
|
return derived().dot(f.derived()); |
| 200 |
|
|
} |
| 201 |
|
|
|
| 202 |
|
49 |
void disp(std::ostream & os) const |
| 203 |
|
|
{ |
| 204 |
|
49 |
derived().disp_impl(os); |
| 205 |
|
49 |
} |
| 206 |
|
49 |
friend std::ostream & operator<<(std::ostream & os, const MotionBase<Derived> & v) |
| 207 |
|
|
{ |
| 208 |
|
49 |
v.disp(os); |
| 209 |
|
49 |
return os; |
| 210 |
|
|
} |
| 211 |
|
|
|
| 212 |
|
|
}; // class MotionBase |
| 213 |
|
|
|
| 214 |
|
|
template<typename MotionDerived> |
| 215 |
|
|
typename internal::RHSScalarMultiplication< |
| 216 |
|
|
MotionDerived, |
| 217 |
|
|
typename MotionDerived::Scalar>::ReturnType |
| 218 |
|
|
operator*(const typename MotionDerived::Scalar & alpha, const MotionBase<MotionDerived> & motion) |
| 219 |
|
|
{ |
| 220 |
|
|
return motion * alpha; |
| 221 |
|
|
} |
| 222 |
|
|
|
| 223 |
|
|
} // namespace pinocchio |
| 224 |
|
|
|
| 225 |
|
|
#endif // ifndef __pinocchio_spatial_motion_base_hpp__ |
| 226 |
|
|
|