| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2019-2021 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/algorithm/center-of-mass.hpp" |
||
6 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
7 |
#include "pinocchio/algorithm/compute-all-terms.hpp" |
||
8 |
#include "pinocchio/algorithm/center-of-mass-derivatives.hpp" |
||
9 |
#include "pinocchio/parsers/sample-models.hpp" |
||
10 |
|||
11 |
#include <boost/test/unit_test.hpp> |
||
12 |
#include <boost/utility/binary.hpp> |
||
13 |
|||
14 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
15 |
|||
16 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematics_derivatives_vcom) |
17 |
{ |
||
18 |
using namespace Eigen; |
||
19 |
using namespace pinocchio; |
||
20 |
|||
21 |
✓✗ | 4 |
Model model; |
22 |
✓✗ | 2 |
buildModels::humanoid(model); |
23 |
|||
24 |
✓✗ | 4 |
Data data_ref(model); |
25 |
|||
26 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
27 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill(1.); |
28 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
29 |
✓✗✓✗ |
4 |
VectorXd vq(VectorXd::Random(model.nv)); |
30 |
✓✗✓✗ |
4 |
VectorXd aq(VectorXd::Random(model.nv)); |
31 |
|||
32 |
// Approximate dvcom_dq by finite diff. |
||
33 |
✓✗ | 2 |
centerOfMass(model,data_ref,q,vq); |
34 |
✓✗ | 2 |
const Eigen::Vector3d vcom0 = data_ref.vcom[0]; |
35 |
2 |
const double alpha = 1e-8; |
|
36 |
✓✗✓✗ |
4 |
Eigen::VectorXd dq = VectorXd::Zero(model.nv); |
37 |
✓✗ | 4 |
Data::Matrix3x dvcom_dqn(3,model.nv); |
38 |
|||
39 |
✓✓ | 70 |
for(int k = 0; k < model.nv; ++k) |
40 |
{ |
||
41 |
✓✗ | 68 |
dq[k] = alpha; |
42 |
✓✗✓✗ |
68 |
centerOfMass(model,data_ref,integrate(model,q,dq),vq); |
43 |
✓✗✓✗ ✓✗✓✗ |
68 |
dvcom_dqn.col(k) = (data_ref.vcom[0]-vcom0)/alpha; |
44 |
✓✗ | 68 |
dq[k] = 0; |
45 |
} |
||
46 |
|||
47 |
{ |
||
48 |
// Compute dvcom_dq using the algorithm |
||
49 |
✓✗ | 4 |
Data data(model); |
50 |
✓✗✓✗ |
4 |
Data::Matrix3x dvcom_dq = Data::Matrix3x::Zero(3,model.nv); |
51 |
✓✗ | 2 |
centerOfMass(model,data,q,vq); |
52 |
✓✗ | 2 |
getCenterOfMassVelocityDerivatives(model,data,dvcom_dq); |
53 |
|||
54 |
// Check that algo result and finite-diff approx are similar. |
||
55 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dvcom_dq.isApprox(dvcom_dqn,sqrt(alpha))); |
56 |
} |
||
57 |
|||
58 |
{ |
||
59 |
// Compute dvcom_dq using the algorithm |
||
60 |
✓✗ | 4 |
Data data(model); |
61 |
✓✗✓✗ |
4 |
Data::Matrix3x dvcom_dq = Data::Matrix3x::Zero(3,model.nv); |
62 |
✓✗ | 2 |
computeAllTerms(model,data,q,vq); |
63 |
✓✗ | 2 |
getCenterOfMassVelocityDerivatives(model,data,dvcom_dq); |
64 |
|||
65 |
// Check that algo result and finite-diff approx are similar. |
||
66 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dvcom_dq.isApprox(dvcom_dqn,sqrt(alpha))); |
67 |
} |
||
68 |
2 |
} |
|
69 |
|||
70 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |