GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: unittest/kinematics.cpp Lines: 153 153 100.0 %
Date: 2024-01-23 21:41:47 Branches: 698 1384 50.4 %

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/parsers/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
















4
BOOST_AUTO_TEST_CASE(test_kinematics_constant_vector_input)
20
{
21
  using namespace Eigen;
22
  using namespace pinocchio;
23
24
4
  Model model;
25
2
  buildModels::humanoidRandom(model);
26
27
4
  Data data(model);
28
29

2
  model.lowerPositionLimit.head<3>().fill(-1.);
30

2
  model.upperPositionLimit.head<3>().fill(1.);
31
2
  VectorXd q = randomConfiguration(model);
32
33

2
  forwardKinematics(model,data,Model::ConfigVectorType::Ones(model.nq));
34
2
}
35
36
















4
BOOST_AUTO_TEST_CASE(test_kinematics_zero)
37
{
38
  using namespace Eigen;
39
  using namespace pinocchio;
40
41
4
  Model model;
42
2
  buildModels::humanoidRandom(model);
43
44

4
  Data data(model), data_ref(model);
45
46

2
  model.lowerPositionLimit.head<3>().fill(-1.);
47

2
  model.upperPositionLimit.head<3>().fill(1.);
48
4
  VectorXd q = randomConfiguration(model);
49
50
2
  forwardKinematics(model,data_ref,q);
51
2
  crba(model,data,q);
52
2
  updateGlobalPlacements(model,data);
53
54
56
  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
55
  {
56



54
    BOOST_CHECK(data.oMi[i] == data_ref.oMi[i]);
57



54
    BOOST_CHECK(data.liMi[i] == data_ref.liMi[i]);
58
  }
59
2
}
60
61
















4
BOOST_AUTO_TEST_CASE(test_kinematics_first)
62
{
63
  using namespace Eigen;
64
  using namespace pinocchio;
65
66
4
  Model model;
67
2
  buildModels::humanoidRandom(model);
68
69
4
  Data data(model);
70
71

2
  model.lowerPositionLimit.head<3>().fill(-1.);
72

2
  model.upperPositionLimit.head<3>().fill(1.);
73
4
  VectorXd q = randomConfiguration(model);
74

4
  VectorXd v(VectorXd::Zero(model.nv));
75
76
2
  forwardKinematics(model,data,q,v);
77
78
56
  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
79
  {
80




54
    BOOST_CHECK(data.v[i] == Motion::Zero());
81
  }
82
2
}
83
84
















4
BOOST_AUTO_TEST_CASE(test_kinematics_second)
85
{
86
  using namespace Eigen;
87
  using namespace pinocchio;
88
89
4
  Model model;
90
2
  buildModels::humanoidRandom(model);
91
92
4
  Data data(model);
93
94

2
  model.lowerPositionLimit.head<3>().fill(-1.);
95

2
  model.upperPositionLimit.head<3>().fill(1.);
96
4
  VectorXd q = randomConfiguration(model);
97

4
  VectorXd v(VectorXd::Zero(model.nv));
98

4
  VectorXd a(VectorXd::Zero(model.nv));
99
100
2
  forwardKinematics(model,data,q,v,a);
101
102
56
  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
103
  {
104




54
    BOOST_CHECK(data.v[i] == Motion::Zero());
105




54
    BOOST_CHECK(data.a[i] == Motion::Zero());
106
  }
107
2
}
108
109
















4
BOOST_AUTO_TEST_CASE(test_get_velocity)
110
{
111
  using namespace Eigen;
112
  using namespace pinocchio;
113
114
4
  Model model;
115
2
  buildModels::humanoidRandom(model);
116
117
4
  Data data(model);
118
119

2
  model.lowerPositionLimit.head<3>().fill(-1.);
120

2
  model.upperPositionLimit.head<3>().fill(1.);
121
4
  VectorXd q = randomConfiguration(model);
122

4
  VectorXd v(VectorXd::Random(model.nv));
123
124
2
  forwardKinematics(model,data,q,v);
125
126
56
  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
127
  {
128




54
    BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i)));
129




54
    BOOST_CHECK(data.v[i].isApprox(getVelocity(model,data,i,LOCAL)));
130




54
    BOOST_CHECK(data.oMi[i].act(data.v[i]).isApprox(getVelocity(model,data,i,WORLD)));
131






54
    BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.v[i]).isApprox(getVelocity(model,data,i,LOCAL_WORLD_ALIGNED)));
132
  }
133
2
}
134
135
















4
BOOST_AUTO_TEST_CASE(test_get_acceleration)
136
{
137
  using namespace Eigen;
138
  using namespace pinocchio;
139
140
4
  Model model;
141
2
  buildModels::humanoidRandom(model);
142
143
4
  Data data(model);
144
145

2
  model.lowerPositionLimit.head<3>().fill(-1.);
146

2
  model.upperPositionLimit.head<3>().fill(1.);
147
4
  VectorXd q = randomConfiguration(model);
148

4
  VectorXd v(VectorXd::Random(model.nv));
149

4
  VectorXd a(VectorXd::Random(model.nv));
150
151
2
  forwardKinematics(model,data,q,v,a);
152
153
56
  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
154
  {
155




54
    BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i)));
156




54
    BOOST_CHECK(data.a[i].isApprox(getAcceleration(model,data,i,LOCAL)));
157




54
    BOOST_CHECK(data.oMi[i].act(data.a[i]).isApprox(getAcceleration(model,data,i,WORLD)));
158






54
    BOOST_CHECK(SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(data.a[i]).isApprox(getAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
159
  }
160
2
}
161
162
















