Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2020-2024 INRIA CNRS |
3 |
|
|
// Copyright (c) 2023 KU Leuven |
4 |
|
|
// |
5 |
|
|
|
6 |
|
|
#ifndef __pinocchio_algorithm_contact_delassus_hxx__ |
7 |
|
|
#define __pinocchio_algorithm_contact_delassus_hxx__ |
8 |
|
|
|
9 |
|
|
#include "pinocchio/algorithm/check.hpp" |
10 |
|
|
#include "pinocchio/algorithm/contact-info.hpp" |
11 |
|
|
#include "pinocchio/algorithm/model.hpp" |
12 |
|
|
#include "pinocchio/multibody/fwd.hpp" |
13 |
|
|
|
14 |
|
|
namespace pinocchio |
15 |
|
|
{ |
16 |
|
|
|
17 |
|
|
template< |
18 |
|
|
typename Scalar, |
19 |
|
|
int Options, |
20 |
|
|
template<typename, int> |
21 |
|
|
class JointCollectionTpl, |
22 |
|
|
typename ConfigVectorType> |
23 |
|
|
struct ComputeOSIMForwardStep |
24 |
|
|
: public fusion::JointUnaryVisitorBase< |
25 |
|
|
ComputeOSIMForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>> |
26 |
|
|
{ |
27 |
|
|
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
28 |
|
|
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; |
29 |
|
|
|
30 |
|
|
typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType; |
31 |
|
|
|
32 |
|
|
template<typename JointModel> |
33 |
|
✗ |
static void algo( |
34 |
|
|
const pinocchio::JointModelBase<JointModel> & jmodel, |
35 |
|
|
pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, |
36 |
|
|
const Model & model, |
37 |
|
|
Data & data, |
38 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q) |
39 |
|
|
{ |
40 |
|
|
typedef typename Model::JointIndex JointIndex; |
41 |
|
|
|
42 |
|
✗ |
const JointIndex i = jmodel.id(); |
43 |
|
✗ |
jmodel.calc(jdata.derived(), q.derived()); |
44 |
|
|
|
45 |
|
✗ |
const JointIndex parent = model.parents[i]; |
46 |
|
✗ |
data.liMi[i] = model.jointPlacements[i] * jdata.M(); |
47 |
|
✗ |
if (parent > 0) |
48 |
|
✗ |
data.oMi[i] = data.oMi[parent] * data.liMi[i]; |
49 |
|
|
else |
50 |
|
✗ |
data.oMi[i] = data.liMi[i]; |
51 |
|
|
|
52 |
|
✗ |
jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S()); |
53 |
|
✗ |
data.oYcrb[i] = data.oMi[i].act(model.inertias[i]); |
54 |
|
✗ |
data.oYaba[i] = data.oYcrb[i].matrix(); |
55 |
|
|
} |
56 |
|
|
}; |
57 |
|
|
|
58 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
59 |
|
|
struct ComputeOSIMBackwardStep |
60 |
|
|
: public fusion::JointUnaryVisitorBase< |
61 |
|
|
ComputeOSIMBackwardStep<Scalar, Options, JointCollectionTpl>> |
62 |
|
|
{ |
63 |
|
|
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
64 |
|
|
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; |
65 |
|
|
|
66 |
|
|
typedef boost::fusion::vector<const Model &, Data &> ArgsType; |
67 |
|
|
|
68 |
|
|
template<typename JointModel> |
69 |
|
✗ |
static void algo( |
70 |
|
|
const JointModelBase<JointModel> & jmodel, |
71 |
|
|
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
72 |
|
|
const Model & model, |
73 |
|
|
Data & data) |
74 |
|
|
{ |
75 |
|
|
typedef typename Model::JointIndex JointIndex; |
76 |
|
|
typedef typename Data::Inertia Inertia; |
77 |
|
|
typedef typename Data::Matrix6 Matrix6; |
78 |
|
|
typedef typename Data::Matrix6x Matrix6x; |
79 |
|
|
|
80 |
|
✗ |
const JointIndex i = jmodel.id(); |
81 |
|
✗ |
const JointIndex parent = model.parents[i]; |
82 |
|
✗ |
typename Inertia::Matrix6 & Ia = data.oYaba[i]; |
83 |
|
|
|
84 |
|
|
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock; |
85 |
|
✗ |
ColBlock Jcols = jmodel.jointCols(data.J); |
86 |
|
|
|
87 |
|
✗ |
jdata.U().noalias() = Ia * Jcols; |
88 |
|
✗ |
jdata.StU().noalias() = Jcols.transpose() * jdata.U(); |
89 |
|
|
|
90 |
|
|
// Account for the rotor inertia contribution |
91 |
|
✗ |
jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); |
92 |
|
|
|
93 |
|
✗ |
internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv()); |
94 |
|
|
|
95 |
|
✗ |
jdata.UDinv().noalias() = |
96 |
|
|
Jcols |
97 |
|
✗ |
* jdata.Dinv().transpose(); //@justin can we remove the transpose since Dinv() is symmetric? |
98 |
|
✗ |
data.oK[i].noalias() = jdata.UDinv() * Jcols.transpose(); |
99 |
|
✗ |
data.oL[i].noalias() = -jdata.UDinv() * jdata.U().transpose(); |
100 |
|
✗ |
data.oL[i] += Matrix6::Identity(); |
101 |
|
|
|
102 |
|
✗ |
if (parent > 0) |
103 |
|
|
{ |
104 |
|
✗ |
jdata.UDinv().noalias() = jdata.U() * jdata.Dinv(); |
105 |
|
✗ |
data.oYaba[parent] += Ia; |
106 |
|
✗ |
data.oYaba[parent].noalias() -= jdata.UDinv() * jdata.U().transpose(); |
107 |
|
|
} |
108 |
|
|
} |
109 |
|
|
}; |
110 |
|
|
|
111 |
|
|
template< |
112 |
|
|
typename Scalar, |
113 |
|
|
int Options, |
114 |
|
|
template<typename, int> |
115 |
|
|
class JointCollectionTpl, |
116 |
|
|
typename ConfigVectorType, |
117 |
|
|
class ModelAllocator, |
118 |
|
|
class DataAllocator, |
119 |
|
|
typename MatrixType> |
120 |
|
✗ |
void computeDelassusMatrix( |
121 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
122 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
123 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
124 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models, |
125 |
|
|
std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data, |
126 |
|
|
const Eigen::MatrixBase<MatrixType> & delassus_, |
127 |
|
|
const Scalar mu) |
128 |
|
|
{ |
129 |
|
✗ |
assert(model.check(data) && "data is not consistent with model."); |
130 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE( |
131 |
|
|
q.size(), model.nq, "The joint configuration vector is not of right size"); |
132 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
133 |
|
|
check_expression_if_real<Scalar>(mu >= Scalar(0)), "mu has to be positive"); |
134 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE( |
135 |
|
|
contact_models.size(), contact_data.size(), "contact models and data size are not the same"); |
136 |
|
|
|
137 |
|
✗ |
MatrixType & delassus = delassus_.const_cast_derived(); |
138 |
|
✗ |
const size_t constraint_total_size = getTotalConstraintSize(contact_models); |
139 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE(delassus_.rows(), (Eigen::DenseIndex)constraint_total_size); |
140 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE(delassus_.cols(), (Eigen::DenseIndex)constraint_total_size); |
141 |
|
|
|
142 |
|
|
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
143 |
|
|
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; |
144 |
|
|
|
145 |
|
|
typedef typename Model::JointIndex JointIndex; |
146 |
|
|
typedef typename Model::SE3 SE3; |
147 |
|
|
typedef typename Model::IndexVector IndexVector; |
148 |
|
|
typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel; |
149 |
|
|
typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData; |
150 |
|
|
|
151 |
|
|
typedef ComputeOSIMForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> Pass1; |
152 |
|
✗ |
for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
153 |
|
|
{ |
154 |
|
✗ |
Pass1::run( |
155 |
|
✗ |
model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); |
156 |
|
|
} |
157 |
|
|
|
158 |
|
✗ |
for (size_t k = 0; k < contact_models.size(); ++k) |
159 |
|
|
{ |
160 |
|
✗ |
const RigidConstraintModel & cmodel = contact_models[k]; |
161 |
|
✗ |
RigidConstraintData & cdata = contact_data[k]; |
162 |
|
|
|
163 |
|
✗ |
const JointIndex joint1_id = cmodel.joint1_id; |
164 |
|
|
|
165 |
|
|
// Compute relative placement between the joint and the contact frame |
166 |
|
✗ |
SE3 & oMc = cdata.oMc1; |
167 |
|
✗ |
oMc = data.oMi[joint1_id] * cmodel.joint1_placement; // contact placement |
168 |
|
|
|
169 |
|
|
typedef typename Data::Inertia Inertia; |
170 |
|
|
typedef typename Inertia::Symmetric3 Symmetric3; |
171 |
|
|
|
172 |
|
|
// Add contact inertia to the joint articulated inertia |
173 |
|
✗ |
Symmetric3 S(Symmetric3::Zero()); |
174 |
|
✗ |
if (cmodel.type == CONTACT_6D) |
175 |
|
✗ |
S.setDiagonal(Symmetric3::Vector3::Constant(mu)); |
176 |
|
|
|
177 |
|
✗ |
const Inertia contact_inertia(mu, oMc.translation(), S); |
178 |
|
✗ |
data.oYaba[joint1_id] += contact_inertia.matrix(); |
179 |
|
|
} |
180 |
|
|
|
181 |
|
|
typedef ComputeOSIMBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; |
182 |
|
✗ |
for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
183 |
|
|
{ |
184 |
|
✗ |
Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
185 |
|
|
} |
186 |
|
|
|
187 |
|
✗ |
Eigen::DenseIndex current_row_id = 0; |
188 |
|
✗ |
for (size_t k = 0; k < contact_models.size(); ++k) |
189 |
|
|
{ |
190 |
|
|
typedef typename RigidConstraintData::VectorOfMatrix6 VectorOfMatrix6; |
191 |
|
✗ |
const RigidConstraintModel & cmodel = contact_models[k]; |
192 |
|
✗ |
RigidConstraintData & cdata = contact_data[k]; |
193 |
|
|
|
194 |
|
✗ |
const JointIndex joint1_id = cmodel.joint1_id; |
195 |
|
✗ |
const SE3 & oMc1 = cdata.oMc1; |
196 |
|
✗ |
const IndexVector & support1 = model.supports[joint1_id]; |
197 |
|
|
|
198 |
|
|
{ |
199 |
|
✗ |
VectorOfMatrix6 & propagators = cdata.extended_motion_propagators_joint1; |
200 |
|
✗ |
VectorOfMatrix6 & lambdas = cdata.lambdas_joint1; |
201 |
|
✗ |
switch (cmodel.type) |
202 |
|
|
{ |
203 |
|
✗ |
case CONTACT_3D: { |
204 |
|
✗ |
oMc1.toActionMatrixInverse(propagators.back()); |
205 |
|
✗ |
propagators.back().template bottomRows<3>().setZero(); |
206 |
|
✗ |
for (size_t j = support1.size() - 1; j > 1; --j) |
207 |
|
|
{ |
208 |
|
✗ |
lambdas[j].template leftCols<3>().noalias() = |
209 |
|
✗ |
data.oK[(size_t)support1[j]] * propagators[j].template topRows<3>().transpose(); |
210 |
|
✗ |
propagators[j - 1].template topRows<3>().noalias() = |
211 |
|
✗ |
propagators[j].template topRows<3>() * data.oL[(size_t)support1[j]]; |
212 |
|
|
} |
213 |
|
✗ |
lambdas[1].template leftCols<3>().noalias() = |
214 |
|
✗ |
data.oK[(size_t)support1[1]] * propagators[1].template topRows<3>().transpose(); |
215 |
|
|
|
216 |
|
✗ |
for (size_t j = 2; j < support1.size(); ++j) |
217 |
|
|
{ |
218 |
|
✗ |
lambdas[j].template leftCols<3>().noalias() += |
219 |
|
✗ |
data.oL[(size_t)support1[j]] * lambdas[j - 1].template leftCols<3>(); |
220 |
|
|
} |
221 |
|
✗ |
break; |
222 |
|
|
} |
223 |
|
✗ |
case CONTACT_6D: { |
224 |
|
✗ |
oMc1.toActionMatrixInverse(propagators.back()); |
225 |
|
✗ |
for (size_t j = support1.size() - 1; j > 1; --j) |
226 |
|
|
{ |
227 |
|
✗ |
lambdas[j].noalias() = data.oK[(size_t)support1[j]] * propagators[j].transpose(); |
228 |
|
✗ |
propagators[j - 1].noalias() = propagators[j] * data.oL[(size_t)support1[j]]; |
229 |
|
|
} |
230 |
|
✗ |
lambdas[1].noalias() = data.oK[(size_t)support1[1]] * propagators[1].transpose(); |
231 |
|
|
|
232 |
|
✗ |
for (size_t j = 2; j < support1.size(); ++j) |
233 |
|
|
{ |
234 |
|
✗ |
lambdas[j].noalias() += data.oL[(size_t)support1[j]] * lambdas[j - 1]; |
235 |
|
|
} |
236 |
|
✗ |
break; |
237 |
|
|
} |
238 |
|
✗ |
default: { |
239 |
|
✗ |
assert(false && "must never happened"); |
240 |
|
|
break; |
241 |
|
|
} |
242 |
|
|
} |
243 |
|
|
} |
244 |
|
|
|
245 |
|
|
// Fill the delassus matrix block-wise |
246 |
|
|
{ |
247 |
|
✗ |
const int size = cmodel.size(); |
248 |
|
|
|
249 |
|
✗ |
const VectorOfMatrix6 & propagators = cdata.extended_motion_propagators_joint1; |
250 |
|
✗ |
const VectorOfMatrix6 & lambdas = cdata.lambdas_joint1; |
251 |
|
|
|
252 |
|
✗ |
Eigen::DenseIndex current_row_id_other = 0; |
253 |
|
✗ |
for (size_t i = 0; i < k; ++i) |
254 |
|
|
{ |
255 |
|
✗ |
const RigidConstraintModel & cmodel_other = contact_models[i]; |
256 |
|
✗ |
const RigidConstraintData & cdata_other = contact_data[i]; |
257 |
|
✗ |
const int size_other = cmodel_other.size(); |
258 |
|
✗ |
const IndexVector & support1_other = model.supports[cmodel_other.joint1_id]; |
259 |
|
|
|
260 |
|
✗ |
const VectorOfMatrix6 & propagators_other = |
261 |
|
|
cdata_other.extended_motion_propagators_joint1; |
262 |
|
|
|
263 |
|
|
size_t id_in_support1, id_in_support1_other; |
264 |
|
✗ |
findCommonAncestor( |
265 |
|
✗ |
model, joint1_id, cmodel_other.joint1_id, id_in_support1, id_in_support1_other); |
266 |
|
|
|
267 |
|
✗ |
delassus.block(current_row_id_other, current_row_id, size_other, size).noalias() = |
268 |
|
✗ |
propagators_other[id_in_support1_other].topRows(size_other) |
269 |
|
✗ |
* lambdas[id_in_support1].leftCols(size); |
270 |
|
|
|
271 |
|
✗ |
current_row_id_other += size_other; |
272 |
|
|
} |
273 |
|
|
|
274 |
|
✗ |
assert(current_row_id_other == current_row_id && "current row indexes do not match."); |
275 |
|
✗ |
delassus.block(current_row_id, current_row_id, size, size).noalias() = |
276 |
|
✗ |
propagators.back().topRows(size) * lambdas.back().leftCols(size); |
277 |
|
✗ |
current_row_id += size; |
278 |
|
|
} |
279 |
|
|
} |
280 |
|
✗ |
assert( |
281 |
|
|
current_row_id == delassus.rows() |
282 |
|
|
&& "current row indexes do not the number of rows in the Delassus matrix."); |
283 |
|
|
} |
284 |
|
|
|
285 |
|
|
template< |
286 |
|
|
typename Scalar, |
287 |
|
|
int Options, |
288 |
|
|
template<typename, int> |
289 |
|
|
class JointCollectionTpl, |
290 |
|
|
class Allocator> |
291 |
|
|
inline void initPvDelassus( |
292 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
293 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
294 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, Allocator> & contact_models) |
295 |
|
|
{ |
296 |
|
|
std::fill(data.constraints_supported_dim.begin(), data.constraints_supported_dim.end(), 0); |
297 |
|
|
for (std::size_t i = 0; i < contact_models.size(); ++i) |
298 |
|
|
{ |
299 |
|
|
const RigidConstraintModelTpl<Scalar, Options> & contact_model = contact_models[i]; |
300 |
|
|
const JointIndex & joint_id = contact_model.joint1_id; |
301 |
|
|
switch (contact_model.reference_frame) |
302 |
|
|
{ |
303 |
|
|
case LOCAL: |
304 |
|
|
if (contact_model.type == CONTACT_6D) |
305 |
|
|
data.constraints_supported_dim[joint_id] += 6; |
306 |
|
|
else if (contact_model.type == CONTACT_3D) |
307 |
|
|
data.constraints_supported_dim[joint_id] += 3; |
308 |
|
|
else |
309 |
|
|
assert(false && "Must never happen"); |
310 |
|
|
break; |
311 |
|
|
case WORLD: |
312 |
|
|
assert(false && "WORLD not implemented"); |
313 |
|
|
break; |
314 |
|
|
case LOCAL_WORLD_ALIGNED: |
315 |
|
|
assert(false && "LOCAL_WORLD_ALIGNED not implemented"); |
316 |
|
|
break; |
317 |
|
|
default: |
318 |
|
|
assert(false && "Must never happen"); |
319 |
|
|
break; |
320 |
|
|
} |
321 |
|
|
data.accumulation_descendant[joint_id] = joint_id; |
322 |
|
|
|
323 |
|
|
data.constraints_on_joint[joint_id].push_back(i); |
324 |
|
|
} |
325 |
|
|
|
326 |
|
|
// Running backprop to get the count of constraints |
327 |
|
|
for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
328 |
|
|
{ |
329 |
|
|
const JointIndex & parent = model.parents[i]; |
330 |
|
|
data.constraints_supported_dim[parent] += data.constraints_supported_dim[i]; |
331 |
|
|
for (std::size_t constraint : data.constraints_on_joint[i]) |
332 |
|
|
data.constraints_supported[i].insert(constraint); |
333 |
|
|
for (std::size_t constraint : data.constraints_supported[i]) |
334 |
|
|
data.constraints_supported[parent].insert( |
335 |
|
|
constraint); // For loops, will need to use a set data-structure |
336 |
|
|
} |
337 |
|
|
// Assign accumulation descendants for branching nodes |
338 |
|
|
for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
339 |
|
|
{ |
340 |
|
|
const JointIndex & parent = model.parents[i]; |
341 |
|
|
if (data.constraints_supported_dim[i] > 0) |
342 |
|
|
{ |
343 |
|
|
data.joints_supporting_constraints.push_back(i); |
344 |
|
|
data.accumulation_descendant[parent] = data.accumulation_descendant[i]; |
345 |
|
|
if (data.constraints_supported_dim[parent] > data.constraints_supported_dim[i]) |
346 |
|
|
{ |
347 |
|
|
data.accumulation_descendant[parent] = parent; |
348 |
|
|
} |
349 |
|
|
} |
350 |
|
|
} |
351 |
|
|
|
352 |
|
|
for (JointIndex i = 1; i < (JointIndex)model.njoints; i++) |
353 |
|
|
{ |
354 |
|
|
if (data.accumulation_descendant[i] == i) |
355 |
|
|
data.accumulation_joints.push_back(i); |
356 |
|
|
} |
357 |
|
|
// Assign accumulation ancestors |
358 |
|
|
for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
359 |
|
|
{ |
360 |
|
|
const JointIndex & parent = model.parents[i]; |
361 |
|
|
if (data.accumulation_descendant[parent] == parent) |
362 |
|
|
{ |
363 |
|
|
if (data.accumulation_descendant[i] != 0) |
364 |
|
|
data.accumulation_ancestor[data.accumulation_descendant[i]] = parent; |
365 |
|
|
} |
366 |
|
|
} |
367 |
|
|
} |
368 |
|
|
|
369 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
370 |
|
✗ |
JointIndex findGreatestCommonAncestor( |
371 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
372 |
|
|
const DataTpl<Scalar, Options, JointCollectionTpl> & data, |
373 |
|
|
JointIndex joint1_id, |
374 |
|
|
JointIndex joint2_id, |
375 |
|
|
size_t & index_ancestor_in_support1, |
376 |
|
|
size_t & index_ancestor_in_support2) |
377 |
|
|
{ |
378 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
379 |
|
|
joint1_id < (JointIndex)model.njoints, "joint1_id is not valid."); |
380 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
381 |
|
|
joint2_id < (JointIndex)model.njoints, "joint2_id is not valid."); |
382 |
|
|
|
383 |
|
✗ |
if (joint1_id == 0 || joint2_id == 0) |
384 |
|
|
{ |
385 |
|
✗ |
index_ancestor_in_support1 = index_ancestor_in_support2 = 0; |
386 |
|
✗ |
return 0; |
387 |
|
|
} |
388 |
|
|
|
389 |
|
✗ |
index_ancestor_in_support1 = 0; |
390 |
|
✗ |
index_ancestor_in_support2 = 0; |
391 |
|
|
|
392 |
|
✗ |
if (data.constraints_supported[joint1_id].size() > 1) |
393 |
|
✗ |
index_ancestor_in_support1++; |
394 |
|
✗ |
if (data.constraints_supported[joint2_id].size() > 1) |
395 |
|
✗ |
index_ancestor_in_support2++; |
396 |
|
|
|
397 |
|
✗ |
while (joint1_id != joint2_id) |
398 |
|
|
{ |
399 |
|
✗ |
if (joint1_id > joint2_id) |
400 |
|
|
{ |
401 |
|
✗ |
joint1_id = data.accumulation_ancestor[joint1_id]; |
402 |
|
✗ |
index_ancestor_in_support1++; |
403 |
|
|
} |
404 |
|
|
else |
405 |
|
|
{ |
406 |
|
✗ |
joint2_id = data.accumulation_ancestor[joint2_id]; |
407 |
|
✗ |
index_ancestor_in_support2++; |
408 |
|
|
} |
409 |
|
|
} |
410 |
|
|
|
411 |
|
✗ |
index_ancestor_in_support1--; |
412 |
|
✗ |
index_ancestor_in_support2--; |
413 |
|
|
|
414 |
|
✗ |
return joint1_id; |
415 |
|
|
} |
416 |
|
|
|
417 |
|
|
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> |
418 |
|
|
struct ComputePvDelassusBackwardStep |
419 |
|
|
: public fusion::JointUnaryVisitorBase< |
420 |
|
|
ComputeOSIMBackwardStep<Scalar, Options, JointCollectionTpl>> |
421 |
|
|
{ |
422 |
|
|
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
423 |
|
|
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; |
424 |
|
|
|
425 |
|
|
typedef boost::fusion::vector<const Model &, Data &> ArgsType; |
426 |
|
|
|
427 |
|
|
template<typename JointModel> |
428 |
|
|
static void algo( |
429 |
|
|
const JointModelBase<JointModel> & jmodel, |
430 |
|
|
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
431 |
|
|
const Model & model, |
432 |
|
|
Data & data) |
433 |
|
|
{ |
434 |
|
|
typedef typename Model::JointIndex JointIndex; |
435 |
|
|
typedef typename Data::Inertia Inertia; |
436 |
|
|
typedef typename Data::Matrix6x Matrix6x; |
437 |
|
|
|
438 |
|
|
const JointIndex i = jmodel.id(); |
439 |
|
|
const JointIndex parent = model.parents[i]; |
440 |
|
|
typename Inertia::Matrix6 & Ia = data.oYaba[i]; |
441 |
|
|
|
442 |
|
|
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock; |
443 |
|
|
ColBlock Jcols = jmodel.jointCols(data.J); |
444 |
|
|
|
445 |
|
|
jdata.U().noalias() = Ia * Jcols; |
446 |
|
|
jdata.StU().noalias() = Jcols.transpose() * jdata.U(); |
447 |
|
|
|
448 |
|
|
// Account for the rotor inertia contribution |
449 |
|
|
jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature); |
450 |
|
|
|
451 |
|
|
internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv()); |
452 |
|
|
|
453 |
|
|
jdata.UDinv().noalias() = Jcols * jdata.Dinv().transpose(); |
454 |
|
|
data.oL[i].setIdentity(); |
455 |
|
|
data.oL[i].noalias() -= jdata.UDinv() * jdata.U().transpose(); |
456 |
|
|
|
457 |
|
|
if (parent > 0) |
458 |
|
|
{ |
459 |
|
|
data.oYaba[parent].noalias() = Ia - jdata.UDinv() * jdata.U().transpose(); |
460 |
|
|
} |
461 |
|
|
} |
462 |
|
|
}; |
463 |
|
|
|
464 |
|
|
template< |
465 |
|
|
typename Scalar, |
466 |
|
|
int Options, |
467 |
|
|
template<typename, int> |
468 |
|
|
class JointCollectionTpl, |
469 |
|
|
typename ConfigVectorType, |
470 |
|
|
class ModelAllocator, |
471 |
|
|
class DataAllocator, |
472 |
|
|
typename MatrixType> |
473 |
|
✗ |
void computePvDelassusMatrix( |
474 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
475 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
476 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
477 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models, |
478 |
|
|
std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data, |
479 |
|
|
const Eigen::MatrixBase<MatrixType> & delassus_, |
480 |
|
|
const Scalar mu) |
481 |
|
|
{ |
482 |
|
✗ |
assert(model.check(data) && "data is not consistent with model."); |
483 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE( |
484 |
|
|
q.size(), model.nq, "The joint configuration vector is not of right size"); |
485 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
486 |
|
|
check_expression_if_real<Scalar>(mu >= Scalar(0)), "mu has to be positive"); |
487 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE( |
488 |
|
|
contact_models.size(), contact_data.size(), "contact models and data size are not the same"); |
489 |
|
|
|
490 |
|
✗ |
MatrixType & delassus = delassus_.const_cast_derived(); |
491 |
|
✗ |
const size_t constraint_total_size = getTotalConstraintSize(contact_models); |
492 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE(delassus_.rows(), (Eigen::DenseIndex)constraint_total_size); |
493 |
|
✗ |
PINOCCHIO_CHECK_ARGUMENT_SIZE(delassus_.cols(), (Eigen::DenseIndex)constraint_total_size); |
494 |
|
|
|
495 |
|
|
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data; |
496 |
|
|
|
497 |
|
✗ |
typename Data::Vector6 scratch_pad_vector = Data::Vector6::Zero(); |
498 |
|
✗ |
typename Data::Vector6 scratch_pad_vector2 = Data::Vector6::Zero(); |
499 |
|
|
|
500 |
|
✗ |
typename Data::Matrix6 scratch_pad1; |
501 |
|
✗ |
typename Data::Matrix6 scratch_pad2; |
502 |
|
|
|
503 |
|
|
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model; |
504 |
|
|
|
505 |
|
|
typedef typename Model::JointIndex JointIndex; |
506 |
|
|
typedef typename Model::SE3 SE3; |
507 |
|
|
typedef RigidConstraintModelTpl<Scalar, Options> RigidConstraintModel; |
508 |
|
|
typedef RigidConstraintDataTpl<Scalar, Options> RigidConstraintData; |
509 |
|
|
|
510 |
|
|
typedef ComputeOSIMForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> Pass1; |
511 |
|
✗ |
for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
512 |
|
|
{ |
513 |
|
✗ |
Pass1::run( |
514 |
|
✗ |
model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived())); |
515 |
|
|
} |
516 |
|
|
|
517 |
|
✗ |
for (size_t k = 0; k < contact_models.size(); ++k) |
518 |
|
|
{ |
519 |
|
✗ |
const RigidConstraintModel & cmodel = contact_models[k]; |
520 |
|
✗ |
RigidConstraintData & cdata = contact_data[k]; |
521 |
|
|
|
522 |
|
✗ |
const JointIndex joint1_id = cmodel.joint1_id; |
523 |
|
|
|
524 |
|
|
// Compute relative placement between the joint and the contact frame |
525 |
|
✗ |
SE3 & oMc = cdata.oMc1; |
526 |
|
✗ |
oMc = data.oMi[joint1_id] * cmodel.joint1_placement; // contact placement |
527 |
|
|
|
528 |
|
|
typedef typename Data::Inertia Inertia; |
529 |
|
|
typedef typename Inertia::Symmetric3 Symmetric3; |
530 |
|
|
|
531 |
|
|
// Add contact inertia to the joint articulated inertia |
532 |
|
✗ |
Symmetric3 S(Symmetric3::Zero()); |
533 |
|
✗ |
if (cmodel.type == CONTACT_6D) |
534 |
|
✗ |
S.setDiagonal(Symmetric3::Vector3::Constant(mu)); |
535 |
|
|
|
536 |
|
✗ |
const Inertia contact_inertia(mu, oMc.translation(), S); |
537 |
|
✗ |
data.oYaba[joint1_id] += contact_inertia.matrix(); |
538 |
|
|
|
539 |
|
|
typedef typename RigidConstraintData::VectorOfMatrix6 VectorOfMatrix6; |
540 |
|
|
|
541 |
|
✗ |
VectorOfMatrix6 & propagators = cdata.extended_motion_propagators_joint1; |
542 |
|
✗ |
switch (cmodel.type) |
543 |
|
|
{ |
544 |
|
✗ |
case CONTACT_3D: { |
545 |
|
✗ |
oMc.toActionMatrixInverse(propagators[0]); |
546 |
|
✗ |
propagators[0].template bottomRows<3>().setZero(); |
547 |
|
✗ |
break; |
548 |
|
|
} |
549 |
|
✗ |
case CONTACT_6D: { |
550 |
|
✗ |
oMc.toActionMatrixInverse(propagators[0]); |
551 |
|
✗ |
break; |
552 |
|
|
} |
553 |
|
|
|
554 |
|
✗ |
default: { |
555 |
|
✗ |
assert(false && "must never happen"); |
556 |
|
|
break; |
557 |
|
|
} |
558 |
|
|
} |
559 |
|
|
} |
560 |
|
|
|
561 |
|
|
// Initialize motion propagator and spatial_inv_inertia at all the accumulation joints |
562 |
|
✗ |
for (JointIndex i : data.accumulation_joints) |
563 |
|
|
{ |
564 |
|
|
|
565 |
|
✗ |
if (data.constraints_supported[i].size() == 1) |
566 |
|
|
{ |
567 |
|
✗ |
size_t constraint = data.constraints_on_joint[i][0]; |
568 |
|
✗ |
data.extended_motion_propagator[i] = |
569 |
|
✗ |
contact_data[constraint].extended_motion_propagators_joint1[0]; |
570 |
|
|
} |
571 |
|
|
else |
572 |
|
|
{ |
573 |
|
✗ |
data.extended_motion_propagator[i].setIdentity(); |
574 |
|
|
} |
575 |
|
✗ |
data.spatial_inv_inertia[i].setZero(); |
576 |
|
|
} |
577 |
|
|
|
578 |
|
|
typedef ComputePvDelassusBackwardStep<Scalar, Options, JointCollectionTpl> Pass2; |
579 |
|
✗ |
for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i) |
580 |
|
|
{ |
581 |
|
✗ |
Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data)); |
582 |
|
|
|
583 |
|
✗ |
if (data.constraints_supported_dim[i] > 0) |
584 |
|
|
{ |
585 |
|
✗ |
size_t ad_i = data.accumulation_descendant[i]; |
586 |
|
✗ |
int nv = model.joints[i].nv(); |
587 |
|
|
// propagate the spatial inverse inertia |
588 |
|
✗ |
if (nv == 1) |
589 |
|
|
{ |
590 |
|
|
// Optimization for single DoF joints |
591 |
|
✗ |
if (data.constraints_supported_dim[ad_i] != 3) |
592 |
|
|
{ |
593 |
|
|
// When propagating 6D constraints |
594 |
|
✗ |
scratch_pad_vector.noalias() = |
595 |
|
✗ |
data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); |
596 |
|
✗ |
scratch_pad_vector2.noalias() = scratch_pad_vector * data.joints[i].Dinv().coeff(0, 0); |
597 |
|
✗ |
data.spatial_inv_inertia[ad_i].noalias() += |
598 |
|
✗ |
scratch_pad_vector2 * scratch_pad_vector.transpose(); |
599 |
|
|
} |
600 |
|
|
else |
601 |
|
|
{ |
602 |
|
|
// Propagating 3D constraints |
603 |
|
✗ |
scratch_pad_vector.template topRows<3>().noalias() = |
604 |
|
✗ |
data.extended_motion_propagator[i].template topRows<3>() |
605 |
|
✗ |
* model.joints[i].jointCols(data.J); |
606 |
|
✗ |
scratch_pad_vector2.template topRows<3>().noalias() = |
607 |
|
✗ |
scratch_pad_vector.template topRows<3>() * data.joints[i].Dinv().coeff(0, 0); |
608 |
|
✗ |
data.spatial_inv_inertia[ad_i].template topLeftCorner<3, 3>().noalias() += |
609 |
|
✗ |
scratch_pad_vector2.template topRows<3>() |
610 |
|
✗ |
* scratch_pad_vector.template topRows<3>().transpose(); |
611 |
|
|
} |
612 |
|
|
} |
613 |
|
✗ |
else if (nv == 6) |
614 |
|
|
{ |
615 |
|
✗ |
scratch_pad1.noalias() = |
616 |
|
✗ |
data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); |
617 |
|
✗ |
scratch_pad2.noalias() = scratch_pad1 * data.joints[i].Dinv(); |
618 |
|
✗ |
data.spatial_inv_inertia[ad_i].noalias() += scratch_pad2 * scratch_pad1.transpose(); |
619 |
|
|
} |
620 |
|
✗ |
else if (nv > 1) |
621 |
|
|
{ |
622 |
|
|
// Joints with more than 1 DoF |
623 |
|
✗ |
scratch_pad1.leftCols(nv).noalias() = |
624 |
|
✗ |
data.extended_motion_propagator[i] * model.joints[i].jointCols(data.J); |
625 |
|
✗ |
scratch_pad2.leftCols(nv).noalias() = scratch_pad1.leftCols(nv) * data.joints[i].Dinv(); |
626 |
|
✗ |
data.spatial_inv_inertia[ad_i].noalias() += |
627 |
|
✗ |
scratch_pad2.leftCols(nv) * scratch_pad1.leftCols(nv).transpose(); |
628 |
|
|
} |
629 |
|
|
else |
630 |
|
|
{ |
631 |
|
✗ |
assert(false && "must never happen"); |
632 |
|
|
} |
633 |
|
|
// propagate the EMP |
634 |
|
✗ |
const JointIndex parent = model.parents[i]; |
635 |
|
✗ |
size_t par_ad_i = data.accumulation_descendant[parent]; |
636 |
|
✗ |
if (parent > 0 && par_ad_i == ad_i) |
637 |
|
|
{ |
638 |
|
|
|
639 |
|
✗ |
if (data.constraints_supported_dim[ad_i] != 3) |
640 |
|
|
{ |
641 |
|
✗ |
data.extended_motion_propagator[parent].noalias() = |
642 |
|
✗ |
data.extended_motion_propagator[i] * data.oL[i]; |
643 |
|
|
} |
644 |
|
|
else |
645 |
|
|
{ |
646 |
|
✗ |
data.extended_motion_propagator[parent].template topRows<3>().noalias() = |
647 |
|
✗ |
data.extended_motion_propagator[i].template topRows<3>() * data.oL[i]; |
648 |
|
|
} |
649 |
|
|
} |
650 |
|
✗ |
else if (par_ad_i != ad_i) // TODO: optimize for 3D constraints |
651 |
|
|
{ |
652 |
|
✗ |
data.extended_motion_propagator2[ad_i].noalias() = |
653 |
|
✗ |
data.extended_motion_propagator[i] * data.oL[i]; |
654 |
|
|
} |
655 |
|
|
} |
656 |
|
|
} |
657 |
|
|
|
658 |
|
✗ |
for (JointIndex i : data.accumulation_joints) |
659 |
|
|
{ |
660 |
|
|
|
661 |
|
✗ |
size_t an_i = data.accumulation_ancestor[i]; |
662 |
|
✗ |
if (an_i != 0) // TODO: optimize for 3D constraints. Not very important here. |
663 |
|
|
{ |
664 |
|
✗ |
scratch_pad1.noalias() = |
665 |
|
✗ |
data.extended_motion_propagator2[i] * data.spatial_inv_inertia[an_i]; |
666 |
|
✗ |
data.spatial_inv_inertia[i].noalias() += |
667 |
|
✗ |
scratch_pad1 * data.extended_motion_propagator2[i].transpose(); |
668 |
|
|
} |
669 |
|
|
} |
670 |
|
|
|
671 |
|
|
typedef typename RigidConstraintData::VectorOfMatrix6 VectorOfMatrix6; |
672 |
|
|
// compute EMP in the reverse direction |
673 |
|
✗ |
for (size_t k = 0; k < contact_models.size(); ++k) |
674 |
|
|
{ |
675 |
|
✗ |
VectorOfMatrix6 & cemp = contact_data[k].extended_motion_propagators_joint1; |
676 |
|
✗ |
JointIndex curr_joint = contact_models[k].joint1_id; |
677 |
|
|
|
678 |
|
|
JointIndex an_i; |
679 |
|
|
|
680 |
|
✗ |
if (data.constraints_supported[curr_joint].size() == 1) |
681 |
|
|
{ |
682 |
|
✗ |
cemp[0] = data.extended_motion_propagator2[curr_joint]; |
683 |
|
✗ |
an_i = data.accumulation_ancestor[curr_joint]; |
684 |
|
|
} |
685 |
|
|
else |
686 |
|
|
{ |
687 |
|
✗ |
an_i = curr_joint; |
688 |
|
|
} |
689 |
|
|
|
690 |
|
✗ |
size_t counter = 1; |
691 |
|
✗ |
while (an_i > 1) |
692 |
|
|
{ |
693 |
|
✗ |
if (contact_models[k].size() != 3) |
694 |
|
✗ |
cemp[counter].noalias() = cemp[counter - 1] * data.extended_motion_propagator2[an_i]; |
695 |
|
|
else |
696 |
|
✗ |
cemp[counter].template topRows<3>().noalias() = |
697 |
|
✗ |
cemp[counter - 1].template topRows<3>() * data.extended_motion_propagator2[an_i]; |
698 |
|
✗ |
curr_joint = an_i; |
699 |
|
✗ |
an_i = data.accumulation_ancestor[curr_joint]; |
700 |
|
✗ |
counter++; |
701 |
|
|
} |
702 |
|
|
} |
703 |
|
|
|
704 |
|
✗ |
Eigen::DenseIndex current_row_id = 0; |
705 |
|
✗ |
for (size_t k = 0; k < contact_models.size(); ++k) |
706 |
|
|
{ |
707 |
|
✗ |
const RigidConstraintModel & cmodel = contact_models[k]; |
708 |
|
✗ |
RigidConstraintData & cdata = contact_data[k]; |
709 |
|
✗ |
const VectorOfMatrix6 & cemp = cdata.extended_motion_propagators_joint1; |
710 |
|
|
|
711 |
|
✗ |
const JointIndex joint1_id = cmodel.joint1_id; |
712 |
|
|
|
713 |
|
|
// Fill the delassus matrix block-wise |
714 |
|
|
{ |
715 |
|
✗ |
const int size = cmodel.size(); |
716 |
|
|
|
717 |
|
✗ |
Eigen::DenseIndex current_row_id_other = 0; |
718 |
|
✗ |
for (size_t i = 0; i < k; ++i) |
719 |
|
|
{ |
720 |
|
✗ |
RigidConstraintData & cdata_other = contact_data[i]; |
721 |
|
✗ |
const RigidConstraintModel & cmodel_other = contact_models[i]; |
722 |
|
✗ |
const int size_other = cmodel_other.size(); |
723 |
|
✗ |
const JointIndex joint1_id_other = cmodel_other.joint1_id; |
724 |
|
|
|
725 |
|
✗ |
const VectorOfMatrix6 & cemp_other = cdata_other.extended_motion_propagators_joint1; |
726 |
|
|
|
727 |
|
|
size_t id_in_support1, id_in_support1_other, gca; |
728 |
|
✗ |
gca = findGreatestCommonAncestor( |
729 |
|
|
model, data, joint1_id, joint1_id_other, id_in_support1, id_in_support1_other); |
730 |
|
|
|
731 |
|
✗ |
scratch_pad1.noalias() = cemp_other[id_in_support1_other] * data.spatial_inv_inertia[gca]; |
732 |
|
✗ |
if (size == 6) |
733 |
|
|
{ |
734 |
|
✗ |
if (size_other == 6) |
735 |
|
✗ |
delassus.template block<6, 6>(current_row_id_other, current_row_id).noalias() = |
736 |
|
✗ |
scratch_pad1 * cemp[id_in_support1].transpose(); |
737 |
|
✗ |
else if (size_other == 3) |
738 |
|
✗ |
delassus.template block<3, 6>(current_row_id_other, current_row_id).noalias() = |
739 |
|
✗ |
scratch_pad1.template topRows<3>() |
740 |
|
✗ |
* cemp[id_in_support1].template topRows<6>().transpose(); |
741 |
|
|
} |
742 |
|
✗ |
else if (size == 3) |
743 |
|
|
{ |
744 |
|
✗ |
if (size_other == 6) |
745 |
|
✗ |
delassus.template block<6, 3>(current_row_id_other, current_row_id).noalias() = |
746 |
|
✗ |
scratch_pad1.template topRows<6>() |
747 |
|
✗ |
* cemp[id_in_support1].template topRows<3>().transpose(); |
748 |
|
✗ |
else if (size_other == 3) |
749 |
|
✗ |
delassus.template block<3, 3>(current_row_id_other, current_row_id).noalias() = |
750 |
|
✗ |
scratch_pad1.template topRows<3>() |
751 |
|
✗ |
* cemp[id_in_support1].template topRows<3>().transpose(); |
752 |
|
|
} |
753 |
|
|
else |
754 |
|
|
{ |
755 |
|
✗ |
delassus.block(current_row_id_other, current_row_id, size_other, size).noalias() = |
756 |
|
✗ |
scratch_pad1.topRows(size_other) * cemp[id_in_support1].topRows(size).transpose(); |
757 |
|
|
} |
758 |
|
✗ |
current_row_id_other += size_other; |
759 |
|
|
} |
760 |
|
|
|
761 |
|
✗ |
assert(current_row_id_other == current_row_id && "current row indexes do not match."); |
762 |
|
✗ |
if ( |
763 |
|
✗ |
data.constraints_supported_dim[joint1_id] > 6 |
764 |
|
✗ |
|| data.constraints_supported[joint1_id].size() > 1) |
765 |
|
|
{ |
766 |
|
✗ |
scratch_pad1.noalias() = cemp[0] * data.spatial_inv_inertia[joint1_id]; |
767 |
|
✗ |
delassus.block(current_row_id, current_row_id, size, size).noalias() = |
768 |
|
✗ |
scratch_pad1.topRows(size) * cemp[0].topRows(size).transpose(); |
769 |
|
|
} |
770 |
|
|
else |
771 |
|
|
{ |
772 |
|
✗ |
if (size == 6) |
773 |
|
✗ |
delassus.template block<6, 6>(current_row_id, current_row_id) = |
774 |
|
✗ |
data.spatial_inv_inertia[joint1_id]; |
775 |
|
✗ |
else if (size == 3) |
776 |
|
✗ |
delassus.template block<3, 3>(current_row_id, current_row_id) = |
777 |
|
✗ |
data.spatial_inv_inertia[joint1_id].template topLeftCorner<3, 3>(); |
778 |
|
|
else |
779 |
|
✗ |
delassus.block(current_row_id, current_row_id, size, size) = |
780 |
|
✗ |
data.spatial_inv_inertia[joint1_id].topLeftCorner(size, size); |
781 |
|
|
} |
782 |
|
✗ |
current_row_id += size; |
783 |
|
|
} |
784 |
|
|
} |
785 |
|
✗ |
assert( |
786 |
|
|
current_row_id == delassus.rows() |
787 |
|
|
&& "current row indexes do not the number of rows in the Delassus matrix."); |
788 |
|
|
} |
789 |
|
|
|
790 |
|
|
template< |
791 |
|
|
typename Scalar, |
792 |
|
|
int Options, |
793 |
|
|
template<typename, int> |
794 |
|
|
class JointCollectionTpl, |
795 |
|
|
typename ConfigVectorType, |
796 |
|
|
class ModelAllocator, |
797 |
|
|
class DataAllocator, |
798 |
|
|
typename MatrixType> |
799 |
|
✗ |
void computeDampedDelassusMatrixInverse( |
800 |
|
|
const ModelTpl<Scalar, Options, JointCollectionTpl> & model, |
801 |
|
|
DataTpl<Scalar, Options, JointCollectionTpl> & data, |
802 |
|
|
const Eigen::MatrixBase<ConfigVectorType> & q, |
803 |
|
|
const std::vector<RigidConstraintModelTpl<Scalar, Options>, ModelAllocator> & contact_models, |
804 |
|
|
std::vector<RigidConstraintDataTpl<Scalar, Options>, DataAllocator> & contact_data, |
805 |
|
|
const Eigen::MatrixBase<MatrixType> & damped_delassus_inverse_, |
806 |
|
|
const Scalar mu, |
807 |
|
|
const bool scaled, |
808 |
|
|
const bool Pv) |
809 |
|
|
{ |
810 |
|
✗ |
PINOCCHIO_CHECK_INPUT_ARGUMENT( |
811 |
|
|
check_expression_if_real<Scalar>(mu >= Eigen::NumTraits<Scalar>::dummy_precision()), |
812 |
|
|
"mu is too small."); |
813 |
|
|
|
814 |
|
✗ |
const Scalar mu_inv = Scalar(1) / mu; |
815 |
|
✗ |
MatrixType & damped_delassus_inverse = damped_delassus_inverse_.const_cast_derived(); |
816 |
|
|
|
817 |
|
✗ |
const Scalar mu_inv_square = mu_inv * mu_inv; |
818 |
|
✗ |
assert( |
819 |
|
|
check_expression_if_real<Scalar>(mu_inv_square != std::numeric_limits<Scalar>::infinity()) |
820 |
|
|
&& "mu_inv**2 is equal to infinity."); |
821 |
|
|
|
822 |
|
✗ |
if (Pv) |
823 |
|
✗ |
computePvDelassusMatrix( |
824 |
|
|
model, data, q, contact_models, contact_data, damped_delassus_inverse, mu_inv); |
825 |
|
|
else |
826 |
|
✗ |
computeDelassusMatrix( |
827 |
|
|
model, data, q, contact_models, contact_data, damped_delassus_inverse, mu_inv); |
828 |
|
|
|
829 |
|
✗ |
damped_delassus_inverse *= -mu_inv; |
830 |
|
✗ |
damped_delassus_inverse.diagonal().array() += Scalar(1); |
831 |
|
✗ |
if (!scaled) |
832 |
|
✗ |
damped_delassus_inverse *= mu_inv; |
833 |
|
|
} |
834 |
|
|
|
835 |
|
|
} // namespace pinocchio |
836 |
|
|
|
837 |
|
|
#endif // ifndef __pinocchio_algorithm_contact_delassus_hxx__ |
838 |
|
|
|