| Directory: | ./ |
|---|---|
| File: | unittest/contact-dynamics-derivatives.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 1659 | 1669 | 99.4% |
| Branches: | 3808 | 7498 | 50.8% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2020-2022 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #include <iostream> | ||
| 6 | |||
| 7 | #include "pinocchio/algorithm/jacobian.hpp" | ||
| 8 | #include "pinocchio/algorithm/frames.hpp" | ||
| 9 | #include "pinocchio/algorithm/kinematics.hpp" | ||
| 10 | #include "pinocchio/algorithm/crba.hpp" | ||
| 11 | #include "pinocchio/parsers/srdf.hpp" | ||
| 12 | #include "pinocchio/algorithm/aba-derivatives.hpp" | ||
| 13 | #include "pinocchio/algorithm/kinematics-derivatives.hpp" | ||
| 14 | #include "pinocchio/algorithm/frames-derivatives.hpp" | ||
| 15 | #include "pinocchio/algorithm/constrained-dynamics.hpp" | ||
| 16 | #include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp" | ||
| 17 | #include "pinocchio/algorithm/joint-configuration.hpp" | ||
| 18 | #include "pinocchio/multibody/sample-models.hpp" | ||
| 19 | #include "pinocchio/spatial/classic-acceleration.hpp" | ||
| 20 | |||
| 21 | #include <boost/test/unit_test.hpp> | ||
| 22 | #include <boost/utility/binary.hpp> | ||
| 23 | |||
| 24 | #ifdef PINOCCHIO_WITH_SDFORMAT | ||
| 25 | |||
| 26 | #include "pinocchio/parsers/sdf.hpp" | ||
| 27 | |||
| 28 | #endif // PINOCCHIO_WITH_SDFORMAT | ||
| 29 | |||
| 30 | #define KP 10 | ||
| 31 | #define KD 10 | ||
| 32 | |||
| 33 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) | ||
| 34 | |||
| 35 | using namespace Eigen; | ||
| 36 | using namespace pinocchio; | ||
| 37 | |||
| 38 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_sparse_constraint_dynamics_derivatives_no_contact) |
| 39 | { | ||
| 40 | using namespace Eigen; | ||
| 41 | using namespace pinocchio; | ||
| 42 | |||
| 43 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 44 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 45 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_ref(model); |
| 46 | |||
| 47 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 48 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 49 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 50 | |||
| 51 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 52 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 53 | |||
| 54 | // Contact models and data | ||
| 55 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_constraint_models; |
| 56 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_constraint_data; |
| 57 | |||
| 58 | 2 | const double mu0 = 0.; | |
| 59 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 60 | |||
| 61 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, empty_constraint_models); |
| 62 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 63 | model, data, q, v, tau, empty_constraint_models, empty_constraint_data, prox_settings); | ||
| 64 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 65 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 66 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 67 | model, data, empty_constraint_models, empty_constraint_data, prox_settings); | ||
| 68 | |||
| 69 | // Reference values | ||
| 70 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeABADerivatives(model, data_ref, q, v, tau); |
| 71 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data_ref, q, v, data_ref.ddq); |
| 72 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.ddq.isApprox(data.ddq)); |
| 73 | |||
| 74 |
2/2✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
|
56 | for (size_t k = 1; k < (size_t)model.njoints; ++k) |
| 75 | { | ||
| 76 |
7/14✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
|
54 | BOOST_CHECK(data_ref.oMi[k].isApprox(data.oMi[k])); |
| 77 |
7/14✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
|
54 | BOOST_CHECK(data_ref.ov[k].isApprox(data.ov[k])); |
| 78 |
7/14✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
|
54 | BOOST_CHECK(data_ref.v[k].isApprox(data.v[k])); |
| 79 |
7/14✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
|
54 | BOOST_CHECK(data_ref.a[k].isApprox(data.a[k])); |
| 80 |
7/14✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 27 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 27 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 27 times.
|
54 | BOOST_CHECK(data_ref.oa[k].isApprox(data.oa[k])); |
| 81 | } | ||
| 82 | |||
| 83 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.ddq.isApprox(data.ddq)); |
| 84 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.dVdq.isApprox(data.dVdq)); |
| 85 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.J.isApprox(data.J)); |
| 86 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.dAdq.isApprox(data.dAdq)); |
| 87 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.dAdv.isApprox(data.dAdv)); |
| 88 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.dFdq.isApprox(data.dFdq)); |
| 89 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.dFdv.isApprox(data.dFdv)); |
| 90 | |||
| 91 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.dtau_dq.isApprox(data.dtau_dq)); |
| 92 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.dtau_dv.isApprox(data.dtau_dv)); |
| 93 | |||
| 94 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.ddq_dq.isApprox(data.ddq_dq)); |
| 95 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.ddq_dv.isApprox(data.ddq_dv)); |
| 96 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_ref.Minv.isApprox(data.ddq_dtau)); |
| 97 | 2 | } | |
| 98 | |||
| 99 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_sparse_constraint_dynamics_derivatives) |
| 100 | { | ||
| 101 | using namespace Eigen; | ||
| 102 | using namespace pinocchio; | ||
| 103 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 104 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 105 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_ref(model); |
| 106 | |||
| 107 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 108 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 109 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 110 | |||
| 111 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 112 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 113 | |||
| 114 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 115 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 116 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 117 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 118 | |||
| 119 | // Contact models and data | ||
| 120 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 121 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 122 | |||
| 123 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); |
| 124 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 125 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); |
| 126 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 127 | |||
| 128 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_LF); |
| 129 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_LF)); |
| 130 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 131 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RF)); |
| 132 | |||
| 133 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 134 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 135 | 4 | constraint_dim += constraint_models[k].size(); | |
| 136 | |||
| 137 | 2 | const double mu0 = 0.; | |
| 138 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 139 | |||
| 140 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 141 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 142 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 143 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 144 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 145 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 146 | |||
| 147 | // Reference values | ||
| 148 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | crba(model, data_ref, q, Convention::WORLD); |
| 149 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data_ref.M.triangularView<Eigen::StrictlyLower>() = |
| 150 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 151 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | container::aligned_vector<Force> fext((size_t)model.njoints, Force::Zero()); |
| 152 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 153 | { | ||
| 154 | 4 | const RigidConstraintModel & cmodel = constraint_models[k]; | |
| 155 | 4 | const RigidConstraintData & cdata = constraint_data[k]; | |
| 156 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
4 | fext[cmodel.joint1_id] = cmodel.joint1_placement.act(cdata.contact_force); |
| 157 | |||
| 158 |
8/16✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✗ Branch 30 not taken.
✓ Branch 31 taken 2 times.
|
4 | BOOST_CHECK(cdata.oMc1.isApprox(data_ref.oMi[cmodel.joint1_id] * cmodel.joint1_placement)); |
| 159 | } | ||
| 160 | |||
| 161 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeABADerivatives(model, data_ref, q, v, tau, fext); |
| 162 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data_ref, q, v); |
| 163 | |||
| 164 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
| 165 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); |
| 166 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.J.isApprox(data_ref.J)); |
| 167 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); |
| 168 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); |
| 169 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq)); |
| 170 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.dFdv.isApprox(data_ref.dFdv)); |
| 171 | |||
| 172 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | MatrixXd vLF_partial_dq(MatrixXd::Zero(6, model.nv)), aLF_partial_dq(MatrixXd::Zero(6, model.nv)), |
| 173 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | aLF_partial_dv(MatrixXd::Zero(6, model.nv)), aLF_partial_da(MatrixXd::Zero(6, model.nv)); |
| 174 | |||
| 175 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | MatrixXd vRF_partial_dq(MatrixXd::Zero(6, model.nv)), aRF_partial_dq(MatrixXd::Zero(6, model.nv)), |
| 176 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | aRF_partial_dv(MatrixXd::Zero(6, model.nv)), aRF_partial_da(MatrixXd::Zero(6, model.nv)); |
| 177 | |||
| 178 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameAccelerationDerivatives( |
| 179 | model, data_ref, LF_id, ci_LF.joint1_placement, LOCAL, vLF_partial_dq, aLF_partial_dq, | ||
| 180 | aLF_partial_dv, aLF_partial_da); | ||
| 181 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameAccelerationDerivatives( |
| 182 | model, data_ref, RF_id, ci_RF.joint1_placement, LOCAL, vRF_partial_dq, aRF_partial_dq, | ||
| 183 | aRF_partial_dv, aRF_partial_da); | ||
| 184 | |||
| 185 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd Jc(constraint_dim, model.nv); |
| 186 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Jc << aLF_partial_da, aRF_partial_da.topRows<3>(); |
| 187 | |||
| 188 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd K(model.nv + constraint_dim, model.nv + constraint_dim); |
| 189 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | K << data_ref.M, Jc.transpose(), Jc, MatrixXd::Zero(constraint_dim, constraint_dim); |
| 190 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const MatrixXd Kinv = K.inverse(); |
| 191 | |||
| 192 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | MatrixXd osim((Jc * data_ref.M.inverse() * Jc.transpose()).inverse()); |
| 193 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data.osim.isApprox(osim)); |
| 194 | |||
| 195 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ac_partial_dq(constraint_dim, model.nv); |
| 196 | |||
| 197 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(data.ov[RF_id].isApprox(data_ref.oMi[RF_id].act(data_ref.v[RF_id]))); |
| 198 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | aRF_partial_dq.topRows<3>() += |
| 199 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
2 | cross(ci_RF.joint1_placement.actInv(data_ref.v[RF_id]).angular(), vRF_partial_dq.topRows<3>()) |
| 200 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
6 | - cross( |
| 201 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
|
6 | ci_RF.joint1_placement.actInv(data_ref.v[RF_id]).linear(), vRF_partial_dq.bottomRows<3>()); |
| 202 | |||
| 203 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(data.ov[LF_id].isApprox(data_ref.oMi[LF_id].act(data_ref.v[LF_id]))); |
| 204 | |||
| 205 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | ac_partial_dq << aLF_partial_dq, aRF_partial_dq.topRows<3>(); |
| 206 | |||
| 207 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dac_dq = ac_partial_dq; // - Jc * data_ref.Minv*data_ref.dtau_dq; |
| 208 | |||
| 209 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data.dac_dq.isApprox(dac_dq, 1e-8)); |
| 210 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK(Kinv.bottomLeftCorner(constraint_dim, model.nv).isApprox(osim * Jc * data_ref.Minv)); |
| 211 | |||
| 212 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | MatrixXd df_dq = Kinv.bottomLeftCorner(constraint_dim, model.nv) * data_ref.dtau_dq |
| 213 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
4 | + Kinv.bottomRightCorner(constraint_dim, constraint_dim) * ac_partial_dq; |
| 214 | |||
| 215 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | MatrixXd ddq_dq = data_ref.Minv * (-data_ref.dtau_dq + Jc.transpose() * df_dq); |
| 216 | |||
| 217 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(df_dq.isApprox(data.dlambda_dq)); |
| 218 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(ddq_dq.isApprox(data.ddq_dq)); |
| 219 | 2 | } | |
| 220 | |||
| 221 | 330 | pinocchio::Motion computeAcceleration( | |
| 222 | const pinocchio::Model & model, | ||
| 223 | const pinocchio::Data & data, | ||
| 224 | const pinocchio::JointIndex & joint_id, | ||
| 225 | const pinocchio::ReferenceFrame reference_frame, | ||
| 226 | const pinocchio::ContactType contact_type, | ||
| 227 | const pinocchio::SE3 & placement = pinocchio::SE3::Identity()) | ||
| 228 | { | ||
| 229 | PINOCCHIO_UNUSED_VARIABLE(model); | ||
| 230 | using namespace pinocchio; | ||
| 231 |
1/2✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
|
330 | Motion res(Motion::Zero()); |
| 232 | |||
| 233 | 330 | const Data::SE3 & oMi = data.oMi[joint_id]; | |
| 234 |
1/2✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
|
330 | const Data::SE3 oMc = oMi * placement; |
| 235 | |||
| 236 | 330 | const Data::SE3 & iMc = placement; | |
| 237 |
1/2✓ Branch 2 taken 330 times.
✗ Branch 3 not taken.
|
330 | const Motion ov = oMi.act(data.v[joint_id]); |
| 238 |
1/2✓ Branch 2 taken 330 times.
✗ Branch 3 not taken.
|
330 | const Motion oa = oMi.act(data.a[joint_id]); |
| 239 | |||
| 240 |
2/4✗ Branch 0 not taken.
✓ Branch 1 taken 132 times.
✓ Branch 2 taken 198 times.
✗ Branch 3 not taken.
|
330 | switch (reference_frame) |
| 241 | { | ||
| 242 | ✗ | case WORLD: | |
| 243 | ✗ | if (contact_type == CONTACT_6D) | |
| 244 | ✗ | return oa; | |
| 245 | ✗ | classicAcceleration(ov, oa, res.linear()); | |
| 246 | ✗ | res.angular() = oa.angular(); | |
| 247 | ✗ | break; | |
| 248 | 132 | case LOCAL_WORLD_ALIGNED: | |
| 249 |
2/2✓ Branch 0 taken 66 times.
✓ Branch 1 taken 66 times.
|
132 | if (contact_type == CONTACT_6D) |
| 250 | { | ||
| 251 |
6/12✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 66 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 66 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 66 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 66 times.
✗ Branch 18 not taken.
|
66 | res.linear() = oMc.rotation() * iMc.actInv(data.a[joint_id]).linear(); |
| 252 |
5/10✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 66 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 66 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 66 times.
✗ Branch 15 not taken.
|
66 | res.angular() = oMi.rotation() * data.a[joint_id].angular(); |
| 253 | } | ||
| 254 | else | ||
| 255 | { | ||
| 256 |
5/10✓ Branch 3 taken 66 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 66 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 66 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 66 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 66 times.
✗ Branch 16 not taken.
|
66 | res.linear() = oMc.rotation() * classicAcceleration(data.v[joint_id], data.a[joint_id], iMc); |
| 257 |
5/10✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 66 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 66 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 66 times.
✗ Branch 15 not taken.
|
66 | res.angular() = oMi.rotation() * data.a[joint_id].angular(); |
| 258 | } | ||
| 259 | 132 | break; | |
| 260 | 198 | case LOCAL: | |
| 261 |
2/2✓ Branch 0 taken 132 times.
✓ Branch 1 taken 66 times.
|
198 | if (contact_type == CONTACT_6D) |
| 262 |
1/2✓ Branch 1 taken 132 times.
✗ Branch 2 not taken.
|
132 | return oMc.actInv(oa); |
| 263 |
2/4✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 66 times.
✗ Branch 7 not taken.
|
66 | classicAcceleration(data.v[joint_id], data.a[joint_id], iMc, res.linear()); |
| 264 |
6/12✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 66 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 66 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 66 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 66 times.
✗ Branch 18 not taken.
|
66 | res.angular() = iMc.rotation().transpose() * data.a[joint_id].angular(); |
| 265 | 66 | break; | |
| 266 | ✗ | default: | |
| 267 | ✗ | break; | |
| 268 | } | ||
| 269 | |||
| 270 |
1/2✓ Branch 1 taken 198 times.
✗ Branch 2 not taken.
|
198 | return res; |
| 271 | } | ||
| 272 | |||
| 273 | 165 | pinocchio::Motion getContactAcceleration( | |
| 274 | const Model & model, | ||
| 275 | const Data & data, | ||
| 276 | const RigidConstraintModel & cmodel, | ||
| 277 | const pinocchio::SE3 & c1Mc2 = SE3::Identity()) | ||
| 278 | { | ||
| 279 | const Motion v1 = getFrameVelocity( | ||
| 280 |
1/2✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
|
165 | model, data, cmodel.joint1_id, cmodel.joint1_placement, cmodel.reference_frame); |
| 281 | const Motion v2 = getFrameVelocity( | ||
| 282 |
1/2✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
|
165 | model, data, cmodel.joint2_id, cmodel.joint2_placement, cmodel.reference_frame); |
| 283 |
2/4✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
|
165 | const Motion v = v1 - c1Mc2.act(v2); |
| 284 | const Motion a1 = computeAcceleration( | ||
| 285 |
1/2✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
|
165 | model, data, cmodel.joint1_id, cmodel.reference_frame, cmodel.type, cmodel.joint1_placement); |
| 286 | const Motion a2 = computeAcceleration( | ||
| 287 |
1/2✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
|
165 | model, data, cmodel.joint2_id, cmodel.reference_frame, cmodel.type, cmodel.joint2_placement); |
| 288 |
5/10✓ Branch 1 taken 165 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 165 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 165 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 165 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 165 times.
✗ Branch 14 not taken.
|
330 | return a1 - c1Mc2.act(a2) + v.cross(c1Mc2.act(v2)); |
| 289 | } | ||
| 290 | |||
| 291 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_fd) |
| 292 | { | ||
| 293 | using namespace Eigen; | ||
| 294 | using namespace pinocchio; | ||
| 295 | |||
| 296 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 297 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 298 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 299 | |||
| 300 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 301 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 302 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 303 | |||
| 304 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 305 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 306 | |||
| 307 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 308 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 309 | |||
| 310 | // Contact models and data | ||
| 311 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 312 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 313 | |||
| 314 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); |
| 315 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 316 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kp.array() = KP; |
| 317 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kd.array() = KD; |
| 318 | |||
| 319 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_LF); |
| 320 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_LF)); |
| 321 | |||
| 322 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 323 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 324 | 2 | constraint_dim += constraint_models[k].size(); | |
| 325 | |||
| 326 | 2 | const double mu0 = 0.; | |
| 327 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 328 | |||
| 329 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 330 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 331 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 332 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 333 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 334 | |||
| 335 | // Data_fd | ||
| 336 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 337 | |||
| 338 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 339 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 340 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 341 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 342 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 343 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 344 | |||
| 345 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 346 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 347 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 348 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 349 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 350 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 351 | |||
| 352 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 353 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 354 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 355 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 356 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 357 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 358 | |||
| 359 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 360 | |||
| 361 | 2 | const double alpha = 1e-8; | |
| 362 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 363 | |||
| 364 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 365 | { | ||
| 366 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 367 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 368 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 369 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 370 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 371 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 372 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 373 | } | ||
| 374 | |||
| 375 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 376 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 377 | |||
| 378 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 379 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 380 | { | ||
| 381 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 382 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 383 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 384 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 385 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 386 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 387 | } | ||
| 388 | |||
| 389 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 390 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 391 | |||
| 392 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 393 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 394 | { | ||
| 395 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 396 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 397 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 398 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 399 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 400 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 401 | } | ||
| 402 | |||
| 403 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 404 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 405 | 2 | } | |
| 406 | |||
| 407 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) | ||
| 408 | 2 | createData( | |
| 409 | const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) | ||
| 410 | & constraint_models) | ||
| 411 | { | ||
| 412 | 2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintData) constraint_datas; | |
| 413 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 2 times.
|
6 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 414 |
2/4✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
|
4 | constraint_datas.push_back(pinocchio::RigidConstraintData(constraint_models[k])); |
| 415 | |||
| 416 | 2 | return constraint_datas; | |
| 417 | } | ||
| 418 | |||
| 419 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_correction_6D) |
| 420 | { | ||
| 421 | using namespace Eigen; | ||
| 422 | using namespace pinocchio; | ||
| 423 | |||
| 424 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 425 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 426 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 427 | |||
| 428 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 429 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 430 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 431 | |||
| 432 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 433 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 434 | 2 | const double mu = 0.; | |
| 435 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu, 1); |
| 436 | |||
| 437 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 438 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 439 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 440 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 441 | |||
| 442 | // Contact models and data | ||
| 443 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 444 | |||
| 445 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL); |
| 446 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 447 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint2_placement.setRandom(); |
| 448 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kp.array() = KP; |
| 449 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kd.array() = KD; |
| 450 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 451 | |||
| 452 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_3D, model, LF_id, LOCAL); |
| 453 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 454 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint2_placement.setRandom(); |
| 455 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kp.array() = KP; |
| 456 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kd.array() = KD; |
| 457 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_LF); |
| 458 | |||
| 459 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 460 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 461 | 4 | constraint_dim += constraint_models[k].size(); | |
| 462 | |||
| 463 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 464 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_datas = createData(constraint_models); |
| 465 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 466 | const Eigen::VectorXd ddq0 = | ||
| 467 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_datas, prox_settings); |
| 468 | |||
| 469 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::MatrixXd ddq_dq(model.nv, model.nv), ddq_dv(model.nv, model.nv), |
| 470 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_dtau(model.nv, model.nv); |
| 471 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::MatrixXd dlambda_dq(constraint_dim, model.nv), dlambda_dv(constraint_dim, model.nv), |
| 472 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dlambda_dtau(constraint_dim, model.nv); |
| 473 | |||
| 474 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 475 | model, data, constraint_models, constraint_datas, prox_settings, ddq_dq, ddq_dv, ddq_dtau, | ||
| 476 | dlambda_dq, dlambda_dv, dlambda_dtau); | ||
| 477 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeForwardKinematicsDerivatives(model, data, q, v, 0 * v); |
| 478 | |||
| 479 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x dv_RF_dq_L(Data::Matrix6x::Zero(6, model.nv)); |
| 480 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x dv_RF_dv_L(Data::Matrix6x::Zero(6, model.nv)); |
| 481 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameVelocityDerivatives( |
| 482 | model, data, ci_RF.joint1_id, ci_RF.joint1_placement, ci_RF.reference_frame, dv_RF_dq_L, | ||
| 483 | dv_RF_dv_L); | ||
| 484 | |||
| 485 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x dv_LF_dq_L(Data::Matrix6x::Zero(6, model.nv)); |
| 486 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data::Matrix6x dv_LF_dv_L(Data::Matrix6x::Zero(6, model.nv)); |
| 487 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getFrameVelocityDerivatives( |
| 488 | model, data, ci_LF.joint1_id, ci_LF.joint1_placement, ci_LF.reference_frame, dv_LF_dq_L, | ||
| 489 | dv_LF_dv_L); | ||
| 490 | |||
| 491 | 2 | const double eps = 1e-8; | |
| 492 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x dacc_corrector_RF_dq(6, model.nv); |
| 493 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dacc_corrector_RF_dq.setZero(); |
| 494 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x dacc_corrector_RF_dv(6, model.nv); |
| 495 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dacc_corrector_RF_dv.setZero(); |
| 496 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix3x dacc_corrector_LF_dq(3, model.nv); |
| 497 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dacc_corrector_LF_dq.setZero(); |
| 498 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix3x dacc_corrector_LF_dv(3, model.nv); |
| 499 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dacc_corrector_LF_dv.setZero(); |
| 500 | |||
| 501 | { | ||
| 502 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | const SE3::Matrix6 Jlog = Jlog6(constraint_datas[0].c1Mc2.inverse()); |
| 503 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | dacc_corrector_RF_dq = -(ci_RF.corrector.Kp.asDiagonal() * Jlog * dv_RF_dv_L); |
| 504 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | dacc_corrector_RF_dq -= ci_RF.corrector.Kd.asDiagonal() * dv_RF_dq_L; |
| 505 | |||
| 506 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | dacc_corrector_RF_dv = -(ci_RF.corrector.Kd.asDiagonal() * dv_RF_dv_L); |
| 507 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 1 times.
|
2 | BOOST_CHECK(dv_RF_dv_L.isApprox(data.contact_chol.matrix().topRightCorner(6, model.nv))); |
| 508 | } | ||
| 509 | |||
| 510 | { | ||
| 511 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | dacc_corrector_LF_dq = -(ci_LF.corrector.Kp.asDiagonal() * dv_LF_dv_L.topRows<3>()); |
| 512 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | dacc_corrector_LF_dq -= ci_LF.corrector.Kd.asDiagonal() * dv_LF_dq_L.topRows<3>(); |
| 513 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 514 | { | ||
| 515 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dacc_corrector_LF_dq.col(k) += |
| 516 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ci_LF.corrector.Kp.asDiagonal() |
| 517 |
6/12✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 32 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 32 times.
✗ Branch 18 not taken.
|
192 | * dv_LF_dv_L.col(k).tail<3>().cross(constraint_datas[1].contact_placement_error.linear()); |
| 518 | } | ||
| 519 | |||
| 520 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | dacc_corrector_LF_dv = -(ci_LF.corrector.Kd.asDiagonal() * dv_LF_dv_L.topRows<3>()); |
| 521 |
11/22✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✗ Branch 39 not taken.
✓ Branch 40 taken 1 times.
|
2 | BOOST_CHECK(dv_LF_dv_L.topRows<3>().isApprox( |
| 522 | data.contact_chol.matrix().topRightCorner(9, model.nv).bottomRows<3>())); | ||
| 523 | } | ||
| 524 | |||
| 525 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 526 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_datas_fd = createData(constraint_models); |
| 527 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 528 | |||
| 529 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x dacc_corrector_RF_dq_fd(6, model.nv); |
| 530 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix3x dacc_corrector_LF_dq_fd(3, model.nv); |
| 531 | |||
| 532 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::MatrixXd ddq_dq_fd(model.nv, model.nv), ddq_dv_fd(model.nv, model.nv), |
| 533 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_dtau_fd(model.nv, model.nv); |
| 534 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Eigen::MatrixXd dlambda_dq_fd(constraint_dim, model.nv), dlambda_dv_fd(constraint_dim, model.nv), |
| 535 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dlambda_dtau_fd(constraint_dim, model.nv); |
| 536 | |||
| 537 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 538 | { | ||
| 539 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
64 | Eigen::VectorXd v_eps = Eigen::VectorXd::Zero(model.nv); |
| 540 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = eps; |
| 541 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | const Eigen::VectorXd q_plus = integrate(model, q, v_eps); |
| 542 | |||
| 543 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | Eigen::VectorXd ddq_plus = constraintDynamics( |
| 544 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_datas_fd, prox_settings); |
| 545 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dacc_corrector_RF_dq_fd.col(k) = (constraint_datas_fd[0].contact_acceleration_error |
| 546 |
1/2✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
|
64 | - constraint_datas[0].contact_acceleration_error) |
| 547 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | .toVector() |
| 548 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
128 | / eps; |
| 549 |
2/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
|
128 | dacc_corrector_LF_dq_fd.col(k) = (constraint_datas_fd[1].contact_acceleration_error.linear() |
| 550 |
2/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
|
128 | - constraint_datas[1].contact_acceleration_error.linear()) |
| 551 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
128 | / eps; |
| 552 | |||
| 553 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_dq_fd.col(k) = (ddq_plus - ddq0) / eps; |
| 554 | 64 | } | |
| 555 | |||
| 556 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_dq_fd.isApprox(ddq_dq, sqrt(eps))); |
| 557 | // std::cout << "ddq_dq_fd:\n" << ddq_dq_fd - ddq_dq << std::endl; | ||
| 558 | // std::cout << "ddq_dq:\n" << ddq_dq << std::endl; | ||
| 559 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dacc_corrector_RF_dq.isApprox(dacc_corrector_RF_dq_fd, sqrt(eps))); |
| 560 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dacc_corrector_LF_dq.isApprox(dacc_corrector_LF_dq_fd, sqrt(eps))); |
| 561 | // std::cout << "dacc_corrector_RF_dq:\n" << dacc_corrector_RF_dq << std::endl; | ||
| 562 | // std::cout << "dacc_corrector_RF_dq_fd:\n" << dacc_corrector_RF_dq_fd << std::endl; | ||
| 563 | |||
| 564 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x dacc_corrector_RF_dv_fd(6, model.nv); |
| 565 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix3x dacc_corrector_LF_dv_fd(3, model.nv); |
| 566 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 567 | { | ||
| 568 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | Eigen::VectorXd v_plus(v); |
| 569 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += eps; |
| 570 | |||
| 571 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | Eigen::VectorXd ddq_plus = constraintDynamics( |
| 572 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_datas_fd, prox_settings); |
| 573 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dacc_corrector_RF_dv_fd.col(k) = (constraint_datas_fd[0].contact_acceleration_error |
| 574 |
1/2✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
|
64 | - constraint_datas[0].contact_acceleration_error) |
| 575 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | .toVector() |
| 576 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
128 | / eps; |
| 577 |
2/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
|
128 | dacc_corrector_LF_dv_fd.col(k) = (constraint_datas_fd[1].contact_acceleration_error.linear() |
| 578 |
2/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
|
128 | - constraint_datas[1].contact_acceleration_error.linear()) |
| 579 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
128 | / eps; |
| 580 | |||
| 581 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_dv_fd.col(k) = (ddq_plus - ddq0) / eps; |
| 582 | 64 | } | |
| 583 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_dv_fd.isApprox(ddq_dv, sqrt(eps))); |
| 584 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dacc_corrector_RF_dv.isApprox(dacc_corrector_RF_dv_fd, sqrt(eps))); |
| 585 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dacc_corrector_LF_dv.isApprox(dacc_corrector_LF_dv_fd, sqrt(eps))); |
| 586 | |||
| 587 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix6x dacc_corrector_RF_dtau_fd(6, model.nv); |
| 588 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data::Matrix3x dacc_corrector_LF_dtau_fd(3, model.nv); |
| 589 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (Eigen::DenseIndex k = 0; k < model.nv; ++k) |
| 590 | { | ||
| 591 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | Eigen::VectorXd tau_plus(tau); |
| 592 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += eps; |
| 593 | |||
| 594 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | Eigen::VectorXd ddq_plus = constraintDynamics( |
| 595 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_datas_fd, prox_settings); |
| 596 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dacc_corrector_RF_dtau_fd.col(k) = (constraint_datas_fd[0].contact_acceleration_error |
| 597 |
1/2✓ Branch 3 taken 32 times.
✗ Branch 4 not taken.
|
64 | - constraint_datas[0].contact_acceleration_error) |
| 598 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | .toVector() |
| 599 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
128 | / eps; |
| 600 |
2/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
|
128 | dacc_corrector_LF_dtau_fd.col(k) = (constraint_datas_fd[1].contact_acceleration_error.linear() |
| 601 |
2/4✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
|
128 | - constraint_datas[1].contact_acceleration_error.linear()) |
| 602 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
128 | / eps; |
| 603 | |||
| 604 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_dtau_fd.col(k) = (ddq_plus - ddq0) / eps; |
| 605 | 64 | } | |
| 606 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_dtau_fd.isApprox(ddq_dtau, sqrt(eps))); |
| 607 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dacc_corrector_RF_dtau_fd.isZero(sqrt(eps))); |
| 608 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dacc_corrector_LF_dtau_fd.isZero(sqrt(eps))); |
| 609 | 2 | } | |
| 610 | |||
| 611 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_fd) |
| 612 | { | ||
| 613 | using namespace Eigen; | ||
| 614 | using namespace pinocchio; | ||
| 615 | |||
| 616 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 617 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 618 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 619 | |||
| 620 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 621 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 622 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 623 | |||
| 624 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 625 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 626 | |||
| 627 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 628 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 629 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 630 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 631 | |||
| 632 | // Contact models and data | ||
| 633 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 634 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 635 | |||
| 636 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); |
| 637 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 638 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kp.array() = KP; |
| 639 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kd.array() = KD; |
| 640 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 641 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RF)); |
| 642 | |||
| 643 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 644 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 645 | 2 | constraint_dim += constraint_models[k].size(); | |
| 646 | |||
| 647 | 2 | const double mu0 = 0.; | |
| 648 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 649 | |||
| 650 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 651 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 652 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 653 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 654 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 655 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 656 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 657 | |||
| 658 | // Data_fd | ||
| 659 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 660 | |||
| 661 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 662 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 663 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 664 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 665 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 666 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 667 | |||
| 668 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 669 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 670 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 671 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 672 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 673 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 674 | |||
| 675 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 676 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 677 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 678 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 679 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 680 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 681 | |||
| 682 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 683 | |||
| 684 | 2 | const double alpha = 1e-8; | |
| 685 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 686 | |||
| 687 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 688 | { | ||
| 689 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 690 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 691 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 692 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 693 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 694 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 695 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 696 | } | ||
| 697 | |||
| 698 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 699 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 700 | |||
| 701 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 702 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 703 | { | ||
| 704 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 705 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 706 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 707 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 708 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 709 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 710 | } | ||
| 711 | |||
| 712 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 713 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 714 | |||
| 715 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 716 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 717 | { | ||
| 718 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 719 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 720 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 721 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 722 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 723 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 724 | } | ||
| 725 | |||
| 726 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 727 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 728 | 2 | } | |
| 729 | |||
| 730 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_fd_prox) |
| 731 | { | ||
| 732 | using namespace Eigen; | ||
| 733 | using namespace pinocchio; | ||
| 734 | |||
| 735 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 736 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 737 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 738 | |||
| 739 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 740 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 741 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 742 | |||
| 743 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 744 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 745 | |||
| 746 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 747 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 748 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 749 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 750 | |||
| 751 | // Contact models and data | ||
| 752 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 753 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 754 | |||
| 755 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); |
| 756 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 757 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kp.array() = KP; |
| 758 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kd.array() = KD; |
| 759 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 760 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RF)); |
| 761 | |||
| 762 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 763 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 764 | 2 | constraint_dim += constraint_models[k].size(); | |
| 765 | |||
| 766 | 2 | const double mu0 = 1e-4; | |
| 767 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 20); |
| 768 | |||
| 769 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 770 | VectorXd a_res = | ||
| 771 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 772 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(prox_settings.iter > 1 && prox_settings.iter <= prox_settings.max_iter); |
| 773 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 774 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 775 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 776 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 777 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 778 | |||
| 779 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings_fd(1e-12, mu0, 20); |
| 780 | // Data_fd | ||
| 781 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 782 | |||
| 783 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 784 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 785 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 786 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 787 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 788 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 789 | |||
| 790 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 791 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 792 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 793 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 794 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 795 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 796 | |||
| 797 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 798 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings_fd); |
| 799 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(a_res.isApprox(ddq0)); |
| 800 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 801 | |||
| 802 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 803 | model, data_fd, constraint_models, constraint_data, prox_settings_fd); | ||
| 804 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_fd.dlambda_dtau.isApprox(data.dlambda_dtau)); |
| 805 | |||
| 806 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 807 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 808 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 809 | |||
| 810 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 811 | |||
| 812 | 2 | const double alpha = 1e-8; | |
| 813 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 814 | |||
| 815 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 816 | { | ||
| 817 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 818 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 819 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 820 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings_fd); |
| 821 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 822 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 823 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 824 | } | ||
| 825 | |||
| 826 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 827 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 828 | |||
| 829 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 830 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 831 | { | ||
| 832 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 833 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 834 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings_fd); |
| 835 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 836 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 837 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 838 | } | ||
| 839 | |||
| 840 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 841 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 842 | |||
| 843 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 844 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 845 | { | ||
| 846 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 847 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 848 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings_fd); |
| 849 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 850 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 851 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 852 | } | ||
| 853 | |||
| 854 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 855 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 856 | 2 | } | |
| 857 | |||
| 858 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_loop_closure_3D_fd_prox) |
| 859 | { | ||
| 860 | using namespace Eigen; | ||
| 861 | using namespace pinocchio; | ||
| 862 | |||
| 863 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 864 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 865 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 866 | |||
| 867 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 868 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 869 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 870 | |||
| 871 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 872 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 873 | |||
| 874 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 875 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 876 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 877 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 878 | |||
| 879 | // Contact models and data | ||
| 880 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 881 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 882 | |||
| 883 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LF_id, LOCAL); |
| 884 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kp.array() = KP; |
| 885 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kd.array() = KD; |
| 886 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 887 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q); |
| 888 | // data.oMi[LF_id] * ci_RF.joint2_placement = data.oMi[RF_id] * ci_RF.joint1_placement; | ||
| 889 |
4/8✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
|
2 | ci_RF.joint2_placement = data.oMi[LF_id].inverse() * data.oMi[RF_id] * ci_RF.joint1_placement; |
| 890 | |||
| 891 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 892 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RF)); |
| 893 | |||
| 894 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 895 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 896 | 2 | constraint_dim += constraint_models[k].size(); | |
| 897 | |||
| 898 | 2 | const double mu0 = 1e-4; | |
| 899 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 20); |
| 900 | |||
| 901 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 902 | VectorXd a_res = | ||
| 903 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 904 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(prox_settings.iter > 1 && prox_settings.iter <= prox_settings.max_iter); |
| 905 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 906 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 907 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 908 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 909 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 910 | |||
| 911 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings_fd(1e-12, mu0, 20); |
| 912 | // Data_fd | ||
| 913 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 914 | |||
| 915 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 916 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 917 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 918 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 919 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 920 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 921 | |||
| 922 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 923 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 924 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 925 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 926 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 927 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 928 | |||
| 929 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 930 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings_fd); |
| 931 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(a_res.isApprox(ddq0)); |
| 932 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 933 | |||
| 934 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 935 | model, data_fd, constraint_models, constraint_data, prox_settings_fd); | ||
| 936 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(data_fd.dlambda_dtau.isApprox(data.dlambda_dtau)); |
| 937 | |||
| 938 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 939 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 940 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 941 | |||
| 942 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 943 | |||
| 944 | 2 | const double alpha = 1e-8; | |
| 945 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 946 | |||
| 947 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 948 | { | ||
| 949 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 950 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 951 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 952 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings_fd); |
| 953 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 954 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 955 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 956 | } | ||
| 957 | |||
| 958 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 959 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 960 | |||
| 961 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 962 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 963 | { | ||
| 964 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 965 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 966 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings_fd); |
| 967 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 968 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 969 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 970 | } | ||
| 971 | |||
| 972 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 973 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 974 | |||
| 975 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 976 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 977 | { | ||
| 978 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 979 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 980 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings_fd); |
| 981 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 982 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 983 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 984 | } | ||
| 985 | |||
| 986 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 987 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 988 | 2 | } | |
| 989 | |||
| 990 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_loop_closure_j2_fd) |
| 991 | { | ||
| 992 | using namespace Eigen; | ||
| 993 | using namespace pinocchio; | ||
| 994 | |||
| 995 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 996 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 997 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 998 | |||
| 999 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 1000 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 1001 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1002 | |||
| 1003 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 1004 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1005 | |||
| 1006 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1007 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 1008 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1009 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 1010 | |||
| 1011 | // Contact models and data | ||
| 1012 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 1013 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 1014 | |||
| 1015 | // Add Loop Closure Constraint | ||
| 1016 | |||
| 1017 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm5_joint"; |
| 1018 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RA_id = model.getJointId(RA); |
| 1019 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LA = "larm5_joint"; |
| 1020 | // const Model::JointIndex LA_id = model.getJointId(LA); | ||
| 1021 | |||
| 1022 | RigidConstraintModel ci_closure( | ||
| 1023 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_3D, model, 0, SE3::Identity(), RA_id, SE3::Random(), LOCAL); |
| 1024 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kp.array() = KP; |
| 1025 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kd.array() = KD; |
| 1026 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_closure); |
| 1027 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_closure)); |
| 1028 | // End of Loopo Closure Constraint | ||
| 1029 | |||
| 1030 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1031 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 1032 | 2 | constraint_dim += constraint_models[k].size(); | |
| 1033 | |||
| 1034 | 2 | const double mu0 = 0.; | |
| 1035 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 1036 | |||
| 1037 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 1038 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1039 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 1040 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 1041 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 1042 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 1043 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 1044 | |||
| 1045 | // Data_fd | ||
| 1046 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 1047 | |||
| 1048 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 1049 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 1050 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 1051 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 1052 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 1053 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 1054 | |||
| 1055 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 1056 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 1057 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 1058 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 1059 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 1060 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 1061 | |||
| 1062 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 1063 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1064 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 1065 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 1066 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 1067 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 1068 | |||
| 1069 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 1070 | |||
| 1071 | 2 | const double alpha = 1e-8; | |
| 1072 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 1073 | |||
| 1074 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1075 | { | ||
| 1076 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 1077 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 1078 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1079 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 1080 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1081 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1082 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 1083 | } | ||
| 1084 | |||
| 1085 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 1086 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 1087 | |||
| 1088 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 1089 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1090 | { | ||
| 1091 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 1092 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1093 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 1094 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1095 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1096 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 1097 | } | ||
| 1098 | |||
| 1099 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 1100 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 1101 | |||
| 1102 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 1103 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1104 | { | ||
| 1105 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 1106 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1107 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 1108 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1109 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1110 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 1111 | } | ||
| 1112 | |||
| 1113 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 1114 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 1115 | 2 | } | |
| 1116 | |||
| 1117 | 130 | void computeVelocityAndAccelerationErrors( | |
| 1118 | const Model & model, | ||
| 1119 | const RigidConstraintModel & cmodel, | ||
| 1120 | const VectorXd & q, | ||
| 1121 | const VectorXd & v, | ||
| 1122 | const VectorXd & a, | ||
| 1123 | Motion & v_error, | ||
| 1124 | Motion & a_error, | ||
| 1125 | const VectorXd & Kp, | ||
| 1126 | const VectorXd & Kd) | ||
| 1127 | { | ||
| 1128 |
1/2✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
|
130 | Data data(model); |
| 1129 |
1/2✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
|
130 | forwardKinematics(model, data, q, v, a); |
| 1130 | |||
| 1131 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
130 | const SE3 oMc1 = data.oMi[cmodel.joint1_id] * cmodel.joint1_placement; |
| 1132 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
130 | const SE3 oMc2 = data.oMi[cmodel.joint2_id] * cmodel.joint2_placement; |
| 1133 | |||
| 1134 |
1/2✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
|
130 | const SE3 c1Mc2 = oMc1.actInv(oMc2); |
| 1135 | |||
| 1136 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
130 | const Motion v1 = cmodel.joint1_placement.actInv(data.v[cmodel.joint1_id]); |
| 1137 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
130 | const Motion v2 = cmodel.joint2_placement.actInv(data.v[cmodel.joint2_id]); |
| 1138 | |||
| 1139 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
130 | const Motion a1 = cmodel.joint1_placement.actInv(data.a[cmodel.joint1_id]); |
| 1140 |
1/2✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
|
130 | const Motion a2 = cmodel.joint2_placement.actInv(data.a[cmodel.joint2_id]); |
| 1141 | |||
| 1142 |
3/6✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
|
130 | v_error = v1 - c1Mc2.act(v2); |
| 1143 |
6/12✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 130 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 130 times.
✗ Branch 17 not taken.
|
130 | a_error = a1 - c1Mc2.act(a2) + v_error.cross(c1Mc2.act(v2)); |
| 1144 |
1/2✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
|
130 | a_error.toVector() += |
| 1145 |
10/20✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 130 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 130 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 130 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 130 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 130 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 130 times.
✗ Branch 29 not taken.
|
260 | Kd.asDiagonal() * v_error.toVector() + Kp.asDiagonal() * (-log6(c1Mc2).toVector()); |
| 1146 | 130 | } | |
| 1147 | |||
| 1148 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_loop_closure_j2_fd) |
| 1149 | { | ||
| 1150 | using namespace Eigen; | ||
| 1151 | using namespace pinocchio; | ||
| 1152 | |||
| 1153 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 1154 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 1155 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 1156 | |||
| 1157 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 1158 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 1159 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1160 | |||
| 1161 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 1162 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1163 | |||
| 1164 | // Contact models and data | ||
| 1165 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 1166 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1167 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data, constraint_data_fd; |
| 1168 | |||
| 1169 | // Add loop closure constraint | ||
| 1170 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm5_joint"; |
| 1171 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RA_id = model.getJointId(RA); |
| 1172 | |||
| 1173 | RigidConstraintModel ci_closure( | ||
| 1174 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_6D, model, 0, SE3::Identity(), RA_id, SE3::Identity(), LOCAL); |
| 1175 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kp.array() = KP; |
| 1176 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kd.array() = KD; |
| 1177 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_closure); |
| 1178 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_closure)); |
| 1179 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data_fd.push_back(RigidConstraintData(ci_closure)); |
| 1180 | |||
| 1181 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1182 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 1183 | 2 | constraint_dim += constraint_models[k].size(); | |
| 1184 | |||
| 1185 | 2 | const double mu0 = 0.; | |
| 1186 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 100); |
| 1187 | |||
| 1188 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 1189 | const VectorXd ddq0 = | ||
| 1190 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1191 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK( |
| 1192 | prox_settings.absolute_residual <= prox_settings.absolute_accuracy | ||
| 1193 | || prox_settings.relative_residual <= prox_settings.relative_accuracy); | ||
| 1194 | // BOOST_CHECK(prox_settings.iter == 1); | ||
| 1195 | |||
| 1196 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd a = data.ddq; |
| 1197 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 1198 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 1199 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 1200 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 1201 | |||
| 1202 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Motion v_error, a_error; |
| 1203 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | computeVelocityAndAccelerationErrors( |
| 1204 | model, ci_closure, q, v, ddq0, v_error, a_error, ci_closure.corrector.Kp, | ||
| 1205 | ci_closure.corrector.Kd); | ||
| 1206 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(a_error.isZero()); |
| 1207 | |||
| 1208 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion constraint_velocity_error = constraint_data[0].contact_velocity_error; |
| 1209 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const VectorXd constraint_acceleration_error = -data.primal_rhs_contact.head(constraint_dim); |
| 1210 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(constraint_velocity_error.isApprox(v_error)); |
| 1211 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK(constraint_acceleration_error.isApprox(a_error.toVector() - data.dac_da * ddq0)); |
| 1212 | |||
| 1213 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data.lambda_c; |
| 1214 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 1215 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 1216 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 1217 | |||
| 1218 | 2 | const double alpha = 1e-8; | |
| 1219 | |||
| 1220 | // d./dq | ||
| 1221 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 1222 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 1223 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 1224 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 1225 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_velocity_error_dq_fd(6, model.nv); |
| 1226 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_velocity_error_dq_fd.setZero(); |
| 1227 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_velocity_error_dq2_fd(6, model.nv); |
| 1228 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_velocity_error_dq2_fd.setZero(); |
| 1229 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_acceleration_error_dq_fd(6, model.nv); |
| 1230 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_acceleration_error_dq_fd.setZero(); |
| 1231 | |||
| 1232 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 1233 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1234 | { | ||
| 1235 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 1236 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 1237 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1238 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data_fd, prox_settings); |
| 1239 | |||
| 1240 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1241 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1242 | |||
| 1243 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
64 | Motion v_error_plus, a_error_plus; |
| 1244 |
3/6✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
|
64 | computeVelocityAndAccelerationErrors( |
| 1245 | model, ci_closure, q_plus, v, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp, | ||
| 1246 | ci_closure.corrector.Kd); | ||
| 1247 | |||
| 1248 | 64 | const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error; | |
| 1249 | const VectorXd constraint_acceleration_error_plus = | ||
| 1250 |
3/6✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
|
64 | -data_fd.primal_rhs_contact.head(constraint_dim); |
| 1251 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dconstraint_velocity_error_dq_fd.col(k) = |
| 1252 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
128 | (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha; |
| 1253 |
5/10✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
|
64 | dconstraint_velocity_error_dq2_fd.col(k) = (v_error_plus - v_error).toVector() / alpha; |
| 1254 |
5/10✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
|
64 | dconstraint_acceleration_error_dq_fd.col(k) = (a_error_plus - a_error).toVector() / alpha; |
| 1255 | |||
| 1256 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 1257 | 64 | } | |
| 1258 | |||
| 1259 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_velocity_error_dq_fd.isApprox(data.dvc_dq, sqrt(alpha))); |
| 1260 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_acceleration_error_dq_fd.isApprox(data.dac_dq, sqrt(alpha))); |
| 1261 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 1262 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 1263 | |||
| 1264 | // d./dv | ||
| 1265 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 1266 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 1267 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 1268 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 1269 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_velocity_error_dv_fd(6, model.nv); |
| 1270 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_velocity_error_dv_fd.setZero(); |
| 1271 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_acceleration_error_dv_fd(6, model.nv); |
| 1272 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_acceleration_error_dv_fd.setZero(); |
| 1273 | |||
| 1274 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 1275 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1276 | { | ||
| 1277 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 1278 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1279 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data_fd, prox_settings); |
| 1280 | |||
| 1281 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1282 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1283 | |||
| 1284 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
64 | Motion v_error_plus, a_error_plus; |
| 1285 |
3/6✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
|
64 | computeVelocityAndAccelerationErrors( |
| 1286 | model, ci_closure, q, v_plus, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp, | ||
| 1287 | ci_closure.corrector.Kd); | ||
| 1288 | |||
| 1289 | 64 | const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error; | |
| 1290 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dconstraint_velocity_error_dv_fd.col(k) = |
| 1291 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
128 | (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha; |
| 1292 |
5/10✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
|
64 | dconstraint_acceleration_error_dv_fd.col(k) = (a_error_plus - a_error).toVector() / alpha; |
| 1293 | |||
| 1294 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 1295 | } | ||
| 1296 | |||
| 1297 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_velocity_error_dv_fd.isApprox(data.dac_da, sqrt(alpha))); |
| 1298 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_acceleration_error_dv_fd.isApprox(data.dac_dv, sqrt(alpha))); |
| 1299 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 1300 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 1301 | |||
| 1302 | // d./dtau | ||
| 1303 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 1304 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 1305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 1306 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 1307 | |||
| 1308 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 1309 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1310 | { | ||
| 1311 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 1312 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1313 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data_fd, prox_settings); |
| 1314 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1315 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1316 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 1317 | } | ||
| 1318 | |||
| 1319 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 1320 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 1321 | 2 | } | |
| 1322 | |||
| 1323 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_6D_loop_closure_j1j2_fd) |
| 1324 | { | ||
| 1325 | using namespace Eigen; | ||
| 1326 | using namespace pinocchio; | ||
| 1327 | |||
| 1328 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 1329 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 1330 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 1331 | |||
| 1332 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 1333 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 1334 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1335 | |||
| 1336 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 1337 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1338 | |||
| 1339 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1340 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1341 | |||
| 1342 | // Contact models and data | ||
| 1343 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 1344 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) | ||
| 1345 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data, constraint_data_fd; |
| 1346 | |||
| 1347 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm5_joint"; |
| 1348 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RA_id = model.getJointId(RA); |
| 1349 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LA = "larm5_joint"; |
| 1350 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LA_id = model.getJointId(LA); |
| 1351 | |||
| 1352 | // Add loop closure constraint | ||
| 1353 | RigidConstraintModel ci_closure( | ||
| 1354 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_6D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL); |
| 1355 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kp.array() = KP; |
| 1356 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kd.array() = KD; |
| 1357 | |||
| 1358 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_closure); |
| 1359 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_closure)); |
| 1360 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data_fd.push_back(RigidConstraintData(ci_closure)); |
| 1361 | |||
| 1362 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1363 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 1364 | 2 | constraint_dim += constraint_models[k].size(); | |
| 1365 | |||
| 1366 | 2 | const double mu0 = 0.; | |
| 1367 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 100); |
| 1368 | |||
| 1369 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 1370 | const VectorXd ddq0 = | ||
| 1371 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1372 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data.lambda_c; |
| 1373 | |||
| 1374 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 1375 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 1376 | |||
| 1377 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Motion v_error, a_error; |
| 1378 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | computeVelocityAndAccelerationErrors( |
| 1379 | model, ci_closure, q, v, ddq0, v_error, a_error, ci_closure.corrector.Kp, | ||
| 1380 | ci_closure.corrector.Kd); | ||
| 1381 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(a_error.isZero()); |
| 1382 | |||
| 1383 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const Motion constraint_velocity_error = constraint_data[0].contact_velocity_error; |
| 1384 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const VectorXd constraint_acceleration_error = -data.primal_rhs_contact.head(constraint_dim); |
| 1385 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(constraint_velocity_error.isApprox(v_error)); |
| 1386 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK(constraint_acceleration_error.isApprox(a_error.toVector() - data.dac_da * ddq0)); |
| 1387 | |||
| 1388 | // Data_fd | ||
| 1389 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 1390 | |||
| 1391 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 1392 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 1393 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 1394 | |||
| 1395 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 1396 | |||
| 1397 | 2 | const double alpha = 1e-8; | |
| 1398 | |||
| 1399 | // d./dq | ||
| 1400 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 1401 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 1402 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 1403 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 1404 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_velocity_error_dq_fd(6, model.nv); |
| 1405 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_velocity_error_dq_fd.setZero(); |
| 1406 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_acceleration_error_dq_fd(6, model.nv); |
| 1407 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_acceleration_error_dq_fd.setZero(); |
| 1408 | |||
| 1409 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1410 | { | ||
| 1411 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 1412 | |||
| 1413 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 1414 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1415 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data_fd, prox_settings); |
| 1416 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1417 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1418 | |||
| 1419 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
64 | Motion v_error_plus, a_error_plus; |
| 1420 |
3/6✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
|
64 | computeVelocityAndAccelerationErrors( |
| 1421 | model, ci_closure, q_plus, v, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp, | ||
| 1422 | ci_closure.corrector.Kd); | ||
| 1423 | |||
| 1424 | 64 | const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error; | |
| 1425 | const VectorXd constraint_acceleration_error_plus = | ||
| 1426 |
3/6✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
|
64 | -data_fd.primal_rhs_contact.head(constraint_dim); |
| 1427 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dconstraint_velocity_error_dq_fd.col(k) = |
| 1428 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
128 | (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha; |
| 1429 |
5/10✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
|
64 | dconstraint_acceleration_error_dq_fd.col(k) = (a_error_plus - a_error).toVector() / alpha; |
| 1430 | |||
| 1431 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 1432 | 64 | } | |
| 1433 | |||
| 1434 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_velocity_error_dq_fd.isApprox(data.dvc_dq, sqrt(alpha))); |
| 1435 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_acceleration_error_dq_fd.isApprox(data.dac_dq, sqrt(alpha))); |
| 1436 | |||
| 1437 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 1438 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 1439 | |||
| 1440 | // d./dv | ||
| 1441 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 1442 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 1443 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 1444 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 1445 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_velocity_error_dv_fd(6, model.nv); |
| 1446 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_velocity_error_dv_fd.setZero(); |
| 1447 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dconstraint_acceleration_error_dv_fd(6, model.nv); |
| 1448 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dconstraint_acceleration_error_dv_fd.setZero(); |
| 1449 | |||
| 1450 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 1451 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1452 | { | ||
| 1453 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 1454 | |||
| 1455 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1456 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data_fd, prox_settings); |
| 1457 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1458 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1459 | |||
| 1460 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
64 | Motion v_error_plus, a_error_plus; |
| 1461 |
3/6✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
|
64 | computeVelocityAndAccelerationErrors( |
| 1462 | model, ci_closure, q, v_plus, ddq0, v_error_plus, a_error_plus, ci_closure.corrector.Kp, | ||
| 1463 | ci_closure.corrector.Kd); | ||
| 1464 | |||
| 1465 | 64 | const Motion & constraint_velocity_error_plus = constraint_data_fd[0].contact_velocity_error; | |
| 1466 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dconstraint_velocity_error_dv_fd.col(k) = |
| 1467 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
128 | (constraint_velocity_error_plus - constraint_velocity_error).toVector() / alpha; |
| 1468 |
5/10✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 32 times.
✗ Branch 14 not taken.
|
64 | dconstraint_acceleration_error_dv_fd.col(k) = (a_error_plus - a_error).toVector() / alpha; |
| 1469 | |||
| 1470 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 1471 | } | ||
| 1472 | |||
| 1473 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_velocity_error_dv_fd.isApprox(data.dac_da, sqrt(alpha))); |
| 1474 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dconstraint_acceleration_error_dv_fd.isApprox(data.dac_dv, sqrt(alpha))); |
| 1475 | |||
| 1476 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 1477 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 1478 | |||
| 1479 | // d./dtau | ||
| 1480 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 1481 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 1482 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 1483 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 1484 | |||
| 1485 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 1486 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1487 | { | ||
| 1488 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 1489 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1490 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data_fd, prox_settings); |
| 1491 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1492 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1493 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 1494 | } | ||
| 1495 | |||
| 1496 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 1497 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 1498 | 2 | } | |
| 1499 | |||
| 1500 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE( |
| 1501 | test_constraint_dynamics_derivatives_LOCAL_WORL_ALIGNED_6D_loop_closure_j1j2_fd) | ||
| 1502 | { | ||
| 1503 | using namespace Eigen; | ||
| 1504 | using namespace pinocchio; | ||
| 1505 | |||
| 1506 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 1507 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 1508 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 1509 | |||
| 1510 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 1511 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 1512 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1513 | |||
| 1514 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 1515 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1516 | |||
| 1517 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1518 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 1519 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1520 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 1521 | |||
| 1522 | // Contact models and data | ||
| 1523 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 1524 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 1525 | |||
| 1526 | // Add Loop Closure Constraint | ||
| 1527 | |||
| 1528 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm5_joint"; |
| 1529 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RA_id = model.getJointId(RA); |
| 1530 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LA = "larm5_joint"; |
| 1531 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LA_id = model.getJointId(LA); |
| 1532 | |||
| 1533 | RigidConstraintModel ci_closure( | ||
| 1534 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_6D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 1535 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kp.array() = 0.; |
| 1536 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kd.array() = 0; |
| 1537 | |||
| 1538 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_closure); |
| 1539 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_closure)); |
| 1540 | // End of Loopo Closure Constraint | ||
| 1541 | |||
| 1542 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1543 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 1544 | 2 | constraint_dim += constraint_models[k].size(); | |
| 1545 | |||
| 1546 | 2 | const double mu0 = 0.; | |
| 1547 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 1548 | |||
| 1549 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 1550 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1551 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 1552 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 1553 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 1554 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 1555 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 1556 | |||
| 1557 | // Data_fd | ||
| 1558 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 1559 | |||
| 1560 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 1561 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 1562 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 1563 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 1564 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 1565 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 1566 | |||
| 1567 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 1568 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 1569 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 1570 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 1571 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 1572 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 1573 | |||
| 1574 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 1575 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1576 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 1577 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 1578 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 1579 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 1580 | |||
| 1581 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 1582 | |||
| 1583 | 2 | const double alpha = 1e-8; | |
| 1584 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 1585 | |||
| 1586 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1587 | { | ||
| 1588 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 1589 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 1590 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1591 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 1592 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1593 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1594 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 1595 | } | ||
| 1596 | |||
| 1597 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 1598 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 1599 | |||
| 1600 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 1601 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1602 | { | ||
| 1603 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 1604 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1605 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 1606 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1607 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1608 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 1609 | } | ||
| 1610 | |||
| 1611 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 1612 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 1613 | |||
| 1614 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 1615 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1616 | { | ||
| 1617 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 1618 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1619 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 1620 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1621 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1622 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 1623 | } | ||
| 1624 | |||
| 1625 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 1626 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 1627 | 2 | } | |
| 1628 | |||
| 1629 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_3D_loop_closure_j1j2_fd) |
| 1630 | { | ||
| 1631 | using namespace Eigen; | ||
| 1632 | using namespace pinocchio; | ||
| 1633 | |||
| 1634 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 1635 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 1636 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 1637 | |||
| 1638 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 1639 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 1640 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1641 | |||
| 1642 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 1643 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1644 | |||
| 1645 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1646 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 1647 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1648 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 1649 | |||
| 1650 | // Contact models and data | ||
| 1651 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 1652 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 1653 | |||
| 1654 | // Add Loop Closure Constraint | ||
| 1655 | |||
| 1656 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm5_joint"; |
| 1657 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RA_id = model.getJointId(RA); |
| 1658 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LA = "larm5_joint"; |
| 1659 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LA_id = model.getJointId(LA); |
| 1660 | |||
| 1661 | RigidConstraintModel ci_closure( | ||
| 1662 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_3D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL); |
| 1663 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kp.array() = KP; |
| 1664 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kd.array() = KD; |
| 1665 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_closure); |
| 1666 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_closure)); |
| 1667 | // End of Loopo Closure Constraint | ||
| 1668 | |||
| 1669 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1670 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 1671 | 2 | constraint_dim += constraint_models[k].size(); | |
| 1672 | |||
| 1673 | 2 | const double mu0 = 0.; | |
| 1674 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 1675 | |||
| 1676 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 1677 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1678 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 1679 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 1680 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 1681 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 1682 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 1683 | |||
| 1684 | // Data_fd | ||
| 1685 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 1686 | |||
| 1687 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 1688 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 1689 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 1690 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 1691 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 1692 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 1693 | |||
| 1694 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 1695 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 1696 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 1697 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 1698 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 1699 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 1700 | |||
| 1701 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 1702 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1703 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 1704 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 1705 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 1706 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 1707 | |||
| 1708 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 1709 | |||
| 1710 | 2 | const double alpha = 1e-8; | |
| 1711 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 1712 | |||
| 1713 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1714 | { | ||
| 1715 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 1716 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 1717 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1718 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 1719 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1720 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1721 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 1722 | } | ||
| 1723 | |||
| 1724 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 1725 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 1726 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 1727 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1728 | { | ||
| 1729 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 1730 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1731 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 1732 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1733 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1734 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 1735 | } | ||
| 1736 | |||
| 1737 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 1738 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 1739 | |||
| 1740 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 1741 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1742 | { | ||
| 1743 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 1744 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1745 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 1746 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1747 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1748 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 1749 | } | ||
| 1750 | |||
| 1751 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 1752 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 1753 | 2 | } | |
| 1754 | |||
| 1755 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE( |
| 1756 | test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_3D_loop_closure_j1j2_fd) | ||
| 1757 | { | ||
| 1758 | using namespace Eigen; | ||
| 1759 | using namespace pinocchio; | ||
| 1760 | |||
| 1761 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 1762 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 1763 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 1764 | |||
| 1765 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 1766 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 1767 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1768 | |||
| 1769 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 1770 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1771 | |||
| 1772 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1773 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 1774 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1775 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 1776 | |||
| 1777 | // Contact models and data | ||
| 1778 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 1779 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 1780 | |||
| 1781 | // Add Loop Closure Constraint | ||
| 1782 | |||
| 1783 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RA = "rarm5_joint"; |
| 1784 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RA_id = model.getJointId(RA); |
| 1785 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LA = "larm5_joint"; |
| 1786 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LA_id = model.getJointId(LA); |
| 1787 | |||
| 1788 | RigidConstraintModel ci_closure( | ||
| 1789 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | CONTACT_3D, model, LA_id, SE3::Random(), RA_id, SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 1790 | |||
| 1791 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kp.array() = KP; |
| 1792 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_closure.corrector.Kd.array() = KD; |
| 1793 | |||
| 1794 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_closure); |
| 1795 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_closure)); |
| 1796 | // End of Loopo Closure Constraint | ||
| 1797 | |||
| 1798 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1799 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 1800 | 2 | constraint_dim += constraint_models[k].size(); | |
| 1801 | |||
| 1802 | 2 | const double mu0 = 0.; | |
| 1803 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 1804 | |||
| 1805 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 1806 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1807 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 1808 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 1809 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 1810 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 1811 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 1812 | |||
| 1813 | // Data_fd | ||
| 1814 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 1815 | |||
| 1816 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 1817 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 1818 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 1819 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 1820 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 1821 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 1822 | |||
| 1823 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 1824 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 1825 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 1826 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 1827 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 1828 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 1829 | |||
| 1830 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 1831 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1832 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 1833 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 1834 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 1835 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 1836 | |||
| 1837 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 1838 | |||
| 1839 | 2 | const double alpha = 1e-8; | |
| 1840 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 1841 | |||
| 1842 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1843 | { | ||
| 1844 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 1845 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 1846 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1847 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 1848 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1849 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1850 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 1851 | } | ||
| 1852 | |||
| 1853 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 1854 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 1855 | |||
| 1856 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 1857 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1858 | { | ||
| 1859 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 1860 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1861 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 1862 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1863 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1864 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 1865 | } | ||
| 1866 | |||
| 1867 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 1868 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 1869 | |||
| 1870 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 1871 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1872 | { | ||
| 1873 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 1874 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1875 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 1876 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1877 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1878 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 1879 | } | ||
| 1880 | |||
| 1881 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 1882 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 1883 | 2 | } | |
| 1884 | |||
| 1885 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_6D_fd) |
| 1886 | { | ||
| 1887 | using namespace Eigen; | ||
| 1888 | using namespace pinocchio; | ||
| 1889 | |||
| 1890 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 1891 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 1892 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 1893 | |||
| 1894 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 1895 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 1896 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 1897 | |||
| 1898 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 1899 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 1900 | |||
| 1901 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 1902 | // const Model::JointIndex RF_id = model.getJointId(RF); | ||
| 1903 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 1904 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 1905 | |||
| 1906 | // Contact models and data | ||
| 1907 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 1908 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 1909 | |||
| 1910 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL_WORLD_ALIGNED); |
| 1911 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kp.array() = 0; // TODO: Add support for KP >0 |
| 1912 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kd.array() = KD; |
| 1913 | |||
| 1914 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 1915 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_LF); |
| 1916 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_LF)); |
| 1917 | |||
| 1918 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 1919 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 1920 | 2 | constraint_dim += constraint_models[k].size(); | |
| 1921 | |||
| 1922 | 2 | const double mu0 = 0.; | |
| 1923 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 1924 | |||
| 1925 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 1926 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1927 | |||
| 1928 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 1929 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 1930 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 1931 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 1932 | |||
| 1933 | // Data_fd | ||
| 1934 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 1935 | |||
| 1936 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 1937 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 1938 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 1939 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 1940 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 1941 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 1942 | |||
| 1943 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 1944 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 1945 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 1946 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 1947 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 1948 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 1949 | |||
| 1950 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 1951 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 1952 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 1953 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 1954 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 1955 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 1956 | |||
| 1957 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 1958 | |||
| 1959 | 2 | const double alpha = 1e-8; | |
| 1960 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1961 | { | ||
| 1962 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 1963 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 1964 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1965 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 1966 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1967 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1968 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 1969 | } | ||
| 1970 | |||
| 1971 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 1972 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 1973 | |||
| 1974 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 1975 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1976 | { | ||
| 1977 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 1978 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1979 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 1980 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1981 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1982 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 1983 | } | ||
| 1984 | |||
| 1985 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 1986 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 1987 | |||
| 1988 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 1989 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 1990 | { | ||
| 1991 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 1992 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 1993 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 1994 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 1995 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 1996 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 1997 | } | ||
| 1998 | |||
| 1999 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 2000 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 2001 | 2 | } | |
| 2002 | |||
| 2003 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_LOCAL_WORLD_ALIGNED_3D_fd) |
| 2004 | { | ||
| 2005 | using namespace Eigen; | ||
| 2006 | using namespace pinocchio; | ||
| 2007 | |||
| 2008 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 2009 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 2010 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 2011 | |||
| 2012 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 2013 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 2014 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 2015 | |||
| 2016 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 2017 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 2018 | |||
| 2019 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 2020 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 2021 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 2022 | // const Model::JointIndex LF_id = model.getJointId(LF); | ||
| 2023 | |||
| 2024 | // Contact models and data | ||
| 2025 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 2026 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 2027 | |||
| 2028 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL_WORLD_ALIGNED); |
| 2029 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kp.array() = KP; |
| 2030 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kd.array() = KD; |
| 2031 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 2032 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 2033 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RF)); |
| 2034 | |||
| 2035 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 2036 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2037 | 2 | constraint_dim += constraint_models[k].size(); | |
| 2038 | |||
| 2039 | 2 | const double mu0 = 0.; | |
| 2040 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 2041 | |||
| 2042 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 2043 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 2044 | |||
| 2045 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 2046 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 2047 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 2048 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 2049 | |||
| 2050 | // Data_fd | ||
| 2051 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 2052 | |||
| 2053 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 2054 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 2055 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 2056 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 2057 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 2058 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 2059 | |||
| 2060 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 2061 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 2062 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 2063 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 2064 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 2065 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 2066 | |||
| 2067 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 2068 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 2069 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 2070 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 2071 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 2072 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 2073 | |||
| 2074 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 2075 | |||
| 2076 | 2 | const double alpha = 1e-8; | |
| 2077 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 2078 | { | ||
| 2079 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 2080 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 2081 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 2082 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 2083 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 2084 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 2085 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 2086 | } | ||
| 2087 | |||
| 2088 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 2089 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 2090 | |||
| 2091 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 2092 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 2093 | { | ||
| 2094 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 2095 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 2096 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 2097 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 2098 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 2099 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 2100 | } | ||
| 2101 | |||
| 2102 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 2103 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 2104 | |||
| 2105 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 2106 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 2107 | { | ||
| 2108 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 2109 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 2110 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 2111 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 2112 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 2113 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 2114 | } | ||
| 2115 | |||
| 2116 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 2117 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 2118 | 2 | } | |
| 2119 | |||
| 2120 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_mix_fd) |
| 2121 | { | ||
| 2122 | using namespace Eigen; | ||
| 2123 | using namespace pinocchio; | ||
| 2124 | |||
| 2125 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 2126 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 2127 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 2128 | |||
| 2129 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 2130 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 2131 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 2132 | |||
| 2133 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 2134 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 2135 | |||
| 2136 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 2137 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 2138 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 2139 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 2140 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RH = "rarm6_joint"; |
| 2141 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RH_id = model.getJointId(RH); |
| 2142 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LH = "larm6_joint"; |
| 2143 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LH_id = model.getJointId(LH); |
| 2144 | |||
| 2145 | // Contact models and data | ||
| 2146 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 2147 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 2148 | |||
| 2149 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL_WORLD_ALIGNED); |
| 2150 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kp.array() = 0; // TODO: fix local_world_aligned for 6d with kp non-zero |
| 2151 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kd.array() = KD; |
| 2152 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LF.joint1_placement.setRandom(); |
| 2153 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_LF); |
| 2154 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_LF)); |
| 2155 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_6D, model, RF_id, LOCAL); |
| 2156 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kp.array() = KP; |
| 2157 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kd.array() = KD; |
| 2158 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RF.joint1_placement.setRandom(); |
| 2159 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 2160 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RF)); |
| 2161 | |||
| 2162 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LH(CONTACT_3D, model, LH_id, LOCAL_WORLD_ALIGNED); |
| 2163 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LH.corrector.Kp.array() = KP; |
| 2164 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LH.corrector.Kd.array() = KD; |
| 2165 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_LH.joint1_placement.setRandom(); |
| 2166 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_LH); |
| 2167 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_LH)); |
| 2168 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RH(CONTACT_3D, model, RH_id, LOCAL); |
| 2169 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RH.corrector.Kp.array() = KP; |
| 2170 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RH.corrector.Kd.array() = KD; |
| 2171 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ci_RH.joint1_placement.setRandom(); |
| 2172 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RH); |
| 2173 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RH)); |
| 2174 | |||
| 2175 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 2176 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
|
10 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2177 | 8 | constraint_dim += constraint_models[k].size(); | |
| 2178 | |||
| 2179 | 2 | const double mu0 = 0.; | |
| 2180 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 2181 | |||
| 2182 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 2183 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 2184 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 2185 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 2186 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 2187 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 2188 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 2189 | |||
| 2190 | // Data_fd | ||
| 2191 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 2192 | |||
| 2193 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); |
| 2194 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dq_fd.setZero(); |
| 2195 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); |
| 2196 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dv_fd.setZero(); |
| 2197 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); |
| 2198 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ddq_partial_dtau_fd.setZero(); |
| 2199 | |||
| 2200 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); |
| 2201 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dtau_fd.setZero(); |
| 2202 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); |
| 2203 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dq_fd.setZero(); |
| 2204 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); |
| 2205 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | lambda_partial_dv_fd.setZero(); |
| 2206 | |||
| 2207 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 2208 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 2209 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 2210 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 2211 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 2212 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 2213 | |||
| 2214 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 2215 | |||
| 2216 | 2 | const double alpha = 1e-8; | |
| 2217 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 2218 | |||
| 2219 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Eigen::MatrixXd Jc = data.dac_da; |
| 2220 | const Eigen::MatrixXd Jc_ref = | ||
| 2221 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | data.contact_chol.matrix().topRightCorner(constraint_dim, model.nv); |
| 2222 | |||
| 2223 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(Jc.isApprox(Jc_ref)); |
| 2224 | |||
| 2225 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Eigen::MatrixXd JMinv = Jc * data.Minv; |
| 2226 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Eigen::MatrixXd dac_dq = data.dac_dq; |
| 2227 | |||
| 2228 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd dac_dq_fd(constraint_dim, model.nv); |
| 2229 | |||
| 2230 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd contact_acc0(constraint_dim); |
| 2231 | 2 | Eigen::DenseIndex row_id = 0; | |
| 2232 | |||
| 2233 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, data.ddq); |
| 2234 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 1 times.
|
10 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2235 | { | ||
| 2236 | 8 | const RigidConstraintModel & cmodel = constraint_models[k]; | |
| 2237 | 8 | const RigidConstraintData & cdata = constraint_data[k]; | |
| 2238 | 8 | const Eigen::DenseIndex size = cmodel.size(); | |
| 2239 | |||
| 2240 |
2/4✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
|
8 | const Motion contact_acc = getContactAcceleration(model, data, cmodel); |
| 2241 | |||
| 2242 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
8 | if (cmodel.type == CONTACT_3D) |
| 2243 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | contact_acc0.segment<3>(row_id) = |
| 2244 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
8 | contact_acc.linear() - cdata.contact_acceleration_error.linear(); |
| 2245 | else | ||
| 2246 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | contact_acc0.segment<6>(row_id) = |
| 2247 |
4/8✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
|
8 | contact_acc.toVector() - cdata.contact_acceleration_error.toVector(); |
| 2248 | |||
| 2249 | 8 | row_id += size; | |
| 2250 | } | ||
| 2251 | |||
| 2252 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 2253 | { | ||
| 2254 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 2255 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 2256 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 2257 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 2258 | |||
| 2259 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 2260 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 2261 | |||
| 2262 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | Eigen::VectorXd contact_acc_plus(constraint_dim); |
| 2263 | 64 | Eigen::DenseIndex row_id = 0; | |
| 2264 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | forwardKinematics(model, data_fd, q_plus, v, data.ddq); |
| 2265 |
2/2✓ Branch 1 taken 128 times.
✓ Branch 2 taken 32 times.
|
320 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2266 | { | ||
| 2267 | 256 | const RigidConstraintModel & cmodel = constraint_models[k]; | |
| 2268 | 256 | const RigidConstraintData & cdata = constraint_data[k]; | |
| 2269 | 256 | const Eigen::DenseIndex size = cmodel.size(); | |
| 2270 | |||
| 2271 |
2/4✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 128 times.
✗ Branch 5 not taken.
|
256 | const Motion contact_acc = getContactAcceleration(model, data_fd, cmodel); |
| 2272 | |||
| 2273 |
2/2✓ Branch 0 taken 64 times.
✓ Branch 1 taken 64 times.
|
256 | if (cmodel.type == CONTACT_3D) |
| 2274 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
128 | contact_acc_plus.segment<3>(row_id) = |
| 2275 |
4/8✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 64 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 64 times.
✗ Branch 11 not taken.
|
256 | contact_acc.linear() - cdata.contact_acceleration_error.linear(); |
| 2276 | else | ||
| 2277 |
1/2✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
|
128 | contact_acc_plus.segment<6>(row_id) = |
| 2278 |
4/8✓ Branch 1 taken 64 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 64 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 64 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 64 times.
✗ Branch 11 not taken.
|
256 | contact_acc.toVector() - cdata.contact_acceleration_error.toVector(); |
| 2279 | |||
| 2280 | 256 | row_id += size; | |
| 2281 | } | ||
| 2282 | |||
| 2283 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | dac_dq_fd.col(k) = (contact_acc_plus - contact_acc0) / alpha; |
| 2284 | |||
| 2285 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 2286 | 64 | } | |
| 2287 | |||
| 2288 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dac_dq_fd.isApprox(dac_dq, 1e-6)); |
| 2289 | |||
| 2290 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 2291 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 2292 | |||
| 2293 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 2294 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 2295 | { | ||
| 2296 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 2297 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 2298 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, tau, constraint_models, constraint_data, prox_settings); |
| 2299 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 2300 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 2301 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 2302 | } | ||
| 2303 | |||
| 2304 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); |
| 2305 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 2306 | |||
| 2307 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd tau_plus(tau); |
| 2308 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 2309 | { | ||
| 2310 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] += alpha; |
| 2311 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 2312 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v, tau_plus, constraint_models, constraint_data, prox_settings); |
| 2313 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; |
| 2314 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; |
| 2315 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | tau_plus[k] -= alpha; |
| 2316 | } | ||
| 2317 | |||
| 2318 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); |
| 2319 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); |
| 2320 | 2 | } | |
| 2321 | |||
| 2322 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_loop_closure_kinematics_fd) |
| 2323 | { | ||
| 2324 | using namespace Eigen; | ||
| 2325 | using namespace pinocchio; | ||
| 2326 | |||
| 2327 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 2328 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 2329 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Data data(model), data_fd(model); |
| 2330 | |||
| 2331 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 2332 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 2333 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 2334 | |||
| 2335 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 2336 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 2337 | |||
| 2338 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RH = "rarm6_joint"; |
| 2339 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RH_id = model.getJointId(RH); |
| 2340 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LH = "larm6_joint"; |
| 2341 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LH_id = model.getJointId(LH); |
| 2342 | |||
| 2343 | // Contact models and data | ||
| 2344 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 2345 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 2346 | |||
| 2347 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | RigidConstraintModel ci_RH(CONTACT_6D, model, RH_id, SE3::Random(), LH_id, SE3::Random(), LOCAL); |
| 2348 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RH.corrector.Kp.array() = 0; |
| 2349 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RH.corrector.Kd.array() = 0; |
| 2350 | |||
| 2351 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RH); |
| 2352 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RH)); |
| 2353 | |||
| 2354 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 2355 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2356 | 2 | constraint_dim += constraint_models[k].size(); | |
| 2357 | |||
| 2358 | 2 | const double mu0 = 0.; | |
| 2359 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 2360 | |||
| 2361 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, constraint_models); |
| 2362 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 2363 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Data::TangentVectorType a = data.ddq; |
| 2364 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | data.M.triangularView<Eigen::StrictlyLower>() = |
| 2365 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
| 2366 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 2367 | model, data, constraint_models, constraint_data, prox_settings); | ||
| 2368 | |||
| 2369 | // Data_fd | ||
| 2370 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, constraint_models); |
| 2371 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd ddq0 = constraintDynamics( |
| 2372 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_fd, q, v, tau, constraint_models, constraint_data, prox_settings); |
| 2373 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd lambda0 = data_fd.lambda_c; |
| 2374 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v_eps(VectorXd::Zero(model.nv)); |
| 2375 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 2376 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd ddq_plus(model.nv); |
| 2377 | |||
| 2378 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd lambda_plus(constraint_dim); |
| 2379 | |||
| 2380 | 2 | const double alpha = 1e-8; | |
| 2381 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, a); |
| 2382 | |||
| 2383 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Eigen::MatrixXd Jc = data.dac_da; |
| 2384 | const Eigen::MatrixXd Jc_ref = | ||
| 2385 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | data.contact_chol.matrix().topRightCorner(constraint_dim, model.nv); |
| 2386 | |||
| 2387 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(Jc.isApprox(Jc_ref)); |
| 2388 | |||
| 2389 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Eigen::MatrixXd JMinv = Jc * data.Minv; |
| 2390 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Eigen::MatrixXd dac_dq = data.dac_dq; |
| 2391 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd dac_dq_fd(constraint_dim, model.nv); |
| 2392 | |||
| 2393 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::VectorXd contact_acc0(constraint_dim); |
| 2394 | 2 | Eigen::DenseIndex row_id = 0; | |
| 2395 | |||
| 2396 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | forwardKinematics(model, data, q, v, data.ddq); |
| 2397 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
4 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2398 | { | ||
| 2399 | 2 | const RigidConstraintModel & cmodel = constraint_models[k]; | |
| 2400 | 2 | const RigidConstraintData & cdata = constraint_data[k]; | |
| 2401 | 2 | const Eigen::DenseIndex size = cmodel.size(); | |
| 2402 | |||
| 2403 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Motion contact_acc = getContactAcceleration(model, data, cmodel, cdata.c1Mc2); |
| 2404 | |||
| 2405 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
2 | if (cmodel.type == CONTACT_3D) |
| 2406 | ✗ | contact_acc0.segment<3>(row_id) = contact_acc.linear(); | |
| 2407 | else | ||
| 2408 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | contact_acc0.segment<6>(row_id) = contact_acc.toVector(); |
| 2409 | |||
| 2410 | 2 | row_id += size; | |
| 2411 | } | ||
| 2412 | |||
| 2413 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 2414 | { | ||
| 2415 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 2416 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 2417 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | ddq_plus = constraintDynamics( |
| 2418 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, tau, constraint_models, constraint_data, prox_settings); |
| 2419 | |||
| 2420 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | Eigen::VectorXd contact_acc_plus(constraint_dim); |
| 2421 | 64 | Eigen::DenseIndex row_id = 0; | |
| 2422 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | forwardKinematics(model, data_fd, q_plus, v, data.ddq); |
| 2423 |
2/2✓ Branch 1 taken 32 times.
✓ Branch 2 taken 32 times.
|
128 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2424 | { | ||
| 2425 | 64 | const RigidConstraintModel & cmodel = constraint_models[k]; | |
| 2426 | 64 | const RigidConstraintData & cdata = constraint_data[k]; | |
| 2427 | 64 | const Eigen::DenseIndex size = cmodel.size(); | |
| 2428 | |||
| 2429 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | const Motion contact_acc = getContactAcceleration(model, data_fd, cmodel, cdata.c1Mc2); |
| 2430 | |||
| 2431 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
|
64 | if (cmodel.type == CONTACT_3D) |
| 2432 | ✗ | contact_acc_plus.segment<3>(row_id) = contact_acc.linear(); | |
| 2433 | else | ||
| 2434 |
3/6✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
|
64 | contact_acc_plus.segment<6>(row_id) = contact_acc.toVector(); |
| 2435 | |||
| 2436 | 64 | row_id += size; | |
| 2437 | } | ||
| 2438 | |||
| 2439 |
4/8✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 32 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
|
64 | dac_dq_fd.col(k) = (contact_acc_plus - contact_acc0) / alpha; |
| 2440 | |||
| 2441 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 2442 | 64 | } | |
| 2443 | |||
| 2444 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(dac_dq_fd.isApprox(dac_dq, 2e-6)); |
| 2445 | 2 | } | |
| 2446 | |||
| 2447 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_dirty_data) |
| 2448 | { | ||
| 2449 | // Verify that a dirty data doesn't affect the results of the contact dynamics derivs | ||
| 2450 | using namespace Eigen; | ||
| 2451 | using namespace pinocchio; | ||
| 2452 | |||
| 2453 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 2454 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 2455 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_dirty(model); |
| 2456 | |||
| 2457 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.lowerPositionLimit.head<3>().fill(-1.); |
| 2458 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | model.upperPositionLimit.head<3>().fill(1.); |
| 2459 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 2460 | |||
| 2461 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd v = VectorXd::Random(model.nv); |
| 2462 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | VectorXd tau = VectorXd::Random(model.nv); |
| 2463 | |||
| 2464 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 2465 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 2466 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 2467 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 2468 | |||
| 2469 | // Contact models and data | ||
| 2470 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) constraint_models; |
| 2471 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_data; |
| 2472 | |||
| 2473 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); |
| 2474 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); |
| 2475 | |||
| 2476 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kp.array() = KP; |
| 2477 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_LF.corrector.Kd.array() = KD; |
| 2478 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kp.array() = KP; |
| 2479 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ci_RF.corrector.Kd.array() = KD; |
| 2480 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_LF); |
| 2481 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_LF)); |
| 2482 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraint_models.push_back(ci_RF); |
| 2483 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | constraint_data.push_back(RigidConstraintData(ci_RF)); |
| 2484 | |||
| 2485 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 2486 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < constraint_models.size(); ++k) |
| 2487 | 4 | constraint_dim += constraint_models[k].size(); | |
| 2488 | |||
| 2489 | 2 | const double mu0 = 0.; | |
| 2490 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 2491 | |||
| 2492 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_dirty, constraint_models); |
| 2493 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 2494 | model, data_dirty, q, v, tau, constraint_models, constraint_data, prox_settings); | ||
| 2495 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 2496 | model, data_dirty, constraint_models, constraint_data, prox_settings); | ||
| 2497 | |||
| 2498 | // Reuse the same data with new configurations | ||
| 2499 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | q = randomConfiguration(model); |
| 2500 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | v = VectorXd::Random(model.nv); |
| 2501 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | tau = VectorXd::Random(model.nv); |
| 2502 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 2503 | model, data_dirty, q, v, tau, constraint_models, constraint_data, prox_settings); | ||
| 2504 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 2505 | model, data_dirty, constraint_models, constraint_data, prox_settings); | ||
| 2506 | |||
| 2507 | // Test with fresh data | ||
| 2508 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_fresh(model); |
| 2509 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fresh, constraint_models); |
| 2510 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | constraintDynamics( |
| 2511 | model, data_fresh, q, v, tau, constraint_models, constraint_data, prox_settings); | ||
| 2512 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeConstraintDynamicsDerivatives( |
| 2513 | model, data_fresh, constraint_models, constraint_data, prox_settings); | ||
| 2514 | 2 | const double alpha = 1e-12; | |
| 2515 | |||
| 2516 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data_dirty.ddq_dq.isApprox(data_fresh.ddq_dq, sqrt(alpha))); |
| 2517 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data_dirty.ddq_dv.isApprox(data_fresh.ddq_dv, sqrt(alpha))); |
| 2518 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data_dirty.ddq_dtau.isApprox(data_fresh.ddq_dtau, sqrt(alpha))); |
| 2519 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data_dirty.dlambda_dq.isApprox(data_fresh.dlambda_dq, sqrt(alpha))); |
| 2520 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data_dirty.dlambda_dv.isApprox(data_fresh.dlambda_dv, sqrt(alpha))); |
| 2521 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
|
2 | BOOST_CHECK(data_dirty.dlambda_dtau.isApprox(data_fresh.dlambda_dtau, sqrt(alpha))); |
| 2522 | 2 | } | |
| 2523 | |||
| 2524 | #ifdef PINOCCHIO_WITH_SDFORMAT | ||
| 2525 | |||
| 2526 | BOOST_AUTO_TEST_CASE_EXPECTED_FAILURES(test_constraint_dynamics_derivatives_cassie_proximal, 6) | ||
| 2527 | BOOST_AUTO_TEST_CASE(test_constraint_dynamics_derivatives_cassie_proximal) | ||
| 2528 | { | ||
| 2529 | // TODO: 4 fd tests (ddq/dtau, ddq/dq, ddq/dv, dlambda/dq, dlambda/dv) fail for cassie | ||
| 2530 | const std::string filename = | ||
| 2531 | PINOCCHIO_MODEL_DIR | ||
| 2532 | + std::string("/example-robot-data/robots/cassie_description/robots/cassie.sdf"); | ||
| 2533 | const std::string srdf_filename = | ||
| 2534 | PINOCCHIO_MODEL_DIR | ||
| 2535 | + std::string("/example-robot-data/robots/cassie_description/srdf/cassie_v2.srdf"); | ||
| 2536 | const std::string dir = PINOCCHIO_MODEL_DIR; | ||
| 2537 | |||
| 2538 | pinocchio::Model model; | ||
| 2539 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(pinocchio::RigidConstraintModel) constraint_models; | ||
| 2540 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) constraint_datas; | ||
| 2541 | |||
| 2542 | pinocchio::sdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), model, constraint_models); | ||
| 2543 | pinocchio::srdf::loadReferenceConfigurations(model, srdf_filename, false); | ||
| 2544 | |||
| 2545 | Eigen::VectorXd q = model.referenceConfigurations["standing"]; | ||
| 2546 | pinocchio::normalize(model, q); | ||
| 2547 | VectorXd v = VectorXd::Random(model.nv); | ||
| 2548 | VectorXd tau = VectorXd::Random(model.nv); | ||
| 2549 | |||
| 2550 | const double mu0 = 1e-5; | ||
| 2551 | ProximalSettings prox_settings(1e-12, mu0, 10); | ||
| 2552 | |||
| 2553 | Data data(model), data_fd(model); | ||
| 2554 | |||
| 2555 | initConstraintDynamics(model, data, constraint_models); | ||
| 2556 | for (int k = 0; k < (int)constraint_models.size(); ++k) | ||
| 2557 | { | ||
| 2558 | constraint_datas.push_back(RigidConstraintData(constraint_models[(pinocchio::JointIndex)k])); | ||
| 2559 | } | ||
| 2560 | |||
| 2561 | Eigen::DenseIndex constraint_dim = 0; | ||
| 2562 | for (size_t k = 0; k < constraint_models.size(); ++k) | ||
| 2563 | constraint_dim += constraint_models[k].size(); | ||
| 2564 | |||
| 2565 | initConstraintDynamics(model, data, constraint_models); | ||
| 2566 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_datas, prox_settings); | ||
| 2567 | data.M.triangularView<Eigen::StrictlyLower>() = | ||
| 2568 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); | ||
| 2569 | computeConstraintDynamicsDerivatives( | ||
| 2570 | model, data, constraint_models, constraint_datas, prox_settings); | ||
| 2571 | |||
| 2572 | initConstraintDynamics(model, data_fd, constraint_models); | ||
| 2573 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); | ||
| 2574 | ddq_partial_dq_fd.setZero(); | ||
| 2575 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); | ||
| 2576 | ddq_partial_dv_fd.setZero(); | ||
| 2577 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); | ||
| 2578 | ddq_partial_dtau_fd.setZero(); | ||
| 2579 | |||
| 2580 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); | ||
| 2581 | lambda_partial_dtau_fd.setZero(); | ||
| 2582 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); | ||
| 2583 | lambda_partial_dq_fd.setZero(); | ||
| 2584 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); | ||
| 2585 | lambda_partial_dv_fd.setZero(); | ||
| 2586 | |||
| 2587 | const VectorXd ddq0 = constraintDynamics( | ||
| 2588 | model, data_fd, q, v, tau, constraint_models, constraint_datas, prox_settings); | ||
| 2589 | const VectorXd lambda0 = data_fd.lambda_c; | ||
| 2590 | VectorXd v_eps(VectorXd::Zero(model.nv)); | ||
| 2591 | VectorXd q_plus(model.nq); | ||
| 2592 | VectorXd ddq_plus(model.nv); | ||
| 2593 | |||
| 2594 | VectorXd lambda_plus(constraint_dim); | ||
| 2595 | const double alpha = 1e-8; | ||
| 2596 | forwardKinematics(model, data, q, v); | ||
| 2597 | for (int k = 0; k < model.nv; ++k) | ||
| 2598 | { | ||
| 2599 | v_eps[k] += alpha; | ||
| 2600 | q_plus = integrate(model, q, v_eps); | ||
| 2601 | ddq_plus = constraintDynamics( | ||
| 2602 | model, data_fd, q_plus, v, tau, constraint_models, constraint_datas, prox_settings); | ||
| 2603 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; | ||
| 2604 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; | ||
| 2605 | v_eps[k] = 0.; | ||
| 2606 | } | ||
| 2607 | |||
| 2608 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); | ||
| 2609 | |||
| 2610 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); | ||
| 2611 | |||
| 2612 | VectorXd v_plus(v); | ||
| 2613 | for (int k = 0; k < model.nv; ++k) | ||
| 2614 | { | ||
| 2615 | v_plus[k] += alpha; | ||
| 2616 | ddq_plus = constraintDynamics( | ||
| 2617 | model, data_fd, q, v_plus, tau, constraint_models, constraint_datas, prox_settings); | ||
| 2618 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; | ||
| 2619 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; | ||
| 2620 | v_plus[k] -= alpha; | ||
| 2621 | } | ||
| 2622 | |||
| 2623 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); | ||
| 2624 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); | ||
| 2625 | |||
| 2626 | VectorXd tau_plus(tau); | ||
| 2627 | for (int k = 0; k < model.nv; ++k) | ||
| 2628 | { | ||
| 2629 | tau_plus[k] += alpha; | ||
| 2630 | ddq_plus = constraintDynamics( | ||
| 2631 | model, data_fd, q, v, tau_plus, constraint_models, constraint_datas, prox_settings); | ||
| 2632 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; | ||
| 2633 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; | ||
| 2634 | tau_plus[k] -= alpha; | ||
| 2635 | } | ||
| 2636 | |||
| 2637 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); | ||
| 2638 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); | ||
| 2639 | } | ||
| 2640 | |||
| 2641 | #endif // PINOCCHIO_WITH_SDFORMAT | ||
| 2642 | |||
| 2643 | BOOST_AUTO_TEST_SUITE_END() | ||
| 2644 |