| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2015-2019 CNRS INRIA |
| 3 |
|
|
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
| 4 |
|
|
// |
| 5 |
|
|
|
| 6 |
|
|
#ifndef __pinocchio_spatial_motion_zero_hpp__ |
| 7 |
|
|
#define __pinocchio_spatial_motion_zero_hpp__ |
| 8 |
|
|
|
| 9 |
|
|
namespace pinocchio |
| 10 |
|
|
{ |
| 11 |
|
|
|
| 12 |
|
|
template<typename Scalar, int Options> |
| 13 |
|
|
struct SE3GroupAction<MotionZeroTpl<Scalar, Options>> |
| 14 |
|
|
{ |
| 15 |
|
|
typedef MotionZeroTpl<Scalar, Options> ReturnType; |
| 16 |
|
|
}; |
| 17 |
|
|
|
| 18 |
|
|
template<typename Scalar, int Options, typename MotionDerived> |
| 19 |
|
|
struct MotionAlgebraAction<MotionZeroTpl<Scalar, Options>, MotionDerived> |
| 20 |
|
|
{ |
| 21 |
|
|
typedef MotionZeroTpl<Scalar, Options> ReturnType; |
| 22 |
|
|
}; |
| 23 |
|
|
|
| 24 |
|
|
template<typename _Scalar, int _Options> |
| 25 |
|
|
struct traits<MotionZeroTpl<_Scalar, _Options>> |
| 26 |
|
|
{ |
| 27 |
|
|
enum |
| 28 |
|
|
{ |
| 29 |
|
|
Options = _Options, |
| 30 |
|
|
LINEAR = 0, |
| 31 |
|
|
ANGULAR = 3 |
| 32 |
|
|
}; |
| 33 |
|
|
typedef _Scalar Scalar; |
| 34 |
|
|
typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3; |
| 35 |
|
|
typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6; |
| 36 |
|
|
typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3; |
| 37 |
|
|
typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4; |
| 38 |
|
|
typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6; |
| 39 |
|
|
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; |
| 40 |
|
|
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; |
| 41 |
|
|
typedef Matrix6 ActionMatrixType; |
| 42 |
|
|
typedef Matrix4 HomogeneousMatrixType; |
| 43 |
|
|
typedef Vector3 AngularType; |
| 44 |
|
|
typedef const Vector3 ConstAngularType; |
| 45 |
|
|
typedef Vector3 LinearType; |
| 46 |
|
|
typedef const Vector3 ConstLinearType; |
| 47 |
|
|
typedef Motion MotionPlain; |
| 48 |
|
|
typedef MotionPlain PlainReturnType; |
| 49 |
|
|
|
| 50 |
|
|
}; // traits MotionZeroTpl |
| 51 |
|
|
|
| 52 |
|
|
template<typename Scalar, int Options> |
| 53 |
|
|
struct MotionZeroTpl : public MotionBase<MotionZeroTpl<Scalar, Options>> |
| 54 |
|
|
{ |
| 55 |
|
|
typedef typename traits<MotionZeroTpl>::MotionPlain MotionPlain; |
| 56 |
|
|
typedef typename traits<MotionZeroTpl>::PlainReturnType PlainReturnType; |
| 57 |
|
|
|
| 58 |
|
✗ |
static PlainReturnType plain() |
| 59 |
|
|
{ |
| 60 |
|
✗ |
return MotionPlain::Zero(); |
| 61 |
|
|
} |
| 62 |
|
|
|
| 63 |
|
|
template<typename D2> |
| 64 |
|
✗ |
static bool isEqual_impl(const MotionDense<D2> & other) |
| 65 |
|
|
{ |
| 66 |
|
✗ |
return other.linear().isZero(0) && other.angular().isZero(0); |
| 67 |
|
|
} |
| 68 |
|
|
|
| 69 |
|
✗ |
static bool isEqual_impl(const MotionZeroTpl &) |
| 70 |
|
|
{ |
| 71 |
|
✗ |
return true; |
| 72 |
|
|
} |
| 73 |
|
|
|
| 74 |
|
|
template<typename D2> |
| 75 |
|
✗ |
static void addTo(const MotionBase<D2> &) |
| 76 |
|
|
{ |
| 77 |
|
|
} |
| 78 |
|
|
|
| 79 |
|
|
template<typename D2> |
| 80 |
|
39212 |
static void setTo(MotionBase<D2> & other) |
| 81 |
|
|
{ |
| 82 |
|
39212 |
other.setZero(); |
| 83 |
|
39212 |
} |
| 84 |
|
|
|
| 85 |
|
|
template<typename M1> |
| 86 |
|
✗ |
MotionZeroTpl motionAction(const MotionBase<M1> &) const |
| 87 |
|
|
{ |
| 88 |
|
✗ |
return MotionZeroTpl(); |
| 89 |
|
|
} |
| 90 |
|
|
|
| 91 |
|
|
template<typename S2, int O2, typename D2> |
| 92 |
|
|
void se3Action_impl(const SE3Tpl<S2, O2> &, MotionDense<D2> & v) const |
| 93 |
|
|
{ |
| 94 |
|
|
v.setZero(); |
| 95 |
|
|
} |
| 96 |
|
|
|
| 97 |
|
|
template<typename S2, int O2> |
| 98 |
|
39212 |
MotionZeroTpl se3Action_impl(const SE3Tpl<S2, O2> &) const |
| 99 |
|
|
{ |
| 100 |
|
39212 |
return MotionZeroTpl(); |
| 101 |
|
|
} |
| 102 |
|
|
|
| 103 |
|
|
template<typename S2, int O2, typename D2> |
| 104 |
|
|
void se3ActionInverse_impl(const SE3Tpl<S2, O2> &, MotionDense<D2> & v) const |
| 105 |
|
|
{ |
| 106 |
|
|
v.setZero(); |
| 107 |
|
|
} |
| 108 |
|
|
|
| 109 |
|
|
template<typename S2, int O2> |
| 110 |
|
✗ |
MotionZeroTpl se3ActionInverse_impl(const SE3Tpl<S2, O2> &) const |
| 111 |
|
|
{ |
| 112 |
|
✗ |
return MotionZeroTpl(); |
| 113 |
|
|
} |
| 114 |
|
|
|
| 115 |
|
|
}; // struct MotionZeroTpl |
| 116 |
|
|
|
| 117 |
|
|
template<typename M1, typename Scalar, int Options> |
| 118 |
|
28241 |
inline const M1 & operator+(const MotionBase<M1> & v, const MotionZeroTpl<Scalar, Options> &) |
| 119 |
|
|
{ |
| 120 |
|
28241 |
return v.derived(); |
| 121 |
|
|
} |
| 122 |
|
|
|
| 123 |
|
|
template<typename Scalar, int Options, typename M1> |
| 124 |
|
16125 |
inline const M1 & operator+(const MotionZeroTpl<Scalar, Options> &, const MotionBase<M1> & v) |
| 125 |
|
|
{ |
| 126 |
|
16125 |
return v.derived(); |
| 127 |
|
|
} |
| 128 |
|
|
|
| 129 |
|
|
} // namespace pinocchio |
| 130 |
|
|
|
| 131 |
|
|
#endif // ifndef __pinocchio_spatial_motion_zero_hpp__ |
| 132 |
|
|
|