serialization_examples.py 14.2 KB
Newer Older
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
1
import pathlib
2
3
import unittest

4
5
import curves  # noqa: requiered to get C++ type exposition

6
7
8
9
10
import pinocchio as pin
from multicontact_api import ContactSequence

pin.switchToNumpyArray()

11
PATH = (pathlib.Path(__file__).parent.parent.parent / 'examples').absolute()
12
13
14
print("PATH : ", PATH)


Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
15
def assertTrajNotNone(testCase, phase, root, wholeBody):
16
17
18
19
20
21
22
23
24
25
26
27
28
29
    testCase.assertIsNotNone(phase.c_t)
    testCase.assertIsNotNone(phase.dc_t)
    testCase.assertIsNotNone(phase.ddc_t)
    testCase.assertIsNotNone(phase.L_t)
    testCase.assertIsNotNone(phase.dL_t)
    if root:
        testCase.assertIsNotNone(phase.root_t)
    if wholeBody:
        testCase.assertIsNotNone(phase.q_t)
        testCase.assertIsNotNone(phase.dq_t)
        testCase.assertIsNotNone(phase.ddq_t)
        testCase.assertIsNotNone(phase.tau_t)


Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
30
def testTrajMinMax(testCase, phase, root, wholeBody):
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
    testCase.assertTrue(phase.c_t.min() >= 0.)
    testCase.assertTrue(phase.dc_t.min() >= 0.)
    testCase.assertTrue(phase.ddc_t.min() >= 0.)
    testCase.assertTrue(phase.L_t.min() >= 0.)
    testCase.assertTrue(phase.dL_t.min() >= 0.)
    testCase.assertTrue(phase.c_t.max() >= 0.)
    testCase.assertTrue(phase.dc_t.max() >= 0.)
    testCase.assertTrue(phase.ddc_t.max() >= 0.)
    testCase.assertTrue(phase.L_t.max() >= 0.)
    testCase.assertTrue(phase.dL_t.max() >= 0.)
    if root:
        testCase.assertTrue(phase.root_t.min() >= 0.)
        testCase.assertTrue(phase.root_t.max() >= 0.)
    if wholeBody:
        testCase.assertTrue(phase.q_t.max() >= 0.)
        testCase.assertTrue(phase.dq_t.max() >= 0.)
        testCase.assertTrue(phase.ddq_t.max() >= 0.)
        testCase.assertTrue(phase.tau_t.max() >= 0.)
        testCase.assertTrue(phase.q_t.min() >= 0.)
        testCase.assertTrue(phase.dq_t.min() >= 0.)
        testCase.assertTrue(phase.ddq_t.min() >= 0.)
        testCase.assertTrue(phase.tau_t.min() >= 0.)

Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
54

55
56
57
58
59
def testCallTraj(testCase, phase, root, quasistatic, wholeBody):
    testCase.assertTrue(phase.c_t((phase.c_t.max() + phase.c_t.min()) / 2.).any())
    if not quasistatic:
        testCase.assertTrue(phase.dc_t((phase.dc_t.max() + phase.dc_t.min()) / 2.).any())
        testCase.assertTrue(phase.ddc_t((phase.ddc_t.max() + phase.ddc_t.min()) / 2.).any())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
60
61
    # testCase.assertTrue(phase.L_t((phase.L_t.max() + phase.L_t.min()) / 2.).any())
    # testCase.assertTrue(phase.dL_t((phase.dL_t.max() + phase.dL_t.min()) / 2.).any())
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
    if root:
        testCase.assertTrue(phase.root_t.max() >= 0.)
    if wholeBody:
        testCase.assertTrue(phase.q_t((phase.q_t.max() + phase.q_t.min()) / 2.).any())
        testCase.assertTrue(phase.dq_t((phase.dq_t.max() + phase.dq_t.min()) / 2.).any())
        testCase.assertTrue(phase.ddq_t((phase.ddq_t.max() + phase.ddq_t.min()) / 2.).any())
        testCase.assertTrue(phase.tau_t((phase.tau_t.max() + phase.tau_t.min()) / 2.).any())


