| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018-2020 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/parsers/sample-models.hpp" |
||
13 |
|||
14 |
#include <iostream> |
||
15 |
|||
16 |
#include <boost/test/unit_test.hpp> |
||
17 |
#include <boost/utility/binary.hpp> |
||
18 |
|||
19 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
20 |
|||
21 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint) |
22 |
{ |
||
23 |
using namespace Eigen; |
||
24 |
using namespace pinocchio; |
||
25 |
|||
26 |
✓✗✓✗ |
4 |
pinocchio::Model model; buildModels::humanoidRandom(model); |
27 |
|||
28 |
✓✗ | 4 |
pinocchio::Data data(model); |
29 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
30 |
|||
31 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
32 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
33 |
|||
34 |
// const std::string joint_name = "larm5_joint"; |
||
35 |
// const JointIndex joint_id = model.getJointId(joint_name); |
||
36 |
|||
37 |
✓✗ | 4 |
const VectorXd q = randomConfiguration(model); |
38 |
|||
39 |
✓✗ | 2 |
forwardKinematics(model,data,q); |
40 |
|||
41 |
2 |
const double eps = 1e-8; |
|
42 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
43 |
{ |
||
44 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
45 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
46 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
47 |
|||
48 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
49 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
50 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
51 |
|||
52 |
✓✗ | 54 |
computeJointKinematicRegressor(model, data, joint_id, LOCAL, kinematic_regressor_L); |
53 |
✓✗ | 54 |
computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA); |
54 |
✓✗ | 54 |
computeJointKinematicRegressor(model, data, joint_id, WORLD, kinematic_regressor_W); |
55 |
|||
56 |
✓✗✓✗ |
108 |
Model model_plus = model; Data data_plus(model_plus); |
57 |
54 |
const SE3 & oMi = data.oMi[joint_id]; |
|
58 |
✓✗✓✗ ✓✗ |
54 |
const SE3 Mi_LWA = SE3(oMi.rotation(),SE3::Vector3::Zero()); |
59 |
54 |
const SE3 & oMi_plus = data_plus.oMi[joint_id]; |
|
60 |
✓✓ | 1512 |
for(int i = 1; i < model.njoints; ++i) |
61 |
{ |
||
62 |
✓✗✓✗ |
1458 |
Motion::Vector6 v = Motion::Vector6::Zero(); |
63 |
1458 |
const SE3 & M_placement = model.jointPlacements[(JointIndex)i]; |
|
64 |
1458 |
SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i]; |
|
65 |
✓✓ | 10206 |
for(Eigen::DenseIndex k = 0; k < 6; ++k) |
66 |
{ |
||
67 |
✓✗ | 8748 |
v[k] = eps; |
68 |
✓✗✓✗ ✓✗ |
8748 |
M_placement_plus = M_placement * exp6(Motion(v)); |
69 |
|||
70 |
✓✗ | 8748 |
forwardKinematics(model_plus,data_plus,q); |
71 |
|||
72 |
✓✗✓✗ |
8748 |
const Motion diff_L = log6(oMi.actInv(oMi_plus)); |
73 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8748 |
kinematic_regressor_L_fd.middleCols<6>(6*(i-1)).col(k) = diff_L.toVector()/eps; |
74 |
✓✗ | 8748 |
const Motion diff_LWA = Mi_LWA.act(diff_L); |
75 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8748 |
kinematic_regressor_LWA_fd.middleCols<6>(6*(i-1)).col(k) = diff_LWA.toVector()/eps; |
76 |
✓✗ | 8748 |
const Motion diff_W = oMi.act(diff_L); |
77 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8748 |
kinematic_regressor_W_fd.middleCols<6>(6*(i-1)).col(k) = diff_W.toVector()/eps; |
78 |
✓✗ | 8748 |
v[k] = 0.; |
79 |
} |
||
80 |
|||
81 |
✓✗ | 1458 |
M_placement_plus = M_placement; |
82 |
} |
||
83 |
|||
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd,sqrt(eps))); |
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd,sqrt(eps))); |
86 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd,sqrt(eps))); |
87 |
} |
||
88 |
2 |
} |
|
89 |
|||
90 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_joint_placement) |
91 |
{ |
||
92 |
using namespace Eigen; |
||
93 |
using namespace pinocchio; |
||
94 |
|||
95 |
✓✗✓✗ |
4 |
pinocchio::Model model; buildModels::humanoidRandom(model); |
96 |
|||
97 |
✓✗ | 4 |
pinocchio::Data data(model); |
98 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
99 |
|||
100 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
101 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
102 |
|||
103 |
✓✗ | 4 |
const VectorXd q = randomConfiguration(model); |
104 |
|||
105 |
✓✗ | 2 |
forwardKinematics(model,data,q); |
106 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q); |
107 |
|||
108 |
✓✓ | 56 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
109 |
{ |
||
110 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
111 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
112 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
113 |
|||
114 |
✓✗✓✗ |
54 |
computeJointKinematicRegressor(model, data, joint_id, LOCAL, SE3::Identity(), kinematic_regressor_L); |
115 |
✓✗✓✗ |
54 |
computeJointKinematicRegressor(model, data, joint_id, LOCAL_WORLD_ALIGNED, SE3::Identity(), kinematic_regressor_LWA); |
116 |
✓✗✓✗ |
54 |
computeJointKinematicRegressor(model, data, joint_id, WORLD, SE3::Identity(), kinematic_regressor_W); |
117 |
|||
118 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
119 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
120 |
✓✗✓✗ |
108 |
Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
121 |
|||
122 |
✓✗ | 54 |
computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL, kinematic_regressor_L_ref); |
123 |
✓✗ | 54 |
computeJointKinematicRegressor(model, data_ref, joint_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA_ref); |
124 |
✓✗ | 54 |
computeJointKinematicRegressor(model, data_ref, joint_id, WORLD, kinematic_regressor_W_ref); |
125 |
|||
126 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref)); |
127 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref)); |
128 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
54 |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref)); |
129 |
} |
||
130 |
2 |
} |
|
131 |
|||
132 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_kinematic_regressor_frame) |
133 |
{ |
||
134 |
using namespace Eigen; |
||
135 |
using namespace pinocchio; |
||
136 |
|||
137 |
✓✗✓✗ |
4 |
pinocchio::Model model; buildModels::humanoidRandom(model); |
138 |
|||
139 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
140 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
141 |
|||
142 |
✓✗ | 4 |
const std::string joint_name = "larm5_joint"; |
143 |
✓✗ | 2 |
const JointIndex joint_id = model.getJointId(joint_name); |
144 |
✓✗✓✗ ✓✗ |
2 |
model.addBodyFrame("test_body", joint_id, SE3::Random(), -1); |
145 |
|||
146 |
✓✗ | 4 |
pinocchio::Data data(model); |
147 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
148 |
|||
149 |
✓✗ | 4 |
const VectorXd q = randomConfiguration(model); |
150 |
|||
151 |
✓✗ | 2 |
forwardKinematics(model,data,q); |
152 |
✓✗ | 2 |
updateFramePlacements(model,data); |
153 |
✓✗ | 2 |
forwardKinematics(model,data_ref,q); |
154 |
|||
155 |
2 |
const double eps = 1e-8; |
|
156 |
✓✓ | 112 |
for(FrameIndex frame_id = 1; frame_id < (FrameIndex)model.nframes; ++frame_id) |
157 |
{ |
||
158 |
110 |
const Frame & frame = model.frames[frame_id]; |
|
159 |
|||
160 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_L(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
161 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_LWA(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
162 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_W(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
163 |
|||
164 |
✓✗ | 110 |
computeFrameKinematicRegressor(model, data, frame_id, LOCAL, kinematic_regressor_L); |
165 |
✓✗ | 110 |
computeFrameKinematicRegressor(model, data, frame_id, LOCAL_WORLD_ALIGNED, kinematic_regressor_LWA); |
166 |
✓✗ | 110 |
computeFrameKinematicRegressor(model, data, frame_id, WORLD, kinematic_regressor_W); |
167 |
|||
168 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_L_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
169 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_LWA_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
170 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_W_ref(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
171 |
|||
172 |
✓✗ | 110 |
computeJointKinematicRegressor(model, data_ref, frame.parent, LOCAL, frame.placement, kinematic_regressor_L_ref); |
173 |
✓✗ | 110 |
computeJointKinematicRegressor(model, data_ref, frame.parent, LOCAL_WORLD_ALIGNED, frame.placement, kinematic_regressor_LWA_ref); |
174 |
✓✗ | 110 |
computeJointKinematicRegressor(model, data_ref, frame.parent, WORLD, frame.placement, kinematic_regressor_W_ref); |
175 |
|||
176 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_ref)); |
177 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_ref)); |
178 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_ref)); |
179 |
|||
180 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_L_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
181 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_LWA_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
182 |
✓✗✓✗ |
220 |
Data::Matrix6x kinematic_regressor_W_fd(Data::Matrix6x::Zero(6,6*(model.njoints-1))); |
183 |
|||
184 |
✓✗✓✗ |
220 |
Model model_plus = model; Data data_plus(model_plus); |
185 |
110 |
const SE3 & oMf = data.oMf[frame_id]; |
|
186 |
✓✗✓✗ ✓✗ |
110 |
const SE3 Mf_LWA = SE3(oMf.rotation(),SE3::Vector3::Zero()); |
187 |
110 |
const SE3 & oMf_plus = data_plus.oMf[frame_id]; |
|
188 |
✓✓ | 3080 |
for(int i = 1; i < model.njoints; ++i) |
189 |
{ |
||
190 |
✓✗✓✗ |
2970 |
Motion::Vector6 v = Motion::Vector6::Zero(); |
191 |
2970 |
const SE3 & M_placement = model.jointPlacements[(JointIndex)i]; |
|
192 |
2970 |
SE3 & M_placement_plus = model_plus.jointPlacements[(JointIndex)i]; |
|
193 |
✓✓ | 20790 |
for(Eigen::DenseIndex k = 0; k < 6; ++k) |
194 |
{ |
||
195 |
✓✗ | 17820 |
v[k] = eps; |
196 |
✓✗✓✗ ✓✗ |
17820 |
M_placement_plus = M_placement * exp6(Motion(v)); |
197 |
|||
198 |
✓✗ | 17820 |
forwardKinematics(model_plus,data_plus,q); |
199 |
✓✗ | 17820 |
updateFramePlacements(model_plus,data_plus); |
200 |
|||
201 |
✓✗✓✗ |
17820 |
const Motion diff_L = log6(oMf.actInv(oMf_plus)); |
202 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
17820 |
kinematic_regressor_L_fd.middleCols<6>(6*(i-1)).col(k) = diff_L.toVector()/eps; |
203 |
✓✗ | 17820 |
const Motion diff_LWA = Mf_LWA.act(diff_L); |
204 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
17820 |
kinematic_regressor_LWA_fd.middleCols<6>(6*(i-1)).col(k) = diff_LWA.toVector()/eps; |
205 |
✓✗ | 17820 |
const Motion diff_W = oMf.act(diff_L); |
206 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
17820 |
kinematic_regressor_W_fd.middleCols<6>(6*(i-1)).col(k) = diff_W.toVector()/eps; |
207 |
✓✗ | 17820 |
v[k] = 0.; |
208 |
} |
||
209 |
|||
210 |
✓✗ | 2970 |
M_placement_plus = M_placement; |
211 |
} |
||
212 |
|||
213 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(kinematic_regressor_L.isApprox(kinematic_regressor_L_fd,sqrt(eps))); |
214 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(kinematic_regressor_LWA.isApprox(kinematic_regressor_LWA_fd,sqrt(eps))); |
215 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
110 |
BOOST_CHECK(kinematic_regressor_W.isApprox(kinematic_regressor_W_fd,sqrt(eps))); |
216 |
} |
||
217 |
2 |
} |
|
218 |
|||
219 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_static_regressor) |
220 |
{ |
||
221 |
using namespace Eigen; |
||
222 |
using namespace pinocchio; |
||
223 |
|||
224 |
✓✗✓✗ |
4 |
pinocchio::Model model; buildModels::humanoidRandom(model); |
225 |
|||
226 |
✓✗ | 4 |
pinocchio::Data data(model); |
227 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
228 |
|||
229 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
230 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
231 |
|||
232 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
233 |
✓✗ | 2 |
computeStaticRegressor(model,data,q); |
234 |
|||
235 |
✓✗ | 4 |
VectorXd phi(4*(model.njoints-1)); |
236 |
✓✓ | 56 |
for(int k = 1; k < model.njoints; ++k) |
237 |
{ |
||
238 |
54 |
const Inertia & Y = model.inertias[(size_t)k]; |
|
239 |
✓✗✓✗ ✓✗✓✗ |
54 |
phi.segment<4>(4*(k-1)) << Y.mass(), Y.mass() * Y.lever(); |
240 |
} |
||
241 |
|||
242 |
✓✗✓✗ |
2 |
Vector3d com = centerOfMass(model,data_ref,q); |
243 |
✓✗ | 2 |
Vector3d static_com_ref; |
244 |
✓✗ | 2 |
static_com_ref << com; |
245 |
|||
246 |
✓✗✓✗ |
2 |
Vector3d static_com = data.staticRegressor * phi; |
247 |
|||
248 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(static_com.isApprox(static_com_ref)); |
249 |
2 |
} |
|
250 |
|||
251 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_body_regressor) |
252 |
{ |
||
253 |
using namespace Eigen; |
||
254 |
using namespace pinocchio; |
||
255 |
|||
256 |
✓✗ | 2 |
Inertia I(Inertia::Random()); |
257 |
✓✗ | 2 |
Motion v(Motion::Random()); |
258 |
✓✗ | 2 |
Motion a(Motion::Random()); |
259 |
|||
260 |
✓✗✓✗ ✓✗ |
2 |
Force f = I*a + I.vxiv(v); |
261 |
|||
262 |
✓✗✓✗ ✓✗✓✗ |
2 |
Inertia::Vector6 f_regressor = bodyRegressor(v,a) * I.toDynamicParameters(); |
263 |
|||
264 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(f_regressor.isApprox(f.toVector())); |
265 |
2 |
} |
|
266 |
|||
267 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_joint_body_regressor) |
268 |
{ |
||
269 |
using namespace Eigen; |
||
270 |
using namespace pinocchio; |
||
271 |
|||
272 |
✓✗ | 4 |
pinocchio::Model model; |
273 |
✓✗ | 2 |
buildModels::manipulator(model); |
274 |
✓✗ | 4 |
pinocchio::Data data(model); |
275 |
|||
276 |
2 |
JointIndex JOINT_ID = JointIndex(model.njoints) - 1; |
|
277 |
|||
278 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
279 |
✓✗✓✗ |
4 |
VectorXd v = Eigen::VectorXd::Random(model.nv); |
280 |
✓✗✓✗ |
4 |
VectorXd a = Eigen::VectorXd::Random(model.nv); |
281 |
|||
282 |
✓✗ | 2 |
rnea(model,data,q,v,a); |
283 |
|||
284 |
✓✗ | 2 |
Force f = data.f[JOINT_ID]; |
285 |
|||
286 |
✓✗✓✗ ✓✗✓✗ |
2 |
Inertia::Vector6 f_regressor = jointBodyRegressor(model,data,JOINT_ID) * model.inertias[JOINT_ID].toDynamicParameters(); |
287 |
|||
288 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(f_regressor.isApprox(f.toVector())); |
289 |
2 |
} |
|
290 |
|||
291 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_frame_body_regressor) |
292 |
{ |
||
293 |
using namespace Eigen; |
||
294 |
using namespace pinocchio; |
||
295 |
|||
296 |
✓✗ | 4 |
pinocchio::Model model; |
297 |
✓✗ | 2 |
buildModels::manipulator(model); |
298 |
|||
299 |
2 |
JointIndex JOINT_ID = JointIndex(model.njoints) - 1; |
|
300 |
|||
301 |
✓✗ | 2 |
const SE3 & framePlacement = SE3::Random(); |
302 |
✓✗✓✗ |
2 |
FrameIndex FRAME_ID = model.addBodyFrame ("test_body", JOINT_ID, framePlacement, -1); |
303 |
|||
304 |
✓✗ | 4 |
pinocchio::Data data(model); |
305 |
|||
306 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
307 |
✓✗✓✗ |
4 |
VectorXd v = Eigen::VectorXd::Random(model.nv); |
308 |
✓✗✓✗ |
4 |
VectorXd a = Eigen::VectorXd::Random(model.nv); |
309 |
|||
310 |
✓✗ | 2 |
rnea(model,data,q,v,a); |
311 |
|||
312 |
✓✗ | 2 |
Force f = framePlacement.actInv(data.f[JOINT_ID]); |
313 |
✓✗ | 2 |
Inertia I = framePlacement.actInv(model.inertias[JOINT_ID]); |
314 |
|||
315 |
✓✗✓✗ ✓✗✓✗ |
2 |
Inertia::Vector6 f_regressor = frameBodyRegressor(model,data,FRAME_ID) * I.toDynamicParameters(); |
316 |
|||
317 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(f_regressor.isApprox(f.toVector())); |
318 |
2 |
} |
|
319 |
|||
320 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_joint_torque_regressor) |
321 |
{ |
||
322 |
using namespace Eigen; |
||
323 |
using namespace pinocchio; |
||
324 |
|||
325 |
✓✗ | 4 |
pinocchio::Model model; |
326 |
✓✗ | 2 |
buildModels::humanoidRandom(model); |
327 |
|||
328 |
✓✗✓✗ |
2 |
model.lowerPositionLimit.head<7>().fill(-1.); |
329 |
✓✗✓✗ |
2 |
model.upperPositionLimit.head<7>().fill(1.); |
330 |
|||
331 |
✓✗ | 4 |
pinocchio::Data data(model); |
332 |
✓✗ | 4 |
pinocchio::Data data_ref(model); |
333 |
|||
334 |
✓✗ | 4 |
VectorXd q = randomConfiguration(model); |
335 |
✓✗✓✗ |
4 |
VectorXd v = Eigen::VectorXd::Random(model.nv); |
336 |
✓✗✓✗ |
4 |
VectorXd a = Eigen::VectorXd::Random(model.nv); |
337 |
|||
338 |
✓✗ | 2 |
rnea(model,data_ref,q,v,a); |
339 |
|||
340 |
✓✗ | 4 |
Eigen::VectorXd params(10*(model.njoints-1)); |
341 |
✓✓ | 56 |
for(JointIndex i=1; i<(Model::JointIndex)model.njoints; ++i) |
342 |
✓✗✓✗ ✓✗ |
54 |
params.segment<10>((int)((i-1)*10)) = model.inertias[i].toDynamicParameters(); |
343 |
|||
344 |
✓✗ | 2 |
computeJointTorqueRegressor(model,data,q,v,a); |
345 |
|||
346 |
✓✗✓✗ |
4 |
Eigen::VectorXd tau_regressor = data.jointTorqueRegressor * params; |
347 |
|||
348 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
2 |
BOOST_CHECK(tau_regressor.isApprox(data_ref.tau)); |
349 |
2 |
} |
|
350 |
|||
351 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |