GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/multibody/liegroup/cartesian-product-variant.hpp Lines: 19 19 100.0 %
Date: 2023-08-09 08:43:58 Branches: 14 28 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2018 CNRS
3
//
4
5
#ifndef __pinocchio_cartesian_product_variant_hpp__
6
#define __pinocchio_cartesian_product_variant_hpp__
7
8
#include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9
#include "pinocchio/multibody/liegroup/liegroup-collection.hpp"
10
11
#include "pinocchio/container/aligned-vector.hpp"
12
13
namespace pinocchio
14
{
15
16
  template<typename Scalar, int Options = 0,
17
    template<typename,int> class LieGroupCollectionTpl = LieGroupCollectionDefaultTpl>
18
      struct CartesianProductOperationVariantTpl;
19
  typedef CartesianProductOperationVariantTpl<double, 0, LieGroupCollectionDefaultTpl> CartesianProductOperationVariant;
20
21
  template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
22
  struct traits<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
23
  {
24
    typedef _Scalar Scalar;
25
    enum {
26
      Options = _Options,
27
      NQ = Eigen::Dynamic,
28
      NV = Eigen::Dynamic
29
    };
30
  };
31
32
  ///
33
  /// \brief Dynamic Cartesian product composed of elementary Lie groups defined in LieGroupVariant
34
  ///
35
  template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
36
  struct CartesianProductOperationVariantTpl
37
  : public LieGroupBase<CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> >
38
  {
39
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40
41
    PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperationVariantTpl);
42
43
    typedef LieGroupCollectionTpl<Scalar, Options> LieGroupCollection;
44
    typedef typename LieGroupCollection::LieGroupVariant LieGroupVariant;
45
    typedef LieGroupGenericTpl<LieGroupCollection> LieGroupGeneric;
46
47
    /// \brief Default constructor
48
8
    CartesianProductOperationVariantTpl()
49
    : m_nq(0), m_nv(0)
50
    , lg_nqs(0), lg_nvs(0)
51

8
    , m_neutral(0)
52
8
    {};
53
54
    ///
55
    /// \brief Constructor with one single Lie group
56
    ///
57
    /// \param[in] lg Lie group variant to insert inside the Cartesian product
58
    ///
59
9
    explicit CartesianProductOperationVariantTpl(const LieGroupGeneric & lg)
60
    : m_nq(0), m_nv(0)
61
    , lg_nqs(0), lg_nvs(0)
62

9
    , m_neutral(0)
63
    {
64
9
      append(lg);
65
9
    };
66
67
    ///
68
    /// \brief Constructor with two Lie groups
69
    ///
70
    /// \param[in] lg1 Lie group variant to insert inside the Cartesian product
71
    /// \param[in] lg2 Lie group variant to insert inside the Cartesian product
72
    ///
73
1
    CartesianProductOperationVariantTpl(const LieGroupGeneric & lg1,
74
                                        const LieGroupGeneric & lg2)
75
    : m_nq(0), m_nv(0)
76
    , lg_nqs(0), lg_nvs(0)
77

1
    , m_neutral(0)
78
    {
79

1
      append(lg1); append(lg2);
80
1
    };
81
82
    ///
83
    /// \brief Append a Lie group to the Cartesian product
84
    ///
85
    /// \param[in] lg Lie group variant to insert inside the Cartesian product
86
    ///
87
    void append(const LieGroupGeneric & lg);
88
89
    ///
90
    /// \brief Append a Lie group to the Cartesian product
91
    ///
92
    /// \param[in] lg Lie group to insert inside the Cartesian product
93
    ///
94
    template<typename LieGroupDerived>
95
16
    void append (const LieGroupBase<LieGroupDerived>& lg)
96
    {
97
32
      LieGroupGeneric lgGeneric (lg);
98
16
      append(lgGeneric);
99
16
    }
100
101
    ///
102
    /// \brief Cartesian product between *this and other.
103
    ///
104
    /// \param[in] other CartesianProductOperation to compose with this
105
    ///
106
    /// \returns A new Cartesian product betwenn *this and other.
107
    ///
108
    CartesianProductOperationVariantTpl operator* (const CartesianProductOperationVariantTpl& other) const;
109
110
    ///
111
    /// \brief Append other to *this.
112
    ///
113
    /// \param[in] other CartesianProductOperation to append to *this.
114
    ///
115
    CartesianProductOperationVariantTpl& operator*= (const CartesianProductOperationVariantTpl& other);
116
117
    ///
118
    /// \brief Append a Lie group to *this.
119
    ///
120
    /// \param[in] lg LieGroupGeneric to append to *this.
121
    ///
122
    inline CartesianProductOperationVariantTpl& operator*= (const LieGroupGeneric& lg)
123
    {
124
      append(lg);
125
      return *this;
126
    }
127
128
    ///
129
    /// \brief Append a Lie group to *this.
130
    ///
131
    /// \param[in] lg LieGroupGeneric to append to *this.
132
    ///
133
    template<typename LieGroupDerived>
134
    inline CartesianProductOperationVariantTpl& operator*= (const LieGroupBase<LieGroupDerived>& lg)
135
    {
136
      append<LieGroupDerived>(lg);
137
      return *this;
138
    }
139
140
9
    int nq() const { return m_nq; }
141
9
    int nv() const { return m_nv; }
142
143
10
    std::string name() const { return m_name; }
144
145
9
    ConfigVector_t neutral() const { return m_neutral; }
146
147
    template <class ConfigL_t, class ConfigR_t, class Tangent_t>
148
    void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
149
                         const Eigen::MatrixBase<ConfigR_t> & q1,
150
                         const Eigen::MatrixBase<Tangent_t> & d) const;
