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__ |