GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/rnea-second-order-derivatives.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 9 9 100.0%
Branches: 0 0 -%
Line Branch Exec Source
1 //
2 // Copyright (c) 2017-2019 CNRS INRIA
3
4 #ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hpp__
5 #define __pinocchio_algorithm_rnea_second_order_derivatives_hpp__
6
7 #include "pinocchio/container/aligned-vector.hpp"
8 #include "pinocchio/multibody/data.hpp"
9 #include "pinocchio/multibody/model.hpp"
10
11 namespace pinocchio
12 {
13 ///
14 /// \brief Computes the Second-Order partial derivatives of the Recursive Newton
15 /// Euler Algorithm w.r.t the joint configuration, the joint velocity and the
16 /// joint acceleration.
17 ///
18 /// \tparam JointCollection Collection of Joint types.
19 /// \tparam ConfigVectorType Type of the joint configuration vector.
20 /// \tparam TangentVectorType1 Type of the joint velocity vector.
21 /// \tparam TangentVectorType2 Type of the joint acceleration vector.
22 /// \tparam Tensor1 Type of the 3D-Tensor containing the SO partial
23 /// derivative with respect to the joint configuration vector. The elements of
24 /// Torque vector are along the 1st dim, and joint config along 2nd,3rd
25 /// dimensions.
26 /// \tparam Tensor2 Type of the 3D-Tensor containing the
27 /// Second-Order partial derivative with respect to the joint velocity vector.
28 /// The elements of Torque vector are along the 1st dim, and the velocity
29 /// along 2nd,3rd dimensions.
30 /// \tparam Tensor3 Type of the 3D-Tensor
31 /// containing the cross Second-Order partial derivative with respect to the
32 /// joint configuration and velocty vector. The elements of Torque vector are
33 /// along the 1st dim, and the config. vector along 2nd dimension, and velocity
34 /// along the third dimension.
35 ///\tparam Tensor4 Type of the 3D-Tensor containing the cross Second-Order
36 /// partial derivative with respect to the joint configuration and acceleration
37 /// vector. This is also the First-order partial derivative of Mass-Matrix (M)
38 /// with respect to configuration vector. The elements of Torque vector are
39 /// along the 1st dim, and the acceleration vector along 2nd dimension, while
40 /// configuration along the third dimension.
41 ///
42 /// \param[in] model The model structure of the rigid body system.
43 /// \param[in] data The data structure of the rigid body system.
44 /// \param[in] q The joint configuration vector (dim model.nq).
45 /// \param[in] v The joint velocity vector (dim model.nv).
46 /// \param[in] a The joint acceleration vector (dim model.nv).
47 /// \param[out] d2tau_dqdq Second-Order Partial derivative of the generalized
48 /// torque vector with respect to the joint configuration.
49 /// \param[out] d2tau_dvdv Second-Order Partial derivative of the generalized
50 /// torque vector with respect to the joint velocity
51 /// \param[out] dtau_dqdv Cross Second-Order Partial derivative of the
52 /// generalized torque vector with respect to the joint configuration and
53 /// velocity.
54 /// \param[out] dtau_dadq Cross Second-Order Partial derivative of
55 /// the generalized torque vector with respect to the joint configuration and
56 /// accleration.
57 /// \remarks d2tau_dqdq,
58 /// d2tau_dvdv, dtau_dqdv and dtau_dadq must be first initialized with zeros
59 /// (d2tau_dqdq.setZero(), etc). The storage order of the 3D-tensor derivatives is
60 /// important. For d2tau_dqdq, the elements of generalized torque varies along
61 /// the rows, while elements of q vary along the columns and pages of the
62 /// tensor. For dtau_dqdv, the elements of generalized torque varies along the
63 /// rows, while elements of v vary along the columns and elements of q along the
64 /// pages of the tensor. Hence, dtau_dqdv is essentially d (d tau/dq)/dv, with
65 /// outer-most derivative representing the third dimension (pages) of the
66 /// tensor. The tensor dtau_dadq reduces down to dM/dq, and hence the elements
67 /// of q vary along the pages of the tensor. In other words, this tensor
68 /// derivative is d(d tau/da)/dq. All other remaining combinations of
69 /// second-order derivatives of generalized torque are zero. \sa
70 ///
71 template<
72 typename Scalar,
73 int Options,
74 template<typename, int> class JointCollectionTpl,
75 typename ConfigVectorType,
76 typename TangentVectorType1,
77 typename TangentVectorType2,
78 typename Tensor1,
79 typename Tensor2,
80 typename Tensor3,
81 typename Tensor4>
82 inline void ComputeRNEASecondOrderDerivatives(
83 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
84 DataTpl<Scalar, Options, JointCollectionTpl> & data,
85 const Eigen::MatrixBase<ConfigVectorType> & q,
86 const Eigen::MatrixBase<TangentVectorType1> & v,
87 const Eigen::MatrixBase<TangentVectorType2> & a,
88 const Tensor1 & d2tau_dqdq,
89 const Tensor2 & d2tau_dvdv,
90 const Tensor3 & dtau_dqdv,
91 const Tensor4 & dtau_dadq);
92
93 ///
94 /// \brief Computes the Second-Order partial derivatives of the Recursive Newton
95 /// Euler Algorithms
96 /// with respect to the joint configuration, the joint velocity and the
97 /// joint acceleration.
98 ///
99 /// \tparam JointCollection Collection of Joint types.
100 /// \tparam ConfigVectorType Type of the joint configuration vector.
101 /// \tparam TangentVectorType1 Type of the joint velocity vector.
102 /// \tparam TangentVectorType2 Type of the joint acceleration vector.
103 ///
104 /// \param[in] model The model structure of the rigid body system.
105 /// \param[in] data The data structure of the rigid body system.
106 /// \param[in] q The joint configuration vector (dim model.nq).
107 /// \param[in] v The joint velocity vector (dim model.nv).
108 /// \param[in] a The joint acceleration vector (dim model.nv).
109 ///
110 /// \returns The results are stored in data.d2tau_dqdq, data.d2tau_dvdv,
111 /// data.d2tau_dqdv, and data.d2tau_dadq which respectively correspond to the
112 /// Second-Order partial derivatives of the joint torque vector with respect to
113 /// the joint configuration, velocity and cross Second-Order partial derivatives
114 /// with respect to configuration/velocity and configuration/acceleration
115 /// respectively.
116 ///
117 /// \remarks d2tau_dqdq,
118 /// d2tau_dvdv2, d2tau_dqdv and d2tau_dadq must be first initialized with zeros
119 /// (d2tau_dqdq.setZero(),etc). The storage order of the 3D-tensor derivatives is
120 /// important. For d2tau_dqdq, the elements of generalized torque varies along
121 /// the rows, while elements of q vary along the columns and pages of the
122 /// tensor. For d2tau_dqdv, the elements of generalized torque varies along the
123 /// rows, while elements of v vary along the columns and elements of q along the
124 /// pages of the tensor. Hence, d2tau_dqdv is essentially d (d tau/dq)/dv, with
125 /// outer-most derivative representing the third dimension (pages) of the
126 /// tensor. The tensor d2tau_dadq reduces down to dM/dq, and hence the elements
127 /// of q vary along the pages of the tensor. In other words, this tensor
128 /// derivative is d(d tau/da)/dq. All other remaining combinations of
129 /// second-order derivatives of generalized torque are zero. \sa
130
131 template<
132 typename Scalar,
133 int Options,
134 template<typename, int> class JointCollectionTpl,
135 typename ConfigVectorType,
136 typename TangentVectorType1,
137 typename TangentVectorType2>
138 1 inline void ComputeRNEASecondOrderDerivatives(
139 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
140 DataTpl<Scalar, Options, JointCollectionTpl> & data,
141 const Eigen::MatrixBase<ConfigVectorType> & q,
142 const Eigen::MatrixBase<TangentVectorType1> & v,
143 const Eigen::MatrixBase<TangentVectorType2> & a)
144 {
145 1 (data.d2tau_dqdq).setZero();
146 1 (data.d2tau_dvdv).setZero();
147 1 (data.d2tau_dqdv).setZero();
148 1 (data.d2tau_dadq).setZero();
149
150 1 ComputeRNEASecondOrderDerivatives(
151 1 model, data, q.derived(), v.derived(), a.derived(), data.d2tau_dqdq, data.d2tau_dvdv,
152 1 data.d2tau_dqdv, data.d2tau_dadq);
153 1 }
154
155 } // namespace pinocchio
156
157 #include "pinocchio/algorithm/rnea-second-order-derivatives.hxx"
158
159 #endif // ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hpp__
160