GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-inverse-dynamics.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 78 80 97.5%
Branches: 102 284 35.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2024 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_contact_inverse_dynamics__hpp__
6 #define __pinocchio_algorithm_contact_inverse_dynamics__hpp__
7
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10 #include "pinocchio/algorithm/constraints/coulomb-friction-cone.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include <boost/optional/optional.hpp>
13 #include <pinocchio/algorithm/contact-cholesky.hpp>
14 #include <pinocchio/algorithm/contact-jacobian.hpp>
15 #include "pinocchio/algorithm/proximal.hpp"
16
17 #include <boost/optional.hpp>
18
19 namespace pinocchio
20 {
21
22 ///
23 /// \brief Compute the contact impulses given a target velocity of contact points.
24 ///
25 /// \tparam JointCollection Collection of Joint types.
26 /// \tparam ConfigVectorType Type of the joint configuration vector.
27 /// \tparam TangentVectorType1 Type of the joint velocity vector.
28 /// \tparam TangentVectorType2 Type of the joint acceleration vector.
29 ///
30 /// \param[in] model The model structure of the rigid body system.
31 /// \param[in] data The data structure of the rigid body system.
32 /// \param[in] c_ref The contact point velocity
33 /// \param[in] contact_models The list of contact models.
34 /// \param[in] contact_datas The list of contact_datas.
35 /// \param[in] cones list of friction cones.
36 /// \param[in] R vector representing the diagonal of the compliance matrix.
37 /// \param[in] constraint_correction vector representing the constraint correction.
38 /// \param[in] settings The settings for the proximal algorithm.
39 /// \param[in] impulse_guess initial guess for the contact impulses.
40 ///
41 /// \return The desired joint torques stored in data.tau.
42 ///
43 template<
44 typename Scalar,
45 int Options,
46 template<typename, int> class JointCollectionTpl,
47 typename VectorLikeC,
48 class ConstraintModelAllocator,
49 class ConstraintDataAllocator,
50 class CoulombFrictionConelAllocator,
51 typename VectorLikeR,
52 typename VectorLikeGamma,
53 typename VectorLikeImp>
54 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
55 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
56 7 TangentVectorType & computeContactImpulses(
57 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
58 DataTpl<Scalar, Options, JointCollectionTpl> & data,
59 const Eigen::MatrixBase<VectorLikeC> & c_ref,
60 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
61 contact_models,
62 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
63 const std::vector<CoulombFrictionConeTpl<Scalar>, CoulombFrictionConelAllocator> & cones,
64 const Eigen::MatrixBase<VectorLikeR> & R,
65 const Eigen::MatrixBase<VectorLikeGamma> & constraint_correction,
66 ProximalSettingsTpl<Scalar> & settings,
67 const boost::optional<VectorLikeImp> & impulse_guess = boost::none)
68 {
69
70 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
71 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
72 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
73
74 7 int problem_size = R.size();
75 7 int n_contacts = (int)problem_size / 3;
76
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 7 PINOCCHIO_CHECK_ARGUMENT_SIZE(constraint_correction.size(), problem_size);
77
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 7 PINOCCHIO_CHECK_ARGUMENT_SIZE(contact_models.size(), n_contacts);
78
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 7 PINOCCHIO_CHECK_ARGUMENT_SIZE(contact_datas.size(), n_contacts);
79
2/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 7 PINOCCHIO_CHECK_INPUT_ARGUMENT(
80 check_expression_if_real<Scalar>(settings.mu > Scalar(0)), "mu has to be strictly positive");
81
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 7 MatrixXs J = MatrixXs::Zero(problem_size, model.nv); // TODO: malloc
82
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 7 getConstraintsJacobian(model, data, contact_models, contact_datas, J);
83
5/10
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
 7 VectorXs c_ref_cor, desaxce_correction, R_prox, impulse_c_prev, dimpulse_c; // TODO: malloc
84
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 7 R_prox = R + VectorXs::Constant(problem_size, settings.mu);
85
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 7 c_ref_cor = c_ref + constraint_correction;
86
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 7 if (impulse_guess)
87 {
88
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 6 data.impulse_c = impulse_guess.get();
89
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 3 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 6 PINOCCHIO_CHECK_ARGUMENT_SIZE(data.impulse_c.size(), problem_size);
90 }
91 else
92 {
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 data.impulse_c.setZero();
94 }
95
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 7 Scalar impulse_c_prev_norm_inf = data.impulse_c.template lpNorm<Eigen::Infinity>();
96 Scalar complementarity, dual_feasibility;
97 7 bool abs_prec_reached = false, rel_prec_reached = false;
98 7 const size_t nc = cones.size(); // num constraints
99 7 settings.iter = 1;
100
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 7 for (; settings.iter <= settings.max_iter; ++settings.iter)
101 {
102
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 7 impulse_c_prev = data.impulse_c;
103
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 4 times.
 21 for (size_t cone_id = 0; cone_id < nc; ++cone_id)
104 {
105 14 const Eigen::DenseIndex row_id = 3 * cone_id;
106 14 const auto & cone = cones[cone_id];
107
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 auto impulse_segment = data.impulse_c.template segment<3>(row_id);
108
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 auto impulse_prev_segment = impulse_c_prev.template segment<3>(row_id);
109
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 auto R_prox_segment = R_prox.template segment<3>(row_id);
110 // Vector3 desaxce_segment;
111 // auto desaxce_segment = desaxce_correction.template segment<3>(row_id);
112
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 auto c_ref_segment = c_ref.template segment<3>(row_id);
113
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 14 Vector3 desaxce_segment = cone.computeNormalCorrection(
114 c_ref_segment
115
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 14 + (R.template segment<3>(row_id).array() * impulse_segment.array()).matrix());
116
7/14
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
 14 impulse_segment =
117
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 -((c_ref_segment + desaxce_segment - settings.mu * impulse_prev_segment).array()
118
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 14 / R_prox_segment.array())
119 .matrix();
120
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 14 impulse_segment = cone.weightedProject(impulse_segment, R_prox_segment);
121 }
122
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 7 dimpulse_c = data.impulse_c - impulse_c_prev;
123
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 7 settings.relative_residual = dimpulse_c.template lpNorm<Eigen::Infinity>();
124
125 // if( check_expression_if_real<Scalar,false>(complementarity <= this->absolute_precision)
126 // && check_expression_if_real<Scalar,false>(dual_feasibility <= this->absolute_precision)
127 // && check_expression_if_real<Scalar,false>(primal_feasibility <=
128 // this->absolute_precision))
129 // abs_prec_reached = true;
130 // else
131 // abs_prec_reached = false;
132
133
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 7 const Scalar impulse_c_norm_inf = data.impulse_c.template lpNorm<Eigen::Infinity>();
134 14 if (check_expression_if_real<Scalar, false>(
135 7 settings.relative_residual
136
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
 7 <= settings.relative_accuracy * math::max(impulse_c_norm_inf, impulse_c_prev_norm_inf)))
137 7 rel_prec_reached = true;
138 else
139 ✗ rel_prec_reached = false;
140
141
2/4
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
 7 if (abs_prec_reached || rel_prec_reached)
142 break;
143
144 ✗ impulse_c_prev_norm_inf = impulse_c_norm_inf;
145 }
146 7 return data.impulse_c;
147 7 }
148
149 ///
150 /// \brief The Contact Inverse Dynamics algorithm. It computes the inverse dynamics in the
151 /// presence of contacts, aka the joint torques according to the current state of the system and
152 /// the desired joint accelerations.
153 ///
154 /// \tparam JointCollection Collection of Joint types.
155 /// \tparam ConfigVectorType Type of the joint configuration vector.
156 /// \tparam TangentVectorType1 Type of the joint velocity vector.
157 /// \tparam TangentVectorType2 Type of the joint acceleration vector.
158 ///
159 /// \param[in] model The model structure of the rigid body system.
160 /// \param[in] data The data structure of the rigid body system.
161 /// \param[in] q The joint configuration vector (dim model.nq).
162 /// \param[in] v The joint velocity vector (dim model.nv).
163 /// \param[in] a The joint acceleration vector (dim model.nv).
164 /// \param[in] dt The time step.
165 /// \param[in] contact_models The list of contact models.
166 /// \param[in] contact_datas The list of contact_datas.
167 /// \param[in] cones list of friction cones.
168 /// \param[in] R vector representing the diagonal of the compliance matrix.
169 /// \param[in] constraint_correction vector representing the constraint correction.
170 /// \param[in] settings The settings for the proximal algorithm.
171 /// \param[in] lambda_guess initial guess for the contact forces.
172 ///
173 /// \return The desired joint torques stored in data.tau.
174 ///
175 template<
176 typename Scalar,
177 int Options,
178 template<typename, int> class JointCollectionTpl,
179 typename ConfigVectorType,
180 typename TangentVectorType1,
181 typename TangentVectorType2,
182 class ConstraintModelAllocator,
183 class ConstraintDataAllocator,
184 class CoulombFrictionConelAllocator,
185 typename VectorLikeR,
186 typename VectorLikeGamma,
187 typename VectorLikeLam>
188 PINOCCHIO_UNSUPPORTED_MESSAGE("The API will change towards more flexibility")
189 const typename DataTpl<Scalar, Options, JointCollectionTpl>::
190 4 TangentVectorType & contactInverseDynamics(
191 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
192 DataTpl<Scalar, Options, JointCollectionTpl> & data,
193 const Eigen::MatrixBase<ConfigVectorType> & q,
194 const Eigen::MatrixBase<TangentVectorType1> & v,
195 const Eigen::MatrixBase<TangentVectorType2> & a,
196 Scalar dt,
197 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
198 contact_models,
199 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
200 const std::vector<CoulombFrictionConeTpl<Scalar>, CoulombFrictionConelAllocator> & cones,
201 const Eigen::MatrixBase<VectorLikeR> & R,
202 const Eigen::MatrixBase<VectorLikeGamma> & constraint_correction,
203 ProximalSettingsTpl<Scalar> & settings,
204 const boost::optional<VectorLikeLam> & lambda_guess = boost::none)
205 {
206
207 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> MatrixXs;
208 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> VectorXs;
209 typedef ForceTpl<Scalar, Options> Force;
210 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
211
212 4 int problem_size = R.size();
213 4 int n_contacts = (int)problem_size / 3;
214
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 MatrixXs J = MatrixXs::Zero(problem_size, model.nv); // TODO: malloc
215
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 getConstraintsJacobian(model, data, contact_models, contact_datas, J);
216
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 VectorXs v_ref, c_ref, tau_c;
217
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 v_ref = v + dt * a;
218
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 c_ref.noalias() = J * v_ref; // TODO should rather use the displacement
219 4 boost::optional<VectorXs> impulse_guess = boost::none;
220
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 if (lambda_guess)
221 {
222
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 data.impulse_c = lambda_guess.get();
223
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 data.impulse_c *= dt;
224
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 impulse_guess = boost::make_optional(data.impulse_c);
225 }
226
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 computeContactImpulses(
227 model, data, c_ref, contact_models, contact_datas, cones, R, constraint_correction, settings,
228 impulse_guess);
229
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 data.lambda_c = data.impulse_c / dt;
230
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 4 container::aligned_vector<Force> fext(model.njoints);
231
2/2
✓ Branch 0 taken 130 times.
✓ Branch 1 taken 4 times.
 134 for (int i = 0; i < model.njoints; i++)
232 {
233
2/4
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 130 times.
✗ Branch 6 not taken.
 130 fext[i] = Force::Zero();
234 }
235
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 4 times.
 12 for (int i = 0; i < n_contacts; i++)
236 {
237 8 const auto & cmodel = contact_models[i];
238 8 const Eigen::DenseIndex row_id = 3 * i;
239
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 auto lambda_segment = data.lambda_c.template segment<3>(row_id);
240 8 typename RigidConstraintData::Matrix6 actInv_transpose1 =
241
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 cmodel.joint1_placement.toActionMatrixInverse();
242
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 actInv_transpose1.transposeInPlace();
243
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
 8 fext[cmodel.joint1_id] += Force(actInv_transpose1.template leftCols<3>() * lambda_segment);
244 8 typename RigidConstraintData::Matrix6 actInv_transpose2 =
245
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 cmodel.joint2_placement.toActionMatrixInverse();
246
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 actInv_transpose2.transposeInPlace();
247
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
 8 fext[cmodel.joint2_id] += Force(actInv_transpose2.template leftCols<3>() * lambda_segment);
248 }
249
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 rnea(model, data, q, v, a, fext);
250 4 return data.tau;
251 4 }
252
253 } // namespace pinocchio
254
255 // #if PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
256 // #include "pinocchio/algorithm/contact-inverse-dynamics.txx"
257 // #endif // PINOCCHIO_ENABLE_TEMPLATE_INSTANTIATION
258
259 #endif // ifndef __pinocchio_algorithm_contact_inverse_dynamics_hpp__
260