1 /*
2 * Copyright (c) 2011-2021, 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 "dart/utils/FileInfoWorld.hpp"
34
35 #include <fstream>
36 #include <string>
37
38 #include "dart/simulation/Recording.hpp"
39
40 namespace dart {
41 namespace utils {
42
43 //==============================================================================
FileInfoWorld()44 FileInfoWorld::FileInfoWorld() : mRecord(nullptr)
45 {
46 std::strcpy(mFileName, "");
47 }
48
49 //==============================================================================
~FileInfoWorld()50 FileInfoWorld::~FileInfoWorld()
51 {
52 delete mRecord;
53 }
54
55 //==============================================================================
loadFile(const char * _fName)56 bool FileInfoWorld::loadFile(const char* _fName)
57 {
58 std::ifstream inFile(_fName);
59 if (inFile.fail() == 1)
60 return false;
61
62 inFile.precision(8);
63 char buffer[256];
64 int numFrames;
65 int numSkeletons;
66 int intVal;
67 double doubleVal;
68 std::vector<int> numDofsForSkels;
69 std::vector<double> tempState;
70 Eigen::VectorXd state;
71
72 inFile >> buffer;
73 inFile >> numFrames;
74 inFile >> buffer;
75 inFile >> numSkeletons;
76
77 for (int i = 0; i < numSkeletons; i++)
78 {
79 inFile >> buffer;
80 inFile >> intVal;
81 numDofsForSkels.push_back(intVal);
82 }
83
84 // Release the previous recording
85 delete mRecord;
86
87 mRecord = new simulation::Recording(numDofsForSkels);
88
89 for (int i = 0; i < numFrames; i++)
90 {
91 for (int j = 0; j < numSkeletons; j++)
92 {
93 for (int k = 0; k < mRecord->getNumDofs(j); k++)
94 {
95 inFile >> doubleVal;
96 tempState.push_back(doubleVal);
97 }
98 }
99
100 inFile >> buffer;
101 inFile >> intVal;
102 for (int j = 0; j < intVal; j++)
103 {
104 for (int k = 0; k < 6; k++)
105 {
106 inFile >> doubleVal;
107 tempState.push_back(doubleVal);
108 }
109 }
110
111 state.resize(tempState.size());
112 for (std::size_t j = 0; j < tempState.size(); j++)
113 state[j] = tempState[j];
114 mRecord->addState(state);
115 tempState.clear();
116 }
117 inFile.close();
118
119 std::string text = _fName;
120 int lastSlash = text.find_last_of("/");
121 text = text.substr(lastSlash + 1);
122 strcpy(mFileName, text.c_str());
123 return true;
124 }
125
126 //==============================================================================
saveFile(const char * _fName,simulation::Recording * _record)127 bool FileInfoWorld::saveFile(const char* _fName, simulation::Recording* _record)
128 {
129 std::ofstream outFile(_fName, std::ios::out);
130 if (outFile.fail())
131 return false;
132
133 outFile.precision(8);
134
135 outFile << "numFrames " << _record->getNumFrames() << std::endl;
136 outFile << "numSkeletons " << _record->getNumSkeletons() << std::endl;
137 for (int i = 0; i < _record->getNumSkeletons(); i++)
138 outFile << "Skeleton" << i << " " << _record->getNumDofs(i) << " ";
139 outFile << std::endl;
140 for (int i = 0; i < _record->getNumFrames(); i++)
141 {
142 for (int j = 0; j < _record->getNumSkeletons(); j++)
143 {
144 for (int k = 0; k < _record->getNumDofs(j); k++)
145 outFile << _record->getGenCoord(i, j, k) << " ";
146 outFile << std::endl;
147 }
148 outFile << "Contacts " << _record->getNumContacts(i) << std::endl;
149
150 for (int j = 0; j < _record->getNumContacts(i); j++)
151 {
152 outFile << _record->getContactPoint(i, j) << std::endl;
153 outFile << _record->getContactForce(i, j) << std::endl;
154 }
155 outFile << std::endl;
156 }
157
158 outFile.close();
159
160 std::string text = _fName;
161 int lastSlash = text.find_last_of("/");
162 text = text.substr(lastSlash + 1);
163 std::strcpy(mFileName, text.c_str());
164 return true;
165 }
166
167 //==============================================================================
getRecording() const168 simulation::Recording* FileInfoWorld::getRecording() const
169 {
170 return mRecord;
171 }
172
173 } // namespace utils
174 } // namespace dart
175