| Directory: | ./ |
|---|---|
| File: | include/pinocchio/spatial/motion-dense.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 107 | 115 | 93.0% |
| Branches: | 123 | 303 | 40.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2017-2020 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_spatial_motion_dense_hpp__ | ||
| 6 | #define __pinocchio_spatial_motion_dense_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/spatial/skew.hpp" | ||
| 9 | |||
| 10 | namespace pinocchio | ||
| 11 | { | ||
| 12 | |||
| 13 | template<typename Derived> | ||
| 14 | struct SE3GroupAction<MotionDense<Derived>> | ||
| 15 | { | ||
| 16 | typedef typename SE3GroupAction<Derived>::ReturnType ReturnType; | ||
| 17 | }; | ||
| 18 | |||
| 19 | template<typename Derived, typename MotionDerived> | ||
| 20 | struct MotionAlgebraAction<MotionDense<Derived>, MotionDerived> | ||
| 21 | { | ||
| 22 | typedef typename MotionAlgebraAction<Derived, MotionDerived>::ReturnType ReturnType; | ||
| 23 | }; | ||
| 24 | |||
| 25 | template<typename Derived> | ||
| 26 | class MotionDense : public MotionBase<Derived> | ||
| 27 | { | ||
| 28 | public: | ||
| 29 | typedef MotionBase<Derived> Base; | ||
| 30 | MOTION_TYPEDEF_TPL(Derived); | ||
| 31 | typedef typename traits<Derived>::MotionRefType MotionRefType; | ||
| 32 | |||
| 33 | using Base::angular; | ||
| 34 | using Base::derived; | ||
| 35 | using Base::isApprox; | ||
| 36 | using Base::isZero; | ||
| 37 | using Base::linear; | ||
| 38 | |||
| 39 | 101936 | Derived & setZero() | |
| 40 | { | ||
| 41 |
1/2✓ Branch 2 taken 51181 times.
✗ Branch 3 not taken.
|
101936 | linear().setZero(); |
| 42 |
1/2✓ Branch 2 taken 51181 times.
✗ Branch 3 not taken.
|
101936 | angular().setZero(); |
| 43 | 101936 | return derived(); | |
| 44 | } | ||
| 45 | 4 | Derived & setRandom() | |
| 46 | { | ||
| 47 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
6 | linear().setRandom(); |
| 48 |
1/2✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
|
6 | angular().setRandom(); |
| 49 | 6 | return derived(); | |
| 50 | } | ||
| 51 | |||
| 52 | 164 | ActionMatrixType toActionMatrix_impl() const | |
| 53 | { | ||
| 54 | 164 | ActionMatrixType X; | |
| 55 |
5/10✓ Branch 1 taken 164 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 164 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 164 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 164 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 164 times.
✗ Branch 14 not taken.
|
164 | X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) = |
| 56 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
164 | skew(angular()); |
| 57 |
3/11✗ Branch 1 not taken.
✓ Branch 2 taken 164 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 164 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 164 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
164 | X.template block<3, 3>(LINEAR, ANGULAR) = skew(linear()); |
| 58 |
1/5✗ Branch 1 not taken.
✓ Branch 2 taken 164 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
164 | X.template block<3, 3>(ANGULAR, LINEAR).setZero(); |
| 59 | |||
| 60 | 164 | return X; | |
| 61 | } | ||
| 62 | |||
| 63 | 6 | ActionMatrixType toDualActionMatrix_impl() const | |
| 64 | { | ||
| 65 | 6 | ActionMatrixType X; | |
| 66 |
5/10✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
|
6 | X.template block<3, 3>(ANGULAR, ANGULAR) = X.template block<3, 3>(LINEAR, LINEAR) = |
| 67 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
6 | skew(angular()); |
| 68 |
3/11✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
|
6 | X.template block<3, 3>(ANGULAR, LINEAR) = skew(linear()); |
| 69 |
1/5✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
6 | X.template block<3, 3>(LINEAR, ANGULAR).setZero(); |
| 70 | |||
| 71 | 6 | return X; | |
| 72 | } | ||
| 73 | |||
| 74 | ✗ | HomogeneousMatrixType toHomogeneousMatrix_impl() const | |
| 75 | { | ||
| 76 | ✗ | HomogeneousMatrixType M; | |
| 77 | ✗ | M.template block<3, 3>(0, 0) = skew(angular()); | |
| 78 | ✗ | M.template block<3, 1>(0, 3) = linear(); | |
| 79 | ✗ | M.template block<1, 4>(3, 0).setZero(); | |
| 80 | ✗ | return M; | |
| 81 | } | ||
| 82 | |||
| 83 | template<typename D2> | ||
| 84 | 3529 | bool isEqual_impl(const MotionDense<D2> & other) const | |
| 85 | { | ||
| 86 |
8/16✓ Branch 1 taken 3495 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3495 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3495 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 3495 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3495 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 3495 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3495 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 3495 times.
✗ Branch 21 not taken.
|
3529 | return linear() == other.linear() && angular() == other.angular(); |
| 87 | } | ||
| 88 | |||
| 89 | template<typename D2> | ||
| 90 | 50 | bool isEqual_impl(const MotionBase<D2> & other) const | |
| 91 | { | ||
| 92 | 50 | return other.derived() == derived(); | |
| 93 | } | ||
| 94 | |||
| 95 | // Arithmetic operators | ||
| 96 | template<typename D2> | ||
| 97 | 15005 | Derived & operator=(const MotionDense<D2> & other) | |
| 98 | { | ||
| 99 | 15005 | return derived().set(other.derived()); | |
| 100 | } | ||
| 101 | |||
| 102 | 10 | Derived & operator=(const MotionDense & other) | |
| 103 | { | ||
| 104 | 10 | return derived().set(other.derived()); | |
| 105 | } | ||
| 106 | |||
| 107 | template<typename D2> | ||
| 108 | 15015 | Derived & set(const MotionDense<D2> & other) | |
| 109 | { | ||
| 110 |
2/4✓ Branch 2 taken 8277 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8277 times.
✗ Branch 6 not taken.
|
15015 | linear() = other.linear(); |
| 111 |
2/4✓ Branch 2 taken 8277 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8277 times.
✗ Branch 6 not taken.
|
15015 | angular() = other.angular(); |
| 112 | 15015 | return derived(); | |
| 113 | } | ||
| 114 | |||
| 115 | template<typename D2> | ||
| 116 | 203020 | Derived & operator=(const MotionBase<D2> & other) | |
| 117 | { | ||
| 118 | 203020 | other.derived().setTo(derived()); | |
| 119 | 203020 | return derived(); | |
| 120 | } | ||
| 121 | |||
| 122 | template<typename V6> | ||
| 123 | 6773 | Derived & operator=(const Eigen::MatrixBase<V6> & v) | |
| 124 | { | ||
| 125 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(V6); | ||
| 126 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 6655 times.
|
6773 | assert(v.size() == 6); |
| 127 |
2/4✓ Branch 2 taken 6655 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6655 times.
✗ Branch 6 not taken.
|
6773 | linear() = v.template segment<3>(LINEAR); |
| 128 |
2/4✓ Branch 2 taken 6655 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6655 times.
✗ Branch 6 not taken.
|
6773 | angular() = v.template segment<3>(ANGULAR); |
| 129 | 6773 | return derived(); | |
| 130 | } | ||
| 131 | |||
| 132 | 3532 | MotionPlain operator-() const | |
| 133 | { | ||
| 134 | 3532 | return derived().__opposite__(); | |
| 135 | } | ||
| 136 | template<typename M1> | ||
| 137 | 104846 | MotionPlain operator+(const MotionDense<M1> & v) const | |
| 138 | { | ||
| 139 | 104846 | return derived().__plus__(v.derived()); | |
| 140 | } | ||
| 141 | template<typename M1> | ||
| 142 | 47746 | MotionPlain operator-(const MotionDense<M1> & v) const | |
| 143 | { | ||
| 144 | 47746 | return derived().__minus__(v.derived()); | |
| 145 | } | ||
| 146 | |||
| 147 | template<typename M1> | ||
| 148 | 503594 | Derived & operator+=(const MotionDense<M1> & v) | |
| 149 | { | ||
| 150 | 503594 | return derived().__pequ__(v.derived()); | |
| 151 | } | ||
| 152 | template<typename M1> | ||
| 153 | 93770 | Derived & operator+=(const MotionBase<M1> & v) | |
| 154 | { | ||
| 155 | 93770 | v.derived().addTo(derived()); | |
| 156 | 93770 | return derived(); | |
| 157 | } | ||
| 158 | |||
| 159 | template<typename M1> | ||
| 160 | 531 | Derived & operator-=(const MotionDense<M1> & v) | |
| 161 | { | ||
| 162 | 531 | return derived().__mequ__(v.derived()); | |
| 163 | } | ||
| 164 | |||
| 165 | 3532 | MotionPlain __opposite__() const | |
| 166 | { | ||
| 167 |
4/8✓ Branch 2 taken 3519 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3519 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3519 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3519 times.
✗ Branch 12 not taken.
|
3532 | return MotionPlain(-linear(), -angular()); |
| 168 | } | ||
| 169 | |||
| 170 | template<typename M1> | ||
| 171 | 128 | MotionPlain __plus__(const MotionDense<M1> & v) const | |
| 172 | { | ||
| 173 |
6/12✓ Branch 2 taken 128 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 128 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 128 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 128 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 128 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 128 times.
✗ Branch 18 not taken.
|
128 | return MotionPlain(linear() + v.linear(), angular() + v.angular()); |
| 174 | } | ||
| 175 | |||
| 176 | template<typename M1> | ||
| 177 | 1 | MotionPlain __minus__(const MotionDense<M1> & v) const | |
| 178 | { | ||
| 179 |
6/12✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
|
1 | return MotionPlain(linear() - v.linear(), angular() - v.angular()); |
| 180 | } | ||
| 181 | |||
| 182 | template<typename M1> | ||
| 183 | Derived & __pequ__(const MotionDense<M1> & v) | ||
| 184 | { | ||
| 185 | linear() += v.linear(); | ||
| 186 | angular() += v.angular(); | ||
| 187 | return derived(); | ||
| 188 | } | ||
| 189 | |||
| 190 | template<typename M1> | ||
| 191 | Derived & __mequ__(const MotionDense<M1> & v) | ||
| 192 | { | ||
| 193 | linear() -= v.linear(); | ||
| 194 | angular() -= v.angular(); | ||
| 195 | return derived(); | ||
| 196 | } | ||
| 197 | |||
| 198 | template<typename OtherScalar> | ||
| 199 | MotionPlain __mult__(const OtherScalar & alpha) const | ||
| 200 | { | ||
| 201 | return MotionPlain(alpha * linear(), alpha * angular()); | ||
| 202 | } | ||
| 203 | |||
| 204 | template<typename OtherScalar> | ||
| 205 | 4 | MotionPlain __div__(const OtherScalar & alpha) const | |
| 206 | { | ||
| 207 |
1/5✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
8 | return derived().__mult__((OtherScalar)(1) / alpha); |
| 208 | } | ||
| 209 | |||
| 210 | template<typename F1> | ||
| 211 | ✗ | Scalar dot(const ForceBase<F1> & phi) const | |
| 212 | { | ||
| 213 | ✗ | return phi.linear().dot(linear()) + phi.angular().dot(angular()); | |
| 214 | } | ||
| 215 | |||
| 216 | template<typename D> | ||
| 217 | 282844 | typename MotionAlgebraAction<D, Derived>::ReturnType cross_impl(const D & d) const | |
| 218 | { | ||
| 219 | 282844 | return d.motionAction(derived()); | |
| 220 | } | ||
| 221 | |||
| 222 | template<typename M1, typename M2> | ||
| 223 | 134712 | void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const | |
| 224 | { | ||
| 225 |
8/16✓ Branch 2 taken 88175 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 88175 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 88175 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 88175 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 88175 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 88175 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 88175 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 88175 times.
✗ Branch 24 not taken.
|
134712 | mout.linear() = v.linear().cross(angular()) + v.angular().cross(linear()); |
| 226 |
4/8✓ Branch 2 taken 88175 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 88175 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 88175 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 88175 times.
✗ Branch 12 not taken.
|
134712 | mout.angular() = v.angular().cross(angular()); |
| 227 | 134712 | } | |
| 228 | |||
| 229 | template<typename M1> | ||
| 230 | 59842 | MotionPlain motionAction(const MotionDense<M1> & v) const | |
| 231 | { | ||
| 232 | 59842 | MotionPlain res; | |
| 233 |
1/2✓ Branch 1 taken 144 times.
✗ Branch 2 not taken.
|
59842 | motionAction(v, res); |
| 234 | 59842 | return res; | |
| 235 | } | ||
| 236 | |||
| 237 | template<typename M2> | ||
| 238 | 16 | bool isApprox( | |
| 239 | const MotionDense<M2> & m2, | ||
| 240 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 241 | { | ||
| 242 | 16 | return derived().isApprox_impl(m2, prec); | |
| 243 | } | ||
| 244 | |||
| 245 | template<typename D2> | ||
| 246 | 4964 | bool isApprox_impl( | |
| 247 | const MotionDense<D2> & m2, | ||
| 248 | const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | ||
| 249 | { | ||
| 250 |
8/16✓ Branch 1 taken 4925 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4925 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4925 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 4925 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4925 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 4925 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4925 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 4925 times.
✗ Branch 21 not taken.
|
4964 | return linear().isApprox(m2.linear(), prec) && angular().isApprox(m2.angular(), prec); |
| 251 | } | ||
| 252 | |||
| 253 | 101 | bool isZero_impl(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const | |
| 254 | { | ||
| 255 |
7/12✓ Branch 1 taken 101 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 101 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 48 times.
✓ Branch 7 taken 53 times.
✓ Branch 9 taken 48 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 48 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 48 times.
✗ Branch 15 not taken.
|
101 | return linear().isZero(prec) && angular().isZero(prec); |
| 256 | } | ||
| 257 | |||
| 258 | template<typename S2, int O2, typename D2> | ||
| 259 | 81788 | void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
| 260 | { | ||
| 261 |
5/10✓ Branch 2 taken 54430 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54430 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 54430 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 54430 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 54430 times.
✗ Branch 15 not taken.
|
81788 | v.angular().noalias() = m.rotation() * angular(); |
| 262 |
8/16✓ Branch 3 taken 54430 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 54430 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 54430 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 54430 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 54430 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 54430 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 54430 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 54430 times.
✗ Branch 25 not taken.
|
81788 | v.linear().noalias() = m.rotation() * linear() + m.translation().cross(v.angular()); |
| 263 | 81788 | } | |
| 264 | |||
| 265 | template<typename S2, int O2> | ||
| 266 | 81788 | typename SE3GroupAction<Derived>::ReturnType se3Action_impl(const SE3Tpl<S2, O2> & m) const | |
| 267 | { | ||
| 268 | 81788 | typename SE3GroupAction<Derived>::ReturnType res; | |
| 269 |
1/2✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
|
81788 | se3Action_impl(m, res); |
| 270 | 81788 | return res; | |
| 271 | } | ||
| 272 | |||
| 273 | template<typename S2, int O2, typename D2> | ||
| 274 | 330189 | void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const | |
| 275 | { | ||
| 276 |
7/14✓ Branch 1 taken 165743 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165743 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 165743 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 165743 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 165743 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 165743 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 30 times.
✗ Branch 20 not taken.
|
990507 | v.linear().noalias() = |
| 277 |
3/6✓ Branch 3 taken 165743 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 165743 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 165713 times.
✗ Branch 10 not taken.
|
991594 | m.rotation().transpose() * (linear() - m.translation().cross(angular())); |
| 278 |
6/12✓ Branch 2 taken 165743 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 165743 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 165743 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 165743 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 165743 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 165743 times.
✗ Branch 18 not taken.
|
330189 | v.angular().noalias() = m.rotation().transpose() * angular(); |
| 279 | 330189 | } | |
| 280 | |||
| 281 | template<typename S2, int O2> | ||
| 282 | typename SE3GroupAction<Derived>::ReturnType | ||
| 283 | 330189 | se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const | |
| 284 | { | ||
| 285 | 330189 | typename SE3GroupAction<Derived>::ReturnType res; | |
| 286 |
1/2✓ Branch 1 taken 691 times.
✗ Branch 2 not taken.
|
330189 | se3ActionInverse_impl(m, res); |
| 287 | 330189 | return res; | |
| 288 | } | ||
| 289 | |||
| 290 | 49 | void disp_impl(std::ostream & os) const | |
| 291 | { | ||
| 292 |
3/6✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 49 times.
✗ Branch 10 not taken.
|
49 | os << " v = " << linear().transpose() << std::endl |
| 293 |
5/10✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 49 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 49 times.
✗ Branch 14 not taken.
|
49 | << " w = " << angular().transpose() << std::endl; |
| 294 | 49 | } | |
| 295 | |||
| 296 | /// \returns a MotionRef on this. | ||
| 297 | MotionRefType ref() | ||
| 298 | { | ||
| 299 | return derived().ref(); | ||
| 300 | } | ||
| 301 | |||
| 302 | protected: | ||
| 303 | 2835076 | MotionDense() {}; | |
| 304 | |||
| 305 | MotionDense(const MotionDense &) = delete; | ||
| 306 | |||
| 307 | }; // class MotionDense | ||
| 308 | |||
| 309 | /// Basic operations specialization | ||
| 310 | template<typename M1, typename M2> | ||
| 311 | 40895 | typename traits<M1>::MotionPlain operator^(const MotionDense<M1> & v1, const MotionDense<M2> & v2) | |
| 312 | { | ||
| 313 | 40895 | return v1.derived().cross(v2.derived()); | |
| 314 | } | ||
| 315 | |||
| 316 | template<typename M1, typename F1> | ||
| 317 | 3 | typename traits<F1>::ForcePlain operator^(const MotionDense<M1> & v, const ForceBase<F1> & f) | |
| 318 | { | ||
| 319 | 3 | return v.derived().cross(f.derived()); | |
| 320 | } | ||
| 321 | |||
| 322 | template<typename M1> | ||
| 323 | typename traits<M1>::MotionPlain | ||
| 324 | 15617 | operator*(const typename traits<M1>::Scalar alpha, const MotionDense<M1> & v) | |
| 325 | { | ||
| 326 | 15617 | return v * alpha; | |
| 327 | } | ||
| 328 | |||
| 329 | } // namespace pinocchio | ||
| 330 | |||
| 331 | #endif // ifndef __pinocchio_spatial_motion_dense_hpp__ | ||
| 332 |