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/multibody/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( |
35 |
|
|
pinocchio::Model & model, |
36 |
|
|
const pinocchio::JointModelBase<JointModel> & joint, |
37 |
|
|
const std::string & parent_name, |
38 |
|
|
const std::string & name, |
39 |
|
|
const pinocchio::SE3 placement = pinocchio::SE3::Random(), |
40 |
|
|
bool setRandomLimits = true) |
41 |
|
|
{ |
42 |
|
|
using namespace pinocchio; |
43 |
|
|
typedef typename JointModel::ConfigVector_t CV; |
44 |
|
|
typedef typename JointModel::TangentVector_t TV; |
45 |
|
|
|
46 |
|
|
Model::JointIndex idx; |
47 |
|
|
|
48 |
|
|
if (setRandomLimits) |
49 |
|
|
idx = model.addJoint( |
50 |
|
|
model.getJointId(parent_name), joint, SE3::Random(), name + "_joint", |
51 |
|
|
TV::Random() + TV::Constant(1), TV::Random() + TV::Constant(1), |
52 |
|
|
CV::Random() - CV::Constant(1), CV::Random() + CV::Constant(1)); |
53 |
|
|
else |
54 |
|
|
idx = model.addJoint(model.getJointId(parent_name), joint, 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 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_crba) |
65 |
|
|
{ |
66 |
|
✗ |
pinocchio::Model model; |
67 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
68 |
|
✗ |
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); |
80 |
|
|
timer.tic(); |
81 |
|
|
SMOOTH(NBT) |
82 |
|
|
{ |
83 |
|
|
pinocchio::crba(model, data, q, pinocchio::Convention::WORLD); |
84 |
|
|
} |
85 |
|
|
timer.toc(std::cout, NBT); |
86 |
|
|
|
87 |
|
|
#else |
88 |
|
✗ |
const size_t NBT = 1; |
89 |
|
|
using namespace Eigen; |
90 |
|
|
using namespace pinocchio; |
91 |
|
|
|
92 |
|
✗ |
Eigen::MatrixXd M(model.nv, model.nv); |
93 |
|
✗ |
Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq); |
94 |
|
✗ |
q.segment<4>(3).normalize(); |
95 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
96 |
|
✗ |
Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); |
97 |
|
✗ |
data.M.fill(0); |
98 |
|
✗ |
pinocchio::crba(model, data, q, Convention::WORLD); |
99 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
100 |
|
✗ |
data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
101 |
|
|
|
102 |
|
|
/* Joint inertia from iterative crba. */ |
103 |
|
✗ |
const Eigen::VectorXd bias = rnea(model, data, q, v, a); |
104 |
|
✗ |
for (int i = 0; i < model.nv; ++i) |
105 |
|
|
{ |
106 |
|
✗ |
M.col(i) = rnea(model, data, q, v, Eigen::VectorXd::Unit(model.nv, i)) - bias; |
107 |
|
|
} |
108 |
|
|
|
109 |
|
|
// std::cout << "Mcrb = [ " << data.M << " ];" << std::endl; |
110 |
|
|
// std::cout << "Mrne = [ " << M << " ]; " << std::endl; |
111 |
|
✗ |
BOOST_CHECK(M.isApprox(data.M, 1e-12)); |
112 |
|
|
|
113 |
|
✗ |
std::cout << "(the time score in debug mode is not relevant) "; |
114 |
|
|
|
115 |
|
✗ |
q = Eigen::VectorXd::Zero(model.nq); |
116 |
|
|
|
117 |
|
✗ |
PinocchioTicToc timer(PinocchioTicToc::US); |
118 |
|
✗ |
timer.tic(); |
119 |
|
✗ |
SMOOTH(NBT) |
120 |
|
|
{ |
121 |
|
✗ |
pinocchio::crba(model, data, q, Convention::WORLD); |
122 |
|
|
} |
123 |
|
✗ |
timer.toc(std::cout, NBT); |
124 |
|
|
|
125 |
|
|
#endif // ifndef NDEBUG |
126 |
|
|
} |
127 |
|
|
|
128 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_minimal_crba) |
129 |
|
|
{ |
130 |
|
✗ |
pinocchio::Model model; |
131 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
132 |
|
✗ |
pinocchio::Data data(model), data_ref(model); |
133 |
|
|
|
134 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
135 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
136 |
|
|
|
137 |
|
|
Eigen::VectorXd q = |
138 |
|
✗ |
randomConfiguration(model, model.lowerPositionLimit, model.upperPositionLimit); |
139 |
|
✗ |
Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv)); |
140 |
|
|
|
141 |
|
✗ |
pinocchio::crba(model, data_ref, q, pinocchio::Convention::LOCAL); |
142 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
143 |
|
✗ |
data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
144 |
|
|
|
145 |
|
✗ |
pinocchio::crba(model, data, q, pinocchio::Convention::WORLD); |
146 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
147 |
|
✗ |
data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
148 |
|
|
|
149 |
|
✗ |
BOOST_CHECK(data.M.isApprox(data_ref.M)); |
150 |
|
|
|
151 |
|
✗ |
ccrba(model, data_ref, q, v); |
152 |
|
✗ |
computeJointJacobians(model, data_ref, q); |
153 |
|
✗ |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
154 |
|
✗ |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
155 |
|
|
|
156 |
|
|
// Check double call |
157 |
|
|
{ |
158 |
|
✗ |
pinocchio::Data data2(model); |
159 |
|
✗ |
pinocchio::crba(model, data2, q, pinocchio::Convention::LOCAL); |
160 |
|
✗ |
pinocchio::crba(model, data2, q, pinocchio::Convention::LOCAL); |
161 |
|
|
|
162 |
|
✗ |
BOOST_CHECK(data2.Ycrb[0] == data.Ycrb[0]); |
163 |
|
|
} |
164 |
|
|
} |
165 |
|
|
|
166 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_roto_inertia_effects) |
167 |
|
|
{ |
168 |
|
✗ |
pinocchio::Model model, model_ref; |
169 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
170 |
|
✗ |
model_ref = model; |
171 |
|
|
|
172 |
|
✗ |
BOOST_CHECK(model == model_ref); |
173 |
|
|
|
174 |
|
✗ |
pinocchio::Data data(model), data_ref(model_ref); |
175 |
|
|
|
176 |
|
✗ |
model.armature = Eigen::VectorXd::Random(model.nv) + Eigen::VectorXd::Constant(model.nv, 1.); |
177 |
|
|
|
178 |
|
✗ |
Eigen::VectorXd q = randomConfiguration(model); |
179 |
|
✗ |
pinocchio::crba(model_ref, data_ref, q, pinocchio::Convention::WORLD); |
180 |
|
✗ |
data_ref.M.triangularView<Eigen::StrictlyLower>() = |
181 |
|
✗ |
data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
182 |
|
✗ |
data_ref.M.diagonal() += model.armature; |
183 |
|
|
|
184 |
|
✗ |
pinocchio::crba(model, data, q, pinocchio::Convention::WORLD); |
185 |
|
✗ |
data.M.triangularView<Eigen::StrictlyLower>() = |
186 |
|
✗ |
data.M.transpose().triangularView<Eigen::StrictlyLower>(); |
187 |
|
|
|
188 |
|
✗ |
BOOST_CHECK(data.M.isApprox(data_ref.M)); |
189 |
|
|
} |
190 |
|
|
|
191 |
|
|
#ifndef NDEBUG |
192 |
|
|
|
193 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_crba_malloc) |
194 |
|
|
{ |
195 |
|
|
using namespace pinocchio; |
196 |
|
✗ |
pinocchio::Model model; |
197 |
|
✗ |
pinocchio::buildModels::humanoidRandom(model); |
198 |
|
|
|
199 |
|
✗ |
model.addJoint( |
200 |
|
✗ |
size_t(model.njoints - 1), pinocchio::JointModelRevoluteUnaligned(SE3::Vector3::UnitX()), |
201 |
|
✗ |
SE3::Random(), "revolute_unaligned"); |
202 |
|
✗ |
pinocchio::Data data(model); |
203 |
|
|
|
204 |
|
✗ |
const Eigen::VectorXd q = pinocchio::neutral(model); |
205 |
|
✗ |
Eigen::internal::set_is_malloc_allowed(false); |
206 |
|
✗ |
crba(model, data, q); |
207 |
|
✗ |
Eigen::internal::set_is_malloc_allowed(true); |
208 |
|
|
} |
209 |
|
|
|
210 |
|
|
#endif |
211 |
|
|
|
212 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
213 |
|
|
|