| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/multibody/model.hpp" |
||
6 |
#include "pinocchio/multibody/data.hpp" |
||
7 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
8 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
9 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
10 |
#include "pinocchio/algorithm/kinematics-derivatives.hpp" |
||
11 |
#include "pinocchio/algorithm/rnea.hpp" |
||
12 |
#include "pinocchio/algorithm/rnea-derivatives.hpp" |
||
13 |
#include "pinocchio/algorithm/aba.hpp" |
||
14 |
#include "pinocchio/algorithm/aba-derivatives.hpp" |
||
15 |
#include "pinocchio/algorithm/crba.hpp" |
||
16 |
#include "pinocchio/parsers/sample-models.hpp" |
||
17 |
|||
18 |
#include <iostream> |
||
19 |
|||
20 |
#include <boost/test/unit_test.hpp> |
||
21 |
#include <boost/utility/binary.hpp> |
||
22 |
|||
23 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
24 |
|||
25 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_aba_derivatives) |
26 |
{ |
||
27 |
using namespace Eigen; |
||
28 |
using namespace pinocchio; |
||
29 |
|||
30 |
✓✗ | 4 |
Model model; |
31 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
32 |
|||
33 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
34 |
|||
35 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
36 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
37 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
38 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
39 |
✓✗✓✗ |
4 |
VectorXd tau(VectorXd::Random(model.nv)); |
40 |
✓✗✓✗ |
4 |
VectorXd a(aba(model,data_ref,q,v,tau)); |
41 |
|||
42 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); |
43 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); |
44 |
✓✗✓✗ |
4 |
Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); |
45 |
|||
46 |
✓✗ | 2 |
computeABADerivatives(model, data, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
47 |
✓✗ | 2 |
computeRNEADerivatives(model,data_ref,q,v,a); |
48 |
✓✓ | 56 |
for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
49 |
{ |
||
50 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
52 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); |
53 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oa_gf[k].isApprox(data_ref.oa_gf[k])); |
54 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.of[k].isApprox(data_ref.of[k])); |
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k])); |
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.doYcrb[k].isApprox(data_ref.doYcrb[k])); |
57 |
} |
||
58 |
|||
59 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
61 |
|||
62 |
✓✗ | 2 |
aba(model,data_ref,q,v,tau); |
63 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
64 |
|||
65 |
✓✗ | 2 |
computeMinverse(model,data_ref,q); |
66 |
✓✗ | 2 |
data_ref.Minv.triangularView<Eigen::StrictlyLower>() |
67 |
✓✗✓✗ ✓✗ |
4 |
= data_ref.Minv.transpose().triangularView<Eigen::StrictlyLower>(); |
68 |
|||
69 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv)); |
70 |
|||
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
73 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); |
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); |
76 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq)); |
77 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv)); |
78 |
|||
79 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dq_fd(model.nv,model.nv); aba_partial_dq_fd.setZero(); |
80 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dv_fd(model.nv,model.nv); aba_partial_dv_fd.setZero(); |
81 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dtau_fd(model.nv,model.nv); aba_partial_dtau_fd.setZero(); |
82 |
|||
83 |
✓✗ | 4 |
Data data_fd(model); |
84 |
✓✗✓✗ |
4 |
VectorXd a0 = aba(model,data_fd,q,v,tau); |
85 |
✓✗✓✗ |
4 |
VectorXd v_eps(VectorXd::Zero(model.nv)); |
86 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
87 |
✓✗ | 4 |
VectorXd a_plus(model.nv); |
88 |
2 |
const double alpha = 1e-8; |
|
89 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
90 |
{ |
||
91 |
✓✗ | 64 |
v_eps[k] += alpha; |
92 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
93 |
✓✗✓✗ |
64 |
a_plus = aba(model,data_fd,q_plus,v,tau); |
94 |
|||
95 |
✓✗✓✗ ✓✗✓✗ |
64 |
aba_partial_dq_fd.col(k) = (a_plus - a0)/alpha; |
96 |
✓✗ | 64 |
v_eps[k] -= alpha; |
97 |
} |
||
98 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd,sqrt(alpha))); |
99 |
|||
100 |
✓✗ | 4 |
VectorXd v_plus(v); |
101 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
102 |
{ |
||
103 |
✓✗ | 64 |
v_plus[k] += alpha; |
104 |
✓✗✓✗ |
64 |
a_plus = aba(model,data_fd,q,v_plus,tau); |
105 |
|||
106 |
✓✗✓✗ ✓✗✓✗ |
64 |
aba_partial_dv_fd.col(k) = (a_plus - a0)/alpha; |
107 |
✓✗ | 64 |
v_plus[k] -= alpha; |
108 |
} |
||
109 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd,sqrt(alpha))); |
110 |
|||
111 |
✓✗ | 4 |
VectorXd tau_plus(tau); |
112 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
113 |
{ |
||
114 |
✓✗ | 64 |
tau_plus[k] += alpha; |
115 |
✓✗✓✗ |
64 |
a_plus = aba(model,data_fd,q,v,tau_plus); |
116 |
|||
117 |
✓✗✓✗ ✓✗✓✗ |
64 |
aba_partial_dtau_fd.col(k) = (a_plus - a0)/alpha; |
118 |
✓✗ | 64 |
tau_plus[k] -= alpha; |
119 |
} |
||
120 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd,sqrt(alpha))); |
121 |
2 |
} |
|
122 |
|||
123 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_aba_minimal_argument) |
124 |
{ |
||
125 |
using namespace Eigen; |
||
126 |
using namespace pinocchio; |
||
127 |
|||
128 |
✓✗ | 4 |
Model model; |
129 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
130 |
|||
131 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
132 |
|||
133 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
134 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
135 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
136 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
137 |
✓✗✓✗ |
4 |
VectorXd tau(VectorXd::Random(model.nv)); |
138 |
✓✗✓✗ |
4 |
VectorXd a(aba(model,data_ref,q,v,tau)); |
139 |
|||
140 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); |
141 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); |
142 |
✓✗✓✗ |
4 |
Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); |
143 |
|||
144 |
✓✗ | 2 |
computeABADerivatives(model, data_ref, q, v, tau, aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
145 |
|||
146 |
✓✗ | 2 |
computeABADerivatives(model, data, q, v, tau); |
147 |
|||
148 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
149 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
150 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dVdq.isApprox(data_ref.dVdq)); |
151 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dAdq.isApprox(data_ref.dAdq)); |
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dAdv.isApprox(data_ref.dAdv)); |
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dtau_dq.isApprox(data_ref.dtau_dq)); |
154 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dtau_dv.isApprox(data_ref.dtau_dv)); |
155 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Minv.isApprox(aba_partial_dtau)); |
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.ddq_dq.isApprox(aba_partial_dq)); |
157 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.ddq_dv.isApprox(aba_partial_dv)); |
158 |
2 |
} |
|
159 |
|||
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_aba_derivatives_fext) |
161 |
{ |
||
162 |
using namespace Eigen; |
||
163 |
using namespace pinocchio; |
||
164 |
|||
165 |
✓✗ | 4 |
Model model; |
166 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
167 |
|||
168 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
169 |
|||
170 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
171 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
172 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
173 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
174 |
✓✗✓✗ |
4 |
VectorXd tau(VectorXd::Random(model.nv)); |
175 |
✓✗✓✗ |
4 |
VectorXd a(aba(model,data_ref,q,v,tau)); |
176 |
|||
177 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; |
||
178 |
✓✗✓✗ |
4 |
ForceVector fext((size_t)model.njoints); |
179 |
✓✓ | 58 |
for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) |
180 |
✓✗ | 56 |
(*it).setRandom(); |
181 |
|||
182 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); |
183 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); |
184 |
✓✗✓✗ |
4 |
Data::RowMatrixXs aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); |
185 |
|||
186 |
✓✗ | 2 |
computeABADerivatives(model, data, q, v, tau, fext, |
187 |
aba_partial_dq, aba_partial_dv, aba_partial_dtau); |
||
188 |
|||
189 |
✓✗ | 2 |
aba(model,data_ref,q,v,tau,fext); |
190 |
// updateGlobalPlacements(model, data_ref); |
||
191 |
// for(size_t k =1; k < (size_t)model.njoints; ++k) |
||
192 |
// { |
||
193 |
// BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
||
194 |
// BOOST_CHECK(daita.of[k].isApprox(data_ref.oMi[k].act(data.f[k]))); |
||
195 |
// |
||
196 |
// } |
||
197 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.ddq.isApprox(data_ref.ddq)); |
198 |
|||
199 |
✓✗ | 2 |
computeABADerivatives(model,data_ref,q,v,tau); |
200 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dv.isApprox(data_ref.ddq_dv)); |
201 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dtau.isApprox(data_ref.Minv)); |
202 |
|||
203 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dq_fd(model.nv,model.nv); aba_partial_dq_fd.setZero(); |
204 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dv_fd(model.nv,model.nv); aba_partial_dv_fd.setZero(); |
205 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dtau_fd(model.nv,model.nv); aba_partial_dtau_fd.setZero(); |
206 |
|||
207 |
✓✗ | 4 |
Data data_fd(model); |
208 |
✓✗✓✗ |
4 |
const VectorXd a0 = aba(model,data_fd,q,v,tau,fext); |
209 |
✓✗✓✗ |
4 |
VectorXd v_eps(VectorXd::Zero(model.nv)); |
210 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
211 |
✓✗ | 4 |
VectorXd a_plus(model.nv); |
212 |
2 |
const double alpha = 1e-8; |
|
213 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
214 |
{ |
||
215 |
✓✗ | 64 |
v_eps[k] += alpha; |
216 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
217 |
✓✗✓✗ |
64 |
a_plus = aba(model,data_fd,q_plus,v,tau,fext); |
218 |
|||
219 |
✓✗✓✗ ✓✗✓✗ |
64 |
aba_partial_dq_fd.col(k) = (a_plus - a0)/alpha; |
220 |
✓✗ | 64 |
v_eps[k] -= alpha; |
221 |
} |
||
222 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dq.isApprox(aba_partial_dq_fd,sqrt(alpha))); |
223 |
|||
224 |
✓✗ | 4 |
VectorXd v_plus(v); |
225 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
226 |
{ |
||
227 |
✓✗ | 64 |
v_plus[k] += alpha; |
228 |
✓✗✓✗ |
64 |
a_plus = aba(model,data_fd,q,v_plus,tau,fext); |
229 |
|||
230 |
✓✗✓✗ ✓✗✓✗ |
64 |
aba_partial_dv_fd.col(k) = (a_plus - a0)/alpha; |
231 |
✓✗ | 64 |
v_plus[k] -= alpha; |
232 |
} |
||
233 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dv.isApprox(aba_partial_dv_fd,sqrt(alpha))); |
234 |
|||
235 |
✓✗ | 4 |
VectorXd tau_plus(tau); |
236 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
237 |
{ |
||
238 |
✓✗ | 64 |
tau_plus[k] += alpha; |
239 |
✓✗✓✗ |
64 |
a_plus = aba(model,data_fd,q,v,tau_plus,fext); |
240 |
|||
241 |
✓✗✓✗ ✓✗✓✗ |
64 |
aba_partial_dtau_fd.col(k) = (a_plus - a0)/alpha; |
242 |
✓✗ | 64 |
tau_plus[k] -= alpha; |
243 |
} |
||
244 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(aba_partial_dtau.isApprox(aba_partial_dtau_fd,sqrt(alpha))); |
245 |
|||
246 |
// test the shortcut |
||
247 |
✓✗ | 4 |
Data data_shortcut(model); |
248 |
✓✗ | 2 |
computeABADerivatives(model,data_shortcut,q,v,tau,fext); |
249 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_shortcut.ddq_dq.isApprox(aba_partial_dq)); |
250 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_shortcut.ddq_dv.isApprox(aba_partial_dv)); |
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_shortcut.Minv.isApprox(aba_partial_dtau)); |
252 |
2 |
} |
|
253 |
|||
254 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_multiple_calls) |
255 |
{ |
||
256 |
using namespace Eigen; |
||
257 |
using namespace pinocchio; |
||
258 |
|||
259 |
✓✗ | 4 |
Model model; |
260 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
261 |
|||
262 |
✓✗✓✗ |
4 |
Data data1(model), data2(model); |
263 |
|||
264 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
265 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
266 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
267 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
268 |
✓✗✓✗ |
4 |
VectorXd tau(VectorXd::Random(model.nv)); |
269 |
|||
270 |
✓✗ | 2 |
computeABADerivatives(model,data1,q,v,tau); |
271 |
✓✗ | 2 |
data2 = data1; |
272 |
|||
273 |
✓✓ | 42 |
for(int k = 0; k < 20; ++k) |
274 |
{ |
||
275 |
✓✗ | 40 |
computeABADerivatives(model,data1,q,v,tau); |
276 |
} |
||
277 |
|||
278 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.J.isApprox(data2.J)); |
279 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dJ.isApprox(data2.dJ)); |
280 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq)); |
281 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq)); |
282 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv)); |
283 |
|||
284 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq)); |
285 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv)); |
286 |
|||
287 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq)); |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv)); |
289 |
|||
290 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.ddq_dq.isApprox(data2.ddq_dq)); |
291 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.ddq_dv.isApprox(data2.ddq_dv)); |
292 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.Minv.isApprox(data2.Minv)); |
293 |
2 |
} |
|
294 |
|||
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_aba_derivatives_vs_kinematics_derivatives) |
296 |
{ |
||
297 |
using namespace Eigen; |
||
298 |
using namespace pinocchio; |
||
299 |
|||
300 |
✓✗ | 4 |
Model model; |
301 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
302 |
|||
303 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
304 |
|||
305 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
306 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
307 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
308 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
309 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
310 |
|||
311 |
✓✗✓✗ |
4 |
VectorXd tau = rnea(model,data_ref,q,v,a); |
312 |
|||
313 |
/// Check againt computeGeneralizedGravityDerivatives |
||
314 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dq(model.nv,model.nv); aba_partial_dq.setZero(); |
315 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dv(model.nv,model.nv); aba_partial_dv.setZero(); |
316 |
✓✗✓✗ |
4 |
MatrixXd aba_partial_dtau(model.nv,model.nv); aba_partial_dtau.setZero(); |
317 |
|||
318 |
✓✗ | 2 |
computeABADerivatives(model,data,q,v,tau,aba_partial_dq,aba_partial_dv,aba_partial_dtau); |
319 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
320 |
|||
321 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
322 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
323 |
|||
324 |
✓✓ | 56 |
for(size_t k = 1; k < (size_t)model.njoints; ++k) |
325 |
{ |
||
326 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
327 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); |
328 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k])); |
329 |
} |
||
330 |
2 |
} |
|
331 |
|||
332 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |