unittest/kinematics.cpp
Go to the documentation of this file.
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 #include "utils/model-generator.hpp"
18 
19 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
20 
21 BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
22 {
23  using namespace Eigen;
24  using namespace pinocchio;
25 
26  Model model;
27  buildModels::humanoidRandom(model);
28 
29  Data data(model);
30 
31  model.lowerPositionLimit.head<3>().fill(-1.);
32  model.upperPositionLimit.head<3>().fill(1.);
33  VectorXd q = randomConfiguration(model);
34 
35  forwardKinematics(model, data, Model::ConfigVectorType::Ones(model.nq));
36 }
37 
38 BOOST_AUTO_TEST_CASE(test_kinematics_zero)
39 {
40  using namespace Eigen;
41  using namespace pinocchio;
42 
43  Model model;
44  buildModels::humanoidRandom(model);
45 
46  Data data(model), data_ref(model);
47 
48  model.lowerPositionLimit.head<3>().fill(-1.);
49  model.upperPositionLimit.head<3>().fill(1.);
50  VectorXd q = randomConfiguration(model);
51 
52  forwardKinematics(model, data_ref, q);
53  crba(model, data, q, Convention::WORLD);
54  updateGlobalPlacements(model, data);
55 
56  for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
57  {
58  BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]);
59  BOOST_CHECK(data.liMi[i] == data_ref.liMi[i]);
60  }
61 }
62 
63 BOOST_AUTO_TEST_CASE(test_kinematics_first)
64 {
65  using namespace Eigen;
66  using namespace pinocchio;
67 
68  Model model;
69  buildModels::humanoidRandom(model);
70 
71  Data data(model);
72 
73  model.lowerPositionLimit.head<3>().fill(-1.);
74  model.upperPositionLimit.head<3>().fill(1.);
75  VectorXd q = randomConfiguration(model);
76  VectorXd v(VectorXd::Zero(model.nv));
77 
78  forwardKinematics(model, data, q, v);
79 
80  for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
81  {
82  BOOST_CHECK(data.v[i] == Motion::Zero());
83  }
84 }
85 
86 BOOST_AUTO_TEST_CASE(test_getRelativePlacement)
87 {
88  using namespace Eigen;
89  using namespace pinocchio;
90 
91  Model model;
92  buildModels::humanoid(model);
93  Data data(model);
94 
95  model.lowerPositionLimit.head<3>().fill(-1.);
96  model.upperPositionLimit.head<3>().fill(1.);
97  forwardKinematics(model, data, randomConfiguration(model));
98 
99  const std::vector<JointIndex> test_joints{
100  0, 1, model.getJointId("rleg_elbow_joint"), model.getJointId("lleg_elbow_joint"),
101  (JointIndex)(model.njoints - 1)};
102 
103  for (const JointIndex i : test_joints)
104  {
105  for (const JointIndex j : test_joints)
106  {
107  SE3 placement_world = getRelativePlacement(model, data, i, j, Convention::WORLD);
108  SE3 placement_local = getRelativePlacement(model, data, i, j, Convention::LOCAL);
109 
110  // Both convention should match
111  BOOST_CHECK(placement_world.isApprox(placement_local));
112 
113  // Relative placement to itself is identity
114  if (i == j)
115  {
116  BOOST_CHECK(placement_world.isIdentity());
117  BOOST_CHECK(placement_local.isIdentity());
118  }
119 
120  // Relative placement to world
121  if (i == 0)
122  {
123  BOOST_CHECK(placement_world.isApprox(data.oMi[j]));
124  BOOST_CHECK(placement_local.isApprox(data.oMi[j]));
125  }
126 
127  // Relative placement from world
128  if (j == 0)
129  {
130  BOOST_CHECK(placement_world.isApprox(data.oMi[i].inverse()));
131  BOOST_CHECK(placement_local.isApprox(data.oMi[i].inverse()));
132  }
133  }
134  }
135 }
136 
137 BOOST_AUTO_TEST_CASE(test_kinematics_second)
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::Zero(model.nv));
151  VectorXd a(VectorXd::Zero(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.v[i] == Motion::Zero());
158  BOOST_CHECK(data.a[i] == Motion::Zero());
159  }
160 }
161 
162 BOOST_AUTO_TEST_CASE(test_get_velocity)
163 {
164  using namespace Eigen;
165  using namespace pinocchio;
166 
167  Model model;
168  buildModels::humanoidRandom(model);
169 
170  Data data(model);
171 
172  model.lowerPositionLimit.head<3>().fill(-1.);
173  model.upperPositionLimit.head<3>().fill(1.);
174  VectorXd q = randomConfiguration(model);
175  VectorXd v(VectorXd::Random(model.nv));
176 
177  forwardKinematics(model, data, q, v);
178 
179  for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
180  {
181  BOOST_CHECK(data.v[i].isApprox(getVelocity(model, data, i)));
182  BOOST_CHECK(data.v[i].isApprox(getVelocity(model, data, i, LOCAL)));
183  BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model, data, i, WORLD)));
184  BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero())
185  .act(data.v[i])
186  .isApprox(getVelocity(model, data, i, LOCAL_WORLD_ALIGNED)));
187  }
188 }
189 
190 BOOST_AUTO_TEST_CASE(test_get_acceleration)
191 {
192  using namespace Eigen;
193  using namespace pinocchio;
194 
195  Model model;
196  buildModels::humanoidRandom(model);
197 
198  Data data(model);
199 
200  model.lowerPositionLimit.head<3>().fill(-1.);
201  model.upperPositionLimit.head<3>().fill(1.);
202  VectorXd q = randomConfiguration(model);
203  VectorXd v(VectorXd::Random(model.nv));
204  VectorXd a(VectorXd::Random(model.nv));
205 
206  forwardKinematics(model, data, q, v, a);
207 
208  for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
209  {
210  BOOST_CHECK(data.a[i].isApprox(getAcceleration(model, data, i)));
211  BOOST_CHECK(data.a[i].isApprox(getAcceleration(model, data, i, LOCAL)));
212  BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model, data, i, WORLD)));
213  BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero())
214  .act(data.a[i])
215  .isApprox(getAcceleration(model, data, i, LOCAL_WORLD_ALIGNED)));
216  }
217 }
218 
219 BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
220 {
221  using namespace Eigen;
222  using namespace pinocchio;
223 
224  Model model;
225  buildModels::humanoidRandom(model);
226 
227  Data data(model);
228 
229  model.lowerPositionLimit.head<3>().fill(-1.);
230  model.upperPositionLimit.head<3>().fill(1.);
231  VectorXd q = randomConfiguration(model);
232  VectorXd v(VectorXd::Random(model.nv));
233  VectorXd a(VectorXd::Random(model.nv));
234 
235  forwardKinematics(model, data, q, v, a);
236 
237  for (Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
238  {
239  SE3 T = data.oMi[i];
240  Motion vel = data.v[i];
241  Motion acc = data.a[i];
242  Vector3d linear;
243 
244  Motion acc_classical_local = acc;
245  linear = acc.linear() + vel.angular().cross(vel.linear());
246  acc_classical_local.linear() = linear;
247 
248  BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model, data, i)));
249  BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model, data, i, LOCAL)));
250 
251  Motion vel_world = T.act(vel);
252  Motion acc_classical_world = T.act(acc);
253  linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
254  acc_classical_world.linear() = linear;
255 
256  BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model, data, i, WORLD)));
257 
258  Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel);
259  Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc);
260  linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
261  acc_classical_aligned.linear() = linear;
262 
263  BOOST_CHECK(acc_classical_aligned.isApprox(
264  getClassicalAcceleration(model, data, i, LOCAL_WORLD_ALIGNED)));
265  }
266 }
267 
268 BOOST_AUTO_TEST_CASE(test_kinematic_getters)
269 {
270  using namespace Eigen;
271  using namespace pinocchio;
272 
273  // Build a simple 2R planar model
274  Model model;
275  JointIndex jointId = 0;
276  jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
277  jointId = model.addJoint(
278  jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");
279 
280  Data data(model);
281 
282  // Predetermined configuration values
283  VectorXd q(model.nq);
284  q << M_PI / 2.0, 0.0;
285 
286  VectorXd v(model.nv);
287  v << 1.0, 0.0;
288 
289  VectorXd a(model.nv);
290  a << 0.0, 0.0;
291 
292  // Expected velocity
293  Motion v_local;
294  v_local.linear() = Vector3d(0.0, 1.0, 0.0);
295  v_local.angular() = Vector3d(0.0, 0.0, 1.0);
296 
297  Motion v_world;
298  v_world.linear() = Vector3d::Zero();
299  v_world.angular() = Vector3d(0.0, 0.0, 1.0);
300 
301  Motion v_align;
302  v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
303  v_align.angular() = Vector3d(0.0, 0.0, 1.0);
304 
305  // Expected classical acceleration
306  Motion ac_local;
307  ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
308  ac_local.angular() = Vector3d::Zero();
309 
310  Motion ac_world = Motion::Zero();
311 
312  Motion ac_align;
313  ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
314  ac_align.angular() = Vector3d::Zero();
315 
316  // Perform kinematics
317  forwardKinematics(model, data, q, v, a);
318 
319  // Check output velocity
320  BOOST_CHECK(v_local.isApprox(getVelocity(model, data, jointId)));
321  BOOST_CHECK(v_local.isApprox(getVelocity(model, data, jointId, LOCAL)));
322  BOOST_CHECK(v_world.isApprox(getVelocity(model, data, jointId, WORLD)));
323  BOOST_CHECK(v_align.isApprox(getVelocity(model, data, jointId, LOCAL_WORLD_ALIGNED)));
324 
325  // Check output acceleration (all zero)
326  BOOST_CHECK(getAcceleration(model, data, jointId).isZero());
327  BOOST_CHECK(getAcceleration(model, data, jointId, LOCAL).isZero());
328  BOOST_CHECK(getAcceleration(model, data, jointId, WORLD).isZero());
329  BOOST_CHECK(getAcceleration(model, data, jointId, LOCAL_WORLD_ALIGNED).isZero());
330 
331  // Check output classical
332  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model, data, jointId)));
333  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model, data, jointId, LOCAL)));
334  BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model, data, jointId, WORLD)));
335  BOOST_CHECK(
336  ac_align.isApprox(getClassicalAcceleration(model, data, jointId, LOCAL_WORLD_ALIGNED)));
337 }
338 
339 BOOST_AUTO_TEST_CASE(test_kinematics_mimic)
340 {
341  for (int i = 0; i < pinocchio::MimicTestCases::N_CASES; i++)
342  {
343  const pinocchio::MimicTestCases mimic_test_case(i);
344  const pinocchio::Model & model_mimic = mimic_test_case.model_mimic;
345  const pinocchio::Model & model_full = mimic_test_case.model_full;
346  const Eigen::MatrixXd & G = mimic_test_case.G;
347 
348  Eigen::VectorXd q = pinocchio::randomConfiguration(model_mimic);
349  Eigen::VectorXd v = Eigen::VectorXd::Random(model_mimic.nv);
350  Eigen::VectorXd a = Eigen::VectorXd::Random(model_mimic.nv);
351 
352  Eigen::VectorXd q_full(model_full.nq);
353  Eigen::VectorXd v_full = G * v;
354  Eigen::VectorXd a_full = G * v;
355 
356  mimic_test_case.toFull(q, q_full);
357 
358  pinocchio::Data dataFKFull(model_full);
359  pinocchio::forwardKinematics(model_full, dataFKFull, q_full, v_full, a_full);
360 
361  pinocchio::Data dataFKRed(model_mimic);
362  pinocchio::forwardKinematics(model_mimic, dataFKRed, q, v, a);
363 
364  for (int i = 0; i < model_full.njoints; i++)
365  {
366  BOOST_CHECK(dataFKRed.oMi[i].isApprox(dataFKFull.oMi[i]));
367  BOOST_CHECK(dataFKRed.liMi[i].isApprox(dataFKFull.liMi[i]));
368  BOOST_CHECK(model_full.inertias[i].isApprox(model_mimic.inertias[i]));
369  }
370  }
371 }
372 
373 BOOST_AUTO_TEST_SUITE_END()
simulation-contact-dynamics.T
int T
Definition: simulation-contact-dynamics.py:89
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::MimicTestCases::toFull
void toFull(const Eigen::VectorXd &q, Eigen::VectorXd &q_full) const
Definition: model-generator.hpp:241
pinocchio::forwardKinematics
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
pinocchio::isZero
bool isZero(const Eigen::MatrixBase< MatrixLike > &m, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Definition: math/matrix.hpp:59
pinocchio::updateGlobalPlacements
void updateGlobalPlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Update the global placement of the joints oMi according to the relative placements of the joints.
pinocchio::SE3
context::SE3 SE3
Definition: spatial/fwd.hpp:59
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
Definition: unittest/kinematics.cpp:21
kinematics.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::context::JointModelRZ
JointModelRevoluteTpl< Scalar, Options, 2 > JointModelRZ
Definition: bindings/python/context/generic.hpp:79
mimic_dynamics.a_full
a_full
Definition: mimic_dynamics.py:49
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::MimicTestCases::G
Eigen::MatrixXd G
Definition: model-generator.hpp:134
pinocchio::MimicTestCases::N_CASES
static const int N_CASES
Definition: model-generator.hpp:128
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true, bool mimic=false)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::getRelativePlacement
SE3Tpl< Scalar, Options > getRelativePlacement(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointIdRef, const JointIndex jointIdTarget, const Convention convention=Convention::LOCAL)
Returns the relative placement of two joints expressed in the desired reference frame....
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
model-generator.hpp
static-contact-dynamics.acc
acc
Definition: static-contact-dynamics.py:187
mimic_dynamics.model_full
model_full
Definition: mimic_dynamics.py:9
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl< Scalar >::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
joint-configuration.hpp
mimic_dynamics.v_full
v_full
Definition: mimic_dynamics.py:48
cartpole.v
v
Definition: cartpole.py:153
data.hpp
pinocchio::MimicTestCases::model_mimic
pinocchio::Model model_mimic
Definition: model-generator.hpp:130
mimic_dynamics.model_mimic
model_mimic
Definition: mimic_dynamics.py:19
q
q
pinocchio::getVelocity
MotionTpl< Scalar, Options > getVelocity(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
Returns the spatial velocity of the joint expressed in the desired reference frame....
a
Vec3f a
mimic_dynamics.q_full
q_full
Definition: mimic_dynamics.py:46
pinocchio::buildModels::humanoid
void humanoid(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a 28-DOF kinematic chain of a floating humanoid robot.
fill
fill
mimic_dynamics.G
G
Definition: mimic_dynamics.py:27
pinocchio::MimicTestCases
Definition: model-generator.hpp:125
pinocchio::getClassicalAcceleration
MotionTpl< Scalar, Options > getClassicalAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
Returns the "classical" acceleration of the joint expressed in the desired reference frame....
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::python::context::SE3
SE3Tpl< Scalar, Options > SE3
Definition: bindings/python/context/generic.hpp:53
sample-models.hpp
pinocchio::MotionTpl
Definition: context/casadi.hpp:28
pinocchio::ModelTpl< Scalar >
pinocchio::MimicTestCases::model_full
pinocchio::Model model_full
Definition: model-generator.hpp:131
crba.hpp
pinocchio::getAcceleration
MotionTpl< Scalar, Options > getAcceleration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex jointId, const ReferenceFrame rf=LOCAL)
Returns the spatial acceleration of the joint expressed in the desired reference frame....
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Wed Apr 16 2025 02:41:49