GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 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/crba.hpp" |
||
10 |
#include "pinocchio/algorithm/compute-all-terms.hpp" |
||
11 |
#include "pinocchio/algorithm/rnea.hpp" |
||
12 |
#include "pinocchio/algorithm/center-of-mass.hpp" |
||
13 |
#include "pinocchio/utils/timer.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_com ) |
24 |
{ |
||
25 |
using namespace Eigen; |
||
26 |
using namespace pinocchio; |
||
27 |
|||
28 |
✓✗ | 4 |
pinocchio::Model model; |
29 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
30 |
✓✗ | 4 |
pinocchio::Data data(model); |
31 |
|||
32 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
33 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
34 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Ones(model.nv); |
35 |
✓✗✓✗ |
4 |
VectorXd a = VectorXd::Ones(model.nv); |
36 |
|||
37 |
✓✗ | 2 |
crba(model,data,q); |
38 |
|||
39 |
|||
40 |
/* Test COM against CRBA*/ |
||
41 |
✓✗✓✗ |
2 |
Vector3d com = centerOfMass(model,data,q); |
42 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(data.com[0].isApprox(getComFromCrba(model,data), 1e-12)); |
43 |
|||
44 |
/* Test COM against Jcom (both use different way to compute the COM). */ |
||
45 |
✓✗✓✗ |
2 |
com = centerOfMass(model,data,q); |
46 |
✓✗ | 2 |
jacobianCenterOfMass(model,data,q); |
47 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(com.isApprox(data.com[0], 1e-12)); |
48 |
|||
49 |
/* Test COM against Jcom (both use different way to compute the COM). */ |
||
50 |
✓✗ | 2 |
centerOfMass(model,data,q,v,a); |
51 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(com.isApprox(data.com[0], 1e-12)); |
52 |
|||
53 |
/* Test vCoM against nle algorithm without gravity field */ |
||
54 |
✓✗ | 2 |
a.setZero(); |
55 |
✓✗ | 2 |
model.gravity.setZero(); |
56 |
✓✗ | 2 |
centerOfMass(model,data,q,v,a); |
57 |
✓✗ | 2 |
nonLinearEffects(model, data, q, v); |
58 |
|||
59 |
✓✗✓✗ ✓✗ |
2 |
pinocchio::SE3::Vector3 acom_from_nle (data.nle.head <3> ()/data.mass[0]); |
60 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((data.liMi[1].rotation() * acom_from_nle).isApprox(data.acom[0], 1e-12)); |
61 |
|||
62 |
/* Test Jcom against CRBA */ |
||
63 |
✓✗✓✗ |
4 |
Eigen::MatrixXd Jcom = jacobianCenterOfMass(model,data,q); |
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(data.Jcom.isApprox(getJacobianComFromCrba(model,data), 1e-12)); |
65 |
|||
66 |
/* Test CoM velocity againt jacobianCenterOfMass */ |
||
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12)); |
68 |
|||
69 |
|||
70 |
✓✗ | 2 |
centerOfMass(model,data,q,v); |
71 |
/* Test CoM velocity againt jacobianCenterOfMass */ |
||
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK((Jcom * v).isApprox(data.vcom[0], 1e-12)); |
73 |
|||
74 |
|||
75 |
// std::cout << "com = [ " << data.com[0].transpose() << " ];" << std::endl; |
||
76 |
// std::cout << "mass = [ " << data.mass[0] << " ];" << std::endl; |
||
77 |
// std::cout << "Jcom = [ " << data.Jcom << " ];" << std::endl; |
||
78 |
// std::cout << "M3 = [ " << data.M.topRows<3>() << " ];" << std::endl; |
||
79 |
2 |
} |
|
80 |
|||
81 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_mass ) |
82 |
{ |
||
83 |
using namespace Eigen; |
||
84 |
using namespace pinocchio; |
||
85 |
|||
86 |
✓✗ | 4 |
pinocchio::Model model; |
87 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
88 |
|||
89 |
✓✗ | 2 |
double mass = computeTotalMass(model); |
90 |
|||
91 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(mass == mass); // checking it is not NaN |
92 |
|||
93 |
2 |
double mass_check = 0.0; |
|
94 |
✓✓ | 56 |
for(size_t i=1; i<(size_t)(model.njoints);++i) |
95 |
54 |
mass_check += model.inertias[i].mass(); |
|
96 |
|||
97 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(mass, mass_check, 1e-12); |
98 |
|||
99 |
✓✗ | 4 |
pinocchio::Data data1(model); |
100 |
|||
101 |
✓✗ | 2 |
double mass_data = computeTotalMass(model,data1); |
102 |
|||
103 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(mass_data == mass_data); // checking it is not NaN |
104 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(mass, mass_data, 1e-12); |
105 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(data1.mass[0], mass_data, 1e-12); |
106 |
|||
107 |
✓✗ | 4 |
pinocchio::Data data2(model); |
108 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
109 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
110 |
✓✗ | 2 |
centerOfMass(model,data2,q); |
111 |
|||
112 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK_CLOSE(data2.mass[0], mass, 1e-12); |
113 |
2 |
} |
|
114 |
|||
115 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_subtree_masses ) |
116 |
{ |
||
117 |
using namespace Eigen; |
||
118 |
using namespace pinocchio; |
||
119 |
|||
120 |
✓✗ | 4 |
pinocchio::Model model; |
121 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
122 |
|||
123 |
✓✗ | 4 |
pinocchio::Data data1(model); |
124 |
|||
125 |
✓✗ | 2 |
computeSubtreeMasses(model,data1); |
126 |
|||
127 |
✓✗ | 4 |
pinocchio::Data data2(model); |
128 |
✓✗✓✗ |
4 |
VectorXd q = VectorXd::Ones(model.nq); |
129 |
✓✗✓✗ |
2 |
q.middleRows<4> (3).normalize(); |
130 |
✓✗ | 2 |
centerOfMass(model,data2,q); |
131 |
|||
132 |
✓✓ | 58 |
for(size_t i=0; i<(size_t)(model.njoints);++i) |
133 |
{ |
||
134 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
56 |
BOOST_CHECK_CLOSE(data1.mass[i], data2.mass[i], 1e-12); |
135 |
} |
||
136 |
2 |
} |
|
137 |
|||
138 |
//BOOST_AUTO_TEST_CASE ( test_timings ) |
||
139 |
//{ |
||
140 |
// using namespace Eigen; |
||
141 |
// using namespace pinocchio; |
||
142 |
// |
||
143 |
// pinocchio::Model model; |
||
144 |
// pinocchio::buildModels::humanoidRandom(model); |
||
145 |
// pinocchio::Data data(model); |
||
146 |
// |
||
147 |
// long flag = BOOST_BINARY(1111); |
||
148 |
// PinocchioTicToc timer(PinocchioTicToc::US); |
||
149 |
// #ifdef NDEBUG |
||
150 |
// #ifdef _INTENSE_TESTING_ |
||
151 |
// const size_t NBT = 1000*1000; |
||
152 |
// #else |
||
153 |
// const size_t NBT = 10; |
||
154 |
// #endif |
||
155 |
// #else |
||
156 |
// const size_t NBT = 1; |
||
157 |
// std::cout << "(the time score in debug mode is not relevant) " ; |
||
158 |
// #endif |
||
159 |
// |
||
160 |
// bool verbose = flag & (flag-1) ; // True is two or more binaries of the flag are 1. |
||
161 |
// if(verbose) std::cout <<"--" << std::endl; |
||
162 |
// Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq); |
||
163 |
// |
||
164 |
// if( flag >> 0 & 1 ) |
||
165 |
// { |
||
166 |
// timer.tic(); |
||
167 |
// SMOOTH(NBT) |
||
168 |
// { |
||
169 |
// centerOfMass(model,data,q); |
||
170 |
// } |
||
171 |
// if(verbose) std::cout << "COM =\t"; |
||
172 |
// timer.toc(std::cout,NBT); |
||
173 |
// } |
||
174 |
// |
||
175 |
// if( flag >> 1 & 1 ) |
||
176 |
// { |
||
177 |
// timer.tic(); |
||
178 |
// SMOOTH(NBT) |
||
179 |
// { |
||
180 |
// centerOfMass(model,data,q,false); |
||
181 |
// } |
||
182 |
// if(verbose) std::cout << "Without sub-tree =\t"; |
||
183 |
// timer.toc(std::cout,NBT); |
||
184 |
// } |
||
185 |
// |
||
186 |
// if( flag >> 2 & 1 ) |
||
187 |
// { |
||
188 |
// timer.tic(); |
||
189 |
// SMOOTH(NBT) |
||
190 |
// { |
||
191 |
// jacobianCenterOfMass(model,data,q); |
||
192 |
// } |
||
193 |
// if(verbose) std::cout << "Jcom =\t"; |
||
194 |
// timer.toc(std::cout,NBT); |
||
195 |
// } |
||
196 |
//} |
||
197 |
|||
198 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_subtree_com_jacobian) |
199 |
{ |
||
200 |
using namespace Eigen; |
||
201 |
using namespace pinocchio; |
||
202 |
|||
203 |
✓✗ | 4 |
Model model; |
204 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
205 |
✓✗ | 4 |
Data data(model); |
206 |
|||
207 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1000); |
208 |
✓✗✓✗ ✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>() = -model.upperPositionLimit.head<3>(); |
209 |
✓✗ | 4 |
VectorXd q = pinocchio::randomConfiguration(model); |
210 |
✓✗✓✗ |
4 |
VectorXd v = VectorXd::Random(model.nv); |
211 |
|||
212 |
✓✗ | 4 |
Data data_ref(model); |
213 |
✓✗ | 2 |
jacobianCenterOfMass(model,data_ref,q,true); |
214 |
|||
215 |
✓✗ | 2 |
centerOfMass(model, data, q, v); |
216 |
✓✗✓✗ |
4 |
Data::Matrix3x Jcom(3,model.nv); Jcom.setZero(); |
217 |
✓✗ | 2 |
jacobianSubtreeCenterOfMass(model, data, 0, Jcom); |
218 |
|||
219 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(Jcom.isApprox(data_ref.Jcom)); |
220 |
|||
221 |
✓✗ | 2 |
centerOfMass(model, data_ref, q, v, true); |
222 |
✓✗ | 2 |
computeJointJacobians(model, data_ref, q); |
223 |
✓✗✓✗ |
4 |
Data::Matrix3x Jcom_extracted(3,model.nv), Jcom_fd(3,model.nv); |
224 |
✓✗✓✗ |
4 |
Data data_extracted(model), data_fd(model); |
225 |
2 |
const double eps = 1e-8; |
|
226 |
✓✗ | 2 |
jacobianCenterOfMass(model,data_extracted,q); |
227 |
|||
228 |
// Get subtree jacobian and check that it is consistent with the com velocity |
||
229 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; joint_id++) |
230 |
{ |
||
231 |
✓✗✓✗ ✓✗ |
54 |
SE3::Vector3 subtreeComVelocityInWorld_ref = data_ref.oMi[joint_id].rotation() * data_ref.vcom[joint_id]; |
232 |
✓✗ | 54 |
Jcom.setZero(); |
233 |
✓✗ | 54 |
data.J.setZero(); |
234 |
✓✗ | 54 |
jacobianSubtreeCenterOfMass(model, data, joint_id, Jcom); |
235 |
|||
236 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(data.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id]).isApprox(data_ref.J.middleCols(model.joints[joint_id].idx_v(),data.nvSubtree[joint_id]))); |
237 |
✓✗✓✗ |
54 |
SE3::Vector3 subtreeComVelocityInWorld = Jcom * v; |
238 |
|||
239 |
✓✗ | 54 |
Jcom_extracted.setZero(); |
240 |
✓✗ | 54 |
getJacobianSubtreeCenterOfMass(model,data_extracted,joint_id,Jcom_extracted); |
241 |
|||
242 |
// Check with finite differences |
||
243 |
✓✗✓✗ |
108 |
Eigen::VectorXd v_plus(model.nv); v_plus.setZero(); |
244 |
✓✗ | 54 |
centerOfMass(model,data_fd,q); |
245 |
✓✗ | 54 |
const SE3::Vector3 com = data_fd.oMi[joint_id].act(data_fd.com[joint_id]); |
246 |
✓✗ | 54 |
Jcom_fd.setZero(); |
247 |
✓✓ | 1782 |
for(Eigen::DenseIndex k = 0; k < model.nv; ++k) |
248 |
{ |
||
249 |
✓✗ | 1728 |
v_plus[k] = eps; |
250 |
✓✗ | 1728 |
Eigen::VectorXd q_plus = integrate(model,q,v_plus); |
251 |
✓✗ | 1728 |
centerOfMass(model,data_fd,q_plus); |
252 |
✓✗ | 1728 |
const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]); |
253 |
✓✗✓✗ ✓✗✓✗ |
1728 |
Jcom_fd.col(k) = (com_plus - com)/eps; |
254 |
✓✗ | 1728 |
v_plus[k] = 0.; |
255 |
} |
||
256 |
|||
257 |
// Eigen::VectorXd q_plus = integrate(model,q,v*eps); |
||
258 |
// centerOfMass(model,data_fd,q_plus); |
||
259 |
// const SE3::Vector3 com_plus = data_fd.oMi[joint_id].act(data_fd.com[joint_id]); |
||
260 |
// |
||
261 |
// const SE3::Vector3 vcom_subtree_fd = (com_plus - com)/eps; |
||
262 |
|||
263 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(Jcom.isApprox(Jcom_fd,sqrt(eps))); |
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(Jcom_extracted.isApprox(Jcom_fd,sqrt(eps))); |
265 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(Jcom_extracted.isApprox(Jcom)); |
266 |
|||
267 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(std::fabs(data.mass[joint_id] - data_ref.mass[joint_id]) <= 1e-12); |
268 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
54 |
BOOST_CHECK(data.com[joint_id].isApprox(data_ref.oMi[joint_id].act(data_ref.com[joint_id]))); |
269 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(subtreeComVelocityInWorld.isApprox(subtreeComVelocityInWorld_ref)); |
270 |
} |
||
271 |
2 |
} |
|
272 |
|||
273 |
BOOST_AUTO_TEST_SUITE_END () |
Generated by: GCOVR (Version 4.2) |