GCC Code Coverage Report

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