GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/impulse-dynamics.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 75 84 89.3%
Branches: 83 197 42.1%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_impulse_dynamics_hxx__
6 #define __pinocchio_algorithm_impulse_dynamics_hxx__
7
8 #include "pinocchio/algorithm/check.hpp"
9 #include "pinocchio/algorithm/constrained-dynamics.hxx"
10 #include <limits>
11
12 namespace pinocchio
13 {
14
15 template<
16 typename Scalar,
17 int Options,
18 template<typename, int> class JointCollectionTpl,
19 typename ConfigVectorType,
20 typename TangentVectorType1,
21 class ConstraintModelAllocator,
22 class ConstraintDataAllocator>
23 138 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & impulseDynamics(
24 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
25 DataTpl<Scalar, Options, JointCollectionTpl> & data,
26 const Eigen::MatrixBase<ConfigVectorType> & q,
27 const Eigen::MatrixBase<TangentVectorType1> & v_before,
28 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
29 contact_models,
30 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> & contact_datas,
31 const Scalar r_coeff,
32 const ProximalSettingsTpl<Scalar> & settings)
33 {
34
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 138 times.
✗ Branch 4 not taken.
 138 assert(model.check(data) && "data is not consistent with model.");
35
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 138 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 138 PINOCCHIO_CHECK_INPUT_ARGUMENT(
36 q.size() == model.nq, "The joint configuration vector is not of right size");
37
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 138 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 138 PINOCCHIO_CHECK_INPUT_ARGUMENT(
38 v_before.size() == model.nv, "The joint velocity vector is not of right size");
39
2/11
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 138 PINOCCHIO_CHECK_INPUT_ARGUMENT(
40 check_expression_if_real<Scalar>(settings.mu >= Scalar(0)), "mu has to be positive");
41
4/20
✓ Branch 0 taken 138 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 138 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✓ Branch 8 taken 138 times.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
 138 PINOCCHIO_CHECK_INPUT_ARGUMENT(
42 check_expression_if_real<Scalar>((r_coeff >= Scalar(0)) && (r_coeff <= Scalar(1))),
43 "r_coeff has to be in [0,1]");
44
1/4
✗ Branch 2 not taken.
✓ Branch 3 taken 138 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 138 PINOCCHIO_CHECK_INPUT_ARGUMENT(
45 contact_models.size() == contact_datas.size(),
46 "The contact models and data do not have the same vector size.");
47
48 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
49
50 typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel;
51 typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData;
52
53 138 typename Data::TangentVectorType & dq_after = data.dq_after;
54 138 typename Data::ContactCholeskyDecomposition & contact_chol = data.contact_chol;
55 138 typename Data::VectorXs & primal_dual_contact_solution = data.primal_dual_contact_solution;
56
57
1/2
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
 138 data.oYcrb[0].setZero();
58 typedef ContactAndImpulseDynamicsForwardStep<
59 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1, false>
60 Pass1;
61
2/2
✓ Branch 0 taken 3726 times.
✓ Branch 1 taken 138 times.
 3864 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
62 {
63
1/2
✓ Branch 1 taken 3726 times.
✗ Branch 2 not taken.
 3726 Pass1::run(
64 3726 model.joints[i], data.joints[i],
65
1/2
✓ Branch 3 taken 3726 times.
✗ Branch 4 not taken.
 7452 typename Pass1::ArgsType(model, data, q.derived(), v_before.derived()));
66 }
67
68 typedef ContactAndImpulseDynamicsBackwardStep<Scalar, Options, JointCollectionTpl, false> Pass2;
69
2/2
✓ Branch 0 taken 3726 times.
✓ Branch 1 taken 138 times.
 3864 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
70 {
71
2/4
✓ Branch 1 taken 3726 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3726 times.
✗ Branch 6 not taken.
 3726 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
72 }
73
74
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 data.M.diagonal() += model.armature;
75
1/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 138 contact_chol.compute(model, data, contact_models, contact_datas, settings.mu);
76
77 // Centroidal computations
78 typedef typename Data::Force Force;
79 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
80
1/2
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 data.com[0] = data.oYcrb[0].lever();
81
82
1/2
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 138 data.Ag = data.dFda;
83
1/2
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 138 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
84
1/2
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 138 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
85
2/2
✓ Branch 0 taken 4416 times.
✓ Branch 1 taken 138 times.
 4554 for (long i = 0; i < model.nv; ++i)
86
4/8
✓ Branch 1 taken 4416 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4416 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4416 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4416 times.
✗ Branch 12 not taken.
 4416 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
87
88
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 primal_dual_contact_solution.tail(model.nv).setZero();
89 138 Eigen::DenseIndex current_row_id = 0;
90
2/2
✓ Branch 1 taken 272 times.
✓ Branch 2 taken 138 times.
 410 for (size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id)
91 {
92 272 const RigidConstraintModel & contact_model = contact_models[contact_id];
93 272 RigidConstraintData & contact_data = contact_datas[contact_id];
94 272 const int contact_dim = contact_model.size();
95
96 272 const JointIndex joint1_id = contact_model.joint1_id;
97 272 const typename Data::SE3 & oMi = data.oMi[joint1_id];
98 272 typename Data::SE3 & oMc = contact_data.oMc1;
99
100 // Update contact placement
101
2/4
✓ Branch 1 taken 272 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 272 times.
✗ Branch 5 not taken.
 272 oMc = oMi * contact_model.joint1_placement;
102
103
1/2
✓ Branch 1 taken 272 times.
✗ Branch 2 not taken.
 272 typename Data::Motion pre_impact_velocity;
104
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 134 times.
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
 272 switch (contact_model.reference_frame)
105 {
106 ✗ case WORLD: {
107 // Temporary assignment
108 ✗ pre_impact_velocity = data.ov[joint1_id];
109 ✗ break;
110 }
111 134 case LOCAL_WORLD_ALIGNED: {
112 // Temporary assignment
113
1/2
✓ Branch 2 taken 134 times.
✗ Branch 3 not taken.
 134 pre_impact_velocity = data.ov[joint1_id];
114
5/10
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 134 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 134 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 134 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 134 times.
✗ Branch 15 not taken.
 134 pre_impact_velocity.linear() -= oMc.translation().cross(data.ov[joint1_id].angular());
115 134 break;
116 }
117 138 case LOCAL: {
118 // Temporary assignment
119
2/4
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 138 times.
✗ Branch 6 not taken.
 138 pre_impact_velocity = oMc.actInv(data.ov[joint1_id]);
120 138 break;
121 }
122 ✗ default:
123 ✗ assert(false && "must never happened");
124 break;
125 }
126
127
2/3
✓ Branch 0 taken 134 times.
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 272 switch (contact_model.type)
128 {
129 134 case CONTACT_3D:
130
2/12
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ 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 16 not taken.
✗ Branch 17 not taken.
 134 primal_dual_contact_solution.segment(current_row_id, contact_dim) =
131
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 -(1 + r_coeff) * pre_impact_velocity.linear();
132 134 break;
133 138 case CONTACT_6D:
134
2/10
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ 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.
 138 primal_dual_contact_solution.segment(current_row_id, contact_dim) =
135
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 -(1 + r_coeff) * pre_impact_velocity.toVector();
136 138 break;
137 ✗ default:
138 ✗ assert(false && "must never happened");
139 break;
140 }
141
142 272 current_row_id += contact_dim;
143 }
144
145 // Solve the system
146
1/2
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 138 contact_chol.solveInPlace(primal_dual_contact_solution);
147
148 // Retrieve the joint space delta v
149
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 data.ddq = primal_dual_contact_solution.tail(model.nv);
150
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 dq_after = data.ddq + v_before;
151
4/8
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 138 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 138 times.
✗ Branch 11 not taken.
 138 data.impulse_c = -primal_dual_contact_solution.head(contact_chol.constraintDim());
152
153 // Retrieve the impulses
154 138 Eigen::DenseIndex current_row_sol_id = 0;
155
2/2
✓ Branch 1 taken 272 times.
✓ Branch 2 taken 138 times.
 410 for (size_t contact_id = 0; contact_id < contact_models.size(); ++contact_id)
156 {
157 272 const RigidConstraintModel & contact_model = contact_models[contact_id];
158 272 RigidConstraintData & contact_data = contact_datas[contact_id];
159 272 typename RigidConstraintData::Force & impulse = contact_data.contact_force;
160 272 const int contact_dim = contact_model.size();
161
162
2/3
✓ Branch 0 taken 134 times.
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
 272 switch (contact_model.type)
163 {
164 134 case CONTACT_3D: {
165
4/8
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 134 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 134 times.
✗ Branch 11 not taken.
 134 impulse.linear() = -primal_dual_contact_solution.template segment<3>(current_row_sol_id);
166
2/4
✓ Branch 1 taken 134 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 134 times.
✗ Branch 5 not taken.
 134 impulse.angular().setZero();
167 134 break;
168 }
169 138 case CONTACT_6D: {
170 typedef typename Data::VectorXs::template FixedSegmentReturnType<6>::Type Segment6d;
171
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 const ForceRef<Segment6d> i_sol(
172 primal_dual_contact_solution.template segment<6>(current_row_sol_id));
173
2/4
✓ Branch 1 taken 138 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 138 times.
✗ Branch 5 not taken.
 138 impulse = -i_sol;
174 138 break;
175 }
176 ✗ default:
177 ✗ assert(false && "must never happened");
178 break;
179 }
180
181 272 current_row_sol_id += contact_dim;
182 }
183
184 138 return dq_after;
185 }
186
187 } // namespace pinocchio
188
189 #endif // ifndef __pinocchio_algorithm_impulse_dynamics_hxx__
190