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 |