GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/algorithm/centroidal.hpp Lines: 6 6 100.0 %
Date: 2024-01-23 21:41:47 Branches: 0 0 - %

Line Branch Exec Source
1
//
2
// Copyright (c) 2015-2019 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_algorithm_centroidal_hpp__
6
#define __pinocchio_algorithm_centroidal_hpp__
7
8
#include "pinocchio/multibody/model.hpp"
9
#include "pinocchio/multibody/data.hpp"
10
#include "pinocchio/algorithm/kinematics.hpp"
11
12
namespace pinocchio
13
{
14
15
  ///
16
  /// \brief Computes the Centroidal momentum, a.k.a. the total momenta of the system
17
  ///        expressed around the center of mass.
18
  ///
19
  /// \tparam Scalar The scalar type.
20
  /// \tparam Options Eigen Alignment options.
21
  /// \tparam JointCollection Collection of Joint types.
22
  ///
23
  /// \param[in] model The model structure of the rigid body system.
24
  /// \param[in] data The data structure of the rigid body system.
25
  ///
26
  /// \returns The centroidal momenta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
27
  ///
28
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
29
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
30
  computeCentroidalMomentum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
31
                            DataTpl<Scalar,Options,JointCollectionTpl> & data);
32
33
  ///
34
  /// \brief Computes the Centroidal momentum, a.k.a. the total momenta of the system
35
  ///        expressed around the center of mass.
36
  ///
37
  /// \tparam Scalar The scalar type.
38
  /// \tparam Options Eigen Alignment options.
39
  /// \tparam JointCollection Collection of Joint types.
40
  /// \tparam ConfigVectorType Type of the joint configuration vector.
41
  /// \tparam TangentVectorType Type of the joint velocity vector.
42
  ///
43
  /// \param[in] model The model structure of the rigid body system.
44
  /// \param[in] data The data structure of the rigid body system.
45
  /// \param[in] q The joint configuration vector (dim model.nq).
46
  /// \param[in] v The joint velocity vector (dim model.nv).
47
  ///
48
  /// \returns The centroidal momenta (stored in data.hg), center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
49
  ///
50
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
51
          typename ConfigVectorType, typename TangentVectorType>
52
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
53
3
  computeCentroidalMomentum(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
54
                            DataTpl<Scalar,Options,JointCollectionTpl> & data,
55
                            const Eigen::MatrixBase<ConfigVectorType> & q,
56
                            const Eigen::MatrixBase<TangentVectorType> & v)
57
  {
58
3
    forwardKinematics(model,data,q.derived(),v.derived());
59
3
    return computeCentroidalMomentum(model,data);
60
  }
61
62
  /// \copydoc pinocchio::computeCentroidalMomentum
63
  ///
64
  /// \deprecated This function has been renamed into \ref computeCentroidalMomentum. This signature will be removed in a future release of Pinocchio.
65
  ///        Please consider using this new naming.
66
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
67
  typename ConfigVectorType, typename TangentVectorType>
68
  PINOCCHIO_DEPRECATED
69
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
70
  computeCentroidalDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
71
                            DataTpl<Scalar,Options,JointCollectionTpl> & data,
72
                            const Eigen::MatrixBase<ConfigVectorType> & q,
73
                            const Eigen::MatrixBase<TangentVectorType> & v)
74
  {
75
    return computeCentroidalMomentum(model,data,q,v);
76
  }
77
78
  ///
79
  /// \brief Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and its time derivative
80
  ///        expressed around the center of mass.
81
  ///
82
  /// \tparam Scalar The scalar type.
83
  /// \tparam Options Eigen Alignment options.
84
  /// \tparam JointCollection Collection of Joint types.
85
  ///
86
  /// \param[in] model The model structure of the rigid body system.
87
  /// \param[in] data The data structure of the rigid body system.
88
  ///
89
  /// \returns The centroidal momenta time derivative (stored in data.dhg), centroidal momemta (stored in data.hg),
90
  ///          center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
91
  ///
92
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
93
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
94
  computeCentroidalMomentumTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
95
                                         DataTpl<Scalar,Options,JointCollectionTpl> & data);
96
97
  ///
98
  /// \brief Computes the Centroidal momemtum and its time derivatives, a.k.a. the total momenta of the system and its time derivative
99
  ///        expressed around the center of mass.
100
  ///
101
  /// \tparam Scalar The scalar type.
102
  /// \tparam Options Eigen Alignment options.
103
  /// \tparam JointCollection Collection of Joint types.
104
  /// \tparam ConfigVectorType Type of the joint configuration vector.
105
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
106
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
107
  ///
108
  /// \param[in] model The model structure of the rigid body system.
109
  /// \param[in] data The data structure of the rigid body system.
110
  /// \param[in] q The joint configuration vector (dim model.nq).
111
  /// \param[in] v The joint velocity vector (dim model.nv).
112
  /// \param[in] a The joint acceleration vector (dim model.nv).
113
  ///
114
  /// \returns The centroidal momenta time derivative (stored in data.dhg), centroidal momemta (stored in data.hg),
115
  ///          center of mass (stored in data.com[0]) and velocity of center of mass (stored in data.vcom[0])
116
  ///
117
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
118
          typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
119
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
120
108
  computeCentroidalMomentumTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
121
                                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
122
                                         const Eigen::MatrixBase<ConfigVectorType> & q,
123
                                         const Eigen::MatrixBase<TangentVectorType1> & v,
124
                                         const Eigen::MatrixBase<TangentVectorType2> & a)
125
  {
126
108
    forwardKinematics(model,data,q,v,a);
127
108
    return computeCentroidalMomentumTimeVariation(model,data);
128
  }
129
130
  /// \copydoc pinocchio::computeCentroidalMomentumTimeVariation
