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 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()
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::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::SE3Tpl< context::Scalar, context::Options >
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:19
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
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
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::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
static-contact-dynamics.acc
acc
Definition: static-contact-dynamics.py:187
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
joint-configuration.hpp
cartpole.v
v
Definition: cartpole.py:153
data.hpp
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
fill
fill
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:27
pinocchio::ModelTpl
Definition: context/generic.hpp:20
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:27
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:11