GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2023 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 |
#ifndef EIGEN_RUNTIME_NO_MALLOC |
||
14 |
#define EIGEN_RUNTIME_NO_MALLOC |
||
15 |
#endif |
||
16 |
|||
17 |
#include "pinocchio/multibody/model.hpp" |
||
18 |
#include "pinocchio/multibody/data.hpp" |
||
19 |
#include "pinocchio/algorithm/crba.hpp" |
||
20 |
#include "pinocchio/algorithm/centroidal.hpp" |
||
21 |
#include "pinocchio/algorithm/rnea.hpp" |
||
22 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
23 |
#include "pinocchio/algorithm/center-of-mass.hpp" |
||
24 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
25 |
#include "pinocchio/parsers/sample-models.hpp" |
||
26 |
#include "pinocchio/utils/timer.hpp" |
||
27 |
|||
28 |
#include <iostream> |
||
29 |
|||
30 |
#include <boost/test/unit_test.hpp> |
||
31 |
#include <boost/utility/binary.hpp> |
||
32 |
|||
33 |
template<typename JointModel> |
||
34 |
static void addJointAndBody(pinocchio::Model & model, |
||
35 |
const pinocchio::JointModelBase<JointModel> & joint, |
||
36 |
const std::string & parent_name, |
||
37 |
const std::string & name, |
||
38 |
const pinocchio::SE3 placement = pinocchio::SE3::Random(), |
||
39 |
bool setRandomLimits = true) |
||
40 |
{ |
||
41 |
using namespace pinocchio; |
||
42 |
typedef typename JointModel::ConfigVector_t CV; |
||
43 |
typedef typename JointModel::TangentVector_t TV; |
||
44 |
|||
45 |
Model::JointIndex idx; |
||
46 |
|||
47 |
if (setRandomLimits) |
||
48 |
idx = model.addJoint(model.getJointId(parent_name),joint, |
||
49 |
SE3::Random(), |
||
50 |
name + "_joint", |
||
51 |
TV::Random() + TV::Constant(1), |
||
52 |
TV::Random() + TV::Constant(1), |
||
53 |
CV::Random() - CV::Constant(1), |
||
54 |
CV::Random() + CV::Constant(1) |
||
55 |
); |
||
56 |
else |
||
57 |
idx = model.addJoint(model.getJointId(parent_name),joint, |
||
58 |
placement, name + "_joint"); |
||
59 |
|||
60 |
model.addJointFrame(idx); |
||
61 |
|||
62 |
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity()); |
||
63 |
model.addBodyFrame(name + "_body", idx); |
||
64 |
} |
||
65 |
|||
66 |
BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE ) |
||
67 |
|||
68 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE ( test_crba ) |
69 |
{ |
||
70 |
✓✗ | 4 |
pinocchio::Model model; |
71 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
72 |
✓✗ | 4 |
pinocchio::Data data(model); |
73 |
|||
74 |
#ifdef NDEBUG |
||
75 |
#ifdef _INTENSE_TESTING_ |
||
76 |
const size_t NBT = 1000*1000; |
||
77 |
#else |
||
78 |
const size_t NBT = 10; |
||
79 |
#endif |
||
80 |
|||
81 |
Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq); |
||
82 |
|||
83 |
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic(); |
||
84 |
SMOOTH(NBT) |
||
85 |
{ |
||
86 |
crba(model,data,q); |
||
87 |
} |
||
88 |
timer.toc(std::cout,NBT); |
||
89 |
|||
90 |
#else |
||
91 |
2 |
const size_t NBT = 1; |
|
92 |
using namespace Eigen; |
||
93 |
using namespace pinocchio; |
||
94 |
|||
95 |
✓✗ | 4 |
Eigen::MatrixXd M(model.nv,model.nv); |
96 |
✓✗✓✗ |
4 |
Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq); |
97 |
✓✗✓✗ |
2 |
q.segment <4> (3).normalize(); |
98 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
99 |
✓✗✓✗ |
4 |
Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); |
100 |
✓✗✓✗ |
2 |
data.M.fill(0); crba(model,data,q); |
101 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
102 |
|||
103 |
/* Joint inertia from iterative crba. */ |
||
104 |
✓✗✓✗ |
4 |
const Eigen::VectorXd bias = rnea(model,data,q,v,a); |
105 |
✓✓ | 66 |
for(int i=0;i<model.nv;++i) |
106 |
{ |
||
107 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
64 |
M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias; |
108 |
} |
||
109 |
|||
110 |
// std::cout << "Mcrb = [ " << data.M << " ];" << std::endl; |
||
111 |
// std::cout << "Mrne = [ " << M << " ]; " << std::endl; |
||
112 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(M.isApprox(data.M,1e-12)); |
113 |
|||
114 |
✓✗ | 2 |
std::cout << "(the time score in debug mode is not relevant) " ; |
115 |
|||
116 |
✓✗✓✗ |
2 |
q = Eigen::VectorXd::Zero(model.nq); |
117 |
|||
118 |
✓✗✓✗ |
4 |
PinocchioTicToc timer(PinocchioTicToc::US); timer.tic(); |
119 |
✓✓ | 4 |
SMOOTH(NBT) |
120 |
{ |
||
121 |
✓✗ | 2 |
crba(model,data,q); |
122 |
} |
||
123 |
✓✗ | 2 |
timer.toc(std::cout,NBT); |
124 |
|||
125 |
#endif // ifndef NDEBUG |
||
126 |
|||
127 |
2 |
} |
|
128 |
|||
129 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_minimal_crba) |
130 |
{ |
||
131 |
✓✗ | 4 |
pinocchio::Model model; |
132 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
133 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_ref(model); |
134 |
|||
135 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
136 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
137 |
|||
138 |
✓✗ | 4 |
Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit); |
139 |
✓✗✓✗ |
4 |
Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv)); |
140 |
|||
141 |
✓✗ | 2 |
crba(model,data_ref,q); |
142 |
✓✗✓✗ ✓✗✓✗ |
2 |
data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
143 |
|||
144 |
✓✗ | 2 |
crbaMinimal(model,data,q); |
145 |
✓✗✓✗ ✓✗✓✗ |
2 |
data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
146 |
|||
147 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.M.isApprox(data_ref.M)); |
148 |
|||
149 |
✓✗ | 2 |
ccrba(model,data_ref,q,v); |
150 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
151 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
152 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
153 |
|||
154 |
// Check double call |
||
155 |
{ |
||
156 |
✓✗ | 4 |
pinocchio::Data data2(model); |
157 |
✓✗ | 2 |
crbaMinimal(model,data2,q); |
158 |
✓✗ | 2 |
crbaMinimal(model,data2,q); |
159 |
|||
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data2.Ycrb[0] == data.Ycrb[0]); |
161 |
} |
||
162 |
|||
163 |
2 |
} |
|
164 |
|||
165 |
#ifndef NDEBUG |
||
166 |
|||
167 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_crba_malloc) |
168 |
{ |
||
169 |
using namespace pinocchio; |
||
170 |
✓✗ | 4 |
pinocchio::Model model; |
171 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
172 |
|||
173 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
model.addJoint(size_t(model.njoints-1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), SE3::Random(), "revolute_unaligned"); |
174 |
✓✗ | 4 |
pinocchio::Data data(model); |
175 |
|||
176 |
✓✗ | 4 |
const Eigen::VectorXd q = pinocchio::neutral(model); |
177 |
2 |
Eigen::internal::set_is_malloc_allowed(false); |
|
178 |
✓✗ | 2 |
crba(model, data, q); |
179 |
2 |
Eigen::internal::set_is_malloc_allowed(true); |
|
180 |
2 |
} |
|
181 |
|||
182 |
#endif |
||
183 |
|||
184 |
BOOST_AUTO_TEST_SUITE_END () |
||
185 |
Generated by: GCOVR (Version 4.2) |