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 |