Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2018-2024 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/spatial/fwd.hpp" |
6 |
|
|
#include "pinocchio/spatial/explog.hpp" |
7 |
|
|
#include "pinocchio/algorithm/regressor.hpp" |
8 |
|
|
#include "pinocchio/algorithm/rnea.hpp" |
9 |
|
|
#include "pinocchio/algorithm/frames.hpp" |
10 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
11 |
|
|
#include "pinocchio/algorithm/center-of-mass.hpp" |
12 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
13 |
|
|
#include "pinocchio/algorithm/compute-all-terms.hpp" |
14 |
|
|
|
15 |
|
|
#include <iostream> |
16 |
|
|
|
17 |
|
|
#include <boost/test/unit_test.hpp> |
18 |
|
|
#include <boost/utility/binary.hpp> |
19 |
|
|
|
20 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
21 |
|
|
|
22 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint) |
23 |
|
|
{ |
24 |
|
|
using namespace Eigen; |
25 |
|
|
using namespace pinocchio; |
26 |
|
|
|
27 |
|
✗ |
pinocchio::Model model; |
28 |
|
✗ |
buildModels::humanoidRandom(model); |
29 |
|
|
|
30 |
|
✗ |
pinocchio::Data data(model); |
31 |
|
✗ |
pinocchio::Data data_ref(model); |
32 |
|
|
|
33 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
34 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
35 |
|
|
|
36 |
|
|
// const std::string joint_name = "larm5_joint"; |
37 |
|
|
// const JointIndex joint_id = model.getJointId(joint_name); |
38 |
|
|
|
39 |
|
✗ |
const VectorXd q = randomConfiguration(model); |
40 |
|
|
|
41 |
|
✗ |
forwardKinematics(model, data, q); |
42 |
|
|
|
43 |
|
✗ |
const double eps = 1e-8; |
44 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
45 |
|
|
{ |
46 |
|
✗ |
Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
47 |
|
✗ |
Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
48 |
|
✗ |
Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
49 |
|
|
|
50 |
|
✗ |
Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
51 |
|
✗ |
Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
52 |
|
✗ |
Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
53 |
|
|
|
54 |
|
✗ |
computeJointKinematicRegressor(model, data, joint_id, LOCAL, kinematic_regressor_L); |
55 |
|
✗ |
computeJointKinematicRegressor( |
56 |
|
|
model, data, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA); |
57 |
|
✗ |
computeJointKinematicRegressor(model, data, joint_id, WORLD, kinematic_regressor_W); |
58 |
|
|
|
59 |
|
✗ |
Model model_plus = model; |
60 |
|
✗ |
Data data_plus(model_plus); |
61 |
|
✗ |
const SE3 & oMi = data.oMi[joint_id]; |
62 |
|
✗ |
const SE3 Mi_LWA = SE3(oMi.rotation(), SE3::Vector3::Zero()); |
63 |
|
✗ |
const SE3 & oMi_plus = data_plus.oMi[joint_id]; |
64 |
|
✗ |
for (int i = 1; i < model.njoints; ++i) |
65 |
|
|
{ |
66 |
|
✗ |
Motion::Vector6 v = Motion::Vector6::Zero(); |
67 |
|
✗ |
const SE3 & M_placement = model.jointPlacements[(JointIndex)i]; |
68 |
|
✗ |
SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i]; |
69 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < 6; ++k) |
70 |
|
|
{ |
71 |
|
✗ |
v[k] = eps; |
72 |
|
✗ |
M_placement_plus = M_placement * exp6(Motion(v)); |
73 |
|
|
|
74 |
|
✗ |
forwardKinematics(model_plus, data_plus, q); |
75 |
|
|
|
76 |
|
✗ |
const Motion diff_L = log6(oMi.actInv(oMi_plus)); |
77 |
|
✗ |
kinematic_regressor_L_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_L.toVector() / eps; |
78 |
|
✗ |
const Motion diff_LWA = Mi_LWA.act(diff_L); |
79 |
|
✗ |
kinematic_regressor_LWA_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_LWA.toVector() / eps; |
80 |
|
✗ |
const Motion diff_W = oMi.act(diff_L); |
81 |
|
✗ |
kinematic_regressor_W_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_W.toVector() / eps; |
82 |
|
✗ |
v[k] = 0.; |
83 |
|
|
} |
84 |
|
|
|
85 |
|
✗ |
M_placement_plus = M_placement; |
86 |
|
|
} |
87 |
|
|
|
88 |
|
✗ |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd, sqrt(eps))); |
89 |
|
✗ |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd, sqrt(eps))); |
90 |
|
✗ |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd, sqrt(eps))); |
91 |
|
|
} |
92 |
|
|
} |
93 |
|
|
|
94 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint_placement) |
95 |
|
|
{ |
96 |
|
|
using namespace Eigen; |
97 |
|
|
using namespace pinocchio; |
98 |
|
|
|
99 |
|
✗ |
pinocchio::Model model; |
100 |
|
✗ |
buildModels::humanoidRandom(model); |
101 |
|
|
|
102 |
|
✗ |
pinocchio::Data data(model); |
103 |
|
✗ |
pinocchio::Data data_ref(model); |
104 |
|
|
|
105 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
106 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
107 |
|
|
|
108 |
|
✗ |
const VectorXd q = randomConfiguration(model); |
109 |
|
|
|
110 |
|
✗ |
forwardKinematics(model, data, q); |
111 |
|
✗ |
forwardKinematics(model, data_ref, q); |
112 |
|
|
|
113 |
|
✗ |
for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
114 |
|
|
{ |
115 |
|
✗ |
Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
116 |
|
✗ |
Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
117 |
|
✗ |
Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
118 |
|
|
|
119 |
|
✗ |
computeJointKinematicRegressor( |
120 |
|
✗ |
model, data, joint_id, LOCAL, SE3::Identity(), kinematic_regressor_L); |
121 |
|
✗ |
computeJointKinematicRegressor( |
122 |
|
✗ |
model, data, joint_id, LOCAL_WORLD_ALIGNED, SE3::Identity(), kinematic_regressor_LWA); |
123 |
|
✗ |
computeJointKinematicRegressor( |
124 |
|
✗ |
model, data, joint_id, WORLD, SE3::Identity(), kinematic_regressor_W); |
125 |
|
|
|
126 |
|
✗ |
Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
127 |
|
✗ |
Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
128 |
|
✗ |
Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
129 |
|
|
|
130 |
|
✗ |
computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL, kinematic_regressor_L_ref); |
131 |
|
✗ |
computeJointKinematicRegressor( |
132 |
|
|
model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA_ref); |
133 |
|
✗ |
computeJointKinematicRegressor(model, data_ref, joint_id, WORLD, kinematic_regressor_W_ref); |
134 |
|
|
|
135 |
|
✗ |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref)); |
136 |
|
✗ |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref)); |
137 |
|
✗ |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref)); |
138 |
|
|
} |
139 |
|
|
} |
140 |
|
|
|
141 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_frame) |
142 |
|
|
{ |
143 |
|
|
using namespace Eigen; |
144 |
|
|
using namespace pinocchio; |
145 |
|
|
|
146 |
|
✗ |
pinocchio::Model model; |
147 |
|
✗ |
buildModels::humanoidRandom(model); |
148 |
|
|
|
149 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
150 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
151 |
|
|
|
152 |
|
✗ |
const std::string joint_name = "larm5_joint"; |
153 |
|
✗ |
const JointIndex joint_id = model.getJointId(joint_name); |
154 |
|
✗ |
model.addBodyFrame("test_body", joint_id, SE3::Random(), -1); |
155 |
|
|
|
156 |
|
✗ |
pinocchio::Data data(model); |
157 |
|
✗ |
pinocchio::Data data_ref(model); |
158 |
|
|
|
159 |
|
✗ |
const VectorXd q = randomConfiguration(model); |
160 |
|
|
|
161 |
|
✗ |
forwardKinematics(model, data, q); |
162 |
|
✗ |
updateFramePlacements(model, data); |
163 |
|
✗ |
forwardKinematics(model, data_ref, q); |
164 |
|
|
|
165 |
|
✗ |
const double eps = 1e-8; |
166 |
|
✗ |
for (FrameIndex frame_id = 1; frame_id < (FrameIndex)model.nframes; ++frame_id) |
167 |
|
|
{ |
168 |
|
✗ |
const Frame & frame = model.frames[frame_id]; |
169 |
|
|
|
170 |
|
✗ |
Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
171 |
|
✗ |
Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
172 |
|
✗ |
Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
173 |
|
|
|
174 |
|
✗ |
computeFrameKinematicRegressor(model, data, frame_id, LOCAL, kinematic_regressor_L); |
175 |
|
✗ |
computeFrameKinematicRegressor( |
176 |
|
|
model, data, frame_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA); |
177 |
|
✗ |
computeFrameKinematicRegressor(model, data, frame_id, WORLD, kinematic_regressor_W); |
178 |
|
|
|
179 |
|
✗ |
Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
180 |
|
✗ |
Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
181 |
|
✗ |
Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
182 |
|
|
|
183 |
|
✗ |
computeJointKinematicRegressor( |
184 |
|
✗ |
model, data_ref, frame.parentJoint, LOCAL, frame.placement, kinematic_regressor_L_ref); |
185 |
|
✗ |
computeJointKinematicRegressor( |
186 |
|
✗ |
model, data_ref, frame.parentJoint, LOCAL_WORLD_ALIGNED, frame.placement, |
187 |
|
|
kinematic_regressor_LWA_ref); |
188 |
|
✗ |
computeJointKinematicRegressor( |
189 |
|
✗ |
model, data_ref, frame.parentJoint, WORLD, frame.placement, kinematic_regressor_W_ref); |
190 |
|
|
|
191 |
|
✗ |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref)); |
192 |
|
✗ |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref)); |
193 |
|
✗ |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref)); |
194 |
|
|
|
195 |
|
✗ |
Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
196 |
|
✗ |
Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
197 |
|
✗ |
Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6, 6 * (model.njoints - 1))); |
198 |
|
|
|
199 |
|
✗ |
Model model_plus = model; |
200 |
|
✗ |
Data data_plus(model_plus); |
201 |
|
✗ |
const SE3 & oMf = data.oMf[frame_id]; |
202 |
|
✗ |
const SE3 Mf_LWA = SE3(oMf.rotation(), SE3::Vector3::Zero()); |
203 |
|
✗ |
const SE3 & oMf_plus = data_plus.oMf[frame_id]; |
204 |
|
✗ |
for (int i = 1; i < model.njoints; ++i) |
205 |
|
|
{ |
206 |
|
✗ |
Motion::Vector6 v = Motion::Vector6::Zero(); |
207 |
|
✗ |
const SE3 & M_placement = model.jointPlacements[(JointIndex)i]; |
208 |
|
✗ |
SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i]; |
209 |
|
✗ |
for (Eigen::DenseIndex k = 0; k < 6; ++k) |
210 |
|
|
{ |
211 |
|
✗ |
v[k] = eps; |
212 |
|
✗ |
M_placement_plus = M_placement * exp6(Motion(v)); |
213 |
|
|
|
214 |
|
✗ |
forwardKinematics(model_plus, data_plus, q); |
215 |
|
✗ |
updateFramePlacements(model_plus, data_plus); |
216 |
|
|
|
217 |
|
✗ |
const Motion diff_L = log6(oMf.actInv(oMf_plus)); |
218 |
|
✗ |
kinematic_regressor_L_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_L.toVector() / eps; |
219 |
|
✗ |
const Motion diff_LWA = Mf_LWA.act(diff_L); |
220 |
|
✗ |
kinematic_regressor_LWA_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_LWA.toVector() / eps; |
221 |
|
✗ |
const Motion diff_W = oMf.act(diff_L); |
222 |
|
✗ |
kinematic_regressor_W_fd.middleCols<6>(6 * (i - 1)).col(k) = diff_W.toVector() / eps; |
223 |
|
✗ |
v[k] = 0.; |
224 |
|
|
} |
225 |
|
|
|
226 |
|
✗ |
M_placement_plus = M_placement; |
227 |
|
|
} |
228 |
|
|
|
229 |
|
✗ |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd, sqrt(eps))); |
230 |
|
✗ |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd, sqrt(eps))); |
231 |
|
✗ |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd, sqrt(eps))); |
232 |
|
|
} |
233 |
|
|
} |
234 |
|
|
|
235 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_static_regressor) |
236 |
|
|
{ |
237 |
|
|
using namespace Eigen; |
238 |
|
|
using namespace pinocchio; |
239 |
|
|
|
240 |
|
✗ |
pinocchio::Model model; |
241 |
|
✗ |
buildModels::humanoidRandom(model); |
242 |
|
|
|
243 |
|
✗ |
pinocchio::Data data(model); |
244 |
|
✗ |
pinocchio::Data data_ref(model); |
245 |
|
|
|
246 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
247 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
248 |
|
|
|
249 |
|
✗ |
VectorXd q = randomConfiguration(model); |
250 |
|
✗ |
computeStaticRegressor(model, data, q); |
251 |
|
|
|
252 |
|
✗ |
VectorXd phi(4 * (model.njoints - 1)); |
253 |
|
✗ |
for (int k = 1; k < model.njoints; ++k) |
254 |
|
|
{ |
255 |
|
✗ |
const Inertia & Y = model.inertias[(size_t)k]; |
256 |
|
✗ |
phi.segment<4>(4 * (k - 1)) << Y.mass(), Y.mass() * Y.lever(); |
257 |
|
|
} |
258 |
|
|
|
259 |
|
✗ |
Vector3d com = centerOfMass(model, data_ref, q); |
260 |
|
✗ |
Vector3d static_com_ref; |
261 |
|
✗ |
static_com_ref << com; |
262 |
|
|
|
263 |
|
✗ |
Vector3d static_com = data.staticRegressor * phi; |
264 |
|
|
|
265 |
|
✗ |
BOOST_CHECK(static_com.isApprox(static_com_ref)); |
266 |
|
|
} |
267 |
|
|
|
268 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_body_regressor) |
269 |
|
|
{ |
270 |
|
|
using namespace Eigen; |
271 |
|
|
using namespace pinocchio; |
272 |
|
|
|
273 |
|
✗ |
Inertia I(Inertia::Random()); |
274 |
|
✗ |
Motion v(Motion::Random()); |
275 |
|
✗ |
Motion a(Motion::Random()); |
276 |
|
|
|
277 |
|
✗ |
Force f = I * a + I.vxiv(v); |
278 |
|
|
|
279 |
|
✗ |
Inertia::Vector6 f_regressor = bodyRegressor(v, a) * I.toDynamicParameters(); |
280 |
|
|
|
281 |
|
✗ |
BOOST_CHECK(f_regressor.isApprox(f.toVector())); |
282 |
|
|
} |
283 |
|
|
|
284 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_joint_body_regressor) |
285 |
|
|
{ |
286 |
|
|
using namespace Eigen; |
287 |
|
|
using namespace pinocchio; |
288 |
|
|
|
289 |
|
✗ |
pinocchio::Model model; |
290 |
|
✗ |
buildModels::manipulator(model); |
291 |
|
✗ |
pinocchio::Data data(model); |
292 |
|
|
|
293 |
|
✗ |
JointIndex JOINT_ID = JointIndex(model.njoints) - 1; |
294 |
|
|
|
295 |
|
✗ |
VectorXd q = randomConfiguration(model); |
296 |
|
✗ |
VectorXd v = Eigen::VectorXd::Random(model.nv); |
297 |
|
✗ |
VectorXd a = Eigen::VectorXd::Random(model.nv); |
298 |
|
|
|
299 |
|
✗ |
rnea(model, data, q, v, a); |
300 |
|
|
|
301 |
|
✗ |
Force f = data.f[JOINT_ID]; |
302 |
|
|
|
303 |
|
|
Inertia::Vector6 f_regressor = |
304 |
|
✗ |
jointBodyRegressor(model, data, JOINT_ID) * model.inertias[JOINT_ID].toDynamicParameters(); |
305 |
|
|
|
306 |
|
✗ |
BOOST_CHECK(f_regressor.isApprox(f.toVector())); |
307 |
|
|
} |
308 |
|
|
|
309 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_frame_body_regressor) |
310 |
|
|
{ |
311 |
|
|
using namespace Eigen; |
312 |
|
|
using namespace pinocchio; |
313 |
|
|
|
314 |
|
✗ |
pinocchio::Model model; |
315 |
|
✗ |
buildModels::manipulator(model); |
316 |
|
|
|
317 |
|
✗ |
JointIndex JOINT_ID = JointIndex(model.njoints) - 1; |
318 |
|
|
|
319 |
|
✗ |
const SE3 & framePlacement = SE3::Random(); |
320 |
|
✗ |
FrameIndex FRAME_ID = model.addBodyFrame("test_body", JOINT_ID, framePlacement, -1); |
321 |
|
|
|
322 |
|
✗ |
pinocchio::Data data(model); |
323 |
|
|
|
324 |
|
✗ |
VectorXd q = randomConfiguration(model); |
325 |
|
✗ |
VectorXd v = Eigen::VectorXd::Random(model.nv); |
326 |
|
✗ |
VectorXd a = Eigen::VectorXd::Random(model.nv); |
327 |
|
|
|
328 |
|
✗ |
rnea(model, data, q, v, a); |
329 |
|
|
|
330 |
|
✗ |
Force f = framePlacement.actInv(data.f[JOINT_ID]); |
331 |
|
✗ |
Inertia I = framePlacement.actInv(model.inertias[JOINT_ID]); |
332 |
|
|
|
333 |
|
|
Inertia::Vector6 f_regressor = |
334 |
|
✗ |
frameBodyRegressor(model, data, FRAME_ID) * I.toDynamicParameters(); |
335 |
|
|
|
336 |
|
✗ |
BOOST_CHECK(f_regressor.isApprox(f.toVector())); |
337 |
|
|
} |
338 |
|
|
|
339 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_joint_torque_regressor) |
340 |
|
|
{ |
341 |
|
|
using namespace Eigen; |
342 |
|
|
using namespace pinocchio; |
343 |
|
|
|
344 |
|
✗ |
pinocchio::Model model; |
345 |
|
✗ |
buildModels::humanoidRandom(model); |
346 |
|
|
|
347 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
348 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
349 |
|
|
|
350 |
|
✗ |
pinocchio::Data data(model); |
351 |
|
✗ |
pinocchio::Data data_ref(model); |
352 |
|
|
|
353 |
|
✗ |
VectorXd q = randomConfiguration(model); |
354 |
|
✗ |
VectorXd v = Eigen::VectorXd::Random(model.nv); |
355 |
|
✗ |
VectorXd a = Eigen::VectorXd::Random(model.nv); |
356 |
|
|
|
357 |
|
✗ |
rnea(model, data_ref, q, v, a); |
358 |
|
|
|
359 |
|
✗ |
Eigen::VectorXd params(10 * (model.njoints - 1)); |
360 |
|
✗ |
for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
361 |
|
✗ |
params.segment<10>((int)((i - 1) * 10)) = model.inertias[i].toDynamicParameters(); |
362 |
|
|
|
363 |
|
✗ |
computeJointTorqueRegressor(model, data, q, v, a); |
364 |
|
|
|
365 |
|
✗ |
Eigen::VectorXd tau_regressor = data.jointTorqueRegressor * params; |
366 |
|
|
|
367 |
|
✗ |
BOOST_CHECK(tau_regressor.isApprox(data_ref.tau)); |
368 |
|
|
} |
369 |
|
|
|
370 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinetic_energy_regressor) |
371 |
|
|
{ |
372 |
|
|
using namespace Eigen; |
373 |
|
|
using namespace pinocchio; |
374 |
|
|
|
375 |
|
✗ |
pinocchio::Model model; |
376 |
|
✗ |
buildModels::humanoidRandom(model); |
377 |
|
|
|
378 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
379 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
380 |
|
|
|
381 |
|
✗ |
pinocchio::Data data(model); |
382 |
|
✗ |
pinocchio::Data data_ref(model); |
383 |
|
|
|
384 |
|
✗ |
const VectorXd q = randomConfiguration(model); |
385 |
|
✗ |
const VectorXd v = Eigen::VectorXd::Random(model.nv); |
386 |
|
|
|
387 |
|
✗ |
computeAllTerms(model, data_ref, q, v); |
388 |
|
✗ |
auto target_energy = computeKineticEnergy(model, data_ref); |
389 |
|
|
|
390 |
|
✗ |
const auto regressor = computeKineticEnergyRegressor(model, data, q, v); |
391 |
|
|
|
392 |
|
✗ |
Eigen::VectorXd params(10 * (model.njoints - 1)); |
393 |
|
✗ |
for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
394 |
|
✗ |
params.segment<10>(Eigen::DenseIndex((i - 1) * 10)) = model.inertias[i].toDynamicParameters(); |
395 |
|
|
|
396 |
|
✗ |
const double kinetic_energy_regressor = data.kineticEnergyRegressor * params; |
397 |
|
|
|
398 |
|
✗ |
BOOST_CHECK_CLOSE(kinetic_energy_regressor, target_energy, 1e-12); |
399 |
|
|
} |
400 |
|
|
|
401 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_potential_energy_regressor) |
402 |
|
|
{ |
403 |
|
|
using namespace Eigen; |
404 |
|
|
using namespace pinocchio; |
405 |
|
|
|
406 |
|
✗ |
pinocchio::Model model; |
407 |
|
✗ |
buildModels::humanoidRandom(model); |
408 |
|
|
|
409 |
|
✗ |
model.lowerPositionLimit.head<7>().fill(-1.); |
410 |
|
✗ |
model.upperPositionLimit.head<7>().fill(1.); |
411 |
|
|
|
412 |
|
✗ |
pinocchio::Data data(model); |
413 |
|
✗ |
pinocchio::Data data_ref(model); |
414 |
|
|
|
415 |
|
✗ |
const VectorXd q = randomConfiguration(model); |
416 |
|
✗ |
const VectorXd v = Eigen::VectorXd::Random(model.nv); |
417 |
|
|
|
418 |
|
✗ |
computeAllTerms(model, data_ref, q, v); |
419 |
|
✗ |
const double target_energy = computePotentialEnergy(model, data_ref); |
420 |
|
|
|
421 |
|
✗ |
Eigen::VectorXd params(10 * (model.njoints - 1)); |
422 |
|
✗ |
for (JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
423 |
|
✗ |
params.segment<10>(Eigen::DenseIndex((i - 1) * 10)) = model.inertias[i].toDynamicParameters(); |
424 |
|
|
|
425 |
|
✗ |
computePotentialEnergyRegressor(model, data, q); |
426 |
|
✗ |
const double potential_energy_regressor = data.potentialEnergyRegressor * params; |
427 |
|
|
|
428 |
|
✗ |
BOOST_CHECK_CLOSE(potential_energy_regressor, target_energy, 1e-12); |
429 |
|
|
} |
430 |
|
|
|
431 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
432 |
|
|
|