pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
rnea-derivatives.hpp
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 
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 
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 
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 
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 
165  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
166  inline void
167  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  computeRNEADerivatives(model,data,q.derived(),v.derived(),a.derived(),
174  data.dtau_dq, data.dtau_dv, data.M);
175  }
176 
199  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
200  inline void
201  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  computeRNEADerivatives(model,data,q.derived(),v.derived(),a.derived(),fext,
209  data.dtau_dq, data.dtau_dv, data.M);
210  }
211 
212 
213 } // namespace pinocchio
214 
215 #include "pinocchio/algorithm/rnea-derivatives.hxx"
216 
217 #endif // ifndef __pinocchio_rnea_derivatives_hpp__
pinocchio::DataTpl::dtau_dq
MatrixXs dtau_dq
Partial derivative of the joint torque vector with respect to the joint configuration.
Definition: data.hpp:290
pinocchio::computeStaticTorqueDerivatives
void computeStaticTorqueDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const container::aligned_vector< ForceTpl< Scalar, Options > > &fext, const Eigen::MatrixBase< ReturnMatrixType > &static_torque_partial_dq)
Computes the partial derivative of the generalized gravity and external forces contributions (a...
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:151
pinocchio::computeRNEADerivatives
void computeRNEADerivatives(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 > &a, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint c...
pinocchio::container::aligned_vector
Specialization of an std::vector with an aligned allocator. This specialization might be used when th...
Definition: aligned-vector.hpp:26
pinocchio::DataTpl::dtau_dv
MatrixXs dtau_dv
Partial derivative of the joint torque vector with respect to the joint velocity. ...
Definition: data.hpp:293
pinocchio::computeGeneralizedGravityDerivatives
void computeGeneralizedGravityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< ReturnMatrixType > &gravity_partial_dq)
Computes the partial derivative of the generalized gravity contribution with respect to the joint con...
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24