4
BOOST_AUTO_TEST_CASE(test_get_classical_acceleration)
163
{
164
  using namespace Eigen;
165
  using namespace pinocchio;
166
167
4
  Model model;
168
2
  buildModels::humanoidRandom(model);
169
170
4
  Data data(model);
171
172

2
  model.lowerPositionLimit.head<3>().fill(-1.);
173

2
  model.upperPositionLimit.head<3>().fill(1.);
174
4
  VectorXd q = randomConfiguration(model);
175

4
  VectorXd v(VectorXd::Random(model.nv));
176

4
  VectorXd a(VectorXd::Random(model.nv));
177
178
2
  forwardKinematics(model,data,q,v,a);
179
180
56
  for(Model::JointIndex i = 1; i < (Model::JointIndex)model.njoints; ++i)
181
  {
182
54
    SE3 T = data.oMi[i];
183
54
    Motion vel = data.v[i];
184
54
    Motion acc = data.a[i];
185
54
    Vector3d linear;
186
187
54
    Motion acc_classical_local = acc;
188



54
    linear = acc.linear() + vel.angular().cross(vel.linear());
189

54
    acc_classical_local.linear() = linear;
190
191




54
    BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i)));
192




54
    BOOST_CHECK(acc_classical_local.isApprox(getClassicalAcceleration(model,data,i,LOCAL)));
193
194
54
    Motion vel_world = T.act(vel);
195
54
    Motion acc_classical_world = T.act(acc);
196



54
    linear = acc_classical_world.linear() + vel_world.angular().cross(vel_world.linear());
197

54
    acc_classical_world.linear() = linear;
198
199




54
    BOOST_CHECK(acc_classical_world.isApprox(getClassicalAcceleration(model,data,i,WORLD)));
200
201


54
    Motion vel_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(vel);
202


54
    Motion acc_classical_aligned = SE3(data.oMi[i].rotation(), Eigen::Vector3d::Zero()).act(acc);
203



54
    linear = acc_classical_aligned.linear() + vel_aligned.angular().cross(vel_aligned.linear());
204

54
    acc_classical_aligned.linear() = linear;
205
206




54
    BOOST_CHECK(acc_classical_aligned.isApprox(getClassicalAcceleration(model,data,i,LOCAL_WORLD_ALIGNED)));
207
  }
208
2
}
209
210
















4
BOOST_AUTO_TEST_CASE(test_kinematic_getters)
211
{
212
  using namespace Eigen;
213
  using namespace pinocchio;
214
215
  // Build a simple 2R planar model
216
4
  Model model;
217
2
  JointIndex jointId = 0;
218


2
  jointId = model.addJoint(jointId, JointModelRZ(), SE3::Identity(), "Joint1");
219



2
  jointId = model.addJoint(jointId, JointModelRZ(), SE3(Matrix3d::Identity(), Vector3d(1.0, 0.0, 0.0)), "Joint2");
220
221
4
  Data data(model);
222
223
  // Predetermined configuration values
224
4
  VectorXd q(model.nq);
225

2
  q << M_PI/2.0, 0.0;
226
227
4
  VectorXd v(model.nv);
228

2
  v << 1.0, 0.0;
229
230
4
  VectorXd a(model.nv);
231

2
  a << 0.0, 0.0;
232
233
  // Expected velocity
234
2
  Motion v_local;
235

2
  v_local.linear() = Vector3d(0.0, 1.0, 0.0);
236

2
  v_local.angular() = Vector3d(0.0, 0.0, 1.0);
237
238
2
  Motion v_world;
239

2
  v_world.linear() = Vector3d::Zero();
240

2
  v_world.angular() = Vector3d(0.0, 0.0, 1.0);
241
242
2
  Motion v_align;
243

2
  v_align.linear() = Vector3d(-1.0, 0.0, 0.0);
244

2
  v_align.angular() = Vector3d(0.0, 0.0, 1.0);
245
246
  // Expected classical acceleration
247
2
  Motion ac_local;
248

2
  ac_local.linear() = Vector3d(-1.0, 0.0, 0.0);
249

2
  ac_local.angular() = Vector3d::Zero();
250
251
2
  Motion ac_world = Motion::Zero();
252
253
2
  Motion ac_align;
254

2
  ac_align.linear() = Vector3d(0.0, -1.0, 0.0);
255

2
  ac_align.angular() = Vector3d::Zero();
256
257
  // Perform kinematics
258
2
  forwardKinematics(model,data,q,v,a);
259
260
  // Check output velocity
261




2
  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId)));
262




2
  BOOST_CHECK(v_local.isApprox(getVelocity(model,data,jointId,LOCAL)));
263




2
  BOOST_CHECK(v_world.isApprox(getVelocity(model,data,jointId,WORLD)));
264




2
  BOOST_CHECK(v_align.isApprox(getVelocity(model,data,jointId,LOCAL_WORLD_ALIGNED)));
265
266
  // Check output acceleration (all zero)
267




2
  BOOST_CHECK(getAcceleration(model,data,jointId).isZero());
268




2
  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL).isZero());
269




2
  BOOST_CHECK(getAcceleration(model,data,jointId,WORLD).isZero());
270




2
  BOOST_CHECK(getAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED).isZero());
271
272
  // Check output classical
273




2
  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId)));
274




2
  BOOST_CHECK(ac_local.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL)));
275




2
  BOOST_CHECK(ac_world.isApprox(getClassicalAcceleration(model,data,jointId,WORLD)));
276




2
  BOOST_CHECK(ac_align.isApprox(getClassicalAcceleration(model,data,jointId,LOCAL_WORLD_ALIGNED)));
277
2
}
278
279
BOOST_AUTO_TEST_SUITE_END()