Directory: | ./ |
---|---|
File: | unittest/contact-dynamics-derivatives.cpp |
Date: | 2024-08-27 18:20:05 |
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, 5) | ||
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 | VectorXd v = VectorXd::Random(model.nv); | ||
2547 | VectorXd tau = VectorXd::Random(model.nv); | ||
2548 | |||
2549 | const double mu0 = 1e-5; | ||
2550 | ProximalSettings prox_settings(1e-12, mu0, 10); | ||
2551 | |||
2552 | Data data(model), data_fd(model); | ||
2553 | |||
2554 | initConstraintDynamics(model, data, constraint_models); | ||
2555 | for (int k = 0; k < (int)constraint_models.size(); ++k) | ||
2556 | { | ||
2557 | constraint_datas.push_back(RigidConstraintData(constraint_models[(pinocchio::JointIndex)k])); | ||
2558 | } | ||
2559 | |||
2560 | Eigen::DenseIndex constraint_dim = 0; | ||
2561 | for (size_t k = 0; k < constraint_models.size(); ++k) | ||
2562 | constraint_dim += constraint_models[k].size(); | ||
2563 | |||
2564 | initConstraintDynamics(model, data, constraint_models); | ||
2565 | constraintDynamics(model, data, q, v, tau, constraint_models, constraint_datas, prox_settings); | ||
2566 | data.M.triangularView<Eigen::StrictlyLower>() = | ||
2567 | data.M.transpose().triangularView<Eigen::StrictlyLower>(); | ||
2568 | computeConstraintDynamicsDerivatives( | ||
2569 | model, data, constraint_models, constraint_datas, prox_settings); | ||
2570 | |||
2571 | initConstraintDynamics(model, data_fd, constraint_models); | ||
2572 | MatrixXd ddq_partial_dq_fd(model.nv, model.nv); | ||
2573 | ddq_partial_dq_fd.setZero(); | ||
2574 | MatrixXd ddq_partial_dv_fd(model.nv, model.nv); | ||
2575 | ddq_partial_dv_fd.setZero(); | ||
2576 | MatrixXd ddq_partial_dtau_fd(model.nv, model.nv); | ||
2577 | ddq_partial_dtau_fd.setZero(); | ||
2578 | |||
2579 | MatrixXd lambda_partial_dtau_fd(constraint_dim, model.nv); | ||
2580 | lambda_partial_dtau_fd.setZero(); | ||
2581 | MatrixXd lambda_partial_dq_fd(constraint_dim, model.nv); | ||
2582 | lambda_partial_dq_fd.setZero(); | ||
2583 | MatrixXd lambda_partial_dv_fd(constraint_dim, model.nv); | ||
2584 | lambda_partial_dv_fd.setZero(); | ||
2585 | |||
2586 | const VectorXd ddq0 = constraintDynamics( | ||
2587 | model, data_fd, q, v, tau, constraint_models, constraint_datas, prox_settings); | ||
2588 | const VectorXd lambda0 = data_fd.lambda_c; | ||
2589 | VectorXd v_eps(VectorXd::Zero(model.nv)); | ||
2590 | VectorXd q_plus(model.nq); | ||
2591 | VectorXd ddq_plus(model.nv); | ||
2592 | |||
2593 | VectorXd lambda_plus(constraint_dim); | ||
2594 | const double alpha = 1e-8; | ||
2595 | forwardKinematics(model, data, q, v); | ||
2596 | for (int k = 0; k < model.nv; ++k) | ||
2597 | { | ||
2598 | v_eps[k] += alpha; | ||
2599 | q_plus = integrate(model, q, v_eps); | ||
2600 | ddq_plus = constraintDynamics( | ||
2601 | model, data_fd, q_plus, v, tau, constraint_models, constraint_datas, prox_settings); | ||
2602 | ddq_partial_dq_fd.col(k) = (ddq_plus - ddq0) / alpha; | ||
2603 | lambda_partial_dq_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; | ||
2604 | v_eps[k] = 0.; | ||
2605 | } | ||
2606 | |||
2607 | BOOST_CHECK(ddq_partial_dq_fd.isApprox(data.ddq_dq, sqrt(alpha))); | ||
2608 | |||
2609 | BOOST_CHECK(lambda_partial_dq_fd.isApprox(data.dlambda_dq, sqrt(alpha))); | ||
2610 | |||
2611 | VectorXd v_plus(v); | ||
2612 | for (int k = 0; k < model.nv; ++k) | ||
2613 | { | ||
2614 | v_plus[k] += alpha; | ||
2615 | ddq_plus = constraintDynamics( | ||
2616 | model, data_fd, q, v_plus, tau, constraint_models, constraint_datas, prox_settings); | ||
2617 | ddq_partial_dv_fd.col(k) = (ddq_plus - ddq0) / alpha; | ||
2618 | lambda_partial_dv_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; | ||
2619 | v_plus[k] -= alpha; | ||
2620 | } | ||
2621 | |||
2622 | BOOST_CHECK(ddq_partial_dv_fd.isApprox(data.ddq_dv, sqrt(alpha))); | ||
2623 | BOOST_CHECK(lambda_partial_dv_fd.isApprox(data.dlambda_dv, sqrt(alpha))); | ||
2624 | |||
2625 | VectorXd tau_plus(tau); | ||
2626 | for (int k = 0; k < model.nv; ++k) | ||
2627 | { | ||
2628 | tau_plus[k] += alpha; | ||
2629 | ddq_plus = constraintDynamics( | ||
2630 | model, data_fd, q, v, tau_plus, constraint_models, constraint_datas, prox_settings); | ||
2631 | ddq_partial_dtau_fd.col(k) = (ddq_plus - ddq0) / alpha; | ||
2632 | lambda_partial_dtau_fd.col(k) = (data_fd.lambda_c - lambda0) / alpha; | ||
2633 | tau_plus[k] -= alpha; | ||
2634 | } | ||
2635 | |||
2636 | BOOST_CHECK(lambda_partial_dtau_fd.isApprox(data.dlambda_dtau, sqrt(alpha))); | ||
2637 | BOOST_CHECK(ddq_partial_dtau_fd.isApprox(data.ddq_dtau, sqrt(alpha))); | ||
2638 | } | ||
2639 | |||
2640 | #endif // PINOCCHIO_WITH_SDFORMAT | ||
2641 | |||
2642 | BOOST_AUTO_TEST_SUITE_END() | ||
2643 |