GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/spatial/motion-base.hpp Lines: 38 41 92.7 %
Date: 2024-01-23 21:41:47 Branches: 0 0 - %

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_motion_base_hpp__
7
#define __pinocchio_motion_base_hpp__
8
9
namespace pinocchio
10
{
11
12
  template<class Derived>
13
  class MotionBase
14
  {
15
  public:
16
    MOTION_TYPEDEF_TPL(Derived);
17
18
2187428
    Derived & derived() { return *static_cast<Derived*>(this); }
19
2205655
    const Derived & derived() const { return *static_cast<const Derived*>(this); }
20
21
1655674
    ConstAngularType angular() const { return derived().angular_impl(); }
22
516354
    ConstLinearType linear() const { return derived().linear_impl(); }
23
503955
    AngularType angular() { return derived().angular_impl(); }
24
333375
    LinearType linear() { return derived().linear_impl(); }
25
26
    template<typename V3Like>
27
    void angular(const Eigen::MatrixBase<V3Like> & w)
28
    { derived().angular_impl(w.derived()); }
29
30
    template<typename V3Like>
31
5
    void linear(const Eigen::MatrixBase<V3Like> & v)
32
5
    { derived().linear_impl(v.derived()); }
33
34
447
    operator PlainReturnType() const { return derived().plain(); }
35
    PlainReturnType plain() const { return derived().plain(); }
36
37
518942
    ToVectorConstReturnType toVector() const { return derived().toVector_impl(); }
38
21966
    ToVectorReturnType toVector() { return derived().toVector_impl(); }
39
4
    operator Vector6() const { return toVector(); }
40
41
158
    ActionMatrixType toActionMatrix() const { return derived().toActionMatrix_impl(); }
42
6
    ActionMatrixType toDualActionMatrix() const { return derived().toDualActionMatrix_impl(); }
43
    operator Matrix6() const { return toActionMatrix(); }
44
45
    /**
46
     * @brief The homogeneous representation of the motion vector \f$ \xi \f$.
47
     *
48
     * With \f$ \hat{\xi} = \left( \begin{array}{cc} \omega & v \\ 0 & 0 \\ \end{array} \right) \f$,
49
     * \f[
50
     * {}^a\dot{M}_b = \hat{\xi} {}^aM_b
51
     * \f]
52
     *
53
     * @note This function is provided for completeness, but it is not the best
54
     * way to use Motion quantities in terms of sparsity exploitation and
55
     * general efficiency. For integration, the recommended way is to use
56
     * Motion vectors along with the \ref integrate function.
57
     */
58
    HomogeneousMatrixType toHomogeneousMatrix() const { return derived().toHomogeneousMatrix_impl(); }
59
60
670
    void setZero() { derived().setZero(); }
61
62
    template<typename M2>
63
7538
    bool operator==(const MotionBase<M2> & other) const
64
7538
    { return derived().isEqual_impl(other.derived()); }
65
66
    template<typename M2>
67
1
    bool operator!=(const MotionBase<M2> & other) const
68
1
    { return !(derived() == other.derived()); }
69
70
    Derived operator-() const { return derived().__opposite__(); }
71
1
    Derived operator+(const MotionBase<Derived> & v) const { return derived().__plus__(v.derived()); }
72
    Derived operator-(const MotionBase<Derived> & v) const { return derived().__minus__(v.derived()); }
73
    Derived & operator+=(const MotionBase<Derived> & v) { return derived().__pequ__(v.derived()); }
74
    Derived & operator-=(const MotionBase<Derived> & v) { return derived().__mequ__(v.derived()); }
75
76
    template<typename OtherScalar>
77
    typename internal::RHSScalarMultiplication<Derived,OtherScalar>::ReturnType
78
36748
    operator*(const OtherScalar & alpha) const
79
36748
    { return derived().__mult__(alpha); }
80
81
    template<typename OtherScalar>
82
1
    Derived operator/(const OtherScalar & alpha) const
83
1
    { return derived().__div__(alpha); }
84
85
    template<typename OtherSpatialType>
86
    typename MotionAlgebraAction<OtherSpatialType,Derived>::ReturnType
87
78692
    cross(const OtherSpatialType & d) const
88
    {
89
78692
      return derived().cross_impl(d);
90
    }
91
92
1231
    bool isApprox(const Derived & other, const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
93
1231
    { return derived().isApprox_impl(other, prec);}
94
95
10
    bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
96
10
    { return derived().isZero_impl(prec);}
97
98
    template<typename S2, int O2>
99
    typename SE3GroupAction<Derived>::ReturnType
100
65425
    se3Action(const SE3Tpl<S2,O2> & m) const
101
65425
    { return derived().se3Action_impl(m); }
102
103
    template<typename S2, int O2>
104
    typename SE3GroupAction<Derived>::ReturnType
105
184359
    se3ActionInverse(const SE3Tpl<S2,O2> & m) const
106
184359
    { return derived().se3ActionInverse_impl(m); }
107
108
    template<typename ForceDerived>
109
    Scalar dot(const ForceDense<ForceDerived> & f) const { return derived().dot(f.derived()); }
110
111
1
    void disp(std::ostream & os) const { derived().disp_impl(os); }
112
1
    friend std::ostream & operator << (std::ostream & os, const MotionBase<Derived> & v)
113
    {
114
1
      v.disp(os);
115
1
      return os;
116
    }
117
118
  }; // class MotionBase
119
120
  template<typename MotionDerived>
121
  typename internal::RHSScalarMultiplication<MotionDerived,typename MotionDerived::Scalar>::ReturnType
122
  operator*(const typename MotionDerived::Scalar & alpha,
123
            const MotionBase<MotionDerived> & motion)
124
  {
125
    return motion*alpha;
126
  }
127
128
} // namespace pinocchio
129
130
#endif // ifndef __pinocchio_motion_base_hpp__