Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2019-2022 INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include <iostream> |
6 |
|
|
|
7 |
|
|
#include "pinocchio/algorithm/jacobian.hpp" |
8 |
|
|
#include "pinocchio/algorithm/cholesky.hpp" |
9 |
|
|
#include "pinocchio/algorithm/contact-info.hpp" |
10 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
11 |
|
|
#include "pinocchio/algorithm/contact-cholesky.hxx" |
12 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
13 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
14 |
|
|
|
15 |
|
|
#include <boost/test/unit_test.hpp> |
16 |
|
|
#include <boost/utility/binary.hpp> |
17 |
|
|
|
18 |
|
|
namespace pinocchio |
19 |
|
|
{ |
20 |
|
|
namespace cholesky |
21 |
|
|
{ |
22 |
|
|
template<typename Scalar, int Options> |
23 |
|
|
struct ContactCholeskyDecompositionAccessorTpl |
24 |
|
|
: public ContactCholeskyDecompositionTpl<Scalar, Options> |
25 |
|
|
{ |
26 |
|
|
typedef ContactCholeskyDecompositionTpl<Scalar, Options> Base; |
27 |
|
|
typedef typename Base::IndexVector IndexVector; |
28 |
|
|
typedef typename Base::BooleanVector BooleanVector; |
29 |
|
|
|
30 |
|
✗ |
ContactCholeskyDecompositionAccessorTpl(const Base & other) |
31 |
|
✗ |
: Base(other) |
32 |
|
|
{ |
33 |
|
|
} |
34 |
|
|
|
35 |
|
✗ |
const IndexVector & getParents_fromRow() const |
36 |
|
|
{ |
37 |
|
✗ |
return this->parents_fromRow; |
38 |
|
|
} |
39 |
|
✗ |
const IndexVector & getLastChild() const |
40 |
|
|
{ |
41 |
|
✗ |
return this->last_child; |
42 |
|
|
} |
43 |
|
✗ |
const IndexVector & getNvSubtree_fromRow() const |
44 |
|
|
{ |
45 |
|
✗ |
return this->nv_subtree_fromRow; |
46 |
|
|
} |
47 |
|
|
const std::vector<BooleanVector> & getJoint1_indexes() const |
48 |
|
|
{ |
49 |
|
|
return this->joint1_indexes; |
50 |
|
|
} |
51 |
|
|
const std::vector<BooleanVector> & getJoint2_indexes() const |
52 |
|
|
{ |
53 |
|
|
return this->joint2_indexes; |
54 |
|
|
} |
55 |
|
|
}; |
56 |
|
|
|
57 |
|
|
typedef ContactCholeskyDecompositionAccessorTpl<double, 0> ContactCholeskyDecompositionAccessor; |
58 |
|
|
} // namespace cholesky |
59 |
|
|
} // namespace pinocchio |
60 |
|
|
|
61 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
62 |
|
|
|
63 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_operator_equal) |
64 |
|
|
{ |
65 |
|
|
|
66 |
|
|
using namespace Eigen; |
67 |
|
|
using namespace pinocchio; |
68 |
|
|
using namespace cholesky; |
69 |
|
|
|
70 |
|
✗ |
pinocchio::Model humanoid_model; |
71 |
|
✗ |
pinocchio::buildModels::humanoidRandom(humanoid_model); |
72 |
|
✗ |
Data humanoid_data(humanoid_model); |
73 |
|
|
|
74 |
|
✗ |
pinocchio::Model manipulator_model; |
75 |
|
✗ |
pinocchio::buildModels::manipulator(manipulator_model); |
76 |
|
✗ |
Data manipulator_data(manipulator_model); |
77 |
|
|
|
78 |
|
✗ |
const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty; |
79 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty; |
80 |
|
|
|
81 |
|
✗ |
humanoid_model.lowerPositionLimit.head<3>().fill(-1.); |
82 |
|
✗ |
humanoid_model.upperPositionLimit.head<3>().fill(1.); |
83 |
|
✗ |
VectorXd humanoid_q = randomConfiguration(humanoid_model); |
84 |
|
✗ |
crba(humanoid_model, humanoid_data, humanoid_q, Convention::WORLD); |
85 |
|
|
|
86 |
|
✗ |
VectorXd manipulator_q = randomConfiguration(manipulator_model); |
87 |
|
✗ |
crba(manipulator_model, manipulator_data, manipulator_q, Convention::WORLD); |
88 |
|
|
|
89 |
|
✗ |
ContactCholeskyDecomposition humanoid_chol(humanoid_model), manipulator_chol(manipulator_model); |
90 |
|
✗ |
humanoid_chol.compute(humanoid_model, humanoid_data, contact_models_empty, contact_datas_empty); |
91 |
|
✗ |
manipulator_chol.compute( |
92 |
|
|
manipulator_model, manipulator_data, contact_models_empty, contact_datas_empty); |
93 |
|
|
|
94 |
|
✗ |
BOOST_CHECK(humanoid_chol == humanoid_chol); |
95 |
|
✗ |
BOOST_CHECK(humanoid_chol != manipulator_chol); |
96 |
|
|
} |
97 |
|
|
|
98 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_cholesky_simple) |
99 |
|
|
{ |
100 |
|
|
using namespace Eigen; |
101 |
|
|
using namespace pinocchio; |
102 |
|
|
using namespace pinocchio::cholesky; |
103 |
|
|
|
104 |
|
✗ |
pinocchio::Model model; |
105 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
106 |
|
✗ |
pinocchio::Data data_ref(model); |
107 |
|
|
|
108 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
109 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
110 |
|
✗ |
VectorXd q = randomConfiguration(model); |
111 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
112 |
|
|
|
113 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
114 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
115 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
116 |
|
|
|
117 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
118 |
|
✗ |
const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty; |
119 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_empty; |
120 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models_empty); |
121 |
|
|
|
122 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.D.size() == model.nv); |
123 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.Dinv.size() == model.nv); |
124 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.U.rows() == model.nv); |
125 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.U.cols() == model.nv); |
126 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.size() == model.nv); |
127 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.U.diagonal().isOnes()); |
128 |
|
|
|
129 |
|
✗ |
Data data(model); |
130 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
131 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
132 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
133 |
|
|
|
134 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models_empty, contact_datas_empty); |
135 |
|
|
|
136 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
137 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
138 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
139 |
|
|
|
140 |
|
✗ |
BOOST_CHECK(data.M.isApprox(data_ref.M)); |
141 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
142 |
|
|
|
143 |
|
✗ |
BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); |
144 |
|
✗ |
BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); |
145 |
|
✗ |
BOOST_CHECK( |
146 |
|
|
data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv))); |
147 |
|
|
|
148 |
|
✗ |
ContactCholeskyDecompositionAccessor access(contact_chol_decomposition); |
149 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
150 |
|
|
{ |
151 |
|
✗ |
BOOST_CHECK(access.getParents_fromRow()[k] == data.parents_fromRow[(size_t)k]); |
152 |
|
|
} |
153 |
|
|
|
154 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.njoints; ++k) |
155 |
|
|
{ |
156 |
|
✗ |
BOOST_CHECK(access.getLastChild()[k] == data.lastChild[(size_t)k]); |
157 |
|
|
} |
158 |
|
|
|
159 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
160 |
|
|
{ |
161 |
|
✗ |
BOOST_CHECK(access.getNvSubtree_fromRow()[k] == data.nvSubtree_fromRow[(size_t)k]); |
162 |
|
|
} |
163 |
|
|
|
164 |
|
|
// Test basic operation |
165 |
|
✗ |
VectorXd v_in(VectorXd::Random(model.nv)); |
166 |
|
✗ |
MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20)); |
167 |
|
|
|
168 |
|
|
// Test Uv |
169 |
|
✗ |
VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); |
170 |
|
|
|
171 |
|
✗ |
contact_chol_decomposition.Uv(Uv_op_res); |
172 |
|
✗ |
pinocchio::cholesky::Uv(model, data_ref, Uv_op_ref); |
173 |
|
|
|
174 |
|
✗ |
BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); |
175 |
|
|
|
176 |
|
✗ |
MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); |
177 |
|
|
|
178 |
|
✗ |
contact_chol_decomposition.Uv(Uv_mat_op_res); |
179 |
|
✗ |
pinocchio::cholesky::Uv(model, data_ref, Uv_mat_op_ref); |
180 |
|
|
|
181 |
|
✗ |
BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); |
182 |
|
|
|
183 |
|
|
// Test Utv |
184 |
|
✗ |
VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); |
185 |
|
|
|
186 |
|
✗ |
contact_chol_decomposition.Utv(Utv_op_res); |
187 |
|
✗ |
pinocchio::cholesky::Utv(model, data_ref, Utv_op_ref); |
188 |
|
|
|
189 |
|
✗ |
BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); |
190 |
|
|
|
191 |
|
✗ |
MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); |
192 |
|
|
|
193 |
|
✗ |
contact_chol_decomposition.Utv(Utv_mat_op_res); |
194 |
|
✗ |
pinocchio::cholesky::Utv(model, data_ref, Utv_mat_op_ref); |
195 |
|
|
|
196 |
|
✗ |
BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); |
197 |
|
|
|
198 |
|
|
// Test Uiv |
199 |
|
✗ |
VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); |
200 |
|
|
|
201 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_op_res); |
202 |
|
✗ |
pinocchio::cholesky::Uiv(model, data_ref, Uiv_op_ref); |
203 |
|
|
|
204 |
|
✗ |
BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); |
205 |
|
|
|
206 |
|
✗ |
MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); |
207 |
|
|
|
208 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_mat_op_res); |
209 |
|
✗ |
pinocchio::cholesky::Uiv(model, data_ref, Uiv_mat_op_ref); |
210 |
|
|
|
211 |
|
✗ |
BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); |
212 |
|
|
|
213 |
|
|
// Test Utiv |
214 |
|
✗ |
VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); |
215 |
|
|
|
216 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_op_res); |
217 |
|
✗ |
pinocchio::cholesky::Utiv(model, data_ref, Utiv_op_ref); |
218 |
|
|
|
219 |
|
✗ |
BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); |
220 |
|
|
|
221 |
|
✗ |
MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); |
222 |
|
|
|
223 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_mat_op_res); |
224 |
|
✗ |
pinocchio::cholesky::Utiv(model, data_ref, Utiv_mat_op_ref); |
225 |
|
|
|
226 |
|
✗ |
BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); |
227 |
|
|
|
228 |
|
|
// SolveInPlace |
229 |
|
✗ |
VectorXd sol(v_in); |
230 |
|
✗ |
contact_chol_decomposition.solveInPlace(sol); |
231 |
|
|
|
232 |
|
✗ |
VectorXd sol_ref(data.M.inverse() * v_in); |
233 |
|
|
|
234 |
|
✗ |
BOOST_CHECK(sol.isApprox(sol_ref)); |
235 |
|
|
|
236 |
|
✗ |
MatrixXd sol_mat(mat_in); |
237 |
|
✗ |
contact_chol_decomposition.solveInPlace(sol_mat); |
238 |
|
|
|
239 |
|
✗ |
MatrixXd sol_mat_ref(data.M.inverse() * mat_in); |
240 |
|
|
|
241 |
|
✗ |
BOOST_CHECK(sol_mat.isApprox(sol_mat_ref)); |
242 |
|
|
|
243 |
|
|
// solve |
244 |
|
✗ |
MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in); |
245 |
|
✗ |
BOOST_CHECK(sol_copy_mat.isApprox(sol_mat)); |
246 |
|
|
|
247 |
|
|
// inverse |
248 |
|
✗ |
MatrixXd M_inv(model.nv, model.nv); |
249 |
|
✗ |
contact_chol_decomposition.inverse(M_inv); |
250 |
|
|
|
251 |
|
✗ |
MatrixXd M_inv_ref = data.M.inverse(); |
252 |
|
✗ |
BOOST_CHECK(M_inv.isApprox(M_inv_ref)); |
253 |
|
|
|
254 |
|
|
// test retrieve Mass Matrix Cholesky Decomposition |
255 |
|
|
ContactCholeskyDecomposition mass_matrix_chol = |
256 |
|
✗ |
contact_chol_decomposition.getMassMatrixChoeslkyDecomposition(model); |
257 |
|
|
|
258 |
|
|
// test Operational Space Inertia Matrix |
259 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
260 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
261 |
|
✗ |
BOOST_CHECK(iosim.size() == 0); |
262 |
|
✗ |
BOOST_CHECK(osim.size() == 0); |
263 |
|
|
|
264 |
|
✗ |
BOOST_CHECK(mass_matrix_chol == contact_chol_decomposition); |
265 |
|
✗ |
BOOST_CHECK(mass_matrix_chol.U.isApprox(data_ref.U)); |
266 |
|
✗ |
BOOST_CHECK(mass_matrix_chol.D.isApprox(data_ref.D)); |
267 |
|
✗ |
BOOST_CHECK(mass_matrix_chol.Dinv.isApprox(data_ref.Dinv)); |
268 |
|
|
} |
269 |
|
|
|
270 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_LOCAL) |
271 |
|
|
{ |
272 |
|
|
using namespace Eigen; |
273 |
|
|
using namespace pinocchio; |
274 |
|
|
using namespace pinocchio::cholesky; |
275 |
|
|
|
276 |
|
✗ |
pinocchio::Model model; |
277 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
278 |
|
✗ |
pinocchio::Data data_ref(model); |
279 |
|
|
|
280 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
281 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
282 |
|
✗ |
VectorXd q = randomConfiguration(model); |
283 |
|
|
|
284 |
|
✗ |
const std::string RF = "rleg6_joint"; |
285 |
|
✗ |
const std::string LF = "lleg6_joint"; |
286 |
|
|
|
287 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
288 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
289 |
|
✗ |
RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL); |
290 |
|
✗ |
contact_models.push_back(ci_RF); |
291 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RF)); |
292 |
|
✗ |
RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
293 |
|
✗ |
contact_models.push_back(ci_LF); |
294 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LF)); |
295 |
|
|
|
296 |
|
|
// Compute Mass Matrix |
297 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
298 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
299 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
300 |
|
|
|
301 |
|
|
// Compute Cholesky decomposition |
302 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
303 |
|
|
|
304 |
|
|
// Compute Jacobians |
305 |
|
✗ |
Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv); |
306 |
|
✗ |
J_RF.setZero(); |
307 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF); |
308 |
|
✗ |
J_LF.setZero(); |
309 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); |
310 |
|
|
|
311 |
|
✗ |
const int constraint_dim = 12; |
312 |
|
✗ |
const int total_dim = model.nv + constraint_dim; |
313 |
|
|
|
314 |
|
✗ |
Data::MatrixXs H(total_dim, total_dim); |
315 |
|
✗ |
H.setZero(); |
316 |
|
✗ |
H.bottomRightCorner(model.nv, model.nv) = data_ref.M; |
317 |
|
✗ |
H.middleRows<6>(0).rightCols(model.nv) = J_RF; |
318 |
|
✗ |
H.middleRows<6>(6).rightCols(model.nv) = J_LF; |
319 |
|
|
|
320 |
|
✗ |
H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose(); |
321 |
|
|
|
322 |
|
✗ |
Data data(model); |
323 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
324 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
325 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
326 |
|
|
|
327 |
|
✗ |
ContactCholeskyDecompositionAccessor access(contact_chol_decomposition); |
328 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
329 |
|
|
{ |
330 |
|
✗ |
if (data.parents_fromRow[(size_t)k] == -1) |
331 |
|
✗ |
BOOST_CHECK(access.getParents_fromRow()[k + constraint_dim] == -1); |
332 |
|
|
else |
333 |
|
✗ |
BOOST_CHECK( |
334 |
|
|
access.getParents_fromRow()[k + constraint_dim] |
335 |
|
|
== data.parents_fromRow[(size_t)k] + constraint_dim); |
336 |
|
|
} |
337 |
|
|
|
338 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas); |
339 |
|
|
|
340 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
341 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
342 |
|
|
|
343 |
|
✗ |
BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); |
344 |
|
✗ |
BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); |
345 |
|
✗ |
BOOST_CHECK( |
346 |
|
|
data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv))); |
347 |
|
|
|
348 |
|
|
Data::MatrixXs M_recomposed = |
349 |
|
✗ |
contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv) |
350 |
|
✗ |
* contact_chol_decomposition.D.tail(model.nv).asDiagonal() |
351 |
|
✗ |
* contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose(); |
352 |
|
✗ |
BOOST_CHECK(M_recomposed.isApprox(data.M)); |
353 |
|
|
|
354 |
|
|
Data::MatrixXs H_recomposed = contact_chol_decomposition.U |
355 |
|
✗ |
* contact_chol_decomposition.D.asDiagonal() |
356 |
|
✗ |
* contact_chol_decomposition.U.transpose(); |
357 |
|
|
|
358 |
|
✗ |
BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M)); |
359 |
|
✗ |
BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv) |
360 |
|
|
.isApprox(H.topRightCorner(constraint_dim, model.nv))); |
361 |
|
✗ |
BOOST_CHECK(H_recomposed.isApprox(H)); |
362 |
|
|
|
363 |
|
|
// test Operational Space Inertia Matrix |
364 |
|
|
{ |
365 |
|
✗ |
MatrixXd JMinvJt = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse() |
366 |
|
✗ |
* H.middleRows<12>(0).rightCols(model.nv).transpose(); |
367 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
368 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
369 |
|
✗ |
Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(constraint_dim, model.nv)); |
370 |
|
✗ |
contact_chol_decomposition.getJMinv(JMinv_test); |
371 |
|
✗ |
MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse(); |
372 |
|
✗ |
BOOST_CHECK(JMinv_ref.isApprox(JMinv_test)); |
373 |
|
|
|
374 |
|
✗ |
BOOST_CHECK(iosim.isApprox(JMinvJt)); |
375 |
|
✗ |
BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); |
376 |
|
|
|
377 |
|
✗ |
const MatrixXd rhs = MatrixXd::Random(12, 12); |
378 |
|
✗ |
const MatrixXd res_delassus = contact_chol_decomposition.getDelassusCholeskyExpression() * rhs; |
379 |
|
✗ |
const MatrixXd res_delassus_ref = iosim * rhs; |
380 |
|
|
|
381 |
|
✗ |
BOOST_CHECK(res_delassus_ref.isApprox(res_delassus)); |
382 |
|
|
|
383 |
|
|
const MatrixXd res_delassus_inverse = |
384 |
|
✗ |
contact_chol_decomposition.getDelassusCholeskyExpression().solve(rhs); |
385 |
|
✗ |
const MatrixXd res_delassus_inverse_ref = osim * rhs; |
386 |
|
|
|
387 |
|
✗ |
BOOST_CHECK(res_delassus_inverse_ref.isApprox(res_delassus_inverse)); |
388 |
|
|
} |
389 |
|
|
|
390 |
|
|
// test Mass matrix cholesky |
391 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
392 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
393 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
394 |
|
|
|
395 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
396 |
|
|
|
397 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition_mu; |
398 |
|
✗ |
contact_chol_decomposition_mu.allocate(model, contact_models); |
399 |
|
✗ |
contact_chol_decomposition_mu.compute(model, data, contact_models, contact_datas, 0.); |
400 |
|
|
|
401 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition_mu.D.isApprox(contact_chol_decomposition.D)); |
402 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition_mu.Dinv.isApprox(contact_chol_decomposition.Dinv)); |
403 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition_mu.U.isApprox(contact_chol_decomposition.U)); |
404 |
|
|
|
405 |
|
✗ |
const double mu = 0.1; |
406 |
|
✗ |
contact_chol_decomposition_mu.compute(model, data, contact_models, contact_datas, mu); |
407 |
|
✗ |
Data::MatrixXs H_mu(H); |
408 |
|
✗ |
H_mu.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu); |
409 |
|
|
|
410 |
|
|
// test damped Operational Space Inertia Matrix |
411 |
|
|
{ |
412 |
|
✗ |
MatrixXd JMinvJt_mu = H_mu.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse() |
413 |
|
✗ |
* H_mu.middleRows<12>(0).rightCols(model.nv).transpose() |
414 |
|
✗ |
+ mu * MatrixXd::Identity(12, 12); |
415 |
|
✗ |
MatrixXd iosim_mu = contact_chol_decomposition_mu.getInverseOperationalSpaceInertiaMatrix(); |
416 |
|
✗ |
MatrixXd osim_mu = contact_chol_decomposition_mu.getOperationalSpaceInertiaMatrix(); |
417 |
|
|
|
418 |
|
✗ |
BOOST_CHECK(iosim_mu.isApprox(JMinvJt_mu)); |
419 |
|
✗ |
BOOST_CHECK(osim_mu.isApprox(JMinvJt_mu.inverse())); |
420 |
|
|
|
421 |
|
✗ |
const MatrixXd rhs = MatrixXd::Random(12, 1); |
422 |
|
✗ |
const MatrixXd res = contact_chol_decomposition_mu.getDelassusCholeskyExpression() * rhs; |
423 |
|
✗ |
const MatrixXd res_ref = iosim_mu * rhs; |
424 |
|
|
|
425 |
|
✗ |
BOOST_CHECK(res_ref.isApprox(res)); |
426 |
|
|
|
427 |
|
|
const MatrixXd res_no_mu = |
428 |
|
✗ |
contact_chol_decomposition_mu.getDelassusCholeskyExpression() * rhs - mu * rhs; |
429 |
|
✗ |
const MatrixXd res_no_mu_ref = contact_chol_decomposition.getDelassusCholeskyExpression() * rhs; |
430 |
|
|
|
431 |
|
✗ |
BOOST_CHECK(res_no_mu.isApprox(res_no_mu_ref)); |
432 |
|
|
} |
433 |
|
|
|
434 |
|
|
Data::MatrixXs H_recomposed_mu = contact_chol_decomposition_mu.U |
435 |
|
✗ |
* contact_chol_decomposition_mu.D.asDiagonal() |
436 |
|
✗ |
* contact_chol_decomposition_mu.U.transpose(); |
437 |
|
|
|
438 |
|
✗ |
BOOST_CHECK(H_recomposed_mu.isApprox(H_mu)); |
439 |
|
|
|
440 |
|
|
// Test basic operation |
441 |
|
✗ |
VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size())); |
442 |
|
✗ |
MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20)); |
443 |
|
|
|
444 |
|
|
// Test Uv |
445 |
|
✗ |
VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); |
446 |
|
|
|
447 |
|
✗ |
contact_chol_decomposition.Uv(Uv_op_res); |
448 |
|
✗ |
Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in; |
449 |
|
|
|
450 |
|
✗ |
BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); |
451 |
|
|
|
452 |
|
✗ |
MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); |
453 |
|
|
|
454 |
|
✗ |
contact_chol_decomposition.Uv(Uv_mat_op_res); |
455 |
|
✗ |
Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in; |
456 |
|
|
|
457 |
|
✗ |
BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); |
458 |
|
|
|
459 |
|
|
// Test Utv |
460 |
|
✗ |
VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); |
461 |
|
|
|
462 |
|
✗ |
contact_chol_decomposition.Utv(Utv_op_res); |
463 |
|
✗ |
Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in; |
464 |
|
|
|
465 |
|
✗ |
BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); |
466 |
|
|
|
467 |
|
✗ |
MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); |
468 |
|
|
|
469 |
|
✗ |
contact_chol_decomposition.Utv(Utv_mat_op_res); |
470 |
|
✗ |
Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in; |
471 |
|
|
|
472 |
|
✗ |
BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); |
473 |
|
|
|
474 |
|
|
// Test Uiv |
475 |
|
✗ |
VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); |
476 |
|
|
|
477 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_op_res); |
478 |
|
✗ |
Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in; |
479 |
|
|
|
480 |
|
✗ |
BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); |
481 |
|
|
|
482 |
|
✗ |
MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); |
483 |
|
|
|
484 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_mat_op_res); |
485 |
|
✗ |
Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in; |
486 |
|
|
|
487 |
|
✗ |
BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); |
488 |
|
|
|
489 |
|
|
// Test Utiv |
490 |
|
✗ |
VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); |
491 |
|
|
|
492 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_op_res); |
493 |
|
✗ |
Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in; |
494 |
|
|
|
495 |
|
✗ |
BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); |
496 |
|
|
|
497 |
|
✗ |
MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); |
498 |
|
|
|
499 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_mat_op_res); |
500 |
|
✗ |
Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in; |
501 |
|
|
|
502 |
|
✗ |
BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); |
503 |
|
|
|
504 |
|
|
// SolveInPlace |
505 |
|
✗ |
VectorXd sol(v_in); |
506 |
|
✗ |
contact_chol_decomposition.solveInPlace(sol); |
507 |
|
|
|
508 |
|
✗ |
VectorXd sol_ref(H.inverse() * v_in); |
509 |
|
|
|
510 |
|
✗ |
BOOST_CHECK(sol.isApprox(sol_ref)); |
511 |
|
|
|
512 |
|
✗ |
MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in); |
513 |
|
|
|
514 |
|
✗ |
contact_chol_decomposition.solveInPlace(sol_mat); |
515 |
|
✗ |
sol_mat_ref.noalias() = H.inverse() * mat_in; |
516 |
|
|
|
517 |
|
✗ |
BOOST_CHECK(sol_mat.isApprox(sol_mat_ref)); |
518 |
|
|
|
519 |
|
|
// solve |
520 |
|
✗ |
MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in); |
521 |
|
✗ |
BOOST_CHECK(sol_copy_mat.isApprox(sol_mat)); |
522 |
|
|
|
523 |
|
|
// inverse |
524 |
|
✗ |
MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size()); |
525 |
|
✗ |
contact_chol_decomposition.inverse(H_inv); |
526 |
|
|
|
527 |
|
✗ |
MatrixXd H_inv_ref = H.inverse(); |
528 |
|
✗ |
BOOST_CHECK(H_inv.isApprox(H_inv_ref)); |
529 |
|
|
|
530 |
|
|
// Check matrix |
531 |
|
✗ |
MatrixXd mat1; |
532 |
|
✗ |
contact_chol_decomposition.matrix(mat1); |
533 |
|
✗ |
BOOST_CHECK(mat1.isApprox(H)); |
534 |
|
|
|
535 |
|
✗ |
MatrixXd mat2(constraint_dim + model.nv, constraint_dim + model.nv); |
536 |
|
✗ |
contact_chol_decomposition.matrix(mat2.middleCols(0, constraint_dim + model.nv)); |
537 |
|
✗ |
BOOST_CHECK(mat2.isApprox(H)); |
538 |
|
|
|
539 |
|
✗ |
MatrixXd mat3 = contact_chol_decomposition.matrix(); |
540 |
|
✗ |
BOOST_CHECK(mat3.isApprox(H)); |
541 |
|
|
} |
542 |
|
|
|
543 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_cholesky_contact3D_6D_LOCAL) |
544 |
|
|
{ |
545 |
|
|
using namespace Eigen; |
546 |
|
|
using namespace pinocchio; |
547 |
|
|
using namespace pinocchio::cholesky; |
548 |
|
|
|
549 |
|
✗ |
pinocchio::Model model; |
550 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
551 |
|
✗ |
pinocchio::Data data_ref(model); |
552 |
|
|
|
553 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
554 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
555 |
|
✗ |
VectorXd q = randomConfiguration(model); |
556 |
|
|
|
557 |
|
✗ |
const std::string RF = "rleg6_joint"; |
558 |
|
✗ |
const std::string LF = "lleg6_joint"; |
559 |
|
✗ |
const std::string RA = "rarm6_joint"; |
560 |
|
✗ |
const std::string LA = "larm6_joint"; |
561 |
|
|
|
562 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
563 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
564 |
|
✗ |
RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL); |
565 |
|
✗ |
contact_models.push_back(ci_RF); |
566 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RF)); |
567 |
|
✗ |
RigidConstraintModel ci_LF(CONTACT_3D, model, model.getJointId(LF), LOCAL); |
568 |
|
✗ |
contact_models.push_back(ci_LF); |
569 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LF)); |
570 |
|
|
// RigidConstraintModel ci_RA(CONTACT_3D,model.getJointId(RA),LOCAL); |
571 |
|
|
// contact_models.push_back(ci_RA); |
572 |
|
|
// contact_datas.push_back(RigidConstraintData(ci_RA)); |
573 |
|
|
// RigidConstraintModel ci_LA(CONTACT_3D,model.getJointId(LA),LOCAL_WORLD_ALIGNED); |
574 |
|
|
// contact_models.push_back(ci_LA); |
575 |
|
|
// contact_datas.push_back(RigidConstraintData(ci_LA)); |
576 |
|
|
|
577 |
|
|
// Compute Mass Matrix |
578 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
579 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
580 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
581 |
|
|
|
582 |
|
|
// Compute Cholesky decomposition |
583 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
584 |
|
|
|
585 |
|
|
// Compute Jacobians |
586 |
|
✗ |
Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv); |
587 |
|
✗ |
J_RF.setZero(); |
588 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF); |
589 |
|
✗ |
J_LF.setZero(); |
590 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); |
591 |
|
✗ |
J_RA.setZero(); |
592 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA); |
593 |
|
✗ |
J_LA.setZero(); |
594 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL_WORLD_ALIGNED, J_LA); |
595 |
|
|
|
596 |
|
✗ |
const int constraint_dim = 9; |
597 |
|
✗ |
const int total_dim = model.nv + constraint_dim; |
598 |
|
|
|
599 |
|
✗ |
Data::MatrixXs H(total_dim, total_dim); |
600 |
|
✗ |
H.setZero(); |
601 |
|
✗ |
H.bottomRightCorner(model.nv, model.nv) = data_ref.M; |
602 |
|
✗ |
H.middleRows<6>(0).rightCols(model.nv) = J_RF; |
603 |
|
✗ |
H.middleRows<3>(6).rightCols(model.nv) = J_LF.middleRows<3>(Motion::LINEAR); |
604 |
|
|
// H.middleRows<3>(9).rightCols(model.nv) = J_RA.middleRows<3>(Motion::LINEAR); |
605 |
|
|
// H.middleRows<3>(12).rightCols(model.nv) = J_LA.middleRows<3>(Motion::LINEAR); |
606 |
|
|
|
607 |
|
✗ |
H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose(); |
608 |
|
|
|
609 |
|
✗ |
Data data(model); |
610 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
611 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
612 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
613 |
|
|
|
614 |
|
✗ |
ContactCholeskyDecompositionAccessor access(contact_chol_decomposition); |
615 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
616 |
|
|
{ |
617 |
|
✗ |
if (data.parents_fromRow[(size_t)k] == -1) |
618 |
|
✗ |
BOOST_CHECK(access.getParents_fromRow()[k + constraint_dim] == -1); |
619 |
|
|
else |
620 |
|
✗ |
BOOST_CHECK( |
621 |
|
|
access.getParents_fromRow()[k + constraint_dim] |
622 |
|
|
== data.parents_fromRow[(size_t)k] + constraint_dim); |
623 |
|
|
} |
624 |
|
|
|
625 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas); |
626 |
|
|
|
627 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
628 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
629 |
|
|
|
630 |
|
✗ |
BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); |
631 |
|
✗ |
BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); |
632 |
|
✗ |
BOOST_CHECK( |
633 |
|
|
data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv))); |
634 |
|
|
|
635 |
|
|
Data::MatrixXs M_recomposed = |
636 |
|
✗ |
contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv) |
637 |
|
✗ |
* contact_chol_decomposition.D.tail(model.nv).asDiagonal() |
638 |
|
✗ |
* contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose(); |
639 |
|
✗ |
BOOST_CHECK(M_recomposed.isApprox(data.M)); |
640 |
|
|
|
641 |
|
|
Data::MatrixXs H_recomposed = contact_chol_decomposition.U |
642 |
|
✗ |
* contact_chol_decomposition.D.asDiagonal() |
643 |
|
✗ |
* contact_chol_decomposition.U.transpose(); |
644 |
|
|
|
645 |
|
✗ |
BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M)); |
646 |
|
✗ |
BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv) |
647 |
|
|
.isApprox(H.topRightCorner(constraint_dim, model.nv))); |
648 |
|
✗ |
BOOST_CHECK(H_recomposed.isApprox(H)); |
649 |
|
|
|
650 |
|
|
// Test basic operation |
651 |
|
✗ |
VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size())); |
652 |
|
✗ |
MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20)); |
653 |
|
|
|
654 |
|
|
// Test Uv |
655 |
|
✗ |
VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); |
656 |
|
|
|
657 |
|
✗ |
contact_chol_decomposition.Uv(Uv_op_res); |
658 |
|
✗ |
Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in; |
659 |
|
|
|
660 |
|
✗ |
BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); |
661 |
|
|
|
662 |
|
✗ |
MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); |
663 |
|
|
|
664 |
|
✗ |
contact_chol_decomposition.Uv(Uv_mat_op_res); |
665 |
|
✗ |
Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in; |
666 |
|
|
|
667 |
|
✗ |
BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); |
668 |
|
|
|
669 |
|
|
// Test Utv |
670 |
|
✗ |
VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); |
671 |
|
|
|
672 |
|
✗ |
contact_chol_decomposition.Utv(Utv_op_res); |
673 |
|
✗ |
Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in; |
674 |
|
|
|
675 |
|
✗ |
BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); |
676 |
|
|
|
677 |
|
✗ |
MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); |
678 |
|
|
|
679 |
|
✗ |
contact_chol_decomposition.Utv(Utv_mat_op_res); |
680 |
|
✗ |
Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in; |
681 |
|
|
|
682 |
|
✗ |
BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); |
683 |
|
|
|
684 |
|
|
// Test Uiv |
685 |
|
✗ |
VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); |
686 |
|
|
|
687 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_op_res); |
688 |
|
✗ |
Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in; |
689 |
|
|
|
690 |
|
✗ |
BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); |
691 |
|
|
|
692 |
|
✗ |
MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); |
693 |
|
|
|
694 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_mat_op_res); |
695 |
|
✗ |
Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in; |
696 |
|
|
|
697 |
|
✗ |
BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); |
698 |
|
|
|
699 |
|
|
// Test Utiv |
700 |
|
✗ |
VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); |
701 |
|
|
|
702 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_op_res); |
703 |
|
✗ |
Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in; |
704 |
|
|
|
705 |
|
✗ |
BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); |
706 |
|
|
|
707 |
|
✗ |
MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); |
708 |
|
|
|
709 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_mat_op_res); |
710 |
|
✗ |
Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in; |
711 |
|
|
|
712 |
|
✗ |
BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); |
713 |
|
|
|
714 |
|
|
// SolveInPlace |
715 |
|
✗ |
VectorXd sol(v_in); |
716 |
|
✗ |
contact_chol_decomposition.solveInPlace(sol); |
717 |
|
|
|
718 |
|
✗ |
VectorXd sol_ref(H.inverse() * v_in); |
719 |
|
|
|
720 |
|
✗ |
BOOST_CHECK(sol.isApprox(sol_ref)); |
721 |
|
|
|
722 |
|
✗ |
MatrixXd sol_mat(mat_in), sol_mat_ref(mat_in); |
723 |
|
|
|
724 |
|
✗ |
contact_chol_decomposition.solveInPlace(sol_mat); |
725 |
|
✗ |
sol_mat_ref.noalias() = H.inverse() * mat_in; |
726 |
|
|
|
727 |
|
✗ |
BOOST_CHECK(sol_mat.isApprox(sol_mat_ref)); |
728 |
|
|
|
729 |
|
|
// solve |
730 |
|
✗ |
MatrixXd sol_copy_mat = contact_chol_decomposition.solve(mat_in); |
731 |
|
✗ |
BOOST_CHECK(sol_copy_mat.isApprox(sol_mat)); |
732 |
|
|
|
733 |
|
|
// inverse |
734 |
|
✗ |
MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size()); |
735 |
|
✗ |
contact_chol_decomposition.inverse(H_inv); |
736 |
|
|
|
737 |
|
✗ |
MatrixXd H_inv_ref = H.inverse(); |
738 |
|
✗ |
BOOST_CHECK(H_inv.isApprox(H_inv_ref)); |
739 |
|
|
|
740 |
|
|
// Check matrix |
741 |
|
✗ |
MatrixXd mat1; |
742 |
|
✗ |
contact_chol_decomposition.matrix(mat1); |
743 |
|
✗ |
BOOST_CHECK(mat1.isApprox(H)); |
744 |
|
|
|
745 |
|
✗ |
MatrixXd mat2(constraint_dim + model.nv, constraint_dim + model.nv); |
746 |
|
✗ |
contact_chol_decomposition.matrix(mat2.middleCols(0, constraint_dim + model.nv)); |
747 |
|
✗ |
BOOST_CHECK(mat2.isApprox(H)); |
748 |
|
|
|
749 |
|
✗ |
MatrixXd mat3 = contact_chol_decomposition.matrix(); |
750 |
|
✗ |
BOOST_CHECK(mat3.isApprox(H)); |
751 |
|
|
|
752 |
|
|
// test Operational Space Inertia Matrix |
753 |
|
✗ |
MatrixXd JMinvJt = H.middleRows<9>(0).rightCols(model.nv) * data_ref.M.inverse() |
754 |
|
✗ |
* H.middleRows<9>(0).rightCols(model.nv).transpose(); |
755 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
756 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
757 |
|
|
|
758 |
|
✗ |
BOOST_CHECK(iosim.isApprox(JMinvJt)); |
759 |
|
✗ |
BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); |
760 |
|
|
|
761 |
|
|
// test Mass matrix cholesky |
762 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
763 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
764 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
765 |
|
|
|
766 |
|
✗ |
Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(9, model.nv)); |
767 |
|
✗ |
contact_chol_decomposition.getJMinv(JMinv_test); |
768 |
|
✗ |
MatrixXd JMinv_ref = H.middleRows<9>(0).rightCols(model.nv) * data_ref.M.inverse(); |
769 |
|
✗ |
BOOST_CHECK(JMinv_ref.isApprox(JMinv_test)); |
770 |
|
|
|
771 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
772 |
|
|
} |
773 |
|
|
|
774 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_LOCAL_WORLD_ALIGNED) |
775 |
|
|
{ |
776 |
|
|
using namespace Eigen; |
777 |
|
|
using namespace pinocchio; |
778 |
|
|
using namespace pinocchio::cholesky; |
779 |
|
|
|
780 |
|
✗ |
pinocchio::Model model; |
781 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
782 |
|
✗ |
pinocchio::Data data_ref(model); |
783 |
|
|
|
784 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
785 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
786 |
|
✗ |
VectorXd q = randomConfiguration(model); |
787 |
|
|
|
788 |
|
✗ |
const std::string RF = "rleg6_joint"; |
789 |
|
✗ |
const std::string LF = "lleg6_joint"; |
790 |
|
|
|
791 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
792 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
793 |
|
✗ |
RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL_WORLD_ALIGNED); |
794 |
|
✗ |
contact_models.push_back(ci_RF); |
795 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RF)); |
796 |
|
✗ |
RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
797 |
|
✗ |
contact_models.push_back(ci_LF); |
798 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LF)); |
799 |
|
|
|
800 |
|
|
// Compute Mass Matrix |
801 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
802 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
803 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
804 |
|
|
|
805 |
|
|
// Compute Cholesky decomposition |
806 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
807 |
|
|
|
808 |
|
|
// Compute Jacobians |
809 |
|
✗ |
Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv); |
810 |
|
✗ |
J_RF.setZero(); |
811 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), ci_RF.reference_frame, J_RF); |
812 |
|
✗ |
J_LF.setZero(); |
813 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), ci_LF.reference_frame, J_LF); |
814 |
|
|
|
815 |
|
✗ |
const int constraint_dim = 12; |
816 |
|
✗ |
const int total_dim = model.nv + constraint_dim; |
817 |
|
|
|
818 |
|
✗ |
Data::MatrixXs H(total_dim, total_dim); |
819 |
|
✗ |
H.setZero(); |
820 |
|
✗ |
H.bottomRightCorner(model.nv, model.nv) = data_ref.M; |
821 |
|
✗ |
H.middleRows<6>(0).rightCols(model.nv) = J_RF; |
822 |
|
✗ |
H.middleRows<6>(6).rightCols(model.nv) = J_LF; |
823 |
|
|
|
824 |
|
✗ |
H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose(); |
825 |
|
|
|
826 |
|
✗ |
Data data(model); |
827 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
828 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
829 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
830 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas); |
831 |
|
|
|
832 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
833 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
834 |
|
|
|
835 |
|
✗ |
Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); |
836 |
|
|
|
837 |
|
✗ |
BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M)); |
838 |
|
✗ |
BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv) |
839 |
|
|
.isApprox(H.topRightCorner(constraint_dim, model.nv))); |
840 |
|
✗ |
BOOST_CHECK(H_recomposed.isApprox(H)); |
841 |
|
|
|
842 |
|
|
// inverse |
843 |
|
✗ |
MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size()); |
844 |
|
✗ |
contact_chol_decomposition.inverse(H_inv); |
845 |
|
|
|
846 |
|
✗ |
MatrixXd H_inv_ref = H_recomposed.inverse(); |
847 |
|
✗ |
BOOST_CHECK(H_inv.isApprox(H_inv_ref)); |
848 |
|
|
|
849 |
|
|
// test Operational Space Inertia Matrix |
850 |
|
✗ |
MatrixXd JMinvJt = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse() |
851 |
|
✗ |
* H.middleRows<12>(0).rightCols(model.nv).transpose(); |
852 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
853 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
854 |
|
|
|
855 |
|
✗ |
BOOST_CHECK(iosim.isApprox(JMinvJt)); |
856 |
|
✗ |
BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); |
857 |
|
|
|
858 |
|
|
// test Mass matrix cholesky |
859 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
860 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
861 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
862 |
|
|
|
863 |
|
✗ |
Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(12, model.nv)); |
864 |
|
✗ |
contact_chol_decomposition.getJMinv(JMinv_test); |
865 |
|
✗ |
MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse(); |
866 |
|
✗ |
BOOST_CHECK(JMinv_ref.isApprox(JMinv_test)); |
867 |
|
|
|
868 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
869 |
|
|
} |
870 |
|
|
|
871 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_cholesky_contact6D_by_joint_2) |
872 |
|
|
{ |
873 |
|
|
using namespace Eigen; |
874 |
|
|
using namespace pinocchio; |
875 |
|
|
using namespace pinocchio::cholesky; |
876 |
|
|
|
877 |
|
✗ |
pinocchio::Model model; |
878 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
879 |
|
✗ |
pinocchio::Data data_ref(model); |
880 |
|
|
|
881 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
882 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
883 |
|
✗ |
VectorXd q = randomConfiguration(model); |
884 |
|
|
|
885 |
|
✗ |
const std::string RF = "rleg6_joint"; |
886 |
|
✗ |
const std::string LF = "lleg6_joint"; |
887 |
|
✗ |
const std::string RA = "rarm6_joint"; |
888 |
|
✗ |
const std::string LA = "larm6_joint"; |
889 |
|
|
|
890 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
891 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
892 |
|
✗ |
RigidConstraintModel ci_RF(CONTACT_6D, model, 0, model.getJointId(RF), LOCAL); |
893 |
|
✗ |
contact_models.push_back(ci_RF); |
894 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RF)); |
895 |
|
✗ |
RigidConstraintModel ci_LF(CONTACT_6D, model, 0, model.getJointId(LF), LOCAL); |
896 |
|
✗ |
contact_models.push_back(ci_LF); |
897 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LF)); |
898 |
|
✗ |
RigidConstraintModel ci_RA(CONTACT_6D, model, 0, model.getJointId(RA), LOCAL_WORLD_ALIGNED); |
899 |
|
✗ |
contact_models.push_back(ci_RA); |
900 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RA)); |
901 |
|
✗ |
RigidConstraintModel ci_LA(CONTACT_6D, model, 0, model.getJointId(LA), LOCAL); |
902 |
|
✗ |
contact_models.push_back(ci_LA); |
903 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LA)); |
904 |
|
|
|
905 |
|
|
// Compute Mass Matrix |
906 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
907 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
908 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
909 |
|
|
|
910 |
|
|
// Compute Cholesky decomposition |
911 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
912 |
|
|
|
913 |
|
|
// Compute Jacobians |
914 |
|
✗ |
Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv); |
915 |
|
✗ |
J_RF.setZero(); |
916 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF); |
917 |
|
✗ |
J_LF.setZero(); |
918 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF); |
919 |
|
✗ |
J_RA.setZero(); |
920 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RA), WORLD, J_RA); |
921 |
|
✗ |
J_LA.setZero(); |
922 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LA), WORLD, J_LA); |
923 |
|
|
|
924 |
|
✗ |
const int constraint_dim = 24; |
925 |
|
✗ |
const int total_dim = model.nv + constraint_dim; |
926 |
|
|
|
927 |
|
✗ |
const SE3 oMRA_wla = SE3(SE3::Matrix3::Identity(), ci_RA.joint1_placement.translation()); |
928 |
|
|
|
929 |
|
✗ |
const double mu = 0.1; |
930 |
|
✗ |
Data::MatrixXs H(total_dim, total_dim); |
931 |
|
✗ |
H.setZero(); |
932 |
|
✗ |
H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu); |
933 |
|
✗ |
H.bottomRightCorner(model.nv, model.nv) = data_ref.M; |
934 |
|
✗ |
H.middleRows<6>(0).rightCols(model.nv) = -ci_RF.joint1_placement.toActionMatrixInverse() * J_RF; |
935 |
|
✗ |
H.middleRows<6>(6).rightCols(model.nv) = -ci_LF.joint1_placement.toActionMatrixInverse() * J_LF; |
936 |
|
✗ |
H.middleRows<6>(12).rightCols(model.nv) = -oMRA_wla.toActionMatrixInverse() * J_RA; |
937 |
|
✗ |
H.middleRows<6>(18).rightCols(model.nv) = -ci_LA.joint1_placement.toActionMatrixInverse() * J_LA; |
938 |
|
|
|
939 |
|
✗ |
H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose(); |
940 |
|
|
|
941 |
|
✗ |
Data data(model); |
942 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
943 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
944 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
945 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu); |
946 |
|
|
|
947 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
948 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
949 |
|
|
|
950 |
|
✗ |
Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); |
951 |
|
|
|
952 |
|
✗ |
BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M)); |
953 |
|
✗ |
BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv) |
954 |
|
|
.isApprox(H.topRightCorner(constraint_dim, model.nv))); |
955 |
|
✗ |
BOOST_CHECK(H_recomposed.isApprox(H)); |
956 |
|
|
|
957 |
|
✗ |
VectorXd v_in(VectorXd::Random(contact_chol_decomposition.size())); |
958 |
|
✗ |
MatrixXd mat_in(MatrixXd::Random(contact_chol_decomposition.size(), 20)); |
959 |
|
|
|
960 |
|
|
// Test Uv |
961 |
|
✗ |
VectorXd Uv_op_res(v_in), Uv_op_ref(v_in); |
962 |
|
|
|
963 |
|
✗ |
contact_chol_decomposition.Uv(Uv_op_res); |
964 |
|
✗ |
Uv_op_ref.noalias() = contact_chol_decomposition.U * v_in; |
965 |
|
|
|
966 |
|
✗ |
BOOST_CHECK(Uv_op_res.isApprox(Uv_op_ref)); |
967 |
|
|
|
968 |
|
✗ |
MatrixXd Uv_mat_op_res(mat_in), Uv_mat_op_ref(mat_in); |
969 |
|
|
|
970 |
|
✗ |
contact_chol_decomposition.Uv(Uv_mat_op_res); |
971 |
|
✗ |
Uv_mat_op_ref.noalias() = contact_chol_decomposition.U * mat_in; |
972 |
|
|
|
973 |
|
✗ |
BOOST_CHECK(Uv_mat_op_res.isApprox(Uv_mat_op_ref)); |
974 |
|
|
|
975 |
|
|
// Test Utv |
976 |
|
✗ |
VectorXd Utv_op_res(v_in), Utv_op_ref(v_in); |
977 |
|
|
|
978 |
|
✗ |
contact_chol_decomposition.Utv(Utv_op_res); |
979 |
|
✗ |
Utv_op_ref.noalias() = contact_chol_decomposition.U.transpose() * v_in; |
980 |
|
|
|
981 |
|
✗ |
BOOST_CHECK(Utv_op_res.isApprox(Utv_op_ref)); |
982 |
|
|
|
983 |
|
✗ |
MatrixXd Utv_mat_op_res(mat_in), Utv_mat_op_ref(mat_in); |
984 |
|
|
|
985 |
|
✗ |
contact_chol_decomposition.Utv(Utv_mat_op_res); |
986 |
|
✗ |
Utv_mat_op_ref.noalias() = contact_chol_decomposition.U.transpose() * mat_in; |
987 |
|
|
|
988 |
|
✗ |
BOOST_CHECK(Utv_mat_op_res.isApprox(Utv_mat_op_ref)); |
989 |
|
|
|
990 |
|
|
// Test Uiv |
991 |
|
✗ |
VectorXd Uiv_op_res(v_in), Uiv_op_ref(v_in); |
992 |
|
|
|
993 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_op_res); |
994 |
|
✗ |
Uiv_op_ref.noalias() = contact_chol_decomposition.U.inverse() * v_in; |
995 |
|
|
|
996 |
|
✗ |
BOOST_CHECK(Uiv_op_res.isApprox(Uiv_op_ref)); |
997 |
|
|
|
998 |
|
✗ |
MatrixXd Uiv_mat_op_res(mat_in), Uiv_mat_op_ref(mat_in); |
999 |
|
|
|
1000 |
|
✗ |
contact_chol_decomposition.Uiv(Uiv_mat_op_res); |
1001 |
|
✗ |
Uiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse() * mat_in; |
1002 |
|
|
|
1003 |
|
✗ |
BOOST_CHECK(Uiv_mat_op_res.isApprox(Uiv_mat_op_ref)); |
1004 |
|
|
|
1005 |
|
|
// Test Utiv |
1006 |
|
✗ |
VectorXd Utiv_op_res(v_in), Utiv_op_ref(v_in); |
1007 |
|
|
|
1008 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_op_res); |
1009 |
|
✗ |
Utiv_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * v_in; |
1010 |
|
|
|
1011 |
|
✗ |
BOOST_CHECK(Utiv_op_res.isApprox(Utiv_op_ref)); |
1012 |
|
|
|
1013 |
|
✗ |
MatrixXd Utiv_mat_op_res(mat_in), Utiv_mat_op_ref(mat_in); |
1014 |
|
|
|
1015 |
|
✗ |
contact_chol_decomposition.Utiv(Utiv_mat_op_res); |
1016 |
|
✗ |
Utiv_mat_op_ref.noalias() = contact_chol_decomposition.U.inverse().transpose() * mat_in; |
1017 |
|
|
|
1018 |
|
✗ |
BOOST_CHECK(Utiv_mat_op_res.isApprox(Utiv_mat_op_ref)); |
1019 |
|
|
|
1020 |
|
|
// inverse |
1021 |
|
✗ |
MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size()); |
1022 |
|
✗ |
contact_chol_decomposition.inverse(H_inv); |
1023 |
|
|
|
1024 |
|
✗ |
MatrixXd H_inv2 = contact_chol_decomposition.U.transpose().inverse() |
1025 |
|
✗ |
* contact_chol_decomposition.Dinv.asDiagonal() |
1026 |
|
✗ |
* contact_chol_decomposition.U.inverse(); |
1027 |
|
|
|
1028 |
|
|
MatrixXd H_inv3 = |
1029 |
|
✗ |
MatrixXd::Identity(contact_chol_decomposition.size(), contact_chol_decomposition.size()); |
1030 |
|
✗ |
contact_chol_decomposition.solveInPlace(H_inv3); |
1031 |
|
|
|
1032 |
|
✗ |
MatrixXd H_inv_ref = H_recomposed.inverse(); |
1033 |
|
✗ |
BOOST_CHECK(H_inv.topLeftCorner(constraint_dim, constraint_dim) |
1034 |
|
|
.isApprox(H_inv_ref.topLeftCorner(constraint_dim, constraint_dim))); |
1035 |
|
✗ |
BOOST_CHECK(H_inv.bottomRightCorner(model.nv, model.nv) |
1036 |
|
|
.isApprox(H_inv_ref.bottomRightCorner(model.nv, model.nv))); |
1037 |
|
✗ |
BOOST_CHECK(H_inv.topRightCorner(constraint_dim, model.nv) |
1038 |
|
|
.isApprox(H_inv_ref.topRightCorner(constraint_dim, model.nv))); |
1039 |
|
|
|
1040 |
|
✗ |
BOOST_CHECK(H_inv_ref.isApprox(H_inv)); |
1041 |
|
✗ |
BOOST_CHECK(H_inv_ref.isApprox(H_inv2)); |
1042 |
|
✗ |
BOOST_CHECK(H_inv_ref.isApprox(H_inv3)); |
1043 |
|
✗ |
const VectorXd ei = VectorXd::Unit(contact_chol_decomposition.size(), constraint_dim); |
1044 |
|
✗ |
VectorXd ei_inv = ei; |
1045 |
|
✗ |
contact_chol_decomposition.solveInPlace(ei_inv); |
1046 |
|
✗ |
VectorXd ei_inv2 = ei; |
1047 |
|
✗ |
contact_chol_decomposition.Uiv(ei_inv2); |
1048 |
|
✗ |
ei_inv2 = contact_chol_decomposition.Dinv.asDiagonal() * ei_inv2; |
1049 |
|
✗ |
contact_chol_decomposition.Utiv(ei_inv2); |
1050 |
|
|
|
1051 |
|
✗ |
BOOST_CHECK(ei_inv.isApprox(H_inv_ref * ei)); |
1052 |
|
✗ |
BOOST_CHECK(ei_inv2.isApprox(H_inv_ref * ei)); |
1053 |
|
✗ |
BOOST_CHECK(ei_inv.isApprox(H_inv * ei)); |
1054 |
|
✗ |
BOOST_CHECK(ei_inv2.isApprox(H_inv * ei)); |
1055 |
|
|
|
1056 |
|
|
// test Operational Space Inertia Matrix |
1057 |
|
✗ |
MatrixXd JMinvJt = H.middleRows<24>(0).rightCols(model.nv) * data_ref.M.inverse() |
1058 |
|
✗ |
* H.middleRows<24>(0).rightCols(model.nv).transpose() |
1059 |
|
✗ |
+ mu * Eigen::MatrixXd::Identity(24, 24); |
1060 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
1061 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
1062 |
|
|
|
1063 |
|
✗ |
BOOST_CHECK(iosim.isApprox(JMinvJt)); |
1064 |
|
✗ |
BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); |
1065 |
|
|
|
1066 |
|
|
// test Mass matrix cholesky |
1067 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
1068 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
1069 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
1070 |
|
|
|
1071 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
1072 |
|
✗ |
Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(24, model.nv)); |
1073 |
|
✗ |
contact_chol_decomposition.getJMinv(JMinv_test); |
1074 |
|
✗ |
MatrixXd JMinv_ref = H.middleRows<24>(0).rightCols(model.nv) * data_ref.M.inverse(); |
1075 |
|
✗ |
BOOST_CHECK(JMinv_ref.isApprox(JMinv_test)); |
1076 |
|
|
} |
1077 |
|
|
|
1078 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_cholesky_contact3D_6D_WORLD_by_joint_2) |
1079 |
|
|
{ |
1080 |
|
|
using namespace Eigen; |
1081 |
|
|
using namespace pinocchio; |
1082 |
|
|
using namespace pinocchio::cholesky; |
1083 |
|
|
|
1084 |
|
✗ |
pinocchio::Model model; |
1085 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
1086 |
|
✗ |
pinocchio::Data data_ref(model); |
1087 |
|
|
|
1088 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
1089 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
1090 |
|
✗ |
VectorXd q = randomConfiguration(model); |
1091 |
|
|
|
1092 |
|
✗ |
const std::string RF = "rleg6_joint"; |
1093 |
|
✗ |
const std::string LF = "lleg6_joint"; |
1094 |
|
✗ |
const std::string RA = "rarm6_joint"; |
1095 |
|
✗ |
const std::string LA = "larm6_joint"; |
1096 |
|
|
|
1097 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
1098 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
1099 |
|
✗ |
RigidConstraintModel ci_RF(CONTACT_6D, model, 0, model.getJointId(RF), LOCAL); |
1100 |
|
✗ |
contact_models.push_back(ci_RF); |
1101 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RF)); |
1102 |
|
✗ |
RigidConstraintModel ci_LF(CONTACT_3D, model, 0, model.getJointId(LF), LOCAL); |
1103 |
|
✗ |
contact_models.push_back(ci_LF); |
1104 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LF)); |
1105 |
|
✗ |
RigidConstraintModel ci_RA(CONTACT_3D, model, 0, model.getJointId(RA), LOCAL); |
1106 |
|
✗ |
contact_models.push_back(ci_RA); |
1107 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RA)); |
1108 |
|
✗ |
RigidConstraintModel ci_LA(CONTACT_3D, model, 0, model.getJointId(LA), LOCAL_WORLD_ALIGNED); |
1109 |
|
✗ |
contact_models.push_back(ci_LA); |
1110 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LA)); |
1111 |
|
|
|
1112 |
|
|
// Compute Mass Matrix |
1113 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
1114 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
1115 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
1116 |
|
|
|
1117 |
|
|
// Compute Cholesky decomposition |
1118 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
1119 |
|
|
|
1120 |
|
|
// Compute Jacobians |
1121 |
|
✗ |
Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA(6, model.nv), J_LA(6, model.nv); |
1122 |
|
✗ |
J_RF.setZero(); |
1123 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF); |
1124 |
|
✗ |
J_LF.setZero(); |
1125 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); |
1126 |
|
✗ |
J_RA.setZero(); |
1127 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA); |
1128 |
|
✗ |
J_LA.setZero(); |
1129 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL_WORLD_ALIGNED, J_LA); |
1130 |
|
|
|
1131 |
|
✗ |
const int constraint_dim = 15; |
1132 |
|
✗ |
const int total_dim = model.nv + constraint_dim; |
1133 |
|
|
|
1134 |
|
✗ |
Data::MatrixXs H(total_dim, total_dim); |
1135 |
|
✗ |
H.setZero(); |
1136 |
|
✗ |
H.bottomRightCorner(model.nv, model.nv) = data_ref.M; |
1137 |
|
✗ |
H.middleRows<6>(0).rightCols(model.nv) = -ci_RF.joint1_placement.toActionMatrix() * J_RF; |
1138 |
|
✗ |
H.middleRows<3>(6).rightCols(model.nv) = |
1139 |
|
✗ |
-data_ref.oMi[model.getJointId(LF)].rotation() * J_LF.middleRows<3>(Motion::LINEAR); |
1140 |
|
✗ |
H.middleRows<3>(9).rightCols(model.nv) = |
1141 |
|
✗ |
-data_ref.oMi[model.getJointId(RA)].rotation() * J_RA.middleRows<3>(Motion::LINEAR); |
1142 |
|
✗ |
H.middleRows<3>(12).rightCols(model.nv) = -J_LA.middleRows<3>(Motion::LINEAR); |
1143 |
|
|
|
1144 |
|
✗ |
H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose(); |
1145 |
|
|
|
1146 |
|
✗ |
Data data(model); |
1147 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
1148 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
1149 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
1150 |
|
|
|
1151 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas); |
1152 |
|
|
|
1153 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
1154 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
1155 |
|
|
|
1156 |
|
✗ |
BOOST_CHECK(data_ref.D.isApprox(contact_chol_decomposition.D.tail(model.nv))); |
1157 |
|
✗ |
BOOST_CHECK(data_ref.Dinv.isApprox(contact_chol_decomposition.Dinv.tail(model.nv))); |
1158 |
|
✗ |
BOOST_CHECK( |
1159 |
|
|
data_ref.U.isApprox(contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv))); |
1160 |
|
|
|
1161 |
|
|
Data::MatrixXs M_recomposed = |
1162 |
|
✗ |
contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv) |
1163 |
|
✗ |
* contact_chol_decomposition.D.tail(model.nv).asDiagonal() |
1164 |
|
✗ |
* contact_chol_decomposition.U.bottomRightCorner(model.nv, model.nv).transpose(); |
1165 |
|
✗ |
BOOST_CHECK(M_recomposed.isApprox(data.M)); |
1166 |
|
|
|
1167 |
|
|
Data::MatrixXs H_recomposed = contact_chol_decomposition.U |
1168 |
|
✗ |
* contact_chol_decomposition.D.asDiagonal() |
1169 |
|
✗ |
* contact_chol_decomposition.U.transpose(); |
1170 |
|
|
|
1171 |
|
✗ |
BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M)); |
1172 |
|
✗ |
BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv) |
1173 |
|
|
.isApprox(H.topRightCorner(constraint_dim, model.nv))); |
1174 |
|
✗ |
BOOST_CHECK(H_recomposed.isApprox(H)); |
1175 |
|
|
|
1176 |
|
|
// test Operational Space Inertia Matrix |
1177 |
|
✗ |
MatrixXd JMinvJt = H.middleRows<15>(0).rightCols(model.nv) * data_ref.M.inverse() |
1178 |
|
✗ |
* H.middleRows<15>(0).rightCols(model.nv).transpose(); |
1179 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
1180 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
1181 |
|
|
|
1182 |
|
✗ |
BOOST_CHECK(iosim.isApprox(JMinvJt)); |
1183 |
|
✗ |
BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); |
1184 |
|
|
|
1185 |
|
|
// test Mass matrix cholesky |
1186 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
1187 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
1188 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
1189 |
|
|
|
1190 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
1191 |
|
✗ |
Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(15, model.nv)); |
1192 |
|
✗ |
contact_chol_decomposition.getJMinv(JMinv_test); |
1193 |
|
✗ |
MatrixXd JMinv_ref = H.middleRows<15>(0).rightCols(model.nv) * data_ref.M.inverse(); |
1194 |
|
✗ |
BOOST_CHECK(JMinv_ref.isApprox(JMinv_test)); |
1195 |
|
|
} |
1196 |
|
|
|
1197 |
|
✗ |
BOOST_AUTO_TEST_CASE(loop_contact_cholesky_contact6D) |
1198 |
|
|
{ |
1199 |
|
|
using namespace Eigen; |
1200 |
|
|
using namespace pinocchio; |
1201 |
|
|
using namespace pinocchio::cholesky; |
1202 |
|
|
|
1203 |
|
✗ |
pinocchio::Model model; |
1204 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
1205 |
|
✗ |
pinocchio::Data data_ref(model); |
1206 |
|
|
|
1207 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
1208 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
1209 |
|
✗ |
VectorXd q = randomConfiguration(model); |
1210 |
|
|
|
1211 |
|
✗ |
const std::string RF = "rleg6_joint"; |
1212 |
|
✗ |
const std::string LF = "lleg6_joint"; |
1213 |
|
✗ |
const std::string RA = "rarm6_joint"; |
1214 |
|
✗ |
const std::string LA = "larm6_joint"; |
1215 |
|
|
|
1216 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
1217 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
1218 |
|
|
RigidConstraintModel loop_RF_LF_local( |
1219 |
|
✗ |
CONTACT_6D, model, model.getJointId(RF), model.getJointId(LF), LOCAL); |
1220 |
|
|
RigidConstraintModel loop_RA_LA_lwa( |
1221 |
|
✗ |
CONTACT_6D, model, model.getJointId(RA), model.getJointId(LA), LOCAL_WORLD_ALIGNED); |
1222 |
|
|
|
1223 |
|
✗ |
loop_RF_LF_local.joint1_placement.setRandom(); |
1224 |
|
✗ |
loop_RF_LF_local.joint2_placement.setRandom(); |
1225 |
|
|
|
1226 |
|
✗ |
loop_RA_LA_lwa.joint1_placement.setRandom(); |
1227 |
|
✗ |
loop_RA_LA_lwa.joint2_placement.setRandom(); |
1228 |
|
|
|
1229 |
|
✗ |
contact_models.push_back(loop_RF_LF_local); |
1230 |
|
✗ |
contact_datas.push_back(RigidConstraintData(loop_RF_LF_local)); |
1231 |
|
|
|
1232 |
|
✗ |
contact_models.push_back(loop_RA_LA_lwa); |
1233 |
|
✗ |
contact_datas.push_back(RigidConstraintData(loop_RA_LA_lwa)); |
1234 |
|
|
|
1235 |
|
|
// Compute Mass Matrix |
1236 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
1237 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
1238 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
1239 |
|
|
|
1240 |
|
|
// Compute Cholesky decomposition |
1241 |
|
✗ |
const double mu = 0.1; |
1242 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
1243 |
|
|
|
1244 |
|
|
// Compute Jacobians |
1245 |
|
✗ |
Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA_local(6, model.nv), |
1246 |
|
✗ |
J_LA_local(6, model.nv); |
1247 |
|
✗ |
J_RF.setZero(); |
1248 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF); |
1249 |
|
✗ |
Data::Matrix6x J_RF_local = loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF; |
1250 |
|
✗ |
J_LF.setZero(); |
1251 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); |
1252 |
|
✗ |
Data::Matrix6x J_LF_local = loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF; |
1253 |
|
✗ |
J_RA_local.setZero(); |
1254 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA_local); |
1255 |
|
✗ |
J_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local; |
1256 |
|
✗ |
J_LA_local.setZero(); |
1257 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL, J_LA_local); |
1258 |
|
✗ |
J_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local; |
1259 |
|
|
|
1260 |
|
✗ |
Data::Matrix6x J_RF_world(6, model.nv), J_LF_world(6, model.nv); |
1261 |
|
✗ |
J_RF_world.setZero(); |
1262 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF_world); |
1263 |
|
✗ |
J_LF_world.setZero(); |
1264 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF_world); |
1265 |
|
|
|
1266 |
|
✗ |
forwardKinematics(model, data_ref, q); |
1267 |
|
|
const SE3 oM1_loop1 = |
1268 |
|
✗ |
data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement; |
1269 |
|
|
const SE3 oM2_loop1 = |
1270 |
|
✗ |
data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement; |
1271 |
|
✗ |
const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1; |
1272 |
|
✗ |
const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement; |
1273 |
|
✗ |
const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement; |
1274 |
|
✗ |
const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2; |
1275 |
|
|
|
1276 |
|
✗ |
const int constraint_dim = 12; |
1277 |
|
✗ |
const int total_dim = model.nv + constraint_dim; |
1278 |
|
|
|
1279 |
|
✗ |
Data::MatrixXs H(total_dim, total_dim); |
1280 |
|
✗ |
H.setZero(); |
1281 |
|
✗ |
H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu); |
1282 |
|
✗ |
H.bottomRightCorner(model.nv, model.nv) = data_ref.M; |
1283 |
|
✗ |
H.middleRows<6>(0).rightCols(model.nv) = J_RF_local - _1M2_loop1.toActionMatrix() * J_LF_local; |
1284 |
|
✗ |
const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(), SE3::Vector3::Zero()); |
1285 |
|
✗ |
H.middleRows<6>(6).rightCols(model.nv) = |
1286 |
|
✗ |
oM1_loop2_lwa.toActionMatrix() * J_RA_local |
1287 |
|
✗ |
- (oM1_loop2_lwa.toActionMatrix() * _1M2_loop2.toActionMatrix()) * J_LA_local; |
1288 |
|
|
|
1289 |
|
✗ |
H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose(); |
1290 |
|
|
|
1291 |
|
✗ |
Data data(model); |
1292 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
1293 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
1294 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
1295 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu); |
1296 |
|
|
|
1297 |
|
✗ |
Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); |
1298 |
|
|
|
1299 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
1300 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
1301 |
|
✗ |
BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M)); |
1302 |
|
✗ |
BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv) |
1303 |
|
|
.isApprox(H.topRightCorner(constraint_dim, model.nv))); |
1304 |
|
✗ |
BOOST_CHECK(H_recomposed.isApprox(H)); |
1305 |
|
|
|
1306 |
|
|
// inverse |
1307 |
|
✗ |
MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size()); |
1308 |
|
✗ |
contact_chol_decomposition.inverse(H_inv); |
1309 |
|
|
|
1310 |
|
✗ |
MatrixXd H_inv_ref = H_recomposed.inverse(); |
1311 |
|
|
|
1312 |
|
✗ |
BOOST_CHECK(H_inv_ref.isApprox(H_inv)); |
1313 |
|
|
|
1314 |
|
|
// test Operational Space Inertia Matrix |
1315 |
|
✗ |
MatrixXd JMinvJt = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse() |
1316 |
|
✗ |
* H.middleRows<12>(0).rightCols(model.nv).transpose() |
1317 |
|
✗ |
+ mu * Eigen::MatrixXd::Identity(12, 12); |
1318 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
1319 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
1320 |
|
|
|
1321 |
|
✗ |
BOOST_CHECK(iosim.isApprox(JMinvJt)); |
1322 |
|
✗ |
BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); |
1323 |
|
|
|
1324 |
|
|
// test Mass matrix cholesky |
1325 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
1326 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
1327 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
1328 |
|
|
|
1329 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
1330 |
|
✗ |
Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(12, model.nv)); |
1331 |
|
✗ |
contact_chol_decomposition.getJMinv(JMinv_test); |
1332 |
|
✗ |
MatrixXd JMinv_ref = H.middleRows<12>(0).rightCols(model.nv) * data_ref.M.inverse(); |
1333 |
|
✗ |
BOOST_CHECK(JMinv_ref.isApprox(JMinv_test)); |
1334 |
|
|
} |
1335 |
|
|
|
1336 |
|
✗ |
BOOST_AUTO_TEST_CASE(loop_contact_cholesky_contact_3d) |
1337 |
|
|
{ |
1338 |
|
|
using namespace Eigen; |
1339 |
|
|
using namespace pinocchio; |
1340 |
|
|
using namespace pinocchio::cholesky; |
1341 |
|
|
|
1342 |
|
✗ |
pinocchio::Model model; |
1343 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
1344 |
|
✗ |
pinocchio::Data data_ref(model); |
1345 |
|
|
|
1346 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
1347 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
1348 |
|
✗ |
VectorXd q = randomConfiguration(model); |
1349 |
|
|
|
1350 |
|
✗ |
const std::string RF = "rleg6_joint"; |
1351 |
|
✗ |
const std::string LF = "lleg6_joint"; |
1352 |
|
✗ |
const std::string RA = "rarm6_joint"; |
1353 |
|
✗ |
const std::string LA = "larm6_joint"; |
1354 |
|
|
|
1355 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
1356 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
1357 |
|
|
RigidConstraintModel loop_RF_LF_local( |
1358 |
|
✗ |
CONTACT_3D, model, model.getJointId(RF), model.getJointId(LF), LOCAL); |
1359 |
|
|
RigidConstraintModel loop_RA_LA_lwa( |
1360 |
|
✗ |
CONTACT_3D, model, model.getJointId(RA), model.getJointId(LA), LOCAL_WORLD_ALIGNED); |
1361 |
|
|
|
1362 |
|
✗ |
loop_RF_LF_local.joint1_placement.setRandom(); |
1363 |
|
✗ |
loop_RF_LF_local.joint2_placement.setRandom(); |
1364 |
|
|
|
1365 |
|
✗ |
loop_RA_LA_lwa.joint1_placement.setRandom(); |
1366 |
|
✗ |
loop_RA_LA_lwa.joint2_placement.setRandom(); |
1367 |
|
|
|
1368 |
|
✗ |
contact_models.push_back(loop_RF_LF_local); |
1369 |
|
✗ |
contact_datas.push_back(RigidConstraintData(loop_RF_LF_local)); |
1370 |
|
|
|
1371 |
|
✗ |
contact_models.push_back(loop_RA_LA_lwa); |
1372 |
|
✗ |
contact_datas.push_back(RigidConstraintData(loop_RA_LA_lwa)); |
1373 |
|
|
|
1374 |
|
|
// Compute Mass Matrix |
1375 |
|
✗ |
crba(model, data_ref, q, Convention::WORLD); |
1376 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
1377 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
1378 |
|
|
|
1379 |
|
|
// Compute Cholesky decomposition |
1380 |
|
✗ |
const double mu = 0.1; |
1381 |
|
✗ |
pinocchio::cholesky::decompose(model, data_ref); |
1382 |
|
|
|
1383 |
|
|
// Compute Jacobians |
1384 |
|
✗ |
Data::Matrix6x J_RF(6, model.nv), J_LF(6, model.nv), J_RA_local(6, model.nv), |
1385 |
|
✗ |
J_LA_local(6, model.nv); |
1386 |
|
✗ |
J_RF.setZero(); |
1387 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF); |
1388 |
|
|
const Data::Matrix6x J_RF_local = |
1389 |
|
✗ |
loop_RF_LF_local.joint1_placement.toActionMatrixInverse() * J_RF; |
1390 |
|
✗ |
J_LF.setZero(); |
1391 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); |
1392 |
|
|
const Data::Matrix6x J_LF_local = |
1393 |
|
✗ |
loop_RF_LF_local.joint2_placement.toActionMatrixInverse() * J_LF; |
1394 |
|
✗ |
J_RA_local.setZero(); |
1395 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RA), LOCAL, J_RA_local); |
1396 |
|
✗ |
J_RA_local = loop_RA_LA_lwa.joint1_placement.toActionMatrixInverse() * J_RA_local; |
1397 |
|
✗ |
J_LA_local.setZero(); |
1398 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LA), LOCAL, J_LA_local); |
1399 |
|
✗ |
J_LA_local = loop_RA_LA_lwa.joint2_placement.toActionMatrixInverse() * J_LA_local; |
1400 |
|
|
|
1401 |
|
✗ |
Data::Matrix6x J_RF_world(6, model.nv), J_LF_world(6, model.nv); |
1402 |
|
✗ |
J_RF_world.setZero(); |
1403 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(RF), WORLD, J_RF_world); |
1404 |
|
✗ |
J_LF_world.setZero(); |
1405 |
|
✗ |
getJointJacobian(model, data_ref, model.getJointId(LF), WORLD, J_LF_world); |
1406 |
|
|
|
1407 |
|
✗ |
forwardKinematics(model, data_ref, q); |
1408 |
|
|
const SE3 oM1_loop1 = |
1409 |
|
✗ |
data_ref.oMi[loop_RF_LF_local.joint1_id] * loop_RF_LF_local.joint1_placement; |
1410 |
|
|
const SE3 oM2_loop1 = |
1411 |
|
✗ |
data_ref.oMi[loop_RF_LF_local.joint2_id] * loop_RF_LF_local.joint2_placement; |
1412 |
|
✗ |
const SE3 _1M2_loop1 = oM1_loop1.inverse() * oM2_loop1; |
1413 |
|
✗ |
const SE3 oM1_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint1_id] * loop_RA_LA_lwa.joint1_placement; |
1414 |
|
✗ |
const SE3 oM2_loop2 = data_ref.oMi[loop_RA_LA_lwa.joint2_id] * loop_RA_LA_lwa.joint2_placement; |
1415 |
|
✗ |
const SE3 _1M2_loop2 = oM1_loop2.inverse() * oM2_loop2; |
1416 |
|
|
|
1417 |
|
✗ |
const int constraint_dim = 6; |
1418 |
|
✗ |
const int total_dim = model.nv + constraint_dim; |
1419 |
|
|
|
1420 |
|
✗ |
Data::MatrixXs H(total_dim, total_dim); |
1421 |
|
✗ |
H.setZero(); |
1422 |
|
✗ |
H.topLeftCorner(constraint_dim, constraint_dim).diagonal().fill(-mu); |
1423 |
|
✗ |
H.bottomRightCorner(model.nv, model.nv) = data_ref.M; |
1424 |
|
✗ |
H.middleRows<3>(0).rightCols(model.nv) = |
1425 |
|
✗ |
J_RF_local.middleRows<3>(Motion::LINEAR) |
1426 |
|
✗ |
- _1M2_loop1.rotation() * J_LF_local.middleRows<3>(Motion::LINEAR); |
1427 |
|
✗ |
const SE3 oM1_loop2_lwa = SE3(oM1_loop2.rotation(), SE3::Vector3::Zero()); |
1428 |
|
✗ |
const SE3 oM2_loop2_lwa = SE3(oM2_loop2.rotation(), SE3::Vector3::Zero()); |
1429 |
|
✗ |
H.middleRows<3>(3).rightCols(model.nv) = |
1430 |
|
✗ |
(oM1_loop2_lwa.toActionMatrix() * J_RA_local - (oM2_loop2_lwa.toActionMatrix()) * J_LA_local) |
1431 |
|
✗ |
.middleRows<3>(Motion::LINEAR); |
1432 |
|
|
|
1433 |
|
✗ |
H.triangularView<Eigen::StrictlyLower>() = H.triangularView<Eigen::StrictlyUpper>().transpose(); |
1434 |
|
|
|
1435 |
|
✗ |
Data data(model); |
1436 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
1437 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
1438 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
1439 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu); |
1440 |
|
✗ |
BOOST_CHECK(contact_datas[0].c1Mc2.isApprox(_1M2_loop1)); |
1441 |
|
✗ |
BOOST_CHECK(contact_datas[1].c1Mc2.isApprox(_1M2_loop2)); |
1442 |
|
|
|
1443 |
|
✗ |
Data::MatrixXs H_recomposed = contact_chol_decomposition.matrix(); |
1444 |
|
|
|
1445 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
1446 |
|
✗ |
data.M.triangularView<Eigen::StrictlyUpper>().transpose(); |
1447 |
|
✗ |
BOOST_CHECK(H_recomposed.bottomRightCorner(model.nv, model.nv).isApprox(data.M)); |
1448 |
|
✗ |
BOOST_CHECK(H_recomposed.topRightCorner(constraint_dim, model.nv) |
1449 |
|
|
.isApprox(H.topRightCorner(constraint_dim, model.nv))); |
1450 |
|
✗ |
BOOST_CHECK(H_recomposed.isApprox(H)); |
1451 |
|
|
|
1452 |
|
|
// std::cout << "H_recomposed.topRightCorner(constraint_dim,model.nv):\n" << |
1453 |
|
|
// H_recomposed.topRightCorner(constraint_dim,model.nv) << std::endl; std::cout << |
1454 |
|
|
// "H.topRightCorner(constraint_dim,model.nv):\n" << H .topRightCorner(constraint_dim,model.nv) |
1455 |
|
|
// << std::endl; |
1456 |
|
|
|
1457 |
|
|
// inverse |
1458 |
|
✗ |
MatrixXd H_inv(contact_chol_decomposition.size(), contact_chol_decomposition.size()); |
1459 |
|
✗ |
contact_chol_decomposition.inverse(H_inv); |
1460 |
|
|
|
1461 |
|
✗ |
MatrixXd H_inv_ref = H_recomposed.inverse(); |
1462 |
|
|
|
1463 |
|
✗ |
BOOST_CHECK(H_inv_ref.isApprox(H_inv)); |
1464 |
|
|
|
1465 |
|
|
// test Operational Space Inertia Matrix |
1466 |
|
✗ |
MatrixXd JMinvJt = H.middleRows<6>(0).rightCols(model.nv) * data_ref.M.inverse() |
1467 |
|
✗ |
* H.middleRows<6>(0).rightCols(model.nv).transpose() |
1468 |
|
✗ |
+ mu * Eigen::MatrixXd::Identity(6, 6); |
1469 |
|
✗ |
MatrixXd iosim = contact_chol_decomposition.getInverseOperationalSpaceInertiaMatrix(); |
1470 |
|
✗ |
MatrixXd osim = contact_chol_decomposition.getOperationalSpaceInertiaMatrix(); |
1471 |
|
|
|
1472 |
|
✗ |
BOOST_CHECK(iosim.isApprox(JMinvJt)); |
1473 |
|
✗ |
BOOST_CHECK(osim.isApprox(JMinvJt.inverse())); |
1474 |
|
|
|
1475 |
|
|
// test Mass matrix cholesky |
1476 |
|
✗ |
data_ref.Minv = data_ref.M.inverse(); |
1477 |
|
✗ |
Eigen::MatrixXd Minv_test(Eigen::MatrixXd::Zero(model.nv, model.nv)); |
1478 |
|
✗ |
contact_chol_decomposition.getInverseMassMatrix(Minv_test); |
1479 |
|
|
|
1480 |
|
✗ |
Eigen::MatrixXd JMinv_test(Eigen::MatrixXd::Zero(6, model.nv)); |
1481 |
|
✗ |
contact_chol_decomposition.getJMinv(JMinv_test); |
1482 |
|
✗ |
MatrixXd JMinv_ref = H.middleRows<6>(0).rightCols(model.nv) * data_ref.M.inverse(); |
1483 |
|
✗ |
BOOST_CHECK(JMinv_ref.isApprox(JMinv_test)); |
1484 |
|
✗ |
BOOST_CHECK(Minv_test.isApprox(data_ref.Minv)); |
1485 |
|
|
} |
1486 |
|
|
|
1487 |
|
✗ |
BOOST_AUTO_TEST_CASE(contact_cholesky_updateDamping) |
1488 |
|
|
{ |
1489 |
|
|
using namespace Eigen; |
1490 |
|
|
using namespace pinocchio; |
1491 |
|
|
using namespace pinocchio::cholesky; |
1492 |
|
|
|
1493 |
|
✗ |
pinocchio::Model model; |
1494 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model, true); |
1495 |
|
✗ |
pinocchio::Data data_ref(model); |
1496 |
|
|
|
1497 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
1498 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
1499 |
|
✗ |
VectorXd q = randomConfiguration(model); |
1500 |
|
|
|
1501 |
|
✗ |
const std::string RF = "rleg6_joint"; |
1502 |
|
✗ |
const std::string LF = "lleg6_joint"; |
1503 |
|
|
|
1504 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
1505 |
|
✗ |
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas; |
1506 |
|
✗ |
RigidConstraintModel ci_RF(CONTACT_6D, model, model.getJointId(RF), LOCAL); |
1507 |
|
✗ |
contact_models.push_back(ci_RF); |
1508 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_RF)); |
1509 |
|
✗ |
RigidConstraintModel ci_LF(CONTACT_6D, model, model.getJointId(LF), LOCAL); |
1510 |
|
✗ |
contact_models.push_back(ci_LF); |
1511 |
|
✗ |
contact_datas.push_back(RigidConstraintData(ci_LF)); |
1512 |
|
|
|
1513 |
|
✗ |
Data data(model); |
1514 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
1515 |
|
|
|
1516 |
|
✗ |
const double mu1 = 1e-2, mu2 = 1e-10; |
1517 |
|
|
|
1518 |
|
|
{ |
1519 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
1520 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
1521 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu1); |
1522 |
|
✗ |
contact_chol_decomposition.updateDamping(mu2); |
1523 |
|
|
|
1524 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition_ref; |
1525 |
|
✗ |
contact_chol_decomposition_ref.allocate(model, contact_models); |
1526 |
|
✗ |
contact_chol_decomposition_ref.compute(model, data, contact_models, contact_datas, mu2); |
1527 |
|
|
|
1528 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.D.isApprox(contact_chol_decomposition_ref.D)); |
1529 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.Dinv.isApprox(contact_chol_decomposition_ref.Dinv)); |
1530 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.U.isApprox(contact_chol_decomposition_ref.U)); |
1531 |
|
|
} |
1532 |
|
|
|
1533 |
|
|
{ |
1534 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition; |
1535 |
|
✗ |
contact_chol_decomposition.allocate(model, contact_models); |
1536 |
|
✗ |
contact_chol_decomposition.compute(model, data, contact_models, contact_datas, mu1); |
1537 |
|
✗ |
contact_chol_decomposition.getDelassusCholeskyExpression().updateDamping(mu2); |
1538 |
|
|
|
1539 |
|
✗ |
ContactCholeskyDecomposition contact_chol_decomposition_ref; |
1540 |
|
✗ |
contact_chol_decomposition_ref.allocate(model, contact_models); |
1541 |
|
✗ |
contact_chol_decomposition_ref.compute(model, data, contact_models, contact_datas, mu2); |
1542 |
|
|
|
1543 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.D.isApprox(contact_chol_decomposition_ref.D)); |
1544 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.Dinv.isApprox(contact_chol_decomposition_ref.Dinv)); |
1545 |
|
✗ |
BOOST_CHECK(contact_chol_decomposition.U.isApprox(contact_chol_decomposition_ref.U)); |
1546 |
|
|
} |
1547 |
|
|
} |
1548 |
|
|
|
1549 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
1550 |
|
|
|