GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2016-2020 CNRS, INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/spatial/se3.hpp" |
||
6 |
#include "pinocchio/multibody/model.hpp" |
||
7 |
#include "pinocchio/multibody/data.hpp" |
||
8 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
9 |
#include "pinocchio/algorithm/contact-dynamics.hpp" |
||
10 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
11 |
#include "pinocchio/parsers/sample-models.hpp" |
||
12 |
#include "pinocchio/utils/timer.hpp" |
||
13 |
|||
14 |
#include <iostream> |
||
15 |
|||
16 |
#include <boost/test/unit_test.hpp> |
||
17 |
#include <boost/utility/binary.hpp> |
||
18 |
|||
19 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
20 |
|||
21 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_FD ) |
22 |
{ |
||
23 |
using namespace Eigen; |
||
24 |
using namespace pinocchio; |
||
25 |
|||
26 |
✓✗ | 4 |
pinocchio::Model model; |
27 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
28 |
✓✗ | 4 |
pinocchio::Data data(model); |
29 |
|||
30 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
31 |
✓✗✓✗ |
2 |
q.segment <4> (3).normalize(); |
32 |
|||
33 |
✓✗ | 2 |
pinocchio::computeJointJacobians(model, data, q); |
34 |
|||
35 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
36 |
✓✗✓✗ |
4 |
VectorXd tau = VectorXd::Zero(model.nv); |
37 |
|||
38 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
39 |
✓✗ | 4 |
const std::string LF = "lleg6_joint"; |
40 |
|||
41 |
✓✗ | 4 |
Data::Matrix6x J_RF (6, model.nv); |
42 |
✓✗ | 2 |
J_RF.setZero(); |
43 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF); |
44 |
✓✗ | 4 |
Data::Matrix6x J_LF (6, model.nv); |
45 |
✓✗ | 2 |
J_LF.setZero(); |
46 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF); |
47 |
|||
48 |
✓✗ | 4 |
Eigen::MatrixXd J (12, model.nv); |
49 |
✓✗ | 2 |
J.setZero(); |
50 |
✓✗✓✗ |
2 |
J.topRows<6> () = J_RF; |
51 |
✓✗✓✗ |
2 |
J.bottomRows<6> () = J_LF; |
52 |
|||
53 |
✓✗✓✗ |
4 |
Eigen::VectorXd gamma (VectorXd::Ones(12)); |
54 |
|||
55 |
✓✗✓✗ |
4 |
Eigen::MatrixXd H(J.transpose()); |
56 |
|||
57 |
✓✗ | 2 |
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.); |
58 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
59 |
|||
60 |
✓✗✓✗ |
4 |
MatrixXd Minv (data.M.inverse()); |
61 |
✓✗✓✗ ✓✗✓✗ |
4 |
MatrixXd JMinvJt (J * Minv * J.transpose()); |
62 |
|||
63 |
✓✗✓✗ |
4 |
Eigen::MatrixXd G_ref(J.transpose()); |
64 |
✓✗ | 2 |
cholesky::Uiv(model, data, G_ref); |
65 |
✓✓✓✗ ✓✗✓✗ |
66 |
for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]); |
66 |
✓✗✓✗ ✓✗ |
4 |
Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref); |
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12)); |
68 |
|||
69 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
VectorXd lambda_ref = -JMinvJt.inverse() * (J*Minv*(tau - data.nle) + gamma); |
70 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.lambda_c.isApprox(lambda_ref, 1e-12)); |
71 |
|||
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
VectorXd a_ref = Minv*(tau - data.nle + J.transpose()*lambda_ref); |
73 |
|||
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd dynamics_residual_ref (data.M * a_ref + data.nle - tau - J.transpose()*lambda_ref); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dynamics_residual_ref.norm() <= 1e-11); // previously 1e-12, may be due to numerical approximations, i obtain 2.03e-12 |
76 |
|||
77 |
✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd constraint_residual (J * data.ddq + gamma); |
78 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(constraint_residual.norm() <= 1e-12); |
79 |
|||
80 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c); |
81 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dynamics_residual.norm() <= 1e-12); |
82 |
|||
83 |
2 |
} |
|
84 |
|||
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_computeKKTMatrix) |
86 |
{ |
||
87 |
using namespace Eigen; |
||
88 |
using namespace pinocchio; |
||
89 |
✓✗ | 4 |
pinocchio::Model model; |
90 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
91 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_ref(model); |
92 |
|||
93 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
94 |
✓✗✓✗ |
2 |
q.segment<4>(3).normalize(); |
95 |
|||
96 |
✓✗ | 2 |
pinocchio::computeJointJacobians(model, data_ref, q); |
97 |
|||
98 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
99 |
✓✗ | 4 |
const std::string LF = "lleg6_joint"; |
100 |
|||
101 |
✓✗ | 4 |
Data::Matrix6x J_RF(6, model.nv); |
102 |
✓✗ | 2 |
J_RF.setZero(); |
103 |
✓✗✓✗ |
2 |
getJointJacobian(model, data_ref, model.getJointId(RF), LOCAL, J_RF); |
104 |
✓✗ | 4 |
Data::Matrix6x J_LF(6, model.nv); |
105 |
✓✗ | 2 |
J_LF.setZero(); |
106 |
✓✗✓✗ |
2 |
getJointJacobian(model, data_ref, model.getJointId(LF), LOCAL, J_LF); |
107 |
|||
108 |
✓✗ | 4 |
Eigen::MatrixXd J(12, model.nv); |
109 |
✓✗ | 2 |
J.setZero(); |
110 |
✓✗✓✗ |
2 |
J.topRows<6>() = J_RF; |
111 |
✓✗✓✗ |
2 |
J.bottomRows<6>() = J_LF; |
112 |
|||
113 |
//Check Forward Dynamics |
||
114 |
✓✗ | 2 |
pinocchio::crba(model,data_ref,q); |
115 |
✓✗✓✗ ✓✗✓✗ |
2 |
data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
116 |
|||
117 |
✓✗ | 4 |
Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12); |
118 |
✓✗✓✗ ✓✗ |
2 |
MJtJ << data_ref.M, J.transpose(), |
119 |
✓✗✓✗ ✓✗ |
2 |
J, Eigen::MatrixXd::Zero(12, 12); |
120 |
|||
121 |
✓✗ | 4 |
Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12); |
122 |
✓✗ | 2 |
computeKKTContactDynamicMatrixInverse(model, data, q, J, KKTMatrix_inv); |
123 |
|||
124 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse())); |
125 |
2 |
} |
|
126 |
|||
127 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_getKKTMatrix) |
128 |
{ |
||
129 |
using namespace Eigen; |
||
130 |
using namespace pinocchio; |
||
131 |
✓✗ | 4 |
pinocchio::Model model; |
132 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
133 |
✓✗ | 4 |
pinocchio::Data data(model); |
134 |
|||
135 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
136 |
✓✗✓✗ |
2 |
q.segment <4> (3).normalize(); |
137 |
|||
138 |
✓✗ | 2 |
pinocchio::computeJointJacobians(model, data, q); |
139 |
|||
140 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
141 |
✓✗✓✗ |
4 |
VectorXd tau = VectorXd::Zero(model.nv); |
142 |
|||
143 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
144 |
✓✗ | 4 |
const std::string LF = "lleg6_joint"; |
145 |
|||
146 |
✓✗ | 4 |
Data::Matrix6x J_RF (6, model.nv); |
147 |
✓✗ | 2 |
J_RF.setZero(); |
148 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF); |
149 |
✓✗ | 4 |
Data::Matrix6x J_LF (6, model.nv); |
150 |
✓✗ | 2 |
J_LF.setZero(); |
151 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF); |
152 |
|||
153 |
✓✗ | 4 |
Eigen::MatrixXd J (12, model.nv); |
154 |
✓✗ | 2 |
J.setZero(); |
155 |
✓✗✓✗ |
2 |
J.topRows<6> () = J_RF; |
156 |
✓✗✓✗ |
2 |
J.bottomRows<6> () = J_LF; |
157 |
|||
158 |
✓✗✓✗ |
4 |
Eigen::VectorXd gamma (VectorXd::Ones(12)); |
159 |
|||
160 |
✓✗✓✗ |
4 |
Eigen::MatrixXd H(J.transpose()); |
161 |
|||
162 |
//Check Forward Dynamics |
||
163 |
✓✗ | 2 |
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.); |
164 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
165 |
|||
166 |
✓✗ | 4 |
Eigen::MatrixXd MJtJ(model.nv+12, model.nv+12); |
167 |
✓✗✓✗ ✓✗ |
2 |
MJtJ << data.M, J.transpose(), |
168 |
✓✗✓✗ ✓✗ |
2 |
J, Eigen::MatrixXd::Zero(12, 12); |
169 |
|||
170 |
✓✗ | 4 |
Eigen::MatrixXd KKTMatrix_inv(model.nv+12, model.nv+12); |
171 |
✓✗ | 2 |
getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv); |
172 |
|||
173 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse())); |
174 |
|||
175 |
//Check Impulse Dynamics |
||
176 |
2 |
const double r_coeff = 1.; |
|
177 |
✓✗✓✗ |
4 |
VectorXd v_before = VectorXd::Ones(model.nv); |
178 |
✓✗ | 2 |
pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.); |
179 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
180 |
✓✗✓✗ ✓✗ |
2 |
MJtJ << data.M, J.transpose(), |
181 |
✓✗✓✗ ✓✗ |
2 |
J, Eigen::MatrixXd::Zero(12, 12); |
182 |
|||
183 |
✓✗ | 2 |
getKKTContactDynamicMatrixInverse(model, data, J, KKTMatrix_inv); |
184 |
|||
185 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(KKTMatrix_inv.isApprox(MJtJ.inverse())); |
186 |
2 |
} |
|
187 |
|||
188 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_FD_with_damping ) |
189 |
{ |
||
190 |
using namespace Eigen; |
||
191 |
using namespace pinocchio; |
||
192 |
|||
193 |
✓✗ | 4 |
pinocchio::Model model; |
194 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
195 |
✓✗ | 4 |
pinocchio::Data data(model); |
196 |
|||
197 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
198 |
✓✗✓✗ |
2 |
q.segment <4> (3).normalize(); |
199 |
|||
200 |
✓✗ | 2 |
pinocchio::computeJointJacobians(model, data, q); |
201 |
|||
202 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
203 |
✓✗✓✗ |
4 |
VectorXd tau = VectorXd::Zero(model.nv); |
204 |
|||
205 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
206 |
|||
207 |
✓✗ | 4 |
Data::Matrix6x J_RF (6, model.nv); |
208 |
✓✗ | 2 |
J_RF.setZero(); |
209 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF); |
210 |
|||
211 |
✓✗ | 4 |
Eigen::MatrixXd J (12, model.nv); |
212 |
✓✗ | 2 |
J.setZero(); |
213 |
✓✗✓✗ |
2 |
J.topRows<6> () = J_RF; |
214 |
✓✗✓✗ |
2 |
J.bottomRows<6> () = J_RF; |
215 |
|||
216 |
✓✗✓✗ |
4 |
Eigen::VectorXd gamma (VectorXd::Ones(12)); |
217 |
|||
218 |
// Forward Dynamics with damping |
||
219 |
✓✗ | 2 |
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 1e-12); |
220 |
|||
221 |
// Matrix Definitions |
||
222 |
✓✗✓✗ |
4 |
Eigen::MatrixXd H(J.transpose()); |
223 |
✓✗ | 2 |
data.M.triangularView<Eigen::StrictlyLower>() = |
224 |
✓✗✓✗ ✓✗ |
4 |
data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
225 |
|||
226 |
✓✗✓✗ |
4 |
MatrixXd Minv (data.M.inverse()); |
227 |
✓✗✓✗ ✓✗✓✗ |
4 |
MatrixXd JMinvJt (J * Minv * J.transpose()); |
228 |
|||
229 |
// Check that JMinvJt is correctly formed |
||
230 |
✓✗✓✗ |
4 |
Eigen::MatrixXd G_ref(J.transpose()); |
231 |
✓✗ | 2 |
cholesky::Uiv(model, data, G_ref); |
232 |
✓✓✓✗ ✓✗✓✗ |
66 |
for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]); |
233 |
✓✗✓✗ ✓✗ |
4 |
Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref); |
234 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12)); |
235 |
|||
236 |
// Actual Residuals |
||
237 |
✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd constraint_residual (J * data.ddq + gamma); |
238 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd dynamics_residual (data.M * data.ddq + data.nle - tau - J.transpose()*data.lambda_c); |
239 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(constraint_residual.norm() <= 1e-9); |
240 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dynamics_residual.norm() <= 1e-12); |
241 |
2 |
} |
|
242 |
|||
243 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_ID ) |
244 |
{ |
||
245 |
using namespace Eigen; |
||
246 |
using namespace pinocchio; |
||
247 |
|||
248 |
✓✗ | 4 |
pinocchio::Model model; |
249 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
250 |
✓✗ | 4 |
pinocchio::Data data(model); |
251 |
|||
252 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
253 |
✓✗✓✗ |
2 |
q.segment <4> (3).normalize(); |
254 |
|||
255 |
✓✗ | 2 |
pinocchio::computeJointJacobians(model, data, q); |
256 |
|||
257 |
✓✗✓✗ |
4 |
VectorXd v_before = VectorXd::Ones(model.nv); |
258 |
|||
259 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
260 |
✓✗ | 4 |
const std::string LF = "lleg6_joint"; |
261 |
|||
262 |
✓✗ | 4 |
Data::Matrix6x J_RF (6, model.nv); |
263 |
✓✗ | 2 |
J_RF.setZero(); |
264 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF); |
265 |
✓✗ | 4 |
Data::Matrix6x J_LF (6, model.nv); |
266 |
✓✗ | 2 |
J_LF.setZero(); |
267 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF); |
268 |
|||
269 |
✓✗ | 4 |
Eigen::MatrixXd J (12, model.nv); |
270 |
✓✗ | 2 |
J.setZero(); |
271 |
✓✗✓✗ |
2 |
J.topRows<6> () = J_RF; |
272 |
✓✗✓✗ |
2 |
J.bottomRows<6> () = J_LF; |
273 |
|||
274 |
2 |
const double r_coeff = 1.; |
|
275 |
|||
276 |
✓✗✓✗ |
4 |
Eigen::MatrixXd H(J.transpose()); |
277 |
|||
278 |
✓✗ | 2 |
pinocchio::impulseDynamics(model, data, q, v_before, J, r_coeff, 0.); |
279 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
280 |
|||
281 |
✓✗✓✗ |
4 |
MatrixXd Minv (data.M.inverse()); |
282 |
✓✗✓✗ ✓✗✓✗ |
4 |
MatrixXd JMinvJt (J * Minv * J.transpose()); |
283 |
|||
284 |
✓✗✓✗ |
4 |
Eigen::MatrixXd G_ref(J.transpose()); |
285 |
✓✗ | 2 |
cholesky::Uiv(model, data, G_ref); |
286 |
✓✓✓✗ ✓✗✓✗ |
66 |
for(int k=0;k<model.nv;++k) G_ref.row(k) /= sqrt(data.D[k]); |
287 |
✓✗✓✗ ✓✗ |
4 |
Eigen::MatrixXd H_ref(G_ref.transpose() * G_ref); |
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(H_ref.isApprox(JMinvJt,1e-12)); |
289 |
|||
290 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
VectorXd lambda_ref = JMinvJt.inverse() * (-r_coeff * J * v_before - J * v_before); |
291 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.impulse_c.isApprox(lambda_ref, 1e-12)); |
292 |
|||
293 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
VectorXd v_after_ref = Minv*(data.M * v_before + J.transpose()*lambda_ref); |
294 |
|||
295 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd constraint_residual (J * data.dq_after + r_coeff * J * v_before); |
296 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(constraint_residual.norm() <= 1e-12); |
297 |
|||
298 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd dynamics_residual (data.M * data.dq_after - data.M * v_before - J.transpose()*data.impulse_c); |
299 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dynamics_residual.norm() <= 1e-12); |
300 |
|||
301 |
2 |
} |
|
302 |
|||
303 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE (timings_fd_llt) |
304 |
{ |
||
305 |
using namespace Eigen; |
||
306 |
using namespace pinocchio; |
||
307 |
|||
308 |
✓✗ | 4 |
pinocchio::Model model; |
309 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model,true); |
310 |
✓✗ | 4 |
pinocchio::Data data(model); |
311 |
|||
312 |
#ifdef NDEBUG |
||
313 |
#ifdef _INTENSE_TESTING_ |
||
314 |
const size_t NBT = 1000*1000; |
||
315 |
#else |
||
316 |
const size_t NBT = 100; |
||
317 |
#endif |
||
318 |
|||
319 |
#else |
||
320 |
2 |
const size_t NBT = 1; |
|
321 |
✓✗ | 2 |
std::cout << "(the time score in debug mode is not relevant) " ; |
322 |
#endif // ifndef NDEBUG |
||
323 |
|||
324 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
325 |
✓✗✓✗ |
2 |
q.segment <4> (3).normalize(); |
326 |
|||
327 |
✓✗ | 2 |
pinocchio::computeJointJacobians(model, data, q); |
328 |
|||
329 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
330 |
✓✗✓✗ |
4 |
VectorXd tau = VectorXd::Zero(model.nv); |
331 |
|||
332 |
✓✗ | 4 |
const std::string RF = "rleg6_joint"; |
333 |
✓✗ | 4 |
const std::string LF = "lleg6_joint"; |
334 |
|||
335 |
✓✗ | 4 |
Data::Matrix6x J_RF (6, model.nv); |
336 |
✓✗ | 2 |
J_RF.setZero(); |
337 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(RF), LOCAL, J_RF); |
338 |
✓✗ | 4 |
Data::Matrix6x J_LF (6, model.nv); |
339 |
✓✗ | 2 |
J_LF.setZero(); |
340 |
✓✗✓✗ |
2 |
getJointJacobian(model, data, model.getJointId(LF), LOCAL, J_LF); |
341 |
|||
342 |
✓✗ | 4 |
Eigen::MatrixXd J (12, model.nv); |
343 |
✓✗✓✗ |
2 |
J.topRows<6> () = J_RF; |
344 |
✓✗✓✗ |
2 |
J.bottomRows<6> () = J_LF; |
345 |
|||
346 |
✓✗✓✗ |
4 |
Eigen::VectorXd gamma (VectorXd::Ones(12)); |
347 |
|||
348 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
349 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill( 1.); |
350 |
|||
351 |
✓✗ | 2 |
q = pinocchio::randomConfiguration(model); |
352 |
|||
353 |
✓✗✓✗ |
4 |
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic(); |
354 |
✓✓ | 4 |
SMOOTH(NBT) |
355 |
{ |
||
356 |
✓✗ | 2 |
pinocchio::forwardDynamics(model, data, q, v, tau, J, gamma, 0.); |
357 |
} |
||
358 |
✓✗ | 2 |
timer.toc(std::cout,NBT); |
359 |
|||
360 |
2 |
} |
|
361 |
|||
362 |
BOOST_AUTO_TEST_SUITE_END () |
Generated by: GCOVR (Version 4.2) |