examples.cpp 8.92 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#include <iostream>

#define BOOST_TEST_MODULE StatsTests
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include "multicontact-api/scenario/contact-sequence.hpp"
#include "multicontact-api/scenario/fwd.hpp"
#include "curves/fwd.h"
#include <curves/so3_linear.h>
#include <curves/se3_curve.h>
#include <curves/polynomial.h>
#include <curves/bezier_curve.h>
#include <curves/piecewise_curve.h>
#include <curves/exact_cubic.h>
#include <curves/cubic_hermite_spline.h>

/**
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
22
23
24
25
26
 * This unit test try to deserialize the ContactSequences in the examples folder
 * and check if they have the given data set.
 * If this test fail, it probably mean that an update of multicontact-api broke the backward compatibility with
 * serialized objects The objects need to be re-generated.
 */
27
28
29

using namespace multicontact_api::scenario;

30
const std::string path = TEST_DATA_PATH;
31
32
33
34
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(com_motion_above_feet_COM) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
35
  cs.loadFromBinary(path + "com_motion_above_feet_COM.cs");
36
37
38
39
40
41
42
43
44
  BOOST_CHECK_EQUAL(cs.size(), 1);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
}

BOOST_AUTO_TEST_CASE(com_motion_above_feet_WB) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
45
  cs.loadFromBinary(path + "com_motion_above_feet_WB.cs");
46
47
48
49
50
51
52
53
54
55
56
57
58
  BOOST_CHECK_EQUAL(cs.size(), 1);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
  BOOST_CHECK(cs.haveJointsTrajectories());
  BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
  BOOST_CHECK(cs.haveContactForcesTrajectories());
  BOOST_CHECK(cs.haveZMPtrajectories());
}

BOOST_AUTO_TEST_CASE(step_in_place) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
59
  cs.loadFromBinary(path + "step_in_place.cs");
60
61
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
62
63
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
64
65
66
67
}

BOOST_AUTO_TEST_CASE(step_in_place_COM) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
68
  cs.loadFromBinary(path + "step_in_place_COM.cs");
69
70
71
72
73
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
74
75
76
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());

77
78
79
80
}

BOOST_AUTO_TEST_CASE(step_in_place_REF) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
81
  cs.loadFromBinary(path + "step_in_place_REF.cs");
82
83
84
85
86
87
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
  BOOST_CHECK(cs.haveEffectorsTrajectories());
88
  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-6, false));
89
90
91
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());

92
93
94
95
}

BOOST_AUTO_TEST_CASE(step_in_place_WB) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
96
  cs.loadFromBinary(path + "step_in_place_WB.cs");
97
98
99
100
101
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
102
  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
103
104
105
106
  BOOST_CHECK(cs.haveJointsTrajectories());
  BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
  BOOST_CHECK(cs.haveContactForcesTrajectories());
  BOOST_CHECK(cs.haveZMPtrajectories());
107
108
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
109
110
111
112
}

BOOST_AUTO_TEST_CASE(step_in_place_quasistatic) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
113
  cs.loadFromBinary(path + "step_in_place_quasistatic.cs");
114
115
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
116
117
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
118
119
120
121
}

BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_COM) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
122
  cs.loadFromBinary(path + "step_in_place_quasistatic_COM.cs");
123
124
125
126
127
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
128
129
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
130
131
132
133
}

BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_REF) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
134
  cs.loadFromBinary(path + "step_in_place_quasistatic_REF.cs");
135
136
137
138
139
140
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
  BOOST_CHECK(cs.haveEffectorsTrajectories());
141
142
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
143
144
145
146
}

BOOST_AUTO_TEST_CASE(step_in_place_quasistatic_WB) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
147
  cs.loadFromBinary(path + "step_in_place_quasistatic_WB.cs");
148
149
150
151
152
  BOOST_CHECK_EQUAL(cs.size(), 9);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
153
  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
154
155
156
157
  BOOST_CHECK(cs.haveJointsTrajectories());
  BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
  BOOST_CHECK(cs.haveContactForcesTrajectories());
  BOOST_CHECK(cs.haveZMPtrajectories());
158
159
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
160
161
162
163
}

BOOST_AUTO_TEST_CASE(walk_20cm) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
164
  cs.loadFromBinary(path + "walk_20cm.cs");
165
166
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
167
168
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
169
170
171
172
}

BOOST_AUTO_TEST_CASE(walk_20cm_COM) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
173
  cs.loadFromBinary(path + "walk_20cm_COM.cs");
174
175
176
177
178
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
179
180
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
181
182
183
184
}

BOOST_AUTO_TEST_CASE(walk_20cm_REF) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
185
  cs.loadFromBinary(path + "walk_20cm_REF.cs");
186
187
188
189
190
191
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
  BOOST_CHECK(cs.haveEffectorsTrajectories());
192
193
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
194
195
196
197
}

BOOST_AUTO_TEST_CASE(walk_20cm_WB) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
198
  cs.loadFromBinary(path + "walk_20cm_WB.cs");
199
200
201
202
203
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
204
  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
205
206
207
208
  BOOST_CHECK(cs.haveJointsTrajectories());
  BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
  BOOST_CHECK(cs.haveContactForcesTrajectories());
  BOOST_CHECK(cs.haveZMPtrajectories());
209
210
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
211
212
213
214
}

BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
215
  cs.loadFromBinary(path + "walk_20cm_quasistatic.cs");
216
217
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
218
219
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
220
221
222
223
}

BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_COM) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
224
  cs.loadFromBinary(path + "walk_20cm_quasistatic_COM.cs");
225
226
227
228
229
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
230
231
  BOOST_CHECK(!cs.haveFriction());
  BOOST_CHECK(!cs.haveContactModelDefined());
232
233
234
235
}

BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_REF) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
236
  cs.loadFromBinary(path + "walk_20cm_quasistatic_REF.cs");
237
238
239
240
241
242
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
  BOOST_CHECK(cs.haveEffectorsTrajectories());
243
244
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
245
246
247
248
}

BOOST_AUTO_TEST_CASE(walk_20cm_quasistatic_WB) {
  ContactSequence cs;
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
249
  cs.loadFromBinary(path + "walk_20cm_quasistatic_WB.cs");
250
251
252
253
254
  BOOST_CHECK_EQUAL(cs.size(), 23);
  BOOST_CHECK(cs.haveConsistentContacts());
  BOOST_CHECK(cs.haveTimings());
  BOOST_CHECK(cs.haveCentroidalValues());
  BOOST_CHECK(cs.haveCentroidalTrajectories());
255
  BOOST_CHECK(cs.haveEffectorsTrajectories(1e-1));
256
257
258
259
  BOOST_CHECK(cs.haveJointsTrajectories());
  BOOST_CHECK(cs.haveJointsDerivativesTrajectories());
  BOOST_CHECK(cs.haveContactForcesTrajectories());
  BOOST_CHECK(cs.haveZMPtrajectories());
260
261
  BOOST_CHECK(cs.haveFriction());
  BOOST_CHECK(cs.haveContactModelDefined());
262
263
264
}

BOOST_AUTO_TEST_SUITE_END()