GCC Code Coverage Report

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