pinocchio  3.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
aba-derivatives.hpp
1 //
2 // Copyright (c) 2018-2021 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_aba_derivatives_hpp__
6 #define __pinocchio_algorithm_aba_derivatives_hpp__
7 
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 
11 #include <type_traits>
12 
13 namespace pinocchio
14 {
42  template<
43  typename Scalar,
44  int Options,
45  template<typename, int> class JointCollectionTpl,
46  typename ConfigVectorType,
47  typename TangentVectorType1,
48  typename TangentVectorType2,
49  typename MatrixType1,
50  typename MatrixType2,
51  typename MatrixType3>
52  void computeABADerivatives(
53  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
54  DataTpl<Scalar, Options, JointCollectionTpl> & data,
55  const Eigen::MatrixBase<ConfigVectorType> & q,
56  const Eigen::MatrixBase<TangentVectorType1> & v,
57  const Eigen::MatrixBase<TangentVectorType2> & tau,
58  const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
59  const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
60  const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
89  template<
90  typename Scalar,
91  int Options,
92  template<typename, int> class JointCollectionTpl,
93  typename ConfigVectorType,
94  typename TangentVectorType1,
95  typename TangentVectorType2,
96  typename MatrixType1,
97  typename MatrixType2,
98  typename MatrixType3>
99  void computeABADerivatives(
100  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
101  DataTpl<Scalar, Options, JointCollectionTpl> & data,
102  const Eigen::MatrixBase<ConfigVectorType> & q,
103  const Eigen::MatrixBase<TangentVectorType1> & v,
104  const Eigen::MatrixBase<TangentVectorType2> & tau,
105  const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
106  const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
107  const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
108  const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
109 
132  template<
133  typename Scalar,
134  int Options,
135  template<typename, int> class JointCollectionTpl,
136  typename ConfigVectorType,
137  typename TangentVectorType1,
138  typename TangentVectorType2>
139  typename std::enable_if<
140  ConfigVectorType::IsVectorAtCompileTime || TangentVectorType1::IsVectorAtCompileTime
141  || TangentVectorType2::IsVectorAtCompileTime,
142  void>::type
143  computeABADerivatives(
144  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
145  DataTpl<Scalar, Options, JointCollectionTpl> & data,
146  const Eigen::MatrixBase<ConfigVectorType> & q,
147  const Eigen::MatrixBase<TangentVectorType1> & v,
148  const Eigen::MatrixBase<TangentVectorType2> & tau);
149 
174  template<
175  typename Scalar,
176  int Options,
177  template<typename, int> class JointCollectionTpl,
178  typename ConfigVectorType,
179  typename TangentVectorType1,
180  typename TangentVectorType2>
181  void computeABADerivatives(
182  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
183  DataTpl<Scalar, Options, JointCollectionTpl> & data,
184  const Eigen::MatrixBase<ConfigVectorType> & q,
185  const Eigen::MatrixBase<TangentVectorType1> & v,
186  const Eigen::MatrixBase<TangentVectorType2> & tau,
187  const container::aligned_vector<ForceTpl<Scalar, Options>> & fext);
188 
212  template<
213  typename Scalar,
214  int Options,
215  template<typename, int> class JointCollectionTpl,
216  typename MatrixType1,
217  typename MatrixType2,
218  typename MatrixType3>
219  typename std::enable_if<
220  !(MatrixType1::IsVectorAtCompileTime || MatrixType2::IsVectorAtCompileTime
221  || MatrixType3::IsVectorAtCompileTime),
222  void>::type
223  computeABADerivatives(
224  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
225  DataTpl<Scalar, Options, JointCollectionTpl> & data,
226  const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
227  const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
228  const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
229 
242  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
243  void computeABADerivatives(
244  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
245  DataTpl<Scalar, Options, JointCollectionTpl> & data);
246 
271  template<
272  typename Scalar,
273  int Options,
274  template<typename, int> class JointCollectionTpl,
275  typename MatrixType1,
276  typename MatrixType2,
277  typename MatrixType3>
278  void computeABADerivatives(
279  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
280  DataTpl<Scalar, Options, JointCollectionTpl> & data,
281  const container::aligned_vector<ForceTpl<Scalar, Options>> & fext,
282  const Eigen::MatrixBase<MatrixType1> & aba_partial_dq,
283  const Eigen::MatrixBase<MatrixType2> & aba_partial_dv,
284  const Eigen::MatrixBase<MatrixType3> & aba_partial_dtau);
285 
303  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
304  void computeABADerivatives(
305  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
306  DataTpl<Scalar, Options, JointCollectionTpl> & data,
307  const container::aligned_vector<ForceTpl<Scalar, Options>> & fext);
308 
309 } // namespace pinocchio
310 
311 /* --- Details -------------------------------------------------------------------- */
312 #include "pinocchio/algorithm/aba-derivatives.hxx"
313 
314 #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
315  #include "pinocchio/algorithm/aba-derivatives.txx"
316 #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
317 
318 #endif // ifndef __pinocchio_algorithm_aba_derivatives_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::computeABADerivatives
void computeABADerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.