def testEffectorTraj(testCase, phase):
    for eeName, traj in phase.effectorTrajectories().items():
        testCase.assertIsNotNone(traj)
        testCase.assertTrue(traj.min() >= 0.)
        testCase.assertTrue(traj.max() >= 0.)
        testCase.assertTrue(traj((traj.min() + traj.max()) / 2.).any())


def testContactForce(testCase, phase):
    for eeName, traj in phase.contactForces().items():
        testCase.assertIsNotNone(traj)
        testCase.assertTrue(traj.min() >= 0.)
        testCase.assertTrue(traj.max() >= 0.)
        testCase.assertTrue(traj((traj.min() + traj.max()) / 2.).any())


Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
87
def checkPhase(testCase, phase, root=False, quasistatic=False, effector=False, wholeBody=False):
88
89
90
91
92
93
94
95
    assertTrajNotNone(testCase, phase, root, wholeBody)
    testTrajMinMax(testCase, phase, root, wholeBody)
    testCallTraj(testCase, phase, root, quasistatic, wholeBody)
    if effector:
        testEffectorTraj(testCase, phase)
    if wholeBody:
        testContactForce(testCase, phase)

Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
96
97

def checkCS(testCase, cs, root=False, quasistatic=False, effector=False, wholeBody=False):
98
99
100
101
    for phase in cs.contactPhases:
        checkPhase(testCase, phase, root, quasistatic, effector, wholeBody)


Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
102
class ExamplesSerialization(unittest.TestCase):
103
104
    def test_com_motion_above_feet_COM(self):
        cs = ContactSequence()
105
        cs.loadFromBinary(str(PATH / "com_motion_above_feet_COM.cs"))
106
107
108
109
110
111
112
113
114
        self.assertEqual(cs.size(), 1)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        checkCS(self, cs)

    def test_com_motion_above_feet_WB(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
115
        cs.loadFromBinary(str(PATH / "com_motion_above_feet_WB.cs"))
116
117
118
119
120
121
122
123
124
        self.assertEqual(cs.size(), 1)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveJointsTrajectories())
        self.assertTrue(cs.haveJointsDerivativesTrajectories())
        self.assertTrue(cs.haveContactForcesTrajectories())
        self.assertTrue(cs.haveZMPtrajectories())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
125
        checkCS(self, cs, wholeBody=True)
