| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2017-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/crba.hpp" |
||
14 |
#include "pinocchio/parsers/sample-models.hpp" |
||
15 |
|||
16 |
#include <iostream> |
||
17 |
|||
18 |
#include <boost/test/unit_test.hpp> |
||
19 |
#include <boost/utility/binary.hpp> |
||
20 |
|||
21 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
22 |
|||
23 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives) |
24 |
{ |
||
25 |
using namespace Eigen; |
||
26 |
using namespace pinocchio; |
||
27 |
|||
28 |
✓✗ | 4 |
Model model; |
29 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
30 |
|||
31 |
✓✗✓✗ |
4 |
Data data(model), data_fd(model); |
32 |
|||
33 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
34 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
35 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
36 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Zero(model.nv)); |
37 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Zero(model.nv)); |
38 |
|||
39 |
/// Check againt non-derivative algo |
||
40 |
✓✗✓✗ |
4 |
MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero(); |
41 |
✓✗ | 2 |
computeGeneralizedGravityDerivatives(model,data,q,g_partial_dq); |
42 |
|||
43 |
✓✗✓✗ |
4 |
VectorXd g0 = computeGeneralizedGravity(model,data_fd,q); |
44 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.g.isApprox(g0)); |
45 |
|||
46 |
✓✗✓✗ |
4 |
MatrixXd g_partial_dq_fd(model.nv,model.nv); g_partial_dq_fd.setZero(); |
47 |
|||
48 |
✓✗✓✗ |
4 |
VectorXd v_eps(Eigen::VectorXd::Zero(model.nv)); |
49 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
50 |
✓✗ | 4 |
VectorXd g_plus(model.nv); |
51 |
2 |
const double alpha = 1e-8; |
|
52 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
53 |
{ |
||
54 |
✓✗ | 64 |
v_eps[k] += alpha; |
55 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
56 |
✓✗✓✗ |
64 |
g_plus = computeGeneralizedGravity(model,data_fd,q_plus); |
57 |
|||
58 |
✓✗✓✗ ✓✗✓✗ |
64 |
g_partial_dq_fd.col(k) = (g_plus - g0)/alpha; |
59 |
✓✗ | 64 |
v_eps[k] -= alpha; |
60 |
} |
||
61 |
|||
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(g_partial_dq.isApprox(g_partial_dq_fd,sqrt(alpha))); |
63 |
2 |
} |
|
64 |
|||
65 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_generalized_gravity_derivatives_fext) |
66 |
{ |
||
67 |
using namespace Eigen; |
||
68 |
using namespace pinocchio; |
||
69 |
|||
70 |
✓✗ | 4 |
Model model; |
71 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
72 |
|||
73 |
✓✗✓✗ |
4 |
Data data(model), data_fd(model); |
74 |
|||
75 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
76 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill( 1.); |
77 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
78 |
|||
79 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; |
||
80 |
✓✗✓✗ |
4 |
ForceVector fext((size_t)model.njoints); |
81 |
✓✓ | 58 |
for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) |
82 |
✓✗ | 56 |
(*it).setRandom(); |
83 |
|||
84 |
// Check againt non-derivative algo |
||
85 |
✓✗✓✗ |
4 |
MatrixXd static_vec_partial_dq(model.nv,model.nv); static_vec_partial_dq.setZero(); |
86 |
✓✗ | 2 |
computeStaticTorqueDerivatives(model,data,q,fext,static_vec_partial_dq); |
87 |
|||
88 |
✓✗✓✗ |
4 |
VectorXd tau0 = computeStaticTorque(model,data_fd,q,fext); |
89 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.tau.isApprox(tau0)); |
90 |
|||
91 |
✓✗✓✗ ✓✗✓✗ |
2 |
std::cout << "data.tau: " << data.tau.transpose() << std::endl; |
92 |
✓✗✓✗ ✓✗✓✗ |
2 |
std::cout << "tau0: " << tau0.transpose() << std::endl; |
93 |
|||
94 |
✓✗ | 4 |
MatrixXd static_vec_partial_dq_fd(model.nv,model.nv); |
95 |
|||
96 |
✓✗✓✗ |
4 |
VectorXd v_eps(Eigen::VectorXd::Zero(model.nv)); |
97 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
98 |
✓✗ | 4 |
VectorXd tau_plus(model.nv); |
99 |
2 |
const double alpha = 1e-8; |
|
100 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
101 |
{ |
||
102 |
✓✗ | 64 |
v_eps[k] += alpha; |
103 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
104 |
✓✗✓✗ |
64 |
tau_plus = computeStaticTorque(model,data_fd,q_plus,fext); |
105 |
|||
106 |
✓✗✓✗ ✓✗✓✗ |
64 |
static_vec_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha; |
107 |
✓✗ | 64 |
v_eps[k] = 0.; |
108 |
} |
||
109 |
|||
110 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(static_vec_partial_dq.isApprox(static_vec_partial_dq_fd,sqrt(alpha))); |
111 |
2 |
} |
|
112 |
|||
113 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_rnea_derivatives) |
114 |
{ |
||
115 |
using namespace Eigen; |
||
116 |
using namespace pinocchio; |
||
117 |
|||
118 |
✓✗ | 4 |
Model model; |
119 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
120 |
|||
121 |
✓✗✓✗ ✓✗ |
4 |
Data data(model), data_fd(model), data_ref(model); |
122 |
|||
123 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
124 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
125 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
126 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
127 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
128 |
|||
129 |
/// Check againt computeGeneralizedGravityDerivatives |
||
130 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero(); |
131 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero(); |
132 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero(); |
133 |
✓✗✓✗ ✓✗ |
2 |
computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da); |
134 |
✓✗✓✗ ✓✗ |
2 |
rnea(model,data_ref,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv)); |
135 |
✓✓ | 56 |
for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
136 |
{ |
||
137 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(data.of[k].isApprox(data.oMi[k].act(data_ref.f[k]))); |
138 |
} |
||
139 |
|||
140 |
✓✗✓✗ |
4 |
MatrixXd g_partial_dq(model.nv,model.nv); g_partial_dq.setZero(); |
141 |
✓✗ | 2 |
computeGeneralizedGravityDerivatives(model,data_ref,q,g_partial_dq); |
142 |
|||
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dFdq.isApprox(data_ref.dFdq)); |
144 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dq.isApprox(g_partial_dq)); |
145 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.tau.isApprox(data_ref.g)); |
146 |
|||
147 |
✓✗✓✗ ✓✗✓✗ |
4 |
VectorXd tau0 = rnea(model,data_fd,q,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv)); |
148 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero(); |
149 |
|||
150 |
✓✗✓✗ |
4 |
VectorXd v_eps(VectorXd::Zero(model.nv)); |
151 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
152 |
✓✗ | 4 |
VectorXd tau_plus(model.nv); |
153 |
2 |
const double alpha = 1e-8; |
|
154 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
155 |
{ |
||
156 |
✓✗ | 64 |
v_eps[k] += alpha; |
157 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
158 |
✓✗✓✗ ✓✗✓✗ |
64 |
tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),VectorXd::Zero(model.nv)); |
159 |
|||
160 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha; |
161 |
✓✗ | 64 |
v_eps[k] -= alpha; |
162 |
} |
||
163 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha))); |
164 |
|||
165 |
// Check with q and a non zero |
||
166 |
✓✗✓✗ ✓✗ |
2 |
tau0 = rnea(model,data_fd,q,0*v,a); |
167 |
✓✗ | 2 |
rnea_partial_dq_fd.setZero(); |
168 |
|||
169 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
170 |
{ |
||
171 |
✓✗ | 64 |
v_eps[k] += alpha; |
172 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
173 |
✓✗✓✗ ✓✗ |
64 |
tau_plus = rnea(model,data_fd,q_plus,VectorXd::Zero(model.nv),a); |
174 |
|||
175 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha; |
176 |
✓✗ | 64 |
v_eps[k] -= alpha; |
177 |
} |
||
178 |
|||
179 |
✓✗ | 2 |
rnea_partial_dq.setZero(); |
180 |
✓✗✓✗ |
2 |
computeRNEADerivatives(model,data,q,VectorXd::Zero(model.nv),a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da); |
181 |
✓✗✓✗ |
2 |
forwardKinematics(model,data_ref,q,VectorXd::Zero(model.nv),a); |
182 |
|||
183 |
✓✓ | 56 |
for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
184 |
{ |
||
185 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.a[k].isApprox(data_ref.a[k])); |
186 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
187 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
188 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(data.oh[k].isApprox(Force::Zero())); |
189 |
} |
||
190 |
|||
191 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.tau.isApprox(tau0)); |
192 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha))); |
193 |
|||
194 |
// Check with q and v non zero |
||
195 |
✓✗ | 2 |
const Motion gravity(model.gravity); |
196 |
✓✗ | 2 |
model.gravity.setZero(); |
197 |
✓✗✓✗ ✓✗ |
2 |
tau0 = rnea(model,data_fd,q,v,VectorXd::Zero(model.nv)); |
198 |
✓✗ | 2 |
rnea_partial_dq_fd.setZero(); |
199 |
|||
200 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
201 |
{ |
||
202 |
✓✗ | 64 |
v_eps[k] += alpha; |
203 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
204 |
✓✗✓✗ ✓✗ |
64 |
tau_plus = rnea(model,data_fd,q_plus,v,VectorXd::Zero(model.nv)); |
205 |
|||
206 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha; |
207 |
✓✗ | 64 |
v_eps[k] -= alpha; |
208 |
} |
||
209 |
|||
210 |
✓✗ | 4 |
VectorXd v_plus(v); |
211 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero(); |
212 |
|||
213 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
214 |
{ |
||
215 |
✓✗ | 64 |
v_plus[k] += alpha; |
216 |
✓✗✓✗ ✓✗ |
64 |
tau_plus = rnea(model,data_fd,q,v_plus,VectorXd::Zero(model.nv)); |
217 |
|||
218 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha; |
219 |
✓✗ | 64 |
v_plus[k] -= alpha; |
220 |
} |
||
221 |
|||
222 |
✓✗ | 2 |
rnea_partial_dq.setZero(); |
223 |
✓✗ | 2 |
rnea_partial_dv.setZero(); |
224 |
✓✗✓✗ |
2 |
computeRNEADerivatives(model,data,q,v,VectorXd::Zero(model.nv),rnea_partial_dq,rnea_partial_dv,rnea_partial_da); |
225 |
✓✗✓✗ |
2 |
forwardKinematics(model,data_ref,q,v,VectorXd::Zero(model.nv)); |
226 |
|||
227 |
✓✓ | 56 |
for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
228 |
{ |
||
229 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.a[k].isApprox(data_ref.a[k])); |
230 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
231 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
232 |
} |
||
233 |
|||
234 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.tau.isApprox(tau0)); |
235 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha))); |
236 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha))); |
237 |
|||
238 |
// std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.block<10,10>(0,0) << std::endl; |
||
239 |
// std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.block<10,10>(0,0) << std::endl; |
||
240 |
// std::cout << "rnea_partial_dv:\n" << rnea_partial_dv.topRows<10>() << std::endl; |
||
241 |
// std::cout << "rnea_partial_dv ref:\n" << rnea_partial_dv_fd.topRows<10>() << std::endl; |
||
242 |
// Check with q, v and a non zero |
||
243 |
✓✗ | 2 |
model.gravity = gravity; |
244 |
✓✗ | 2 |
v_plus = v; |
245 |
✓✗✓✗ |
2 |
tau0 = rnea(model,data_fd,q,v,a); |
246 |
✓✗ | 2 |
rnea_partial_dq_fd.setZero(); |
247 |
|||
248 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
249 |
{ |
||
250 |
✓✗ | 64 |
v_eps[k] += alpha; |
251 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
252 |
✓✗✓✗ |
64 |
tau_plus = rnea(model,data_fd,q_plus,v,a); |
253 |
|||
254 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dq_fd.col(k) = (tau_plus - tau0)/alpha; |
255 |
✓✗ | 64 |
v_eps[k] -= alpha; |
256 |
} |
||
257 |
|||
258 |
✓✗ | 2 |
rnea_partial_dv_fd.setZero(); |
259 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
260 |
{ |
||
261 |
✓✗ | 64 |
v_plus[k] += alpha; |
262 |
✓✗✓✗ |
64 |
tau_plus = rnea(model,data_fd,q,v_plus,a); |
263 |
|||
264 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dv_fd.col(k) = (tau_plus - tau0)/alpha; |
265 |
✓✗ | 64 |
v_plus[k] -= alpha; |
266 |
} |
||
267 |
|||
268 |
✓✗ | 2 |
rnea_partial_dq.setZero(); |
269 |
✓✗ | 2 |
rnea_partial_dv.setZero(); |
270 |
✓✗ | 2 |
computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da); |
271 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v,a); |
272 |
|||
273 |
✓✓ | 56 |
for(Model::JointIndex k = 1; k < (Model::JointIndex)model.njoints; ++k) |
274 |
{ |
||
275 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.a[k].isApprox(data_ref.a[k])); |
276 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
277 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
278 |
} |
||
279 |
|||
280 |
✓✗ | 2 |
computeJointJacobiansTimeVariation(model,data_ref,q,v); |
281 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
282 |
✓✗ | 2 |
crba(model,data_ref,q); |
283 |
|||
284 |
✓✗ | 2 |
rnea_partial_da.triangularView<Eigen::StrictlyLower>() |
285 |
✓✗✓✗ ✓✗ |
4 |
= rnea_partial_da.transpose().triangularView<Eigen::StrictlyLower>(); |
286 |
✓✗ | 2 |
data_ref.M.triangularView<Eigen::StrictlyLower>() |
287 |
✓✗✓✗ ✓✗ |
4 |
= data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M)); |
289 |
|||
290 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.tau.isApprox(tau0)); |
291 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(alpha))); |
292 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(alpha))); |
293 |
|||
294 |
✓✗ | 4 |
Data data2(model); |
295 |
✓✗ | 2 |
computeRNEADerivatives(model,data2,q,v,a); |
296 |
✓✗ | 2 |
data2.M.triangularView<Eigen::StrictlyLower>() |
297 |
✓✗✓✗ ✓✗ |
4 |
= data2.M.transpose().triangularView<Eigen::StrictlyLower>(); |
298 |
|||
299 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dq.isApprox(data2.dtau_dq)); |
300 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dv.isApprox(data2.dtau_dv)); |
301 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_da.isApprox(data2.M)); |
302 |
|||
303 |
2 |
} |
|
304 |
|||
305 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_fext) |
306 |
{ |
||
307 |
using namespace Eigen; |
||
308 |
using namespace pinocchio; |
||
309 |
|||
310 |
✓✗ | 4 |
Model model; |
311 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
312 |
typedef Model::Force Force; |
||
313 |
|||
314 |
✓✗✓✗ ✓✗ |
4 |
Data data(model), data_fd(model), data_ref(model); |
315 |
|||
316 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
317 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
318 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
319 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
320 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
321 |
|||
322 |
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceVector; |
||
323 |
✓✗✓✗ |
4 |
ForceVector fext((size_t)model.njoints); |
324 |
✓✓ | 58 |
for(ForceVector::iterator it = fext.begin(); it != fext.end(); ++it) |
325 |
✓✗ | 56 |
(*it).setRandom(); |
326 |
|||
327 |
/// Check againt computeGeneralizedGravityDerivatives |
||
328 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero(); |
329 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero(); |
330 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero(); |
331 |
|||
332 |
✓✗ | 2 |
computeRNEADerivatives(model,data,q,v,a,fext,rnea_partial_dq,rnea_partial_dv,rnea_partial_da); |
333 |
✓✗ | 2 |
rnea(model,data_ref,q,v,a,fext); |
334 |
|||
335 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.tau.isApprox(data_ref.tau)); |
336 |
|||
337 |
✓✗ | 2 |
computeRNEADerivatives(model,data_ref,q,v,a); |
338 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dv.isApprox(data_ref.dtau_dv)); |
339 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_da.isApprox(data_ref.M)); |
340 |
|||
341 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dq_fd(model.nv,model.nv); rnea_partial_dq_fd.setZero(); |
342 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dv_fd(model.nv,model.nv); rnea_partial_dv_fd.setZero(); |
343 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_da_fd(model.nv,model.nv); rnea_partial_da_fd.setZero(); |
344 |
|||
345 |
✓✗✓✗ |
4 |
VectorXd v_eps(VectorXd::Zero(model.nv)); |
346 |
✓✗ | 4 |
VectorXd q_plus(model.nq); |
347 |
✓✗ | 4 |
VectorXd tau_plus(model.nv); |
348 |
2 |
const double eps = 1e-8; |
|
349 |
|||
350 |
✓✗✓✗ |
4 |
const VectorXd tau_ref = rnea(model,data_ref,q,v,a,fext); |
351 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
352 |
{ |
||
353 |
✓✗ | 64 |
v_eps[k] = eps; |
354 |
✓✗ | 64 |
q_plus = integrate(model,q,v_eps); |
355 |
✓✗✓✗ |
64 |
tau_plus = rnea(model,data_fd,q_plus,v,a,fext); |
356 |
|||
357 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dq_fd.col(k) = (tau_plus - tau_ref) / eps; |
358 |
|||
359 |
✓✗ | 64 |
v_eps[k] = 0.; |
360 |
} |
||
361 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dq.isApprox(rnea_partial_dq_fd,sqrt(eps))); |
362 |
|||
363 |
✓✗ | 4 |
VectorXd v_plus(v); |
364 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
365 |
{ |
||
366 |
✓✗ | 64 |
v_plus[k] += eps; |
367 |
|||
368 |
✓✗✓✗ |
64 |
tau_plus = rnea(model,data_fd,q,v_plus,a,fext); |
369 |
|||
370 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_dv_fd.col(k) = (tau_plus - tau_ref) / eps; |
371 |
|||
372 |
✓✗ | 64 |
v_plus[k] -= eps; |
373 |
} |
||
374 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_dv.isApprox(rnea_partial_dv_fd,sqrt(eps))); |
375 |
|||
376 |
✓✗ | 4 |
VectorXd a_plus(a); |
377 |
✓✓ | 66 |
for(int k = 0; k < model.nv; ++k) |
378 |
{ |
||
379 |
✓✗ | 64 |
a_plus[k] += eps; |
380 |
|||
381 |
✓✗✓✗ |
64 |
tau_plus = rnea(model,data_fd,q,v,a_plus,fext); |
382 |
|||
383 |
✓✗✓✗ ✓✗✓✗ |
64 |
rnea_partial_da_fd.col(k) = (tau_plus - tau_ref) / eps; |
384 |
|||
385 |
✓✗ | 64 |
a_plus[k] -= eps; |
386 |
} |
||
387 |
|||
388 |
✓✗✓✗ ✓✗✓✗ |
2 |
rnea_partial_da.triangularView<Eigen::Lower>() = rnea_partial_da.transpose().triangularView<Eigen::Lower>(); |
389 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(rnea_partial_da.isApprox(rnea_partial_da_fd,sqrt(eps))); |
390 |
|||
391 |
// test the shortcut |
||
392 |
✓✗ | 4 |
Data data_shortcut(model); |
393 |
✓✗ | 2 |
computeRNEADerivatives(model,data_shortcut,q,v,a,fext); |
394 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_shortcut.dtau_dq.isApprox(rnea_partial_dq)); |
395 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_shortcut.dtau_dv.isApprox(rnea_partial_dv)); |
396 |
✓✗✓✗ ✓✗✓✗ |
2 |
data_shortcut.M.triangularView<Eigen::Lower>() = data_shortcut.M.transpose().triangularView<Eigen::Lower>(); |
397 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_shortcut.M.isApprox(rnea_partial_da)); |
398 |
2 |
} |
|
399 |
|||
400 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_rnea_derivatives_vs_kinematics_derivatives) |
401 |
{ |
||
402 |
using namespace Eigen; |
||
403 |
using namespace pinocchio; |
||
404 |
|||
405 |
✓✗ | 4 |
Model model; |
406 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
407 |
|||
408 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
409 |
|||
410 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
411 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
412 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
413 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
414 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
415 |
|||
416 |
/// Check againt computeGeneralizedGravityDerivatives |
||
417 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dq(model.nv,model.nv); rnea_partial_dq.setZero(); |
418 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_dv(model.nv,model.nv); rnea_partial_dv.setZero(); |
419 |
✓✗✓✗ |
4 |
MatrixXd rnea_partial_da(model.nv,model.nv); rnea_partial_da.setZero(); |
420 |
|||
421 |
✓✗ | 2 |
computeRNEADerivatives(model,data,q,v,a,rnea_partial_dq,rnea_partial_dv,rnea_partial_da); |
422 |
✓✗ | 2 |
computeForwardKinematicsDerivatives(model,data_ref,q,v,a); |
423 |
|||
424 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
425 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
426 |
|||
427 |
✓✓ | 56 |
for(size_t k = 1; k < (size_t)model.njoints; ++k) |
428 |
{ |
||
429 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oMi[k].isApprox(data_ref.oMi[k])); |
430 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.ov[k].isApprox(data_ref.ov[k])); |
431 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oa[k].isApprox(data_ref.oa[k])); |
432 |
} |
||
433 |
2 |
} |
|
434 |
|||
435 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_multiple_calls) |
436 |
{ |
||
437 |
using namespace Eigen; |
||
438 |
using namespace pinocchio; |
||
439 |
|||
440 |
✓✗ | 4 |
Model model; |
441 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
442 |
|||
443 |
✓✗✓✗ |
4 |
Data data1(model), data2(model); |
444 |
|||
445 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
446 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
447 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
448 |
✓✗✓✗ |
4 |
VectorXd v(VectorXd::Random(model.nv)); |
449 |
✓✗✓✗ |
4 |
VectorXd a(VectorXd::Random(model.nv)); |
450 |
|||
451 |
✓✗ | 2 |
computeRNEADerivatives(model,data1,q,v,a); |
452 |
✓✗ | 2 |
data2 = data1; |
453 |
|||
454 |
✓✓ | 42 |
for(int k = 0; k < 20; ++k) |
455 |
{ |
||
456 |
✓✗ | 40 |
computeRNEADerivatives(model,data1,q,v,a); |
457 |
} |
||
458 |
|||
459 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.J.isApprox(data2.J)); |
460 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dJ.isApprox(data2.dJ)); |
461 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dVdq.isApprox(data2.dVdq)); |
462 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dAdq.isApprox(data2.dAdq)); |
463 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dAdv.isApprox(data2.dAdv)); |
464 |
|||
465 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dFdq.isApprox(data2.dFdq)); |
466 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dFdv.isApprox(data2.dFdv)); |
467 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dFda.isApprox(data2.dFda)); |
468 |
|||
469 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dtau_dq.isApprox(data2.dtau_dq)); |
470 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.dtau_dv.isApprox(data2.dtau_dv)); |
471 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data1.M.isApprox(data2.M)); |
472 |
2 |
} |
|
473 |
|||
474 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_get_coriolis) |
475 |
{ |
||
476 |
using namespace Eigen; |
||
477 |
using namespace pinocchio; |
||
478 |
|||
479 |
✓✗ | 4 |
Model model; |
480 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
481 |
|||
482 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
483 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill( 1.); |
484 |
|||
485 |
✓✗ | 4 |
Data data_ref(model); |
486 |
✓✗ | 4 |
Data data(model); |
487 |
|||
488 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
489 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Random(model.nv); |
490 |
✓✗✓✗ |
4 |
VectorXd tau = VectorXd::Random(model.nv); |
491 |
|||
492 |
✓✗ | 2 |
computeCoriolisMatrix(model,data_ref,q,v); |
493 |
|||
494 |
✓✗ | 2 |
computeRNEADerivatives(model,data,q,v,tau); |
495 |
✓✗ | 2 |
getCoriolisMatrix(model,data); |
496 |
|||
497 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
498 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
499 |
✓✓ | 56 |
for(JointIndex k = 1; k < model.joints.size(); ++k) |
500 |
{ |
||
501 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.B[k].isApprox(data_ref.B[k])); |
502 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.oYcrb[k].isApprox(data_ref.oYcrb[k])); |
503 |
} |
||
504 |
|||
505 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.C.isApprox(data_ref.C)); |
506 |
2 |
} |
|
507 |
|||
508 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |