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/mjcf/detail/Worldbody.hpp"
34
35 #include "dart/utils/XmlHelpers.hpp"
36 #include "dart/utils/mjcf/detail/Compiler.hpp"
37 #include "dart/utils/mjcf/detail/Size.hpp"
38 #include "dart/utils/mjcf/detail/Utils.hpp"
39
40 namespace dart {
41 namespace utils {
42 namespace MjcfParser {
43 namespace detail {
44
45 //==============================================================================
read(tinyxml2::XMLElement * element,const common::optional<Size> & size,const Defaults & defaults,const Default * currentDefault,const common::Uri & baseUri,const common::ResourceRetrieverPtr & retriever)46 Errors Worldbody::read(
47 tinyxml2::XMLElement* element,
48 const common::optional<Size>& size,
49 const Defaults& defaults,
50 const Default* currentDefault,
51 const common::Uri& baseUri,
52 const common::ResourceRetrieverPtr& retriever)
53 {
54 assert(currentDefault != nullptr);
55
56 Errors errors;
57
58 if (std::string(element->Name()) != "worldbody")
59 {
60 errors.emplace_back(
61 ErrorCode::INCORRECT_ELEMENT_TYPE,
62 "Failed to find <worldbody> from the provided element");
63 return errors;
64 }
65
66 // childclass
67 if (hasAttribute(element, "childclass"))
68 {
69 const std::string className = getAttributeString(element, "childclass");
70 const auto& defaultClass = defaults.getDefault(className);
71 if (defaultClass)
72 {
73 currentDefault = &(*defaultClass);
74 }
75 else
76 {
77 errors.push_back(Error(
78 ErrorCode::ATTRIBUTE_INVALID,
79 "Failed to find default with childclass name '" + className + "'"));
80 }
81 }
82
83 // Handle multiple <include>
84 const Errors includeErrors = handleInclude(element, baseUri, retriever);
85 errors.insert(errors.end(), includeErrors.begin(), includeErrors.end());
86
87 // Read multiple <geom>
88 ElementEnumerator geomElements(element, "geom");
89 while (geomElements.next())
90 {
91 Geom geom = Geom();
92 const auto geomErrors = geom.read(
93 geomElements.get(), defaults, currentDefault->getGeomAttributes());
94 errors.insert(errors.end(), geomErrors.begin(), geomErrors.end());
95
96 if (geomErrors.empty())
97 {
98 mGeoms.emplace_back(std::move(geom));
99 }
100 }
101
102 // Read multiple <site>
103 ElementEnumerator siteElements(element, "site");
104 while (siteElements.next())
105 {
106 Site site = Site();
107 const auto siteErrors = site.read(geomElements.get());
108 errors.insert(errors.end(), siteErrors.begin(), siteErrors.end());
109
110 if (siteErrors.empty())
111 {
112 mSites.emplace_back(std::move(site));
113 }
114 }
115
116 // Read multiple <body>
117 ElementEnumerator bodyElements(element, "body");
118 while (bodyElements.next())
119 {
120 Body rootBody = Body();
121 const auto bodyErrors
122 = rootBody.read(bodyElements.get(), size, defaults, currentDefault);
123 errors.insert(errors.end(), bodyErrors.begin(), bodyErrors.end());
124
125 if (bodyErrors.empty())
126 {
127 mRootBodies.emplace_back(std::move(rootBody));
128 }
129 }
130
131 return errors;
132 }
133
134 //==============================================================================
preprocess(const Compiler & compiler)135 Errors Worldbody::preprocess(const Compiler& compiler)
136 {
137 Errors errors;
138
139 for (Geom& geom : mGeoms)
140 {
141 const Errors geomErrors = geom.preprocess(compiler);
142 errors.insert(errors.end(), geomErrors.begin(), geomErrors.end());
143 }
144
145 for (Body& body : mRootBodies)
146 {
147 const Errors bodyErrors = body.preprocess(compiler);
148 errors.insert(errors.end(), bodyErrors.begin(), bodyErrors.end());
149 }
150
151 return errors;
152 }
153
154 //==============================================================================
compile(const Compiler & compiler)155 Errors Worldbody::compile(const Compiler& compiler)
156 {
157 Errors errors;
158
159 for (Geom& geom : mGeoms)
160 {
161 const Errors geomErrors = geom.compile(compiler);
162 errors.insert(errors.end(), geomErrors.begin(), geomErrors.end());
163 }
164
165 for (Body& body : mRootBodies)
166 {
167 const Errors bodyErrors = body.compile(compiler);
168 errors.insert(errors.end(), bodyErrors.begin(), bodyErrors.end());
169 }
170
171 return errors;
172 }
173
174 //==============================================================================
postprocess(const Compiler & compiler)175 Errors Worldbody::postprocess(const Compiler& compiler)
176 {
177 Errors errors;
178
179 for (Geom& geom : mGeoms)
180 {
181 const Errors geomErrors = geom.postprocess(nullptr, compiler);
182 errors.insert(errors.end(), geomErrors.begin(), geomErrors.end());
183 }
184
185 for (Body& body : mRootBodies)
186 {
187 const Errors bodyErrors = body.postprocess(nullptr, compiler);
188 errors.insert(errors.end(), bodyErrors.begin(), bodyErrors.end());
189 }
190
191 return errors;
192 }
193
194 //==============================================================================
getNumGeoms() const195 std::size_t Worldbody::getNumGeoms() const
196 {
197 return mGeoms.size();
198 }
199
200 //==============================================================================
getGeom(std::size_t index) const201 const Geom& Worldbody::getGeom(std::size_t index) const
202 {
203 return mGeoms[index];
204 }
205
206 //==============================================================================
getNumSites() const207 std::size_t Worldbody::getNumSites() const
208 {
209 return mSites.size();
210 }
211
212 //==============================================================================
getSite(std::size_t index) const213 const Site& Worldbody::getSite(std::size_t index) const
214 {
215 return mSites[index];
216 }
217
218 //==============================================================================
getNumRootBodies() const219 std::size_t Worldbody::getNumRootBodies() const
220 {
221 return mRootBodies.size();
222 }
223
224 //==============================================================================
getRootBody(std::size_t index) const225 const Body& Worldbody::getRootBody(std::size_t index) const
226 {
227 return mRootBodies[index];
228 }
229
230 } // namespace detail
231 } // namespace MjcfParser
232 } // namespace utils
233 } // namespace dart
234