Debian Codegraph @

unittests/integration/test_FileInfoWorld.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <fstream>
34 #include <iostream>
35
36 #include <gtest/gtest.h>
37
38 #include "dart/dynamics/BodyNode.hpp"
39 #include "dart/dynamics/RevoluteJoint.hpp"
40 #include "dart/dynamics/Skeleton.hpp"
41 #include "dart/math/Geometry.hpp"
42 #include "dart/simulation/World.hpp"
43 #include "dart/utils/FileInfoWorld.hpp"
44 #include "dart/utils/SkelParser.hpp"
45
46 #include "TestHelpers.hpp"
47
48 using namespace dart;
49 using namespace math;
50 using namespace dynamics;
51 using namespace simulation;
52 using namespace utils;
53
54 //==============================================================================
55 TEST(FileInfoWorld, Basic)
56 {
57 const std::size_t numFrames = 100;
58 const std::string fileName = "testWorld.txt";
59 double tol = 1e-6;
60 bool result = false;
61 FileInfoWorld worldFile;
62
63 WorldPtr world = SkelParser::readWorld(
64 "dart://sample/skel/test/file_info_world_test.skel");
65 EXPECT_TRUE(world != nullptr);
66
67 Recording* recording1 = nullptr;
68 Recording* recording2 = nullptr;
69
70 // Do some simulation with recording
71 for (std::size_t i = 0; i < numFrames; ++i)
72 {
73 world->step();
74 world->bake();
75 }
76
77 recording1 = world->getRecording();
78 EXPECT_TRUE(recording1 != nullptr);
79
80 // Save the recording to a file
81 result = worldFile.saveFile(fileName.c_str(), world->getRecording());
82 EXPECT_TRUE(result);
83
84 // Load the file
85 result = worldFile.loadFile(fileName.c_str());
86 EXPECT_TRUE(result);
87 recording2 = worldFile.getRecording();
88 EXPECT_TRUE(recording2 != nullptr);
89
90 // Check number of frames
91 EXPECT_EQ(recording1->getNumFrames(), (int)numFrames);
92 EXPECT_EQ(recording2->getNumFrames(), (int)numFrames);
93
94 // Check number of skeletons
95 std::size_t numSkeletons = recording1->getNumSkeletons();
96 EXPECT_EQ(recording1->getNumSkeletons(), recording2->getNumSkeletons());
97
98 // Check number of dofs of the skeletons
99 for (std::size_t i = 0; i < numSkeletons; ++i)
100 EXPECT_EQ(recording1->getNumDofs(i), recording2->getNumDofs(i));
101
102 // Check generalized positions and contact info
103 for (std::size_t i = 0; i < numFrames; ++i)
104 {
105 // Check generalized positions
106 for (std::size_t j = 0; j < numSkeletons; ++j)
107 {
108 std::size_t dofs = recording1->getNumDofs(j);
109
110 for (std::size_t k = 0; k < dofs; ++k)
111 {
112 EXPECT_NEAR(
113 recording1->getGenCoord(i, j, k),
114 recording2->getGenCoord(i, j, k),
115 tol);
116 }
117 }
118
119 // Check contact info
120 tol = 1e-3;
121 std::size_t numContacts = recording1->getNumContacts(i);
122 EXPECT_EQ(recording1->getNumContacts(i), recording2->getNumContacts(i));
123
124 for (std::size_t j = 0; j < numContacts; ++j)
125 {
126 for (std::size_t k = 0; k < 3; ++k)
127 {
128 EXPECT_NEAR(
129 recording1->getContactForce(i, j)[k],
130 recording2->getContactForce(i, j)[k],
131 tol);
132
133 EXPECT_NEAR(
134 recording1->getContactPoint(i, j)[k],
135 recording2->getContactPoint(i, j)[k],
136 tol);
137 }
138 }
139 }
140 }