131
  ///
132
  /// \deprecated This function has been renamed into \ref computeCentroidalMomentumTimeVariation. This signature will be removed in a future release of Pinocchio.
133
  ///        Please consider using this new naming.
134
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
135
          typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
136
  PINOCCHIO_DEPRECATED
137
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Force &
138
  computeCentroidalDynamics(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
139
                            DataTpl<Scalar,Options,JointCollectionTpl> & data,
140
                            const Eigen::MatrixBase<ConfigVectorType> & q,
141
                            const Eigen::MatrixBase<TangentVectorType1> & v,
142
                            const Eigen::MatrixBase<TangentVectorType2> & a)
143
  {
144
    return computeCentroidalMomentumTimeVariation(model,data,q,v,a);
145
  }
146
147
  ///
148
  /// \brief Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta
149
  ///        according to the current joint configuration and velocity.
150
  ///
151
  /// \tparam JointCollection Collection of Joint types.
152
  /// \tparam ConfigVectorType Type of the joint configuration vector.
153
  /// \tparam TangentVectorType Type of the joint velocity vector.
154
  ///
155
  /// \param[in] model The model structure of the rigid body system.
156
  /// \param[in] data The data structure of the rigid body system.
157
  /// \param[in] q The joint configuration vector (dim model.nq).
158
  /// \param[in] v The joint velocity vector (dim model.nv).
159
  ///
160
  /// \return The Centroidal Momentum Matrix Ag.
161
  ///
162
  /// \remarks As another output, this algorithm also computes the Joint Jacobian matrix (accessible via data.J).
163
  ///
164
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
165
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
166
  ccrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
167
        DataTpl<Scalar,Options,JointCollectionTpl> & data,
168
        const Eigen::MatrixBase<ConfigVectorType> & q,
169
        const Eigen::MatrixBase<TangentVectorType> & v);
170
171
  ///
172
  /// \brief Computes the Centroidal Momentum Matrix,.
173
  ///
174
  /// \tparam JointCollection Collection of Joint types.
175
  /// \tparam ConfigVectorType Type of the joint configuration vector.
176
  ///
177
  /// \param[in] model The model structure of the rigid body system.
178
  /// \param[in] data The data structure of the rigid body system.
179
  /// \param[in] q The joint configuration vector (dim model.nq).
180
  ///
181
  /// \return The Centroidal Momentum Matrix Ag.
182
  ///
183
  /// \remarks As another output, this algorithm also computes the Joint Jacobian matrix (accessible via data.J).
184
  ///
185
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
186
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
187
  computeCentroidalMap(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
188
                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
189
                       const Eigen::MatrixBase<ConfigVectorType> & q);
190
191
  ///
192
  /// \brief Computes the time derivative of the Centroidal Momentum Matrix according to the current configuration and velocity vectors.
193
  ///
194
  /// \note The computed terms allow to decomposed the spatial momentum variation as following: \f$ \dot{h} = A_g \ddot{q} + \dot{A_g}(q,\dot{q})\dot{q}\f$.
195
  ///
196
  /// \tparam JointCollection Collection of Joint types.
197
  /// \tparam ConfigVectorType Type of the joint configuration vector.
198
  /// \tparam TangentVectorType Type of the joint velocity vector.
199
  ///
200
  /// \param[in] model The model structure of the rigid body system.
201
  /// \param[in] data The data structure of the rigid body system.
202
  /// \param[in] q The joint configuration vector (dim model.nq).
203
  /// \param[in] v The joint velocity vector (dim model.nv).
204
  ///
205
  /// \return The Centroidal Momentum Matrix time derivative dAg (accessible via data.dAg).
206
  ///
207
  /// \remarks As another output, this algorithm also computes the Centroidal Momentum Matrix Ag (accessible via data.Ag), the Joint Jacobian matrix (accessible via data.J) and the time derivatibe of the Joint Jacobian matrix (accessible via data.dJ).
208
  ///
209
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
210
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
211
  dccrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
212
         DataTpl<Scalar,Options,JointCollectionTpl> & data,
213
         const Eigen::MatrixBase<ConfigVectorType> & q,
214
         const Eigen::MatrixBase<TangentVectorType> & v);
215
216
  ///
217
  /// \brief Computes the Centroidal Momentum Matrix time derivative.
218
  ///
219
  /// \tparam JointCollection Collection of Joint types.
220
  /// \tparam ConfigVectorType Type of the joint configuration vector.
221
  /// \tparam TangentVectorType Type of the joint velocity vector.
222
  ///
223
  /// \param[in] model The model structure of the rigid body system.
224
  /// \param[in] data The data structure of the rigid body system.
225
  /// \param[in] q The joint configuration vector (dim model.nq).
226
  /// \param[in] v The joint velocity vector (dim model.nv).
227
  ///
228
  /// \return The Centroidal Momentum Matrix time derivative dAg (accessible via data.dAg).
229
  ///
230
  /// \remarks As another output, this algorithm also computes the Centroidal Momentum Matrix Ag (accessible via data.Ag), the Joint Jacobian matrix (accessible via data.J) and the time derivatibe of the Joint Jacobian matrix (accessible via data.dJ).
231
  ///
232
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
233
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x &
234
  computeCentroidalMapTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
235
                                    DataTpl<Scalar,Options,JointCollectionTpl> & data,
236
                                    const Eigen::MatrixBase<ConfigVectorType> & q,
237
                                    const Eigen::MatrixBase<TangentVectorType> & v);
238
239
} // namespace pinocchio
240
241
/* --- Details -------------------------------------------------------------------- */
242
#include "pinocchio/algorithm/centroidal.hxx"
243
244
#endif // ifndef __pinocchio_algorithm_centroidal_hpp__