GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
/* |
||
6 |
* Test the CRBA algorithm. The code validates both the computation times and |
||
7 |
* the value by comparing the results of the CRBA with the reconstruction of |
||
8 |
* the mass matrix using the RNEA. |
||
9 |
* For a strong timing benchmark, see benchmark/timings. |
||
10 |
* |
||
11 |
*/ |
||
12 |
|||
13 |
#include "pinocchio/multibody/model.hpp" |
||
14 |
#include "pinocchio/multibody/data.hpp" |
||
15 |
#include "pinocchio/algorithm/crba.hpp" |
||
16 |
#include "pinocchio/algorithm/centroidal.hpp" |
||
17 |
#include "pinocchio/algorithm/rnea.hpp" |
||
18 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
19 |
#include "pinocchio/algorithm/center-of-mass.hpp" |
||
20 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
21 |
#include "pinocchio/parsers/sample-models.hpp" |
||
22 |
#include "pinocchio/utils/timer.hpp" |
||
23 |
|||
24 |
#include <iostream> |
||
25 |
|||
26 |
#include <boost/test/unit_test.hpp> |
||
27 |
#include <boost/utility/binary.hpp> |
||
28 |
|||
29 |
template<typename JointModel> |
||
30 |
static void addJointAndBody(pinocchio::Model & model, |
||
31 |
const pinocchio::JointModelBase<JointModel> & joint, |
||
32 |
const std::string & parent_name, |
||
33 |
const std::string & name, |
||
34 |
const pinocchio::SE3 placement = pinocchio::SE3::Random(), |
||
35 |
bool setRandomLimits = true) |
||
36 |
{ |
||
37 |
using namespace pinocchio; |
||
38 |
typedef typename JointModel::ConfigVector_t CV; |
||
39 |
typedef typename JointModel::TangentVector_t TV; |
||
40 |
|||
41 |
Model::JointIndex idx; |
||
42 |
|||
43 |
if (setRandomLimits) |
||
44 |
idx = model.addJoint(model.getJointId(parent_name),joint, |
||
45 |
SE3::Random(), |
||
46 |
name + "_joint", |
||
47 |
TV::Random() + TV::Constant(1), |
||
48 |
TV::Random() + TV::Constant(1), |
||
49 |
CV::Random() - CV::Constant(1), |
||
50 |
CV::Random() + CV::Constant(1) |
||
51 |
); |
||
52 |
else |
||
53 |
idx = model.addJoint(model.getJointId(parent_name),joint, |
||
54 |
placement, name + "_joint"); |
||
55 |
|||
56 |
model.addJointFrame(idx); |
||
57 |
|||
58 |
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity()); |
||
59 |
model.addBodyFrame(name + "_body", idx); |
||
60 |
} |
||
61 |
|||
62 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
63 |
|||
64 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_crba ) |
65 |
{ |
||
66 |
✓✗ | 4 |
pinocchio::Model model; |
67 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
68 |
✓✗ | 4 |
pinocchio::Data data(model); |
69 |
|||
70 |
#ifdef NDEBUG |
||
71 |
#ifdef _INTENSE_TESTING_ |
||
72 |
const size_t NBT = 1000*1000; |
||
73 |
#else |
||
74 |
const size_t NBT = 10; |
||
75 |
#endif |
||
76 |
|||
77 |
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq); |
||
78 |
|||
79 |
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic(); |
||
80 |
SMOOTH(NBT) |
||
81 |
{ |
||
82 |
crba(model,data,q); |
||
83 |
} |
||
84 |
timer.toc(std::cout,NBT); |
||
85 |
|||
86 |
#else |
||
87 |
2 |
const size_t NBT = 1; |
|
88 |
using namespace Eigen; |
||
89 |
using namespace pinocchio; |
||
90 |
|||
91 |
✓✗ | 4 |
Eigen::MatrixXd M(model.nv,model.nv); |
92 |
✓✗✓✗ |
4 |
Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq); |
93 |
✓✗✓✗ |
2 |
q.segment <4> (3).normalize(); |
94 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
95 |
✓✗✓✗ |
4 |
Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); |
96 |
✓✗✓✗ |
2 |
data.M.fill(0); crba(model,data,q); |
97 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
98 |
|||
99 |
/* Joint inertia from iterative crba. */ |
||
100 |
✓✗✓✗ |
4 |
const Eigen::VectorXd bias = rnea(model,data,q,v,a); |
101 |
✓✓ | 66 |
for(int i=0;i<model.nv;++i) |
102 |
{ |
||
103 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias; |
104 |
} |
||
105 |
|||
106 |
// std::cout << "Mcrb = [ " << data.M << " ];" << std::endl; |
||
107 |
// std::cout << "Mrne = [ " << M << " ]; " << std::endl; |
||
108 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.isApprox(data.M,1e-12)); |
109 |
|||
110 |
✓✗ | 2 |
std::cout << "(the time score in debug mode is not relevant) " ; |
111 |
|||
112 |
✓✗✓✗ |
2 |
q = Eigen::VectorXd::Zero(model.nq); |
113 |
|||
114 |
✓✗✓✗ |
4 |
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic(); |
115 |
✓✓ | 4 |
SMOOTH(NBT) |
116 |
{ |
||
117 |
✓✗ | 2 |
crba(model,data,q); |
118 |
} |
||
119 |
✓✗ | 2 |
timer.toc(std::cout,NBT); |
120 |
|||
121 |
#endif // ifndef NDEBUG |
||
122 |
|||
123 |
2 |
} |
|
124 |
|||
125 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_minimal_crba) |
126 |
{ |
||
127 |
✓✗ | 4 |
pinocchio::Model model; |
128 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
129 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_ref(model); |
130 |
|||
131 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
132 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
133 |
|||
134 |
✓✗ | 4 |
Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit); |
135 |
✓✗✓✗ |
4 |
Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv)); |
136 |
|||
137 |
✓✗ | 2 |
crba(model,data_ref,q); |
138 |
✓✗✓✗ ✓✗✓✗ |
2 |
data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
139 |
|||
140 |
✓✗ | 2 |
crbaMinimal(model,data,q); |
141 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
142 |
|||
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.M.isApprox(data_ref.M)); |
144 |
|||
145 |
✓✗ | 2 |
ccrba(model,data_ref,q,v); |
146 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
147 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
148 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
149 |
|||
150 |
2 |
} |
|
151 |
|||
152 |
BOOST_AUTO_TEST_SUITE_END () |
||
153 |
Generated by: GCOVR (Version 4.2) |