18 #include <boost/test/unit_test.hpp>
19 #include <boost/utility/binary.hpp>
21 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
25 using namespace Eigen;
32 model.lowerPositionLimit.head<3>().
fill(-1.);
33 model.upperPositionLimit.head<3>().
fill(1.);
35 VectorXd
v(VectorXd::Random(
model.nv));
36 VectorXd
tau(VectorXd::Random(
model.nv));
43 aba_partial_dq.setZero();
45 aba_partial_dv.setZero();
47 aba_partial_dtau.setZero();
49 const double prec = Eigen::NumTraits<double>::dummy_precision();
70 data_ref.oMi[k].toDualActionMatrix() * data_ref.Yaba[k]
71 * data_ref.oMi[k].inverse().toActionMatrix()));
97 data_ref.
Minv.triangularView<Eigen::StrictlyLower>() =
98 data_ref.
Minv.transpose().triangularView<Eigen::StrictlyLower>();
110 MatrixXd aba_partial_dq_fd(
model.nv,
model.nv);
111 aba_partial_dq_fd.setZero();
112 MatrixXd aba_partial_dv_fd(
model.nv,
model.nv);
113 aba_partial_dv_fd.setZero();
114 MatrixXd aba_partial_dtau_fd(
model.nv,
model.nv);
115 aba_partial_dtau_fd.setZero();
119 VectorXd v_eps(VectorXd::Zero(
model.nv));
120 VectorXd q_plus(
model.nq);
121 VectorXd a_plus(
model.nv);
122 const double alpha = 1e-8;
123 for (
int k = 0; k <
model.nv; ++k)
129 aba_partial_dq_fd.col(k) = (a_plus -
a0) /
alpha;
135 for (
int k = 0; k <
model.nv; ++k)
140 aba_partial_dv_fd.col(k) = (a_plus -
a0) /
alpha;
145 VectorXd tau_plus(
tau);
146 for (
int k = 0; k <
model.nv; ++k)
148 tau_plus[k] +=
alpha;
151 aba_partial_dtau_fd.col(k) = (a_plus -
a0) /
alpha;
152 tau_plus[k] -=
alpha;
159 using namespace Eigen;
167 model.lowerPositionLimit.head<3>().
fill(-1.);
168 model.upperPositionLimit.head<3>().
fill(1.);
170 VectorXd
v(VectorXd::Random(
model.nv));
171 VectorXd
tau(VectorXd::Random(
model.nv));
175 aba_partial_dq.setZero();
177 aba_partial_dv.setZero();
179 aba_partial_dtau.setZero();
182 model, data_ref,
q,
v,
tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
200 using namespace Eigen;
208 model.lowerPositionLimit.head<3>().
fill(-1.);
209 model.upperPositionLimit.head<3>().
fill(1.);
211 VectorXd
v(VectorXd::Random(
model.nv));
212 VectorXd
tau(VectorXd::Random(
model.nv));
216 ForceVector fext((
size_t)
model.njoints);
217 for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
221 aba_partial_dq.setZero();
223 aba_partial_dv.setZero();
225 aba_partial_dtau.setZero();
228 model,
data,
q,
v,
tau, fext, aba_partial_dq, aba_partial_dv, aba_partial_dtau);
244 MatrixXd aba_partial_dq_fd(
model.nv,
model.nv);
245 aba_partial_dq_fd.setZero();
246 MatrixXd aba_partial_dv_fd(
model.nv,
model.nv);
247 aba_partial_dv_fd.setZero();
248 MatrixXd aba_partial_dtau_fd(
model.nv,
model.nv);
249 aba_partial_dtau_fd.setZero();
253 VectorXd v_eps(VectorXd::Zero(
model.nv));
254 VectorXd q_plus(
model.nq);
255 VectorXd a_plus(
model.nv);
256 const double alpha = 1e-8;
257 for (
int k = 0; k <
model.nv; ++k)
263 aba_partial_dq_fd.col(k) = (a_plus -
a0) /
alpha;
269 for (
int k = 0; k <
model.nv; ++k)
274 aba_partial_dv_fd.col(k) = (a_plus -
a0) /
alpha;
279 VectorXd tau_plus(
tau);
280 for (
int k = 0; k <
model.nv; ++k)
282 tau_plus[k] +=
alpha;
285 aba_partial_dtau_fd.col(k) = (a_plus -
a0) /
alpha;
286 tau_plus[k] -=
alpha;
300 using namespace Eigen;
308 model.lowerPositionLimit.head<3>().
fill(-1.);
309 model.upperPositionLimit.head<3>().
fill(1.);
311 VectorXd
v(VectorXd::Random(
model.nv));
312 VectorXd
tau(VectorXd::Random(
model.nv));
317 for (
int k = 0; k < 20; ++k)
341 using namespace Eigen;
349 model.lowerPositionLimit.head<3>().
fill(-1.);
350 model.upperPositionLimit.head<3>().
fill(1.);
352 VectorXd
v(VectorXd::Random(
model.nv));
353 VectorXd
a(VectorXd::Random(
model.nv));
358 aba_partial_dq.setZero();
360 aba_partial_dv.setZero();
361 MatrixXd aba_partial_dtau(
model.nv,
model.nv);
362 aba_partial_dtau.setZero();
370 for (
size_t k = 1; k < (size_t)
model.njoints; ++k)
380 using namespace Eigen;
388 model.lowerPositionLimit.head<3>().
fill(-1.);
389 model.upperPositionLimit.head<3>().
fill(1.);
391 VectorXd
v(VectorXd::Random(
model.nv));
392 VectorXd
tau(VectorXd::Random(
model.nv));
395 aba_partial_dq.setZero();
397 aba_partial_dv.setZero();
398 MatrixXd aba_partial_dtau(
model.nv,
model.nv);
399 aba_partial_dtau.setZero();
404 MatrixXd aba_partial_dq_ref(
model.nv,
model.nv);
405 aba_partial_dq_ref.setZero();
406 MatrixXd aba_partial_dv_ref(
model.nv,
model.nv);
407 aba_partial_dv_ref.setZero();
408 MatrixXd aba_partial_dtau_ref(
model.nv,
model.nv);
409 aba_partial_dtau_ref.setZero();
412 model, data_ref,
q,
v,
tau, aba_partial_dq_ref, aba_partial_dv_ref, aba_partial_dtau_ref);
417 BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
418 BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
419 BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
422 const int num_calls = 20;
424 for (
int it = 0; it < num_calls; ++it)
452 BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
453 BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
454 BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
460 using namespace Eigen;
468 model.lowerPositionLimit.head<3>().
fill(-1.);
469 model.upperPositionLimit.head<3>().
fill(1.);
471 VectorXd
v(VectorXd::Random(
model.nv));
472 VectorXd
tau(VectorXd::Random(
model.nv));
475 ForceVector fext((
size_t)
model.njoints);
476 for (ForceVector::iterator it = fext.begin(); it != fext.end(); ++it)
480 aba_partial_dq.setZero();
482 aba_partial_dv.setZero();
483 MatrixXd aba_partial_dtau(
model.nv,
model.nv);
484 aba_partial_dtau.setZero();
489 MatrixXd aba_partial_dq_ref(
model.nv,
model.nv);
490 aba_partial_dq_ref.setZero();
491 MatrixXd aba_partial_dv_ref(
model.nv,
model.nv);
492 aba_partial_dv_ref.setZero();
493 MatrixXd aba_partial_dtau_ref(
model.nv,
model.nv);
494 aba_partial_dtau_ref.setZero();
497 model, data_ref,
q,
v,
tau, fext, aba_partial_dq_ref, aba_partial_dv_ref, aba_partial_dtau_ref);
502 BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
503 BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
504 BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
507 const int num_calls = 20;
509 for (
int it = 0; it < num_calls; ++it)
537 BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_ref));
538 BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_ref));
539 BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_ref));
543 BOOST_AUTO_TEST_SUITE_END()