| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#include "pinocchio/algorithm/crba.hpp" |
||
6 |
#include "pinocchio/algorithm/centroidal.hpp" |
||
7 |
#include "pinocchio/algorithm/rnea.hpp" |
||
8 |
#include "pinocchio/algorithm/jacobian.hpp" |
||
9 |
#include "pinocchio/algorithm/center-of-mass.hpp" |
||
10 |
#include "pinocchio/algorithm/joint-configuration.hpp" |
||
11 |
|||
12 |
#include "pinocchio/parsers/sample-models.hpp" |
||
13 |
|||
14 |
#include "pinocchio/utils/timer.hpp" |
||
15 |
|||
16 |
#include <iostream> |
||
17 |
|||
18 |
#include <boost/test/unit_test.hpp> |
||
19 |
#include <boost/utility/binary.hpp> |
||
20 |
|||
21 |
template<typename JointModel> |
||
22 |
2 |
static void addJointAndBody(pinocchio::Model & model, |
|
23 |
const pinocchio::JointModelBase<JointModel> & joint, |
||
24 |
const std::string & parent_name, |
||
25 |
const std::string & name, |
||
26 |
const pinocchio::SE3 placement = pinocchio::SE3::Random(), |
||
27 |
bool setRandomLimits = true) |
||
28 |
{ |
||
29 |
using namespace pinocchio; |
||
30 |
typedef typename JointModel::ConfigVector_t CV; |
||
31 |
typedef typename JointModel::TangentVector_t TV; |
||
32 |
|||
33 |
Model::JointIndex idx; |
||
34 |
|||
35 |
✓✗ | 2 |
if(setRandomLimits) |
36 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
idx = model.addJoint(model.getJointId(parent_name),joint, |
37 |
SE3::Random(), |
||
38 |
name + "_joint", |
||
39 |
TV::Random() + TV::Constant(1), |
||
40 |
TV::Random() + TV::Constant(1), |
||
41 |
CV::Random() - CV::Constant(1), |
||
42 |
CV::Random() + CV::Constant(1) |
||
43 |
); |
||
44 |
else |
||
45 |
idx = model.addJoint(model.getJointId(parent_name),joint, |
||
46 |
placement, name + "_joint"); |
||
47 |
|||
48 |
✓✗ | 2 |
model.addJointFrame(idx); |
49 |
|||
50 |
✓✗✓✗ ✓✗ |
2 |
model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity()); |
51 |
✓✗✓✗ ✓✗ |
2 |
model.addBodyFrame(name + "_body", idx); |
52 |
2 |
} |
|
53 |
|||
54 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
55 |
|||
56 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_ccrba) |
57 |
{ |
||
58 |
✓✗ | 4 |
pinocchio::Model model; |
59 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
60 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_ref(model); |
61 |
|||
62 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
63 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill( 1.); |
64 |
✓✗ | 4 |
Eigen::VectorXd q = randomConfiguration(model); |
65 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv); |
66 |
|||
67 |
✓✗ | 2 |
crba(model,data_ref,q); |
68 |
✓✗✓✗ ✓✗✓✗ |
2 |
data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
69 |
✓✗✓✗ |
2 |
data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]); |
70 |
|||
71 |
✓✗✓✗ ✓✗✓✗ |
2 |
pinocchio::SE3 cMo (pinocchio::SE3::Matrix3::Identity(), -getComFromCrba(model, data_ref)); |
72 |
|||
73 |
✓✗ | 2 |
ccrba(model, data, q, v); |
74 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.com[0].isApprox(-cMo.translation(),1e-12)); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.oYcrb[0].matrix().isApprox(data_ref.Ycrb[0].matrix(),1e-12)); |
76 |
|||
77 |
✓✗ | 2 |
pinocchio::Inertia Ig_ref (cMo.act(data.oYcrb[0])); |
78 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ig.matrix().isApprox(Ig_ref.matrix(),1e-12)); |
79 |
|||
80 |
✓✗ | 2 |
pinocchio::SE3 oM1 (data_ref.liMi[1]); |
81 |
✓✗ | 2 |
pinocchio::SE3 cM1 (cMo * oM1); |
82 |
|||
83 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
4 |
pinocchio::Data::Matrix6x Ag_ref (cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows <6> ()); |
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ag.isApprox(Ag_ref,1e-12)); |
85 |
2 |
} |
|
86 |
|||
87 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_centroidal_mapping) |
88 |
{ |
||
89 |
✓✗ | 4 |
pinocchio::Model model; |
90 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
91 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_ref(model); |
92 |
|||
93 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
94 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill( 1.); |
95 |
✓✗ | 4 |
Eigen::VectorXd q = randomConfiguration(model); |
96 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
97 |
|||
98 |
✓✗ | 2 |
computeCentroidalMap(model, data, q); |
99 |
✓✗ | 2 |
ccrba(model,data_ref,q,v); |
100 |
|||
101 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
102 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
103 |
|||
104 |
✓✗ | 2 |
computeJointJacobians(model,data_ref,q); |
105 |
|||
106 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
107 |
2 |
} |
|
108 |
|||
109 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_dccrb) |
110 |
{ |
||
111 |
using namespace pinocchio; |
||
112 |
✓✗ | 4 |
Model model; |
113 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
114 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7"); |
115 |
✓✗✓✗ |
4 |
Data data(model), data_ref(model); |
116 |
|||
117 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
118 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
119 |
|||
120 |
✓✗ | 4 |
Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit); |
121 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); |
122 |
✓✗✓✗ |
4 |
Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); |
123 |
|||
124 |
✓✗✓✗ ✓✗✓✗ |
4 |
const Eigen::VectorXd g = rnea(model,data_ref,q,0*v,0*a); |
125 |
✓✗ | 2 |
rnea(model,data_ref,q,v,a); |
126 |
|||
127 |
✓✗ | 2 |
crba(model,data_ref,q); |
128 |
✓✗✓✗ ✓✗✓✗ |
2 |
data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>(); |
129 |
|||
130 |
✓✗ | 2 |
SE3 cMo(SE3::Identity()); |
131 |
✓✗✓✗ ✓✗✓✗ |
2 |
cMo.translation() = -getComFromCrba(model, data_ref); |
132 |
|||
133 |
✓✗ | 2 |
SE3 oM1 (data_ref.liMi[1]); |
134 |
✓✗ | 2 |
SE3 cM1 (cMo * oM1); |
135 |
✓✗✓✗ ✓✗✓✗ |
4 |
Data::Matrix6x Ag_ref (cM1.toDualActionMatrix() * data_ref.M.topRows <6> ()); |
136 |
|||
137 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>()))); |
138 |
|||
139 |
✓✗ | 2 |
ccrba(model,data_ref,q,v); |
140 |
✓✗ | 2 |
dccrba(model,data,q,v); |
141 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ag.isApprox(Ag_ref)); |
142 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.hg.isApprox(data_ref.hg)); |
144 |
|||
145 |
✓✗ | 2 |
centerOfMass(model,data_ref,q,v,a); |
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(data_ref.vcom[0].isApprox(data.hg.linear()/data_ref.M(0,0))); |
147 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_ref.vcom[0].isApprox(data.vcom[0])); |
148 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear()/data_ref.M(0,0))); |
149 |
|||
150 |
✓✗✓✗ ✓✗✓✗ |
2 |
Force hdot(data.Ag * a + data.dAg * v); |
151 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(hdot.isApprox(hdot_ref)); |
152 |
|||
153 |
✓✗✓✗ |
2 |
dccrba(model,data,q,0*v); |
154 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dAg.isZero()); |
155 |
|||
156 |
// Check that dYcrb is equal to doYcrb |
||
157 |
{ |
||
158 |
// Compute dYcrb |
||
159 |
✓✗✓✗ ✓✗ |
4 |
Data data_ref(model), data_ref_plus(model), data(model); |
160 |
|||
161 |
2 |
const double alpha = 1e-8; |
|
162 |
✓✗ | 4 |
Eigen::VectorXd q_plus(model.nq); |
163 |
✓✗✓✗ |
2 |
q_plus = integrate(model,q,alpha*v); |
164 |
|||
165 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q); |
166 |
✓✗ | 2 |
crba(model,data_ref,q); |
167 |
✓✗ | 2 |
crba(model,data_ref_plus,q_plus); |
168 |
✓✗ | 2 |
forwardKinematics(model,data_ref_plus,q_plus); |
169 |
✓✗ | 2 |
dccrba(model,data,q,v); |
170 |
|||
171 |
✓✓ | 58 |
for(size_t i = 1; i < (size_t)model.njoints; ++i) |
172 |
{ |
||
173 |
✓✗✓✗ ✓✗ |
56 |
Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix() - |
174 |
✓✗✓✗ ✓✗✓✗ |
112 |
data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix())/alpha; |
175 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb,sqrt(alpha))); |
176 |
} |
||
177 |
} |
||
178 |
|||
179 |
{ |
||
180 |
✓✗ | 4 |
Data data(model); |
181 |
✓✗ | 2 |
ccrba(model,data_ref,q,v); |
182 |
✓✗ | 2 |
SE3 oMc_ref(SE3::Identity()); |
183 |
✓✗✓✗ |
2 |
oMc_ref.translation() = data_ref.com[0]; |
184 |
✓✗✓✗ ✓✗ |
4 |
const Data::Matrix6x Ag_ref = oMc_ref.toDualActionMatrix() * data_ref.Ag; |
185 |
✓✗ | 2 |
crba(model,data_ref,q); |
186 |
✓✗✓✗ ✓✗✓✗ |
4 |
const Data::Matrix6x Ag_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>(); |
187 |
|||
188 |
2 |
const double alpha = 1e-8; |
|
189 |
✓✗ | 4 |
Eigen::VectorXd q_plus(model.nq); |
190 |
✓✗✓✗ |
2 |
q_plus = integrate(model,q,alpha*v); |
191 |
✓✗ | 2 |
ccrba(model,data_ref,q_plus,v); |
192 |
✓✗ | 2 |
SE3 oMc_ref_plus(SE3::Identity()); |
193 |
✓✗✓✗ |
2 |
oMc_ref_plus.translation() = data_ref.com[0]; |
194 |
✓✗✓✗ ✓✗ |
4 |
const Data::Matrix6x Ag_plus_ref = oMc_ref_plus.toDualActionMatrix() * data_ref.Ag; |
195 |
✓✗ | 2 |
crba(model,data_ref,q_plus); |
196 |
✓✗✓✗ ✓✗✓✗ |
4 |
const Data::Matrix6x Ag_plus_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>(); |
197 |
✓✗✓✗ ✓✗ |
4 |
const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref)/alpha; |
198 |
✓✗✓✗ ✓✗ |
4 |
const Data::Matrix6x dAg_ref_from_M = (Ag_plus_ref_from_M - Ag_ref_from_M)/alpha; |
199 |
|||
200 |
✓✗ | 2 |
dccrba(model, data, q, v); |
201 |
✓✗ | 2 |
SE3 oMc(SE3::Identity()); |
202 |
✓✗✓✗ |
2 |
oMc.translation() = data.com[0]; |
203 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg; |
204 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(oMc.isApprox(oMc_ref)); |
205 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dAg_ref.isApprox(dAg_ref_from_M, sqrt(alpha))); |
206 |
} |
||
207 |
|||
208 |
// Check for dAg/dt |
||
209 |
{ |
||
210 |
✓✗ | 4 |
Data data(model); |
211 |
✓✗ | 2 |
ccrba(model,data_ref,q,v); |
212 |
✓✗ | 4 |
const Data::Matrix6x Ag_ref = data_ref.Ag; |
213 |
|||
214 |
2 |
const double alpha = 1e-8; |
|
215 |
✓✗ | 4 |
Eigen::VectorXd q_plus(model.nq); |
216 |
✓✗✓✗ |
2 |
q_plus = integrate(model,q,alpha*v); |
217 |
✓✗ | 2 |
ccrba(model,data_ref,q_plus,v); |
218 |
✓✗ | 4 |
const Data::Matrix6x Ag_plus_ref = data_ref.Ag; |
219 |
✓✗✓✗ ✓✗ |
4 |
const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref)/alpha; |
220 |
|||
221 |
✓✗ | 2 |
dccrba(model, data, q, v); |
222 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dAg.isApprox(dAg_ref,sqrt(alpha))); |
223 |
} |
||
224 |
|||
225 |
// Compute tensor dAg/dq |
||
226 |
{ |
||
227 |
✓✗✓✗ ✓✗ |
6 |
std::vector<Data::Matrix6x> dAgdq((size_t)model.nv,Data::Matrix6x::Zero(6,model.nv)); |
228 |
✓✗✓✗ |
4 |
Data data(model), data_fd(model); |
229 |
✓✗✓✗ |
4 |
Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv)); |
230 |
✓✗ | 2 |
ccrba(model,data_fd,q,v); |
231 |
✓✗ | 2 |
SE3 oMc_ref(SE3::Identity()); |
232 |
✓✗✓✗ |
2 |
oMc_ref.translation() = data_fd.com[0]; |
233 |
|||
234 |
✓✗✓✗ ✓✗ |
4 |
Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag; |
235 |
✓✗ | 2 |
const Force hg0 = oMc_ref.act(data_fd.hg); |
236 |
|||
237 |
✓✗ | 4 |
Data::Matrix6x Ag_fd(6,model.nv); |
238 |
✓✗ | 2 |
Force hg_fd; |
239 |
2 |
const double alpha = 1e-8; |
|
240 |
✓✗ | 4 |
Eigen::VectorXd q_plus(model.nq); |
241 |
✓✗ | 4 |
Data::Matrix6x dhdq(6,model.nv); |
242 |
✓✓ | 72 |
for(int k = 0; k < model.nv; ++k) |
243 |
{ |
||
244 |
✓✗ | 70 |
v_fd[k] = alpha; |
245 |
✓✗ | 70 |
q_plus = integrate(model,q,v_fd); |
246 |
✓✗ | 70 |
ccrba(model,data_fd,q_plus,v); |
247 |
✓✗ | 70 |
SE3 oMc_fd(SE3::Identity()); |
248 |
✓✗✓✗ |
70 |
oMc_fd.translation() = data_fd.com[0]; |
249 |
✓✗✓✗ ✓✗ |
70 |
Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag; |
250 |
✓✗✓✗ |
70 |
hg_fd = oMc_fd.act(data_fd.hg); |
251 |
✓✗✓✗ ✓✗ |
70 |
dAgdq[(size_t)k] = (Ag_fd - Ag0)/alpha; |
252 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
70 |
dhdq.col(k) = (hg_fd - hg0).toVector()/alpha; |
253 |
✓✗ | 70 |
v_fd[k] = 0.; |
254 |
} |
||
255 |
|||
256 |
✓✗✓✗ |
4 |
Data::Matrix6x dAg_ref(6,model.nv); dAg_ref.setZero(); |
257 |
✓✓ | 72 |
for(int k = 0; k < model.nv; ++k) |
258 |
{ |
||
259 |
✓✗✓✗ ✓✗ |
70 |
dAg_ref.col(k) = dAgdq[(size_t)k] * v; |
260 |
} |
||
261 |
|||
262 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(dhdq.isApprox(dAg_ref, sqrt(alpha))); |
263 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK((dAg_ref * v).isApprox(dhdq * v, sqrt(alpha))); |
264 |
} |
||
265 |
2 |
} |
|
266 |
|||
267 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_centroidal_mapping_time_derivative) |
268 |
{ |
||
269 |
✓✗ | 4 |
pinocchio::Model model; |
270 |
✓✗ | 2 |
pinocchio::buildModels::humanoidRandom(model); |
271 |
✓✗✓✗ |
4 |
pinocchio::Data data(model), data_ref(model); |
272 |
|||
273 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<3>().fill(-1.); |
274 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<3>().fill( 1.); |
275 |
✓✗ | 4 |
Eigen::VectorXd q = randomConfiguration(model); |
276 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
277 |
|||
278 |
✓✗ | 2 |
computeCentroidalMapTimeVariation(model, data, q, v); |
279 |
✓✗ | 2 |
dccrba(model,data_ref,q,v); |
280 |
|||
281 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
282 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
283 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.Ag.isApprox(data_ref.Ag)); |
284 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dAg.isApprox(data_ref.dAg)); |
285 |
|||
286 |
✓✗ | 2 |
computeJointJacobiansTimeVariation(model,data_ref,q,v); |
287 |
|||
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.J.isApprox(data_ref.J)); |
289 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dJ.isApprox(data_ref.dJ)); |
290 |
2 |
} |
|
291 |
|||
292 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_computeCentroidalMomentum_computeCentroidalMomentumTimeVariation) |
293 |
{ |
||
294 |
using namespace pinocchio; |
||
295 |
✓✗ | 4 |
Model model; |
296 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
297 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7"); |
298 |
✓✗✓✗ ✓✗✓✗ |
4 |
Data data(model), data_fk1(model), data_fk2(model), data_ref(model); |
299 |
|||
300 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
301 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill( 1.); |
302 |
|||
303 |
✓✗ | 4 |
Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit); |
304 |
✓✗✓✗ |
4 |
Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); |
305 |
✓✗✓✗ |
4 |
Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv); |
306 |
|||
307 |
✓✗ | 2 |
ccrba(model,data_ref,q,v); |
308 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q,v); |
309 |
✓✗ | 2 |
centerOfMass(model,data_ref,q,v,false); |
310 |
✓✗ | 2 |
computeCentroidalMomentum(model,data,q,v); |
311 |
|||
312 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(data.mass[0] == data_ref.mass[0]); |
313 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.com[0].isApprox(data_ref.com[0])); |
314 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.hg.isApprox(data_ref.hg)); |
315 |
✓✓ | 58 |
for(size_t k = 1; k < (size_t)model.njoints; ++k) |
316 |
{ |
||
317 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
56 |
BOOST_CHECK(data.mass[k] == data_ref.mass[k]); |
318 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK(data.com[k].isApprox(data_ref.com[k])); |
319 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
320 |
} |
||
321 |
|||
322 |
// Check other signature |
||
323 |
✓✗ | 2 |
forwardKinematics(model,data_fk1,q,v); |
324 |
✓✗ | 2 |
computeCentroidalMomentum(model,data_fk1); |
325 |
|||
326 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_fk1.hg.isApprox(data.hg)); |
327 |
|||
328 |
✓✗ | 2 |
computeCentroidalMomentumTimeVariation(model,data,q,v,a); |
329 |
✓✗ | 2 |
model.gravity.setZero(); |
330 |
✓✗ | 2 |
rnea(model,data_ref,q,v,a); |
331 |
✓✗ | 2 |
dccrba(model,data_ref,q,v); |
332 |
✓✗✓✗ ✓✗✓✗ |
2 |
const Force hgdot(data_ref.Ag * a + data_ref.dAg * v); |
333 |
|||
334 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(data.mass[0] == data_ref.mass[0]); |
335 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.com[0].isApprox(data_ref.com[0])); |
336 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.hg.isApprox(data_ref.hg)); |
337 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dhg.isApprox(hgdot)); |
338 |
✓✓ | 58 |
for(size_t k = 1; k < (size_t)model.njoints; ++k) |
339 |
{ |
||
340 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
56 |
BOOST_CHECK(data.mass[k] == data_ref.mass[k]); |
341 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK(data.com[k].isApprox(data_ref.com[k])); |
342 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK(data.v[k].isApprox(data_ref.v[k])); |
343 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK(data.a[k].isApprox(data_ref.a_gf[k])); |
344 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
56 |
BOOST_CHECK(data.f[k].isApprox(data_ref.f[k])); |
345 |
} |
||
346 |
|||
347 |
// Check other signature |
||
348 |
✓✗ | 2 |
forwardKinematics(model,data_fk2,q,v,a); |
349 |
✓✗ | 2 |
computeCentroidalMomentumTimeVariation(model,data_fk2); |
350 |
|||
351 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_fk2.hg.isApprox(data.hg)); |
352 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data_fk2.dhg.isApprox(data.dhg)); |
353 |
|||
354 |
// Check against finite differences |
||
355 |
✓✗ | 4 |
Data data_fd(model); |
356 |
2 |
const double eps = 1e-8; |
|
357 |
✓✗✓✗ ✓✗ |
4 |
Eigen::VectorXd v_plus = v + eps * a; |
358 |
✓✗✓✗ |
4 |
Eigen::VectorXd q_plus = integrate(model,q,eps*v); |
359 |
|||
360 |
✓✗✓✗ |
2 |
const Force hg = computeCentroidalMomentum(model,data_fd,q,v); |
361 |
✓✗ | 2 |
const SE3::Vector3 com = data_fd.com[0]; |
362 |
✓✗✓✗ |
2 |
const Force hg_plus = computeCentroidalMomentum(model,data_fd,q_plus,v_plus); |
363 |
✓✗ | 2 |
const SE3::Vector3 com_plus = data_fd.com[0]; |
364 |
|||
365 |
✓✗ | 2 |
SE3 transform(SE3::Identity()); |
366 |
✓✗✓✗ ✓✗ |
2 |
transform.translation() = com_plus - com; |
367 |
✓✗✓✗ ✓✗ |
2 |
Force dhg_ref = (transform.act(hg_plus) - hg)/eps; |
368 |
|||
369 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(data.dhg.isApprox(dhg_ref,sqrt(eps))); |
370 |
2 |
} |
|
371 |
|||
372 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |