Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2018-2019 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include "pinocchio/multibody/model.hpp" |
6 |
|
|
#include "pinocchio/multibody/data.hpp" |
7 |
|
|
#include "pinocchio/algorithm/kinematics.hpp" |
8 |
|
|
#include "pinocchio/algorithm/crba.hpp" |
9 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
10 |
|
|
#include "pinocchio/multibody/sample-models.hpp" |
11 |
|
|
|
12 |
|
|
#include <iostream> |
13 |
|
|
|
14 |
|
|
#include <boost/test/unit_test.hpp> |
15 |
|
|
#include <boost/utility/binary.hpp> |
16 |
|
|
|
17 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
18 |
|
|
|
19 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input) |
20 |
|
|
{ |
21 |
|
|
using namespace Eigen; |
22 |
|
|
using namespace pinocchio; |
23 |
|
|
|
24 |
|
✗ |
Model model; |
25 |
|
✗ |
buildModels::humanoidRandom(model); |
26 |
|
|
|
27 |
|
✗ |
Data data(model); |
28 |
|
|
|
29 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
30 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
31 |
|
✗ |
VectorXd q = randomConfiguration(model); |
32 |
|
|
|
33 |
|
✗ |
forwardKinematics(model, data, Model::ConfigVectorType::Ones(model.nq)); |
34 |
|
|
} |
35 |
|
|
|
36 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_zero) |
37 |
|
|
{ |
38 |
|
|
using namespace Eigen; |
39 |
|
|
using namespace pinocchio; |
40 |
|
|
|
41 |
|
✗ |
Model model; |
42 |
|
✗ |
buildModels::humanoidRandom(model); |
43 |
|
|
|
44 |
|
✗ |
Data data(model), data_ref(model); |
45 |
|
|
|
46 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
47 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
48 |
|
✗ |
VectorXd q = randomConfiguration(model); |
49 |
|
|
|
50 |
|
✗ |
forwardKinematics(model, data_ref, q); |
51 |
|
✗ |
crba(model, data, q, Convention::WORLD); |
52 |
|
✗ |
updateGlobalPlacements(model, data); |
53 |
|
|
|
54 |
|
✗ |
for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
55 |
|
|
{ |
56 |
|
✗ |
BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]); |
57 |
|
✗ |
BOOST_CHECK(data.liMi[i] == data_ref.liMi[i]); |
58 |
|
|
} |
59 |
|
|
} |
60 |
|
|
|
61 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_first) |
62 |
|
|
{ |
63 |
|
|
using namespace Eigen; |
64 |
|
|
using namespace pinocchio; |
65 |
|
|
|
66 |
|
✗ |
Model model; |
67 |
|
✗ |
buildModels::humanoidRandom(model); |
68 |
|
|
|
69 |
|
✗ |
Data data(model); |
70 |
|
|
|
71 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
72 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
73 |
|
✗ |
VectorXd q = randomConfiguration(model); |
74 |
|
✗ |
VectorXd v(VectorXd::Zero(model.nv)); |
75 |
|
|
|
76 |
|
✗ |
forwardKinematics(model, data, q, v); |
77 |
|
|
|
78 |
|
✗ |
for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
79 |
|
|
{ |
80 |
|
✗ |
BOOST_CHECK(data.v[i] == Motion::Zero()); |
81 |
|
|
} |
82 |
|
|
} |
83 |
|
|
|
84 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematics_second) |
85 |
|
|
{ |
86 |
|
|
using namespace Eigen; |
87 |
|
|
using namespace pinocchio; |
88 |
|
|
|
89 |
|
✗ |
Model model; |
90 |
|
✗ |
buildModels::humanoidRandom(model); |
91 |
|
|
|
92 |
|
✗ |
Data data(model); |
93 |
|
|
|
94 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
95 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
96 |
|
✗ |
VectorXd q = randomConfiguration(model); |
97 |
|
✗ |
VectorXd v(VectorXd::Zero(model.nv)); |
98 |
|
✗ |
VectorXd a(VectorXd::Zero(model.nv)); |
99 |
|
|
|
100 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
101 |
|
|
|
102 |
|
✗ |
for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
103 |
|
|
{ |
104 |
|
✗ |
BOOST_CHECK(data.v[i] == Motion::Zero()); |
105 |
|
✗ |
BOOST_CHECK(data.a[i] == Motion::Zero()); |
106 |
|
|
} |
107 |
|
|
} |
108 |
|
|
|
109 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_get_velocity) |
110 |
|
|
{ |
111 |
|
|
using namespace Eigen; |
112 |
|
|
using namespace pinocchio; |
113 |
|
|
|
114 |
|
✗ |
Model model; |
115 |
|
✗ |
buildModels::humanoidRandom(model); |
116 |
|
|
|
117 |
|
✗ |
Data data(model); |
118 |
|
|
|
119 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
120 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
121 |
|
✗ |
VectorXd q = randomConfiguration(model); |
122 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
123 |
|
|
|
124 |
|
✗ |
forwardKinematics(model, data, q, v); |
125 |
|
|
|
126 |
|
✗ |
for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
127 |
|
|
{ |
128 |
|
✗ |
BOOST_CHECK(data.v[i].isApprox(getVelocity(model, data, i))); |
129 |
|
✗ |
BOOST_CHECK(data.v[i].isApprox(getVelocity(model, data, i, LOCAL))); |
130 |
|
✗ |
BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model, data, i, WORLD))); |
131 |
|
✗ |
BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()) |
132 |
|
|
.act(data.v[i]) |
133 |
|
|
.isApprox(getVelocity(model, data, i, LOCAL_WORLD_ALIGNED))); |
134 |
|
|
} |
135 |
|
|
} |
136 |
|
|
|
137 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_get_acceleration) |
138 |
|
|
{ |
139 |
|
|
using namespace Eigen; |
140 |
|
|
using namespace pinocchio; |
141 |
|
|
|
142 |
|
✗ |
Model model; |
143 |
|
✗ |
buildModels::humanoidRandom(model); |
144 |
|
|
|
145 |
|
✗ |
Data data(model); |
146 |
|
|
|
147 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
148 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
149 |
|
✗ |
VectorXd q = randomConfiguration(model); |
150 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
151 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
152 |
|
|
|
153 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
154 |
|
|
|
155 |
|
✗ |
for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
156 |
|
|
{ |
157 |
|
✗ |
BOOST_CHECK(data.a[i].isApprox(getAcceleration(model, data, i))); |
158 |
|
✗ |
BOOST_CHECK(data.a[i].isApprox(getAcceleration(model, data, i, LOCAL))); |
159 |
|
✗ |
BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model, data, i, WORLD))); |
160 |
|
✗ |
BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()) |
161 |
|
|
.act(data.a[i]) |
162 |
|
|
.isApprox(getAcceleration(model, data, i, LOCAL_WORLD_ALIGNED))); |
163 |
|
|
} |
164 |
|
|
} |
165 |
|
|
|
166 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_get_classical_acceleration) |
167 |
|
|
{ |
168 |
|
|
using namespace Eigen; |
169 |
|
|
using namespace pinocchio; |
170 |
|
|
|
171 |
|
✗ |
Model model; |
172 |
|
✗ |
buildModels::humanoidRandom(model); |
173 |
|
|
|
174 |
|
✗ |
Data data(model); |
175 |
|
|
|
176 |
|
✗ |
model.lowerPositionLimit.head<3>().fill(-1.); |
177 |
|
✗ |
model.upperPositionLimit.head<3>().fill(1.); |
178 |
|
✗ |
VectorXd q = randomConfiguration(model); |
179 |
|
✗ |
VectorXd v(VectorXd::Random(model.nv)); |
180 |
|
✗ |
VectorXd a(VectorXd::Random(model.nv)); |
181 |
|
|
|
182 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
183 |
|
|
|
184 |
|
✗ |
for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i) |
185 |
|
|
{ |
186 |
|
✗ |
SE3 T = data.oMi[i]; |
187 |
|
✗ |
Motion vel = data.v[i]; |
188 |
|
✗ |
Motion acc = data.a[i]; |
189 |
|
✗ |
Vector3d linear; |
190 |
|
|
|
191 |
|
✗ |
Motion acc_classical_local = acc; |
192 |
|
✗ |
linear = acc.linear() + vel.angular().cross(vel.linear()); |
193 |
|
✗ |
acc_classical_local.linear() = linear; |
194 |
|
|
|
195 |
|
✗ |
BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model, data, i))); |
196 |
|
✗ |
BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model, data, i, LOCAL))); |
197 |
|
|
|
198 |
|
✗ |
Motion vel_world = T.act(vel); |
199 |
|
✗ |
Motion acc_classical_world = T.act(acc); |
200 |
|
✗ |
linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear()); |
201 |
|
✗ |
acc_classical_world.linear() = linear; |
202 |
|
|
|
203 |
|
✗ |
BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model, data, i, WORLD))); |
204 |
|
|
|
205 |
|
✗ |
Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel); |
206 |
|
✗ |
Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc); |
207 |
|
✗ |
linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear()); |
208 |
|
✗ |
acc_classical_aligned.linear() = linear; |
209 |
|
|
|
210 |
|
✗ |
BOOST_CHECK(acc_classical_aligned.isApprox( |
211 |
|
|
getClassicalAcceleration(model, data, i, LOCAL_WORLD_ALIGNED))); |
212 |
|
|
} |
213 |
|
|
} |
214 |
|
|
|
215 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_kinematic_getters) |
216 |
|
|
{ |
217 |
|
|
using namespace Eigen; |
218 |
|
|
using namespace pinocchio; |
219 |
|
|
|
220 |
|
|
// Build a simple 2R planar model |
221 |
|
✗ |
Model model; |
222 |
|
✗ |
JointIndex jointId = 0; |
223 |
|
✗ |
jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1"); |
224 |
|
✗ |
jointId = model.addJoint( |
225 |
|
✗ |
jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2"); |
226 |
|
|
|
227 |
|
✗ |
Data data(model); |
228 |
|
|
|
229 |
|
|
// Predetermined configuration values |
230 |
|
✗ |
VectorXd q(model.nq); |
231 |
|
✗ |
q << M_PI / 2.0, 0.0; |
232 |
|
|
|
233 |
|
✗ |
VectorXd v(model.nv); |
234 |
|
✗ |
v << 1.0, 0.0; |
235 |
|
|
|
236 |
|
✗ |
VectorXd a(model.nv); |
237 |
|
✗ |
a << 0.0, 0.0; |
238 |
|
|
|
239 |
|
|
// Expected velocity |
240 |
|
✗ |
Motion v_local; |
241 |
|
✗ |
v_local.linear() = Vector3d(0.0, 1.0, 0.0); |
242 |
|
✗ |
v_local.angular() = Vector3d(0.0, 0.0, 1.0); |
243 |
|
|
|
244 |
|
✗ |
Motion v_world; |
245 |
|
✗ |
v_world.linear() = Vector3d::Zero(); |
246 |
|
✗ |
v_world.angular() = Vector3d(0.0, 0.0, 1.0); |
247 |
|
|
|
248 |
|
✗ |
Motion v_align; |
249 |
|
✗ |
v_align.linear() = Vector3d(-1.0, 0.0, 0.0); |
250 |
|
✗ |
v_align.angular() = Vector3d(0.0, 0.0, 1.0); |
251 |
|
|
|
252 |
|
|
// Expected classical acceleration |
253 |
|
✗ |
Motion ac_local; |
254 |
|
✗ |
ac_local.linear() = Vector3d(-1.0, 0.0, 0.0); |
255 |
|
✗ |
ac_local.angular() = Vector3d::Zero(); |
256 |
|
|
|
257 |
|
✗ |
Motion ac_world = Motion::Zero(); |
258 |
|
|
|
259 |
|
✗ |
Motion ac_align; |
260 |
|
✗ |
ac_align.linear() = Vector3d(0.0, -1.0, 0.0); |
261 |
|
✗ |
ac_align.angular() = Vector3d::Zero(); |
262 |
|
|
|
263 |
|
|
// Perform kinematics |
264 |
|
✗ |
forwardKinematics(model, data, q, v, a); |
265 |
|
|
|
266 |
|
|
// Check output velocity |
267 |
|
✗ |
BOOST_CHECK(v_local.isApprox(getVelocity(model, data, jointId))); |
268 |
|
✗ |
BOOST_CHECK(v_local.isApprox(getVelocity(model, data, jointId, LOCAL))); |
269 |
|
✗ |
BOOST_CHECK(v_world.isApprox(getVelocity(model, data, jointId, WORLD))); |
270 |
|
✗ |
BOOST_CHECK(v_align.isApprox(getVelocity(model, data, jointId, LOCAL_WORLD_ALIGNED))); |
271 |
|
|
|
272 |
|
|
// Check output acceleration (all zero) |
273 |
|
✗ |
BOOST_CHECK(getAcceleration(model, data, jointId).isZero()); |
274 |
|
✗ |
BOOST_CHECK(getAcceleration(model, data, jointId, LOCAL).isZero()); |
275 |
|
✗ |
BOOST_CHECK(getAcceleration(model, data, jointId, WORLD).isZero()); |
276 |
|
✗ |
BOOST_CHECK(getAcceleration(model, data, jointId, LOCAL_WORLD_ALIGNED).isZero()); |
277 |
|
|
|
278 |
|
|
// Check output classical |
279 |
|
✗ |
BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model, data, jointId))); |
280 |
|
✗ |
BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model, data, jointId, LOCAL))); |
281 |
|
✗ |
BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model, data, jointId, WORLD))); |
282 |
|
✗ |
BOOST_CHECK( |
283 |
|
|
ac_align.isApprox(getClassicalAcceleration(model, data, jointId, LOCAL_WORLD_ALIGNED))); |
284 |
|
|
} |
285 |
|
|
|
286 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
287 |
|
|
|