151
152
    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
153
    void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
154
                          const Eigen::MatrixBase<ConfigR_t> & q1,
155
                          const Eigen::MatrixBase<JacobianOut_t> & J) const;
156
157
    template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
158
    void dDifference_product_impl(const ConfigL_t & q0,
159
                                  const ConfigR_t & q1,
160
                                  const JacobianIn_t & Jin,
161
                                  JacobianOut_t & Jout,
162
                                  bool dDifferenceOnTheLeft,
163
                                  const AssignmentOperatorType op) const;
164
165
    template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
166
    void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
167
                        const Eigen::MatrixBase<Velocity_t> & v,
168
                        const Eigen::MatrixBase<ConfigOut_t> & qout) const;
169
170
    template <class Config_t, class Jacobian_t>
171
    void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q,
172
                                         const Eigen::MatrixBase<Jacobian_t> & J) const;
173
174
    template <class Config_t, class Tangent_t, class JacobianOut_t>
175
    void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
176
                            const Eigen::MatrixBase<Tangent_t> & v,
177
                            const Eigen::MatrixBase<JacobianOut_t> & J,
178
                            const AssignmentOperatorType op=SETTO) const;
179
180
    template <class Config_t, class Tangent_t, class JacobianOut_t>
181
    void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
182
                            const Eigen::MatrixBase<Tangent_t> & v,
183
                            const Eigen::MatrixBase<JacobianOut_t> & J,
184
                            const AssignmentOperatorType op=SETTO) const;
185
186
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
187
    void dIntegrate_product_impl(const Config_t & q,
188
                                 const Tangent_t & v,
189
                                 const JacobianIn_t & Jin,
190
                                 JacobianOut_t & Jout,
191
                                 bool dIntegrateOnTheLeft,
192
                                 const ArgumentPosition arg,
193
                                 const AssignmentOperatorType op) const;
194
195
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
196
    void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
197
                                     const Eigen::MatrixBase<Tangent_t> & v,
198
                                     const Eigen::MatrixBase<JacobianIn_t> & J_in,
199
                                     const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
200
201
    template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
202
    void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
203
                                     const Eigen::MatrixBase<Tangent_t> & v,
204
                                     const Eigen::MatrixBase<JacobianIn_t> & J_in,
205
                                     const Eigen::MatrixBase<JacobianOut_t> & J_out) const;
206
207
    template <class Config_t, class Tangent_t, class JacobianOut_t>
208
    void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
209
                                     const Eigen::MatrixBase<Tangent_t> & v,
210
                                     const Eigen::MatrixBase<JacobianOut_t> & J) const;
211
212
    template <class Config_t, class Tangent_t, class JacobianOut_t>
213
    void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
214
                                     const Eigen::MatrixBase<Tangent_t> & v,
215
                                     const Eigen::MatrixBase<JacobianOut_t> & J) const;
216
217
    template <class ConfigL_t, class ConfigR_t>
218
    Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
219
                                const Eigen::MatrixBase<ConfigR_t> & q1) const;
220
221
    template <class Config_t>
222
    void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const;
223
224
    template <class Config_t>
225
    bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qout,
226
                            const Scalar& prec) const;
227
228
    template <class Config_t>
229
    void random_impl (const Eigen::MatrixBase<Config_t>& qout) const;
230
231
    template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
232
    void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower,
233
                                  const Eigen::MatrixBase<ConfigR_t> & upper,
234
                                  const Eigen::MatrixBase<ConfigOut_t> & qout) const;
235
236
    template <class ConfigL_t, class ConfigR_t>
237
    bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
238
                                  const Eigen::MatrixBase<ConfigR_t> & q1,
239
                                  const Scalar & prec) const;
240
241
    bool isEqual_impl (const CartesianProductOperationVariantTpl& other) const;
242
243
    template <typename LieGroup1, typename LieGroup2>
244
    bool isEqual(const CartesianProductOperation<LieGroup1, LieGroup2> & other) const;
245
246
  protected:
247
248
    PINOCCHIO_ALIGNED_STD_VECTOR(LieGroupGeneric) liegroups;
249
    Index m_nq, m_nv;
250
    std::vector<Index> lg_nqs, lg_nvs;
251
    std::string m_name;
252
253
    ConfigVector_t m_neutral;
254
255
  };
256
257
} // namespace pinocchio
258
259
#include <pinocchio/multibody/liegroup/cartesian-product-variant.hxx>
260
261
#endif // ifndef __pinocchio_cartesian_product_variant_hpp__