21 #include <boost/test/unit_test.hpp>
22 #include <boost/utility/binary.hpp>
24 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
28 using namespace Eigen;
35 model.lowerPositionLimit.head<3>().
fill(-1.);
36 model.upperPositionLimit.head<3>().
fill(1.);
39 VectorXd
v = VectorXd::Random(
model.nv);
40 VectorXd
tau = VectorXd::Random(
model.nv);
46 const double mu0 = 0.;
50 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
51 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
56 data.M.triangularView<Eigen::StrictlyLower>() =
57 data.M.transpose().triangularView<Eigen::StrictlyLower>();
66 using namespace Eigen;
73 model.lowerPositionLimit.head<3>().
fill(-1.);
74 model.upperPositionLimit.head<3>().
fill(1.);
77 VectorXd
v = VectorXd::Random(
model.nv);
78 VectorXd
tau = VectorXd::Random(
model.nv);
80 const std::string RF =
"rleg6_joint";
82 const std::string LF =
"lleg6_joint";
99 const double mu0 = 0.;
107 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
108 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
119 Eigen::MatrixXd KKT_matrix_ref =
121 KKT_matrix_ref.bottomRightCorner(
model.nv,
model.nv) = data_ref.
M;
130 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
131 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
140 data.M.triangularView<Eigen::StrictlyLower>() =
141 data.M.transpose().triangularView<Eigen::StrictlyLower>();
160 Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
163 .isApprox(KKT_matrix_ref.bottomRightCorner(
model.nv,
model.nv)));
170 Eigen::DenseIndex constraint_id = 0;
186 data_ref.
impulse_c.segment<6>(constraint_id));
195 constraint_id +=
cmodel.size();
201 using namespace Eigen;
208 model.lowerPositionLimit.head<3>().
fill(-1.);
209 model.upperPositionLimit.head<3>().
fill(1.);
212 VectorXd
v = VectorXd::Random(
model.nv);
213 VectorXd
tau = VectorXd::Random(
model.nv);
215 const std::string RF =
"rleg6_joint";
217 const std::string LF =
"lleg6_joint";
234 const double mu0 = 0.;
242 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
243 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
253 rhs_ref.segment<6>(0) =
255 rhs_ref.segment<6>(6) =
258 Eigen::MatrixXd KKT_matrix_ref =
260 KKT_matrix_ref.bottomRightCorner(
model.nv,
model.nv) = data_ref.
M;
269 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
270 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
279 data.M.triangularView<Eigen::StrictlyLower>() =
280 data.M.transpose().triangularView<Eigen::StrictlyLower>();
299 Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
302 .isApprox(KKT_matrix_ref.bottomRightCorner(
model.nv,
model.nv)));
309 Eigen::DenseIndex constraint_id = 0;
325 data_ref.
impulse_c.segment<6>(constraint_id));
334 constraint_id +=
cmodel.size();
340 using namespace Eigen;
347 model.lowerPositionLimit.head<3>().
fill(-1.);
348 model.upperPositionLimit.head<3>().
fill(1.);
351 VectorXd
v = VectorXd::Random(
model.nv);
352 VectorXd
tau = VectorXd::Random(
model.nv);
354 const std::string RF =
"rleg6_joint";
356 const std::string LF =
"lleg6_joint";
373 const double mu0 = 0.;
382 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
383 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
388 J_ref.middleRows<3>(6) = Jtmp.middleRows<3>(Motion::LINEAR);
395 Eigen::MatrixXd KKT_matrix_ref =
397 KKT_matrix_ref.bottomRightCorner(
model.nv,
model.nv) = data_ref.
M;
406 data_ref.
M.triangularView<Eigen::StrictlyLower>() =
407 data_ref.
M.transpose().triangularView<Eigen::StrictlyLower>();
416 data.M.triangularView<Eigen::StrictlyLower>() =
417 data.M.transpose().triangularView<Eigen::StrictlyLower>();
436 Eigen::MatrixXd KKT_matrix = contact_chol.matrix();
439 .isApprox(KKT_matrix_ref.bottomRightCorner(
model.nv,
model.nv)));
446 Eigen::DenseIndex constraint_id = 0;
462 data_ref.
impulse_c.segment<6>(constraint_id));
471 constraint_id +=
cmodel.size();
475 BOOST_AUTO_TEST_SUITE_END()