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 |
|
|
|