centroidal-derivatives.hpp 5.56 KB
Newer Older
1
2
3
4
//
// Copyright (c) 2018 INRIA
//

5
6
#ifndef __pinocchio_algorithm_centroidal_derivatives_hpp__
#define __pinocchio_algorithm_centroidal_derivatives_hpp__
7
8
9
10
11

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/check.hpp"

12
namespace pinocchio
13
14
15
16
17
18
{
  
  ///
  /// \brief Computes the analytical derivatives of the centroidal dynamics with respect to
  ///        the joint configuration vector, velocity and acceleration.
  ///
19
20
21
  /// \details Computes the first order approximation of the centroidal dynamics time derivative
  ///          and corresponds to the following equation
  ///          \f$
22
23
24
  ///               d\dot{h_{g}} = \frac{\partial \dot{h_{g}}}{\partial \mathbf{q}} d\mathbf{q}
  ///                            + \frac{\partial \dot{h_{g}}}{\partial \mathbf{v}} d\mathbf{v}
  ///                            + \frac{\partial \dot{h_{g}}}{\partial \mathbf{a}} d\mathbf{a}
25
26
  ///          \f$
  ///
27
28
  /// \tparam JointCollection Collection of Joint types.
  /// \tparam ConfigVectorType Type of the joint configuration vector.
29
30
  /// \tparam TangentVectorType1 Type of the joint velocity vector.
  /// \tparam TangentVectorType2 Type of the joint acceleration vector.
31
32
33
34
35
36
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[in] q The joint configuration vector (dim model.nq).
  /// \param[in] v The joint velocity vector (dim model.nv).
  /// \param[in] a The joint acceleration vector (dim model.nv).
37
  /// \param[out] dh_dq The partial derivative of the centroidal momentum with respect to the configuration vector (dim 6 x model.nv).
38
39
40
41
  /// \param[out] dhdot_dq The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv).
  /// \param[out] dhdot_dv The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv).
  /// \param[out] dhdot_da The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv).
  ///
42
43
44
  /// \result It also computes the current centroidal dynamics and its time derivative.
  ///         For information, the centroidal momentum matrix is equivalent to dhdot_da.
  ///
45
46
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
           typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
47
           typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3>
48
49
50
51
52
53
  inline void
  computeCentroidalDynamicsDerivatives(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,
54
                                       const Eigen::MatrixBase<Matrix6xLike0> & dh_dq,
55
56
57
                                       const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq,
                                       const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
                                       const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da);
58
59
  ///
  /// \brief Retrive the analytical derivatives of the centroidal dynamics from the RNEA derivatives.
60
  ///        pinocchio::computeRNEADerivatives should have been called first.
61
62
63
64
  ///
  /// \details Computes the first order approximation of the centroidal dynamics time derivative
  ///          and corresponds to the following equation
  ///          \f$
65
66
67
  ///               d\dot{h_{g}} = \frac{\partial \dot{h_{g}}}{\partial \mathbf{q}} d\mathbf{q}
  ///                            + \frac{\partial \dot{h_{g}}}{\partial \mathbf{v}} d\mathbf{v}
  ///                            + \frac{\partial \dot{h_{g}}}{\partial \mathbf{a}} d\mathbf{a}
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
  ///          \f$
  ///
  /// \param[in] model The model structure of the rigid body system.
  /// \param[in] data The data structure of the rigid body system.
  /// \param[out] dhdot_dq The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv).
  /// \param[out] dhdot_dv The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv).
  /// \param[out] dhdot_da The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv).
  ///
  /// \result It also computes the current centroidal dynamics and its time derivative.
  ///         For information, the centroidal momentum matrix is equivalent to dhdot_da.
  ///
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
           typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3>
  inline void
  getCentroidalDynamicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                   const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq,
                                   const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
                                   const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da);
87
88
  
  
89
} // namespace pinocchio
90
91
92
93

/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/centroidal-derivatives.hxx"

94
#endif // ifndef __pinocchio_algorithm_centroidal_derivatives_hpp__