GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/rnea-second-order-derivatives.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 8 0.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>
75 class JointCollectionTpl,
76 typename ConfigVectorType,
77 typename TangentVectorType1,
78 typename TangentVectorType2,
79 typename Tensor1,
80 typename Tensor2,
81 typename Tensor3,
82 typename Tensor4>
83 inline void ComputeRNEASecondOrderDerivatives(
84 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
85 DataTpl<Scalar, Options, JointCollectionTpl> & data,
86 const Eigen::MatrixBase<ConfigVectorType> & q,
87 const Eigen::MatrixBase<TangentVectorType1> & v,
88 const Eigen::MatrixBase<TangentVectorType2> & a,
89 const Tensor1 & d2tau_dqdq,
90 const Tensor2 & d2tau_dvdv,
91 const Tensor3 & dtau_dqdv,
92 const Tensor4 & dtau_dadq);
93
94 ///
95 /// \brief Computes the Second-Order partial derivatives of the Recursive Newton
96 /// Euler Algorithms
97 /// with respect to the joint configuration, the joint velocity and the
98 /// joint acceleration.
99 ///
100 /// \tparam JointCollection Collection of Joint types.
101 /// \tparam ConfigVectorType Type of the joint configuration vector.
102 /// \tparam TangentVectorType1 Type of the joint velocity vector.
103 /// \tparam TangentVectorType2 Type of the joint acceleration vector.
104 ///
105 /// \param[in] model The model structure of the rigid body system.
106 /// \param[in] data The data structure of the rigid body system.
107 /// \param[in] q The joint configuration vector (dim model.nq).
108 /// \param[in] v The joint velocity vector (dim model.nv).
109 /// \param[in] a The joint acceleration vector (dim model.nv).
110 ///
111 /// \returns The results are stored in data.d2tau_dqdq, data.d2tau_dvdv,
112 /// data.d2tau_dqdv, and data.d2tau_dadq which respectively correspond to the
113 /// Second-Order partial derivatives of the joint torque vector with respect to
114 /// the joint configuration, velocity and cross Second-Order partial derivatives
115 /// with respect to configuration/velocity and configuration/acceleration
116 /// respectively.
117 ///
118 /// \remarks d2tau_dqdq,
119 /// d2tau_dvdv2, d2tau_dqdv and d2tau_dadq must be first initialized with zeros
120 /// (d2tau_dqdq.setZero(),etc). The storage order of the 3D-tensor derivatives is
121 /// important. For d2tau_dqdq, the elements of generalized torque varies along
122 /// the rows, while elements of q vary along the columns and pages of the
123 /// tensor. For d2tau_dqdv, the elements of generalized torque varies along the
124 /// rows, while elements of v vary along the columns and elements of q along the
125 /// pages of the tensor. Hence, d2tau_dqdv is essentially d (d tau/dq)/dv, with
126 /// outer-most derivative representing the third dimension (pages) of the
127 /// tensor. The tensor d2tau_dadq reduces down to dM/dq, and hence the elements
128 /// of q vary along the pages of the tensor. In other words, this tensor
129 /// derivative is d(d tau/da)/dq. All other remaining combinations of
130 /// second-order derivatives of generalized torque are zero. \sa
131
132 template<
133 typename Scalar,
134 int Options,
135 template<typename, int>
136 class JointCollectionTpl,
137 typename ConfigVectorType,
138 typename TangentVectorType1,
139 typename TangentVectorType2>
140 ✗ inline void ComputeRNEASecondOrderDerivatives(
141 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
142 DataTpl<Scalar, Options, JointCollectionTpl> & data,
143 const Eigen::MatrixBase<ConfigVectorType> & q,
144 const Eigen::MatrixBase<TangentVectorType1> & v,
145 const Eigen::MatrixBase<TangentVectorType2> & a)
146 {
147 ✗ (data.d2tau_dqdq).setZero();
148 ✗ (data.d2tau_dvdv).setZero();
149 ✗ (data.d2tau_dqdv).setZero();
150 ✗ (data.d2tau_dadq).setZero();
151
152 ✗ ComputeRNEASecondOrderDerivatives(
153 ✗ model, data, q.derived(), v.derived(), a.derived(), data.d2tau_dqdq, data.d2tau_dvdv,
154 ✗ data.d2tau_dqdv, data.d2tau_dadq);
155 }
156
157 } // namespace pinocchio
158
159 #include "pinocchio/algorithm/rnea-second-order-derivatives.hxx"
160
161 #endif // ifndef __pinocchio_algorithm_rnea_second_order_derivatives_hpp__
162