126
127
128

    def test_step_in_place(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
129
        cs.loadFromBinary(str(PATH / "step_in_place.cs"))
130
131
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
132
133
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
134
135
136

    def test_step_in_place_COM(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
137
        cs.loadFromBinary(str(PATH / "step_in_place_COM.cs"))
138
139
140
141
142
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
143
144
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
145
        checkCS(self, cs, effector=False, wholeBody=False)
146
147
148

    def test_step_in_place_REF(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
149
        cs.loadFromBinary(str(PATH / "step_in_place_REF.cs"))
150
151
152
153
154
155
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories())
156
        self.assertTrue(cs.haveEffectorsTrajectories(1e-6, False))
157
158
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
159
        checkCS(self, cs, root=True, effector=True, wholeBody=False)
160
161
162

    def test_step_in_place_WB(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
163
        cs.loadFromBinary(str(PATH / "step_in_place_WB.cs"))
164
165
166
167
168
169
170
171
172
173
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
        self.assertTrue(cs.haveJointsTrajectories())
        self.assertTrue(cs.haveJointsDerivativesTrajectories())
        self.assertTrue(cs.haveContactForcesTrajectories())
        self.assertTrue(cs.haveZMPtrajectories())
174
175
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
176
        checkCS(self, cs, effector=True, wholeBody=True)
177
178
179

    def test_step_in_place_quasistatic(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
180
        cs.loadFromBinary(str(PATH / "step_in_place_quasistatic.cs"))
181
182
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
183
184
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
185
186
187

    def test_step_in_place_quasistatic_COM(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
188
        cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_COM.cs"))
189
190
191
192
193
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
194
195
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
196
        checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
197
198
199

    def test_step_in_place_quasistatic_REF(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
200
        cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_REF.cs"))
201
202
203
204
205
206
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories())
207
208
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
209
        checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False)
210
211
212

    def test_step_in_place_quasistatic_WB(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
213
        cs.loadFromBinary(str(PATH / "step_in_place_quasistatic_WB.cs"))
214
215
216
217
218
219
220
221
222
223
        self.assertEqual(cs.size(), 9)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
        self.assertTrue(cs.haveJointsTrajectories())
        self.assertTrue(cs.haveJointsDerivativesTrajectories())
        self.assertTrue(cs.haveContactForcesTrajectories())
        self.assertTrue(cs.haveZMPtrajectories())
224
225
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
226
        checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)
227
228
229

    def test_walk_20cm(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
230
        cs.loadFromBinary(str(PATH / "walk_20cm.cs"))
231
        self.assertEqual(cs.size(), 23)
232
233
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
234
235
236
237
        self.assertTrue(cs.haveConsistentContacts())

    def test_walk_20cm_COM(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
238
        cs.loadFromBinary(str(PATH / "walk_20cm_COM.cs"))
239
240
241
242
243
        self.assertEqual(cs.size(), 23)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
244
245
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
246
        checkCS(self, cs, effector=False, wholeBody=False)
247
248
249

    def test_walk_20cm_REF(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
250
        cs.loadFromBinary(str(PATH / "walk_20cm_REF.cs"))
251
252
253
254
255
256
        self.assertEqual(cs.size(), 23)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories())
257
258
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
259
        checkCS(self, cs, root=True, effector=True, wholeBody=False)
260
261
262

    def test_walk_20cm_WB(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
263
        cs.loadFromBinary(str(PATH / "walk_20cm_WB.cs"))
264
265
266
267
268
269
270
271
272
273
        self.assertEqual(cs.size(), 23)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
        self.assertTrue(cs.haveJointsTrajectories())
        self.assertTrue(cs.haveJointsDerivativesTrajectories())
        self.assertTrue(cs.haveContactForcesTrajectories())
        self.assertTrue(cs.haveZMPtrajectories())
274
275
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
276
        checkCS(self, cs, effector=True, wholeBody=True)
277
278
279

    def test_walk_20cm_quasistatic(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
280
        cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic.cs"))
281
        self.assertEqual(cs.size(), 23)
282
283
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
284
285
286
287
        self.assertTrue(cs.haveConsistentContacts())

    def test_walk_20cm_quasistatic_COM(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
288
        cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_COM.cs"))
289
290
291
292
293
        self.assertEqual(cs.size(), 23)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
294
295
        self.assertFalse(cs.haveFriction())
        self.assertFalse(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
296
        checkCS(self, cs, quasistatic=True, effector=False, wholeBody=False)
297
298
299

    def test_walk_20cm_quasistatic_REF(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
300
        cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_REF.cs"))
301
302
303
304
305
306
        self.assertEqual(cs.size(), 23)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories())
307
308
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
309
        checkCS(self, cs, root=True, quasistatic=True, effector=True, wholeBody=False)
310
311
312

    def test_walk_20cm_quasistatic_WB(self):
        cs = ContactSequence()
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
313
        cs.loadFromBinary(str(PATH / "walk_20cm_quasistatic_WB.cs"))
314
315
316
317
318
319
320
321
322
323
        self.assertEqual(cs.size(), 23)
        self.assertTrue(cs.haveConsistentContacts())
        self.assertTrue(cs.haveTimings())
        self.assertTrue(cs.haveCentroidalValues())
        self.assertTrue(cs.haveCentroidalTrajectories())
        self.assertTrue(cs.haveEffectorsTrajectories(1e-1))
        self.assertTrue(cs.haveJointsTrajectories())
        self.assertTrue(cs.haveJointsDerivativesTrajectories())
        self.assertTrue(cs.haveContactForcesTrajectories())
        self.assertTrue(cs.haveZMPtrajectories())
324
325
        self.assertTrue(cs.haveFriction())
        self.assertTrue(cs.haveContactModelDefined())
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
326
327
        checkCS(self, cs, quasistatic=True, effector=True, wholeBody=True)

328
329
330

if __name__ == '__main__':
    unittest.main()