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

Line Branch Exec Source
1
//
2
// Copyright (c) 2017-2019 CNRS INRIA
3
//
4
5
#ifndef __pinocchio_rnea_derivatives_hpp__
6
#define __pinocchio_rnea_derivatives_hpp__
7
8
#include "pinocchio/multibody/model.hpp"
9
#include "pinocchio/multibody/data.hpp"
10
11
#include "pinocchio/container/aligned-vector.hpp"
12
13
namespace pinocchio
14
{
15
16
  ///
17
  /// \brief Computes the partial derivative of the generalized gravity contribution
18
  ///        with respect to the joint configuration.
19
  ///
20
  /// \tparam JointCollection Collection of Joint types.
21
  /// \tparam ConfigVectorType Type of the joint configuration vector.
22
  /// \tparam ReturnMatrixType Type of the matrix containing the partial derivative of the gravity vector with respect to the joint configuration vector.
23
  ///
24
  /// \param[in] model The model structure of the rigid body system.
25
  /// \param[in] data The data structure of the rigid body system.
26
  /// \param[in] q The joint configuration vector (dim model.nq).
27
  /// \param[out] gravity_partial_dq Partial derivative of the generalized gravity vector with respect to the joint configuration.
28
  ///
29
  /// \remarks gravity_partial_dq must be first initialized with zeros (gravity_partial_dq.setZero).
30
  ///
31
  /// \sa pinocchio::computeGeneralizedGravity
32
  ///
33
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename ReturnMatrixType>
34
  inline void
35
  computeGeneralizedGravityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
36
                                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
37
                                       const Eigen::MatrixBase<ConfigVectorType> & q,
38
                                       const Eigen::MatrixBase<ReturnMatrixType> & gravity_partial_dq);
39
40
  ///
41
  /// \brief Computes the partial derivative of the generalized gravity and external forces contributions (a.k.a static torque vector)
42
  ///        with respect to the joint configuration.
43
  ///
44
  /// \tparam JointCollection Collection of Joint types.
45
  /// \tparam ConfigVectorType Type of the joint configuration vector.
46
  /// \tparam ReturnMatrixType Type of the matrix containing the partial derivative of the gravity vector with respect to the joint configuration vector.
47
  ///
48
  /// \param[in] model The model structure of the rigid body system.
49
  /// \param[in] data The data structure of the rigid body system.
50
  /// \param[in] q The joint configuration vector (dim model.nq).
51
  /// \param[in] fext External forces expressed in the local frame of the joints (dim model.njoints).
52
  /// \param[out] static_torque_partial_dq Partial derivative of the static torque vector with respect to the joint configuration.
53
  ///
54
  /// \remarks gravity_partial_dq must be first initialized with zeros (gravity_partial_dq.setZero).
55
  ///
56
  /// \sa pinocchio::computeGeneralizedTorque
57
  ///
58
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename ReturnMatrixType>
59
  inline void
60
  computeStaticTorqueDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
61
                                 DataTpl<Scalar,Options,JointCollectionTpl> & data,
62
                                 const Eigen::MatrixBase<ConfigVectorType> & q,
63
                                 const container::aligned_vector< ForceTpl<Scalar,Options> > & fext,
64
                                 const Eigen::MatrixBase<ReturnMatrixType> & static_torque_partial_dq);
65
66
  ///
67
  /// \brief Computes the partial derivatives of the Recursive Newton Euler Algorithms
68
  ///        with respect to the joint configuration, the joint velocity and the joint acceleration.
69
  ///
70
  /// \tparam JointCollection Collection of Joint types.
71
  /// \tparam ConfigVectorType Type of the joint configuration vector.
72
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
73
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
74
  /// \tparam MatrixType1 Type of the matrix containing the partial derivative with respect to the joint configuration vector.
75
  /// \tparam MatrixType2 Type of the matrix containing the partial derivative with respect to the joint velocity vector.
76
  /// \tparam MatrixType3 Type of the matrix containing the partial derivative with respect to the joint acceleration vector.
