Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/constrained-dynamics.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 7 | 7 | 100.0% |
Branches: | 4 | 8 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2019-2021 INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_algorithm_constrained_dynamics_hpp__ | ||
6 | #define __pinocchio_algorithm_constrained_dynamics_hpp__ | ||
7 | |||
8 | #include "pinocchio/algorithm/contact-info.hpp" | ||
9 | |||
10 | #include "pinocchio/algorithm/proximal.hpp" | ||
11 | |||
12 | namespace pinocchio | ||
13 | { | ||
14 | |||
15 | /// | ||
16 | /// \brief Init the forward dynamics data according to the contact information contained in | ||
17 | /// contact_models. | ||
18 | /// | ||
19 | /// \tparam JointCollection Collection of Joint types. | ||
20 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
21 | /// \tparam TangentVectorType1 Type of the joint velocity vector. | ||
22 | /// \tparam TangentVectorType2 Type of the joint torque vector. | ||
23 | /// \tparam Allocator Allocator class for the std::vector. | ||
24 | /// | ||
25 | /// \param[in] model The model structure of the rigid body system. | ||
26 | /// \param[in] data The data structure of the rigid body system. | ||
27 | /// \param[in] contact_models Vector of contact information related to the problem. | ||
28 | /// | ||
29 | template< | ||
30 | typename Scalar, | ||
31 | int Options, | ||
32 | template<typename, int> | ||
33 | class JointCollectionTpl, | ||
34 | class Allocator> | ||
35 | PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") | ||
36 | inline void initConstraintDynamics( | ||
37 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
38 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
39 | const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models); | ||
40 | |||
41 | /// | ||
42 | /// \brief Computes the forward dynamics with contact constraints according to a given list of | ||
43 | /// contact information. | ||
44 | /// | ||
45 | /// \note When using forwardDynamics for the first time, you should call first | ||
46 | /// initConstraintDynamics to initialize the internal memory used in the algorithm. | ||
47 | /// | ||
48 | /// It computes the following problem: \[ | ||
49 | /// \f[ | ||
50 | /// \begin{eqnarray} | ||
51 | /// \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ | ||
52 | /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 | ||
53 | /// \end{eqnarray} | ||
54 | /// \f] where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without | ||
55 | /// constraints), \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ | ||
56 | /// \gamma \f$ is the constraint drift. | ||
57 | /// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse | ||
58 | /// is performed. | ||
59 | /// | ||
60 | /// \tparam JointCollection Collection of Joint types. | ||
61 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
62 | /// \tparam TangentVectorType1 Type of the joint velocity vector. | ||
63 | /// \tparam TangentVectorType2 Type of the joint torque vector. | ||
64 | /// \tparam Allocator Allocator class for the std::vector. | ||
65 | /// | ||
66 | /// \param[in] model The model structure of the rigid body system. | ||
67 | /// \param[in] data The data structure of the rigid body system. | ||
68 | /// \param[in] q The joint configuration (size model.nq). | ||
69 | /// \param[in] v The joint velocity (size model.nv). | ||
70 | /// \param[in] tau The joint torque vector (size model.nv). | ||
71 | /// \param[in] contact_models Vector of contact models. | ||
72 | /// \param[in] contact_datas Vector of contact data. | ||
73 | /// \param[in] settings Proximal settings (mu, accuracy and maximal number of iterations). | ||
74 | /// | ||
75 | /// \note A hint: a typical value for mu is 1e-12 when two contact constraints are redundant. | ||
76 | /// | ||
77 | /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers | ||
78 | /// linked to the contact forces are available throw data.lambda_c vector. | ||
79 | /// | ||
80 | template< | ||
81 | typename Scalar, | ||
82 | int Options, | ||
83 | template<typename, int> | ||
84 | class JointCollectionTpl, | ||
85 | typename ConfigVectorType, | ||
86 | typename TangentVectorType1, | ||
87 | typename TangentVectorType2, | ||
88 | class ConstraintModelAllocator, | ||
89 | class ConstraintDataAllocator> | ||
90 | PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") | ||
91 | inline const | ||
92 | typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics( | ||
93 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
94 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
95 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
96 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
97 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
98 | const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & | ||
99 | contact_models, | ||
100 | std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas, | ||
101 | ProximalSettingsTpl<Scalar> & settings); | ||
102 | |||
103 | /// | ||
104 | /// \brief Computes the forward dynamics with contact constraints according to a given list of | ||
105 | /// Contact information. | ||
106 | /// | ||
107 | /// \note When using forwardDynamics for the first time, you should call first | ||
108 | /// initConstraintDynamics to initialize the internal memory used in the algorithm. | ||
109 | /// | ||
110 | /// It computes the following problem: \f[ | ||
111 | /// \begin{eqnarray} | ||
112 | /// \underset{\ddot{q}}{\min} & & \| \ddot{q} - \ddot{q}_{\text{free}} \|_{M(q)} \\ | ||
113 | /// \text{s.t.} & & J (q) \ddot{q} + \gamma (q, \dot{q}) = 0 | ||
114 | /// \end{eqnarray} | ||
115 | /// \f] where \f$ \ddot{q}_{\text{free}} \f$ is the free acceleration (i.e. without | ||
116 | /// constraints), \f$ M \f$ is the mass matrix, \f$ J \f$ the constraint Jacobian and \f$ | ||
117 | /// \gamma \f$ is the constraint drift. | ||
118 | /// By default, the constraint Jacobian is assumed to be full rank, and undamped Cholesky inverse | ||
119 | /// is performed. | ||
120 | /// | ||
121 | /// \tparam JointCollection Collection of Joint types. | ||
122 | /// \tparam ConfigVectorType Type of the joint configuration vector. | ||
123 | /// \tparam TangentVectorType1 Type of the joint velocity vector. | ||
124 | /// \tparam TangentVectorType2 Type of the joint torque vector. | ||
125 | /// \tparam Allocator Allocator class for the std::vector. | ||
126 | /// | ||
127 | /// \param[in] model The model structure of the rigid body system. | ||
128 | /// \param[in] data The data structure of the rigid body system. | ||
129 | /// \param[in] q The joint configuration (size model.nq). | ||
130 | /// \param[in] v The joint velocity (size model.nv). | ||
131 | /// \param[in] tau The joint torque vector (size model.nv). | ||
132 | /// \param[in] contact_models Vector of contact models. | ||
133 | /// \param[in] contact_datas Vector of contact data. | ||
134 | /// | ||
135 | /// \return A reference to the joint acceleration stored in data.ddq. The Lagrange Multipliers | ||
136 | /// linked to the contact forces are available throw data.lambda_c vector. | ||
137 | /// | ||
138 | template< | ||
139 | typename Scalar, | ||
140 | int Options, | ||
141 | template<typename, int> | ||
142 | class JointCollectionTpl, | ||
143 | typename ConfigVectorType, | ||
144 | typename TangentVectorType1, | ||
145 | typename TangentVectorType2, | ||
146 | class ConstraintModelAllocator, | ||
147 | class ConstraintDataAllocator> | ||
148 | PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") | ||
149 | inline const | ||
150 | 2 | typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & constraintDynamics( | |
151 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
152 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
153 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
154 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
155 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
156 | const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> & | ||
157 | contact_models, | ||
158 | std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas) | ||
159 | { | ||
160 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | ProximalSettingsTpl<Scalar> settings; |
161 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return constraintDynamics(model, data, q, v, tau, contact_models, contact_datas, settings); |
162 | } | ||
163 | |||
164 | template< | ||
165 | typename Scalar, | ||
166 | int Options, | ||
167 | template<typename, int> | ||
168 | class JointCollectionTpl, | ||
169 | typename ConfigVectorType, | ||
170 | typename TangentVectorType1, | ||
171 | typename TangentVectorType2, | ||
172 | class ModelAllocator, | ||
173 | class DataAllocator> | ||
174 | PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility") | ||
175 | inline const | ||
176 | 3 | typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & contactABA( | |
177 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model, | ||
178 | DataTpl<Scalar, Options, JointCollectionTpl> & data, | ||
179 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
180 | const Eigen::MatrixBase<TangentVectorType1> & v, | ||
181 | const Eigen::MatrixBase<TangentVectorType2> & tau, | ||
182 | const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models, | ||
183 | std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data) | ||
184 | { | ||
185 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | ProximalSettingsTpl<Scalar> settings = ProximalSettingsTpl<Scalar>(); |
186 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | return contactABA( |
187 | 9 | model, data, q.derived(), v.derived(), tau.derived(), contact_models, contact_data, settings); | |
188 | } | ||
189 | |||
190 | } // namespace pinocchio | ||
191 | |||
192 | #include "pinocchio/algorithm/constrained-dynamics.hxx" | ||
193 | |||
194 | #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
195 | #include "pinocchio/algorithm/constrained-dynamics.txx" | ||
196 | #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION | ||
197 | |||
198 | #endif // ifndef __pinocchio_algorithm_constrained_dynamics_hpp__ | ||
199 |