| Directory: | ./ |
|---|---|
| File: | unittest/impulse-dynamics-derivatives.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 219 | 219 | 100.0% |
| Branches: | 512 | 1006 | 50.9% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2020-2021 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #include "pinocchio/algorithm/jacobian.hpp" | ||
| 6 | #include "pinocchio/algorithm/frames.hpp" | ||
| 7 | #include "pinocchio/algorithm/kinematics.hpp" | ||
| 8 | #include "pinocchio/algorithm/rnea.hpp" | ||
| 9 | #include "pinocchio/algorithm/rnea-derivatives.hpp" | ||
| 10 | #include "pinocchio/algorithm/impulse-dynamics.hpp" | ||
| 11 | #include "pinocchio/algorithm/impulse-dynamics-derivatives.hpp" | ||
| 12 | #include "pinocchio/algorithm/joint-configuration.hpp" | ||
| 13 | #include "pinocchio/multibody/sample-models.hpp" | ||
| 14 | |||
| 15 | #include <iostream> | ||
| 16 | |||
| 17 | #include <boost/test/unit_test.hpp> | ||
| 18 | #include <boost/utility/binary.hpp> | ||
| 19 | |||
| 20 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) | ||
| 21 | |||
| 22 | using namespace Eigen; | ||
| 23 | using namespace pinocchio; | ||
| 24 | |||
| 25 |
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_impulse_dynamics_derivatives_no_contact) |
| 26 | { | ||
| 27 | // result: (dMdq)(dqafter-v) = drnea(q,0,dqafter-v) | ||
| 28 | using namespace Eigen; | ||
| 29 | using namespace pinocchio; | ||
| 30 | |||
| 31 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 32 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 33 |
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); |
| 34 | |||
| 35 |
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.); |
| 36 |
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.); |
| 37 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 38 | |||
| 39 |
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); |
| 40 | |||
| 41 | // Contact models and data | ||
| 42 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) empty_contact_models; |
| 43 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) empty_contact_data; |
| 44 | |||
| 45 | 2 | const double mu0 = 0.; | |
| 46 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 47 | |||
| 48 | 2 | const double r_coeff = 0.5; | |
| 49 | |||
| 50 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, empty_contact_models); |
| 51 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulseDynamics( |
| 52 | model, data, q, v, empty_contact_models, empty_contact_data, r_coeff, prox_settings); | ||
| 53 | |||
| 54 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Eigen::VectorXd dv = data.dq_after - v; |
| 55 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeImpulseDynamicsDerivatives( |
| 56 | model, data, empty_contact_models, empty_contact_data, r_coeff, prox_settings); | ||
| 57 | |||
| 58 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Motion gravity_bk = model.gravity; |
| 59 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model.gravity.setZero(); |
| 60 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeRNEADerivatives(model, data_ref, q, Eigen::VectorXd::Zero(model.nv), dv); |
| 61 | // Reference values | ||
| 62 |
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)); |
| 63 | 2 | } | |
| 64 | |||
| 65 |
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_impulse_dynamics_derivatives) |
| 66 | { | ||
| 67 | using namespace Eigen; | ||
| 68 | using namespace pinocchio; | ||
| 69 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 70 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 71 |
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); |
| 72 | |||
| 73 |
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.); |
| 74 |
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.); |
| 75 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 76 |
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); |
| 77 | |||
| 78 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 79 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 80 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 81 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 82 | |||
| 83 | // Contact models and data | ||
| 84 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 85 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; |
| 86 | |||
| 87 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, LOCAL); |
| 88 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, LOCAL); |
| 89 | |||
| 90 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 91 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_data.push_back(RigidConstraintData(ci_LF)); |
| 92 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 93 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_data.push_back(RigidConstraintData(ci_RF)); |
| 94 | |||
| 95 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 96 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 97 | 4 | constraint_dim += contact_models[k].size(); | |
| 98 | |||
| 99 | 2 | const double mu0 = 0.; | |
| 100 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 101 | 2 | const double r_coeff = 0.5; | |
| 102 | |||
| 103 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 104 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulseDynamics(model, data, q, v, contact_models, contact_data, r_coeff, prox_settings); |
| 105 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeImpulseDynamicsDerivatives( |
| 106 | model, data, contact_models, contact_data, r_coeff, prox_settings); | ||
| 107 | |||
| 108 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; | ||
| 109 | |||
| 110 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | ForceVector iext((size_t)model.njoints); |
| 111 |
2/2✓ Branch 4 taken 28 times.
✓ Branch 5 taken 1 times.
|
58 | for (ForceVector::iterator it = iext.begin(); it != iext.end(); ++it) |
| 112 |
1/2✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
|
56 | (*it).setZero(); |
| 113 | |||
| 114 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
2 | iext[model.getJointId(LF)] = contact_data[0].contact_force; |
| 115 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
2 | iext[model.getJointId(RF)] = contact_data[1].contact_force; |
| 116 | |||
| 117 |
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 | Eigen::VectorXd effective_v = (1 + r_coeff) * v + data.ddq; |
| 118 | |||
| 119 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeForwardKinematicsDerivatives( |
| 120 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model, data_ref, q, effective_v, Eigen::VectorXd::Zero(model.nv)); |
| 121 | |||
| 122 |
2/2✓ Branch 1 taken 28 times.
✓ Branch 2 taken 1 times.
|
58 | for (size_t i = 0; i < data.ov.size(); i++) |
| 123 | { | ||
| 124 |
10/20✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 28 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 28 times.
✗ Branch 12 not taken.
✓ Branch 17 taken 28 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 28 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 28 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 28 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 28 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 28 times.
✗ Branch 34 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 28 times.
|
56 | BOOST_CHECK(((1 + r_coeff) * data.ov[i] + data.oa[i] - data_ref.ov[i]).isZero()); |
| 125 | } | ||
| 126 | |||
| 127 |
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 | Eigen::MatrixXd Jc(9, model.nv), dv_dq(9, model.nv), Jc_tmp(6, model.nv), dv_dq_tmp(6, model.nv); |
| 128 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jc.setZero(); |
| 129 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dv_dq.setZero(); |
| 130 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dv_dq_tmp.setZero(); |
| 131 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Jc_tmp.setZero(); |
| 132 | |||
| 133 |
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 | getJointVelocityDerivatives(model, data_ref, LF_id, LOCAL, dv_dq.topRows<6>(), Jc.topRows<6>()); |
| 134 | |||
| 135 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | getJointVelocityDerivatives(model, data_ref, RF_id, LOCAL, dv_dq_tmp, Jc_tmp); |
| 136 | |||
| 137 |
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.bottomRows<3>() = Jc_tmp.topRows<3>(); |
| 138 |
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 | dv_dq.bottomRows<3>() = dv_dq_tmp.topRows<3>(); |
| 139 | |||
| 140 |
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)); |
| 141 | |||
| 142 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Motion gravity_bk = model.gravity; |
| 143 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model.gravity.setZero(); |
| 144 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | computeRNEADerivatives(model, data_ref, q, Eigen::VectorXd::Zero(model.nv), data.ddq, iext); |
| 145 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | model.gravity = gravity_bk; |
| 146 | |||
| 147 |
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.dac_da.isApprox(Jc)); |
| 148 | // BOOST_CHECK((data.dvc_dq-(dv_dq-Jc*data.Minv*data_ref.dtau_dq)).norm()<=1e-12); | ||
| 149 | |||
| 150 |
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 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 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((data.dlambda_dv + (1 + r_coeff) * data.osim * Jc).isZero()); |
| 151 | 2 | } | |
| 152 | |||
| 153 |
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_impulse_dynamics_derivatives_LOCAL_fd) |
| 154 | { | ||
| 155 | using namespace Eigen; | ||
| 156 | using namespace pinocchio; | ||
| 157 | |||
| 158 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 159 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 160 |
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); |
| 161 | |||
| 162 |
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.); |
| 163 |
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.); |
| 164 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 165 | |||
| 166 |
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); |
| 167 | |||
| 168 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 169 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 170 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 171 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 172 | |||
| 173 | // Contact models and data | ||
| 174 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 175 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; |
| 176 | |||
| 177 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, SE3::Random(), LOCAL); |
| 178 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, SE3::Random(), LOCAL); |
| 179 | |||
| 180 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 181 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_data.push_back(RigidConstraintData(ci_LF)); |
| 182 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 183 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_data.push_back(RigidConstraintData(ci_RF)); |
| 184 | |||
| 185 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 186 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 187 | 4 | constraint_dim += contact_models[k].size(); | |
| 188 | |||
| 189 | 2 | const double mu0 = 0.; | |
| 190 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 191 | 2 | const double r_coeff = 0.5; | |
| 192 | |||
| 193 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 194 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulseDynamics(model, data, q, v, contact_models, contact_data, r_coeff, prox_settings); |
| 195 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeImpulseDynamicsDerivatives( |
| 196 | model, data, contact_models, contact_data, r_coeff, prox_settings); | ||
| 197 | |||
| 198 | // Data_fd | ||
| 199 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, contact_models); |
| 200 | |||
| 201 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dqafter_partial_dq_fd(model.nv, model.nv); |
| 202 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dqafter_partial_dq_fd.setZero(); |
| 203 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dqafter_partial_dv_fd(model.nv, model.nv); |
| 204 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dqafter_partial_dv_fd.setZero(); |
| 205 | |||
| 206 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd impulse_partial_dq_fd(constraint_dim, model.nv); |
| 207 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulse_partial_dq_fd.setZero(); |
| 208 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd impulse_partial_dv_fd(constraint_dim, model.nv); |
| 209 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulse_partial_dv_fd.setZero(); |
| 210 | |||
| 211 | const VectorXd dqafter0 = | ||
| 212 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | impulseDynamics(model, data_fd, q, v, contact_models, contact_data, r_coeff, prox_settings); |
| 213 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd impulse0 = data_fd.impulse_c; |
| 214 |
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)); |
| 215 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 216 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd dqafter_plus(model.nv); |
| 217 | |||
| 218 |
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 Eigen::MatrixXd Jc = data.contact_chol.matrix().topRightCorner(constraint_dim, model.nv); |
| 219 |
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 | const Eigen::VectorXd vel_jump = Jc * (dqafter0 + r_coeff * v); |
| 220 | |||
| 221 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Data data_plus(model); |
| 222 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd impulse_plus(constraint_dim); |
| 223 | |||
| 224 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Eigen::MatrixXd dvc_dq_fd(constraint_dim, model.nv); |
| 225 | 2 | const double alpha = 1e-8; | |
| 226 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 227 | { | ||
| 228 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 229 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 230 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dqafter_plus = impulseDynamics( |
| 231 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, contact_models, contact_data, r_coeff, prox_settings); |
| 232 | |||
| 233 | const Eigen::MatrixXd Jc_plus = | ||
| 234 |
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.contact_chol.matrix().topRightCorner(constraint_dim, model.nv); |
| 235 |
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 | const Eigen::VectorXd vel_jump_plus = Jc_plus * (dqafter0 + r_coeff * v); |
| 236 | |||
| 237 |
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 | dqafter_partial_dq_fd.col(k) = (dqafter_plus - dqafter0) / alpha; |
| 238 |
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 | impulse_partial_dq_fd.col(k) = (data_fd.impulse_c - impulse0) / alpha; |
| 239 |
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 | dvc_dq_fd.col(k) = (vel_jump_plus - vel_jump) / alpha; |
| 240 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 241 | 64 | } | |
| 242 | |||
| 243 |
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(Jc.isApprox(data.dac_da, sqrt(alpha))); |
| 244 |
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(dqafter_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 245 |
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(impulse_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 246 | |||
| 247 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 248 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 249 | { | ||
| 250 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 251 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dqafter_plus = impulseDynamics( |
| 252 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, contact_models, contact_data, r_coeff, prox_settings); |
| 253 | |||
| 254 |
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 | dqafter_partial_dv_fd.col(k) = (dqafter_plus - dqafter0) / alpha; |
| 255 |
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 | impulse_partial_dv_fd.col(k) = (data_fd.impulse_c - impulse0) / alpha; |
| 256 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 257 | } | ||
| 258 | |||
| 259 |
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 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 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
|
2 | BOOST_CHECK(dqafter_partial_dv_fd.isApprox( |
| 260 | Eigen::MatrixXd::Identity(model.nv, model.nv) + data.ddq_dv, sqrt(alpha))); | ||
| 261 |
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(impulse_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 262 | 2 | } | |
| 263 | |||
| 264 |
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_impulse_dynamics_derivatives_LOCAL_WORLD_ALIGNED_fd) |
| 265 | { | ||
| 266 | using namespace Eigen; | ||
| 267 | using namespace pinocchio; | ||
| 268 | |||
| 269 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Model model; |
| 270 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | buildModels::humanoidRandom(model, true); |
| 271 |
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); |
| 272 | |||
| 273 |
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.); |
| 274 |
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.); |
| 275 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q = randomConfiguration(model); |
| 276 | |||
| 277 |
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); |
| 278 | |||
| 279 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string RF = "rleg6_joint"; |
| 280 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex RF_id = model.getJointId(RF); |
| 281 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | const std::string LF = "lleg6_joint"; |
| 282 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Model::JointIndex LF_id = model.getJointId(LF); |
| 283 | |||
| 284 | // Contact models and data | ||
| 285 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models; |
| 286 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data; |
| 287 | |||
| 288 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_LF(CONTACT_6D, model, LF_id, SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 289 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | RigidConstraintModel ci_RF(CONTACT_3D, model, RF_id, SE3::Random(), LOCAL_WORLD_ALIGNED); |
| 290 | |||
| 291 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_LF); |
| 292 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_data.push_back(RigidConstraintData(ci_LF)); |
| 293 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | contact_models.push_back(ci_RF); |
| 294 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | contact_data.push_back(RigidConstraintData(ci_RF)); |
| 295 | |||
| 296 | 2 | Eigen::DenseIndex constraint_dim = 0; | |
| 297 |
2/2✓ Branch 1 taken 2 times.
✓ Branch 2 taken 1 times.
|
6 | for (size_t k = 0; k < contact_models.size(); ++k) |
| 298 | 4 | constraint_dim += contact_models[k].size(); | |
| 299 | |||
| 300 | 2 | const double mu0 = 0.; | |
| 301 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | ProximalSettings prox_settings(1e-12, mu0, 1); |
| 302 | 2 | const double r_coeff = 0.5; | |
| 303 | |||
| 304 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data, contact_models); |
| 305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulseDynamics(model, data, q, v, contact_models, contact_data, r_coeff, prox_settings); |
| 306 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | computeImpulseDynamicsDerivatives( |
| 307 | model, data, contact_models, contact_data, r_coeff, prox_settings); | ||
| 308 | |||
| 309 | // Data_fd | ||
| 310 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | initConstraintDynamics(model, data_fd, contact_models); |
| 311 | |||
| 312 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dqafter_partial_dq_fd(model.nv, model.nv); |
| 313 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dqafter_partial_dq_fd.setZero(); |
| 314 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd dqafter_partial_dv_fd(model.nv, model.nv); |
| 315 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dqafter_partial_dv_fd.setZero(); |
| 316 | |||
| 317 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd impulse_partial_dq_fd(constraint_dim, model.nv); |
| 318 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulse_partial_dq_fd.setZero(); |
| 319 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXd impulse_partial_dv_fd(constraint_dim, model.nv); |
| 320 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | impulse_partial_dv_fd.setZero(); |
| 321 | |||
| 322 | const VectorXd dqafter0 = | ||
| 323 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | impulseDynamics(model, data_fd, q, v, contact_models, contact_data, r_coeff, prox_settings); |
| 324 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const VectorXd impulse0 = data_fd.impulse_c; |
| 325 |
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)); |
| 326 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd q_plus(model.nq); |
| 327 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd dqafter_plus(model.nv); |
| 328 | |||
| 329 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd impulse_plus(constraint_dim); |
| 330 | 2 | const double alpha = 1e-8; | |
| 331 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 332 | { | ||
| 333 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] += alpha; |
| 334 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | q_plus = integrate(model, q, v_eps); |
| 335 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dqafter_plus = impulseDynamics( |
| 336 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q_plus, v, contact_models, contact_data, r_coeff, prox_settings); |
| 337 |
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 | dqafter_partial_dq_fd.col(k) = (dqafter_plus - dqafter0) / alpha; |
| 338 |
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 | impulse_partial_dq_fd.col(k) = (data_fd.impulse_c - impulse0) / alpha; |
| 339 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_eps[k] = 0.; |
| 340 | } | ||
| 341 | |||
| 342 |
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(dqafter_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); |
| 343 |
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(impulse_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); |
| 344 | |||
| 345 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | VectorXd v_plus(v); |
| 346 |
2/2✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
|
66 | for (int k = 0; k < model.nv; ++k) |
| 347 | { | ||
| 348 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] += alpha; |
| 349 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | dqafter_plus = impulseDynamics( |
| 350 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | model, data_fd, q, v_plus, contact_models, contact_data, r_coeff, prox_settings); |
| 351 |
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 | dqafter_partial_dv_fd.col(k) = (dqafter_plus - dqafter0) / alpha; |
| 352 |
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 | impulse_partial_dv_fd.col(k) = (data_fd.impulse_c - impulse0) / alpha; |
| 353 |
1/2✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
|
64 | v_plus[k] -= alpha; |
| 354 | } | ||
| 355 | |||
| 356 |
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 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 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 27 not taken.
✗ Branch 31 not taken.
✓ Branch 32 taken 1 times.
|
2 | BOOST_CHECK(dqafter_partial_dv_fd.isApprox( |
| 357 | Eigen::MatrixXd::Identity(model.nv, model.nv) + data.ddq_dv, sqrt(alpha))); | ||
| 358 |
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(impulse_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); |
| 359 | 2 | } | |
| 360 | |||
| 361 | BOOST_AUTO_TEST_SUITE_END() | ||
| 362 |