77
  ///
78
  /// \param[in] model The model structure of the rigid body system.
79
  /// \param[in] data The data structure of the rigid body system.
80
  /// \param[in] q The joint configuration vector (dim model.nq).
81
  /// \param[in] v The joint velocity vector (dim model.nv).
82
  /// \param[in] a The joint acceleration vector (dim model.nv).
83
  /// \param[out] rnea_partial_dq Partial derivative of the generalized torque vector with respect to the joint configuration.
84
  /// \param[out] rnea_partial_dv Partial derivative of the generalized torque vector with respect to the joint velocity.
85
  /// \param[out] rnea_partial_da Partial derivative of the generalized torque vector with respect to the joint acceleration.
86
  ///
87
  /// \remarks rnea_partial_dq, rnea_partial_dv and rnea_partial_da must be first initialized with zeros (rnea_partial_dq.setZero(),etc).
88
  ///         As for pinocchio::crba, only the upper triangular part of rnea_partial_da is filled.
89
  ///
90
  /// \sa pinocchio::rnea
91
  ///
92
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
93
  typename MatrixType1, typename MatrixType2, typename MatrixType3>
94
  inline void
95
  computeRNEADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
96
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
97
                         const Eigen::MatrixBase<ConfigVectorType> & q,
98
                         const Eigen::MatrixBase<TangentVectorType1> & v,
99
                         const Eigen::MatrixBase<TangentVectorType2> & a,
100
                         const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq,
101
                         const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
102
                         const Eigen::MatrixBase<MatrixType3> & rnea_partial_da);
103
104
  ///
105
  /// \brief Computes the derivatives of the Recursive Newton Euler Algorithms
106
  ///        with respect to the joint configuration, the joint velocity and the joint acceleration.
107
  ///
108
  /// \tparam JointCollection Collection of Joint types.
109
  /// \tparam ConfigVectorType Type of the joint configuration vector.
110
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
111
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
112
  /// \tparam MatrixType1 Type of the matrix containing the partial derivative with respect to the joint configuration vector.
113
  /// \tparam MatrixType2 Type of the matrix containing the partial derivative with respect to the joint velocity vector.
114
  /// \tparam MatrixType3 Type of the matrix containing the partial derivative with respect to the joint acceleration vector.
115
  ///
116
  /// \param[in] model The model structure of the rigid body system.
117
  /// \param[in] data The data structure of the rigid body system.
118
  /// \param[in] q The joint configuration vector (dim model.nq).
119
  /// \param[in] v The joint velocity vector (dim model.nv).
120
  /// \param[in] a The joint acceleration vector (dim model.nv).
121
  /// \param[in] fext External forces expressed in the local frame of the joints (dim model.njoints).
122
  /// \param[out] rnea_partial_dq Partial derivative of the generalized torque vector with respect to the joint configuration.
123
  /// \param[out] rnea_partial_dv Partial derivative of the generalized torque vector with respect to the joint velocity.
124
  /// \param[out] rnea_partial_da Partial derivative of the generalized torque vector with respect to the joint acceleration.
125
  ///
126
  /// \remarks rnea_partial_dq, rnea_partial_dv and rnea_partial_da must be first initialized with zeros (rnea_partial_dq.setZero(),etc).
127
  ///         As for pinocchio::crba, only the upper triangular part of rnea_partial_da is filled.
128
  ///
129
  /// \sa pinocchio::rnea
130
  ///
131
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
132
  typename MatrixType1, typename MatrixType2, typename MatrixType3>
133
  inline void
134
  computeRNEADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
135
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
136
                         const Eigen::MatrixBase<ConfigVectorType> & q,
137
                         const Eigen::MatrixBase<TangentVectorType1> & v,
138
                         const Eigen::MatrixBase<TangentVectorType2> & a,
139
                         const container::aligned_vector< ForceTpl<Scalar,Options> > & fext,
140
                         const Eigen::MatrixBase<MatrixType1> & rnea_partial_dq,
141
                         const Eigen::MatrixBase<MatrixType2> & rnea_partial_dv,
142
                         const Eigen::MatrixBase<MatrixType3> & rnea_partial_da);
143
144
  ///
145
  /// \brief Computes the derivatives of the Recursive Newton Euler Algorithms
146
  ///        with respect to the joint configuration, the joint velocity and the joint acceleration.
147
  ///
148
  /// \tparam JointCollection Collection of Joint types.
149
  /// \tparam ConfigVectorType Type of the joint configuration vector.
150
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
151
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
152
  ///
153
  /// \param[in] model The model structure of the rigid body system.
154
  /// \param[in] data The data structure of the rigid body system.
155
  /// \param[in] q The joint configuration vector (dim model.nq).
156
  /// \param[in] v The joint velocity vector (dim model.nv).
157
  /// \param[in] a The joint acceleration vector (dim model.nv).
158
  ///
159
  /// \returns The results are stored in data.dtau_dq, data.dtau_dv and data.M which respectively correspond
160
  ///          to the partial derivatives of the joint torque vector with respect to the joint configuration, velocity and acceleration.
161
  ///          As for pinocchio::crba, only the upper triangular part of data.M is filled.
162
  ///
163
  /// \sa pinocchio::rnea, pinocchio::crba, pinocchio::cholesky::decompose
164
  ///
165
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
166
  inline void
167
30
  computeRNEADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
168
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
169
                         const Eigen::MatrixBase<ConfigVectorType> & q,
170
                         const Eigen::MatrixBase<TangentVectorType1> & v,
171
                         const Eigen::MatrixBase<TangentVectorType2> & a)
172
  {
173
30
    computeRNEADerivatives(model,data,q.derived(),v.derived(),a.derived(),
174
30
                           data.dtau_dq, data.dtau_dv, data.M);
175
30
  }
176
177
  ///
178
  /// \brief Computes the derivatives of the Recursive Newton Euler Algorithms
179
  ///        with respect to the joint configuration, the joint velocity and the joint acceleration.
180
  ///
181
  /// \tparam JointCollection Collection of Joint types.
182
  /// \tparam ConfigVectorType Type of the joint configuration vector.
183
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
184
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
185
  ///
186
  /// \param[in] model The model structure of the rigid body system.
187
  /// \param[in] data The data structure of the rigid body system.
188
  /// \param[in] q The joint configuration vector (dim model.nq).
189
  /// \param[in] v The joint velocity vector (dim model.nv).
190
  /// \param[in] a The joint acceleration vector (dim model.nv).
191
  /// \param[in] fext External forces expressed in the local frame of the joints (dim model.njoints).
192
  ///
193
  /// \returns The results are stored in data.dtau_dq, data.dtau_dv and data.M which respectively correspond
194
  ///          to the partial derivatives of the joint torque vector with respect to the joint configuration, velocity and acceleration.
195
  ///          As for pinocchio::crba, only the upper triangular part of data.M is filled.
196
  ///
197
  /// \sa pinocchio::rnea, pinocchio::crba, pinocchio::cholesky::decompose
198
  ///
199
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
200
  inline void
201
3
  computeRNEADerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
202
                         DataTpl<Scalar,Options,JointCollectionTpl> & data,
203
                         const Eigen::MatrixBase<ConfigVectorType> & q,
204
                         const Eigen::MatrixBase<TangentVectorType1> & v,
205
                         const Eigen::MatrixBase<TangentVectorType2> & a,
206
                         const container::aligned_vector< ForceTpl<Scalar,Options> > & fext)
207
  {
208
3
    computeRNEADerivatives(model,data,q.derived(),v.derived(),a.derived(),fext,
209
3
                           data.dtau_dq, data.dtau_dv, data.M);
210
3
  }
211
212
213
} // namespace pinocchio
214
215
#include "pinocchio/algorithm/rnea-derivatives.hxx"
216
217
#endif // ifndef __pinocchio_rnea_derivatives_hpp__