Directory: | ./ |
---|---|
File: | include/pinocchio/parsers/mjcf/mjcf-graph.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 39 | 39 | 100.0% |
Branches: | 50 | 94 | 53.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2024 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_parsers_mjcf_graph_hpp__ | ||
6 | #define __pinocchio_parsers_mjcf_graph_hpp__ | ||
7 | |||
8 | #include "pinocchio/parsers/urdf.hpp" | ||
9 | #include "pinocchio/multibody/model.hpp" | ||
10 | #include "pinocchio/multibody/joint/joints.hpp" | ||
11 | |||
12 | #include <boost/property_tree/xml_parser.hpp> | ||
13 | #include <boost/property_tree/ptree.hpp> | ||
14 | #include <boost/foreach.hpp> | ||
15 | #include <boost/math/constants/constants.hpp> | ||
16 | #include <boost/filesystem.hpp> | ||
17 | #include <boost/logic/tribool.hpp> | ||
18 | #include <boost/lexical_cast.hpp> | ||
19 | |||
20 | #include <sstream> | ||
21 | #include <limits> | ||
22 | #include <iostream> | ||
23 | #include <unordered_map> | ||
24 | |||
25 | namespace pinocchio | ||
26 | { | ||
27 | namespace mjcf | ||
28 | { | ||
29 | namespace details | ||
30 | { | ||
31 | struct MjcfGraph; | ||
32 | struct MjcfJoint; | ||
33 | struct MjcfGeom; | ||
34 | struct MjcfSite; | ||
35 | |||
36 | /// @brief Informations that are stocked in the XML tag compile. | ||
37 | /// | ||
38 | struct PINOCCHIO_PARSERS_DLLAPI MjcfCompiler | ||
39 | { | ||
40 | public: | ||
41 | // Global attribute to use limit that are in the model or not | ||
42 | bool autolimits = true; | ||
43 | |||
44 | // Attribute to keep or not the full path of files specified in the model | ||
45 | bool strippath = false; | ||
46 | // Directory where all the meshes are (can be relative or absolute) | ||
47 | std::string meshdir; | ||
48 | // Directory where all the textures are (can be relative or absolute) | ||
49 | std::string texturedir; | ||
50 | |||
51 | // Value for angle conversion (Mujoco default - degrees) | ||
52 | double angle_converter = boost::math::constants::pi<double>() / 180.0; | ||
53 | // Euler Axis to use to convert angles representation to quaternion | ||
54 | Eigen::Matrix3d mapEulerAngles; | ||
55 | |||
56 | // Value to crop the mass (if mass < boundMass, mass = boundMass) | ||
57 | double boundMass = 0; | ||
58 | // Value to crop the diagonal of the inertia matrix (if mass < boundMass, mass = boundMass) | ||
59 | double boundInertia = 0; | ||
60 | |||
61 | // True, false or auto - auto = indeterminate | ||
62 | boost::logic::tribool inertiafromgeom = boost::logic::indeterminate; | ||
63 | |||
64 | /// @brief Convert the angle in radian if model was declared to use degree | ||
65 | /// @param angle_ angle to convert | ||
66 | /// @return converted angle | ||
67 | double convertAngle(const double & angle_) const; | ||
68 | |||
69 | /// @brief Convert the euler angles according to the convention declared in the compile tag. | ||
70 | /// @param angles Euler angles | ||
71 | /// @return Quaternion representation of the euler angles | ||
72 | Eigen::Matrix3d convertEuler(const Eigen::Vector3d & angles) const; | ||
73 | }; | ||
74 | |||
75 | /// @brief Structure to stock all default classes information | ||
76 | struct MjcfClass | ||
77 | { | ||
78 | public: | ||
79 | typedef boost::property_tree::ptree ptree; | ||
80 | |||
81 | // name of the default class | ||
82 | std::string className; | ||
83 | // Ptree associated with the class name | ||
84 | ptree classElement; | ||
85 | }; | ||
86 | |||
87 | /// @brief All Bodies informations extracted from mjcf model | ||
88 | struct MjcfBody | ||
89 | { | ||
90 | public: | ||
91 | // Name of the body | ||
92 | std::string bodyName; | ||
93 | // Name of the parent | ||
94 | std::string bodyParent; | ||
95 | // Name of the default class used by this body (optional) | ||
96 | std::string bodyClassName; | ||
97 | // Special default class, that is common to all bodies and children if not specified | ||
98 | // otherwise | ||
99 | std::string childClass; | ||
100 | |||
101 | // Position of the body wrt to the previous body | ||
102 | SE3 bodyPlacement = SE3::Identity(); | ||
103 | // Body inertia | ||
104 | Inertia bodyInertia = Inertia::Identity(); | ||
105 | |||
106 | // Vector of joints associated with the body | ||
107 | std::vector<MjcfJoint> jointChildren; | ||
108 | // Vector of geometries associated with the body | ||
109 | std::vector<MjcfGeom> geomChildren; | ||
110 | // Vector of sites | ||
111 | std::vector<MjcfSite> siteChildren; | ||
112 | }; | ||
113 | |||
114 | /// @brief All joint limits | ||
115 | struct PINOCCHIO_PARSERS_DLLAPI RangeJoint | ||
116 | { | ||
117 | // Max effort | ||
118 | Eigen::VectorXd maxEffort; | ||
119 | // Max velocity | ||
120 | Eigen::VectorXd maxVel; | ||
121 | // Max position | ||
122 | Eigen::VectorXd maxConfig; | ||
123 | // Min position | ||
124 | Eigen::VectorXd minConfig; | ||
125 | |||
126 | // Join Stiffness | ||
127 | Eigen::VectorXd springStiffness; | ||
128 | // joint position or angle in which the joint spring (if any) achieves equilibrium | ||
129 | Eigen::VectorXd springReference; | ||
130 | |||
131 | // friction applied in this joint | ||
132 | Eigen::VectorXd friction; | ||
133 | // Damping applied by this joint. | ||
134 | Eigen::VectorXd damping; | ||
135 | |||
136 | // Armature inertia created by this joint | ||
137 | double armature = 0.; | ||
138 | // Dry friction. | ||
139 | double frictionLoss = 0.; | ||
140 | |||
141 |
7/14✓ Branch 2 taken 156 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 156 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 156 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 156 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 156 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 156 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 156 times.
✗ Branch 21 not taken.
|
156 | RangeJoint() = default; |
142 | 85 | explicit RangeJoint(double v) | |
143 |
7/14✓ Branch 2 taken 85 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 85 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 85 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 85 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 85 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 85 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 85 times.
✗ Branch 21 not taken.
|
85 | { |
144 | 85 | const double infty = std::numeric_limits<double>::infinity(); | |
145 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | maxVel = Eigen::VectorXd::Constant(1, infty); |
146 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | maxEffort = Eigen::VectorXd::Constant(1, infty); |
147 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | minConfig = Eigen::VectorXd::Constant(1, -infty); |
148 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | maxConfig = Eigen::VectorXd::Constant(1, infty); |
149 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | springStiffness = Eigen::VectorXd::Constant(1, v); |
150 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | springReference = Eigen::VectorXd::Constant(1, v); |
151 | ; | ||
152 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | friction = Eigen::VectorXd::Constant(1, 0.); |
153 |
2/4✓ Branch 1 taken 85 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 85 times.
✗ Branch 5 not taken.
|
85 | damping = Eigen::VectorXd::Constant(1, 0.); |
154 | 85 | } | |
155 | |||
156 | /// @brief Set dimension to the limits to match the joint nq and nv. | ||
157 | /// @tparam Nq joint configuration | ||
158 | /// @tparam Nv joint velocity | ||
159 | /// @return Range with new dimension | ||
160 | template<int Nq, int Nv> | ||
161 | RangeJoint setDimension() const; | ||
162 | |||
163 | /// @brief Concatenate 2 rangeJoint | ||
164 | /// @tparam Nq old_range, joint configuration | ||
165 | /// @tparam Nv old_range, joint velocity | ||
166 | /// @param range to concatenate with | ||
167 | /// @return Concatenated range. | ||
168 | template<int Nq, int Nv> | ||
169 | RangeJoint concatenate(const RangeJoint & range) const; | ||
170 | }; | ||
171 | |||
172 | /// @brief All joint information parsed from the mjcf model | ||
173 | struct PINOCCHIO_PARSERS_DLLAPI MjcfJoint | ||
174 | { | ||
175 | public: | ||
176 | typedef boost::property_tree::ptree ptree; | ||
177 | |||
178 | // Name of the joint | ||
179 | std::string jointName = "free"; | ||
180 | // Placement of the joint wrt to its body - default Identity | ||
181 | SE3 jointPlacement = SE3::Identity(); | ||
182 | |||
183 | // axis of the joint - default "0 0 1" | ||
184 | Eigen::Vector3d axis = Eigen::Vector3d::UnitZ(); | ||
185 | // Limits that applie to this joint | ||
186 | RangeJoint range{1}; | ||
187 | |||
188 | // type of the joint (hinge, ball, slide, free) - default "hinge" | ||
189 | std::string jointType = "hinge"; | ||
190 | |||
191 | double posRef = 0.; // only possible for hinge and slides | ||
192 | |||
193 | /// @param el ptree joint node | ||
194 | /// @param currentBody body to which the joint belongs to | ||
195 | /// @param currentGraph current Mjcf graph (needed to get compiler information) | ||
196 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
197 | |||
198 | /// @brief Go through a joint node (default class or not) and parse info into the structure | ||
199 | /// @param el ptree joint node | ||
200 | /// @param use_limits whether to parse the limits or not | ||
201 | void | ||
202 | goThroughElement(const ptree & el, bool use_limits, const MjcfCompiler & currentCompiler); | ||
203 | }; | ||
204 | /// @brief All informations related to a mesh are stored here | ||
205 | struct MjcfMesh | ||
206 | { | ||
207 | // Scale of the mesh | ||
208 | Eigen::Vector3d scale = Eigen::Vector3d::Constant(1); | ||
209 | // Path to the mesh file | ||
210 | std::string filePath; | ||
211 | }; | ||
212 | |||
213 | /// @brief All informations related to a texture are stored here | ||
214 | struct MjcfTexture | ||
215 | { | ||
216 | // [2d, cube, skybox], “cube” | ||
217 | std::string textType = "cube"; | ||
218 | // Path to the texture file | ||
219 | std::string filePath; | ||
220 | // Size of the grid if needed | ||
221 | Eigen::Vector2d gridsize = Eigen::Vector2d::Constant(1); | ||
222 | }; | ||
223 | |||
224 | /// @brief All informations related to material are stored here | ||
225 | struct PINOCCHIO_PARSERS_DLLAPI MjcfMaterial | ||
226 | { | ||
227 | typedef boost::property_tree::ptree ptree; | ||
228 | // Color of the material | ||
229 | Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1); | ||
230 | |||
231 | float reflectance = 0; | ||
232 | |||
233 | float shininess = 0.5; | ||
234 | |||
235 | float specular = 0.5; | ||
236 | |||
237 | float emission = 0; | ||
238 | // name of the texture to apply on the material | ||
239 | std::string texture; | ||
240 | |||
241 | /// @brief Go through a ptree node to look for material tag related | ||
242 | /// @param el ptree material node | ||
243 | void goThroughElement(const ptree & el); | ||
244 | }; | ||
245 | |||
246 | struct PINOCCHIO_PARSERS_DLLAPI MjcfGeom | ||
247 | { | ||
248 | public: | ||
249 | typedef boost::property_tree::ptree ptree; | ||
250 | |||
251 | // Kind of possible geometry | ||
252 | enum TYPE | ||
253 | { | ||
254 | VISUAL, | ||
255 | COLLISION, | ||
256 | BOTH | ||
257 | }; | ||
258 | // name of the geometry object | ||
259 | std::string geomName; | ||
260 | |||
261 | // [plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh, sdf], “sphere” | ||
262 | std::string geomType = "sphere"; | ||
263 | |||
264 | // Kind of the geometry object | ||
265 | TYPE geomKind = BOTH; | ||
266 | |||
267 | // Contact filtering and dynamic pair (used here to determine geometry kind) | ||
268 | int contype = 1; | ||
269 | int conaffinity = 1; | ||
270 | // Geometry group (used to determine geometry kind) | ||
271 | int group = 0; | ||
272 | |||
273 | // String that hold size parameter | ||
274 | std::string sizeS; | ||
275 | // Optional in case fromto tag is used | ||
276 | boost::optional<std::string> fromtoS; | ||
277 | // Size parameter | ||
278 | Eigen::VectorXd size; | ||
279 | |||
280 | // Color of the geometry | ||
281 | Eigen::Vector4d rgba = Eigen::Vector4d::Constant(1); | ||
282 | |||
283 | // Name of the material applied on the geometry | ||
284 | std::string materialName; | ||
285 | // Name of the mesh (optional) | ||
286 | std::string meshName; | ||
287 | |||
288 | // Density for computing the mass | ||
289 | double density = 1000; | ||
290 | // If mass is only on the outer layer of the geometry | ||
291 | bool shellinertia = false; | ||
292 | |||
293 | // Geometry Placement in parent body. Center of the frame of geometry is the center of mass. | ||
294 | SE3 geomPlacement = SE3::Identity(); | ||
295 | // Inertia of the geometry obj | ||
296 | Inertia geomInertia = Inertia::Identity(); | ||
297 | // optional mass (if not defined, will use density) | ||
298 | boost::optional<double> massGeom; | ||
299 | |||
300 | /// @brief Find the geometry kind | ||
301 | void findKind(); | ||
302 | |||
303 | /// @brief Compute Geometry size based on sizeS and fromtoS | ||
304 | void computeSize(); | ||
305 | |||
306 | /// @brief Compute geometry inertia | ||
307 | void computeInertia(); | ||
308 | |||
309 | /// @brief Fill Geometry element with info from ptree nodes | ||
310 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
311 | |||
312 | /// @bried Go through a geom ptree node, to gather informations | ||
313 | void goThroughElement(const ptree & el, const MjcfGraph & currentGraph); | ||
314 | }; | ||
315 | |||
316 | struct PINOCCHIO_PARSERS_DLLAPI MjcfSite | ||
317 | { | ||
318 | typedef boost::property_tree::ptree ptree; | ||
319 | |||
320 | SE3 sitePlacement = SE3::Identity(); | ||
321 | |||
322 | std::string siteName; | ||
323 | |||
324 | void fill(const ptree & el, const MjcfBody & currentBody, const MjcfGraph & currentGraph); | ||
325 | void goThroughElement(const ptree & el, const MjcfGraph & currentGraph); | ||
326 | }; | ||
327 | |||
328 | /// @brief The graph which contains all information taken from the mjcf file | ||
329 | struct PINOCCHIO_PARSERS_DLLAPI MjcfGraph | ||
330 | { | ||
331 | public: | ||
332 | typedef boost::property_tree::ptree ptree; | ||
333 | typedef std::vector<std::string> VectorOfStrings; | ||
334 | typedef std::unordered_map<std::string, MjcfBody> BodyMap_t; | ||
335 | typedef std::unordered_map<std::string, MjcfClass> ClassMap_t; | ||
336 | typedef std::unordered_map<std::string, MjcfMaterial> MaterialMap_t; | ||
337 | typedef std::unordered_map<std::string, MjcfMesh> MeshMap_t; | ||
338 | typedef std::unordered_map<std::string, MjcfTexture> TextureMap_t; | ||
339 | typedef std::unordered_map<std::string, Eigen::VectorXd> ConfigMap_t; | ||
340 | |||
341 | // Compiler Info needed to properly parse the rest of file | ||
342 | MjcfCompiler compilerInfo; | ||
343 | // Map of default classes | ||
344 | ClassMap_t mapOfClasses; | ||
345 | // Map of bodies | ||
346 | BodyMap_t mapOfBodies; | ||
347 | // Map of Materials | ||
348 | MaterialMap_t mapOfMaterials; | ||
349 | // Map of Meshes | ||
350 | MeshMap_t mapOfMeshes; | ||
351 | // Map of textures | ||
352 | TextureMap_t mapOfTextures; | ||
353 | // Map of model configurations | ||
354 | ConfigMap_t mapOfConfigs; | ||
355 | |||
356 | // reference configuration | ||
357 | Eigen::VectorXd referenceConfig; | ||
358 | |||
359 | // property tree where xml file is stored | ||
360 | ptree pt; | ||
361 | |||
362 | // Ordered list of bodies | ||
363 | VectorOfStrings bodiesList; | ||
364 | |||
365 | // Name of the model | ||
366 | std::string modelName; | ||
367 | std::string modelPath; | ||
368 | |||
369 | // Urdf Visitor to add joint and body | ||
370 | typedef pinocchio::urdf::details:: | ||
371 | UrdfVisitor<double, 0, ::pinocchio::JointCollectionDefaultTpl> | ||
372 | UrdfVisitor; | ||
373 | UrdfVisitor & urdfVisitor; | ||
374 | |||
375 | /// @brief graph constructor | ||
376 | /// @param urdfVisitor | ||
377 | 26 | MjcfGraph(UrdfVisitor & urdfVisitor, const std::string & modelPath) | |
378 |
1/2✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
|
26 | : modelPath(modelPath) |
379 |
2/4✓ Branch 8 taken 26 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26 times.
✗ Branch 12 not taken.
|
26 | , urdfVisitor(urdfVisitor) |
380 | { | ||
381 | 26 | } | |
382 | |||
383 | /// @brief Convert pose of an mjcf element into SE3 | ||
384 | /// @param el ptree element with all the pose element | ||
385 | /// @return pose in SE3 | ||
386 | SE3 convertPosition(const ptree & el) const; | ||
387 | |||
388 | /// @brief Convert Inertia of an mjcf element into Inertia model of pinocchio | ||
389 | /// @param el ptree element with all the inertial information | ||
390 | /// @return Inertia element in pinocchio | ||
391 | Inertia convertInertiaFromMjcf(const ptree & el) const; | ||
392 | |||
393 | /// @brief Go through the default part of the file and get all the class name. Fill the | ||
394 | /// mapOfDefault for later use. | ||
395 | /// @param el ptree element. Root of the default | ||
396 | void parseDefault(ptree & el, const ptree & parent); | ||
397 | |||
398 | /// @brief Go through the main body of the mjcf file "worldbody" to get all the info ready | ||
399 | /// to create the model. | ||
400 | /// @param el root of the tree | ||
401 | /// @param parentName name of the parentBody in the robot tree | ||
402 | void parseJointAndBody( | ||
403 | const ptree & el, | ||
404 | const boost::optional<std::string> & childClass, | ||
405 | const std::string & parentName = ""); | ||
406 | |||
407 | /// @brief Parse all the info from the compile node into compilerInfo | ||
408 | /// @param el ptree compile node | ||
409 | void parseCompiler(const ptree & el); | ||
410 | |||
411 | /// @brief Parse all the info from a texture node | ||
412 | /// @param el ptree texture node | ||
413 | void parseTexture(const ptree & el); | ||
414 | |||
415 | /// @brief Parse all the info from a material node | ||
416 | /// @param el ptree material node | ||
417 | void parseMaterial(const ptree & el); | ||
418 | |||
419 | /// @brief Parse all the info from a mesh node | ||
420 | /// @param el ptree mesh node | ||
421 | void parseMesh(const ptree & el); | ||
422 | |||
423 | /// @brief Parse all the info from the meta tag asset (mesh, material, texture) | ||
424 | /// @param el ptree texture node | ||
425 | void parseAsset(const ptree & el); | ||
426 | |||
427 | /// @brief Parse all the info from the meta tag keyframe | ||
428 | /// @param el ptree keyframe node | ||
429 | void parseKeyFrame(const ptree & el); | ||
430 | |||
431 | /// @brief parse the mjcf file into a graph | ||
432 | void parseGraph(); | ||
433 | |||
434 | /// @brief parse the mjcf file into a graph | ||
435 | /// @param xmlStr xml file name | ||
436 | void parseGraphFromXML(const std::string & xmlStr); | ||
437 | |||
438 | /// @brief Create a joint to add to the joint composite if needed | ||
439 | /// @tparam TypeX joint with axis X | ||
440 | /// @tparam TypeY joint with axis Y | ||
441 | /// @tparam TypeZ joint with axis Z | ||
442 | /// @tparam TypeUnaligned joint with axis unaligned | ||
443 | /// @param axis axis of the joint | ||
444 | /// @return one of the joint with the right axis | ||
445 | template<typename TypeX, typename TypeY, typename TypeZ, typename TypeUnaligned> | ||
446 | JointModel createJoint(const Eigen::Vector3d & axis); | ||
447 | |||
448 | /// @brief Add a joint to the model. only needed when a body has a solo joint child | ||
449 | /// @param jointInfo The joint to add to the tree | ||
450 | /// @param currentBody The body associated with the joint | ||
451 | /// @param bodyInJoint Position of the body wrt to its joint | ||
452 | void | ||
453 | addSoloJoint(const MjcfJoint & jointInfo, const MjcfBody & currentBody, SE3 & bodyInJoint); | ||
454 | |||
455 | /// @brief Use all the infos that were parsed from the xml file to add a body and joint to | ||
456 | /// the model | ||
457 | /// @param nameOfBody Name of the body to add | ||
458 | void fillModel(const std::string & nameOfBody); | ||
459 | |||
460 | /// @brief Fill the pinocchio model with all the infos from the graph | ||
461 | void parseRootTree(); | ||
462 | |||
463 | /// @brief Fill reference configuration for a body and all it's associated dof | ||
464 | /// @param currentBody body to check | ||
465 | void fillReferenceConfig(const MjcfBody & currentBody); | ||
466 | |||
467 | /// @brief Add a keyframe to the model (ie reference configuration) | ||
468 | /// @param keyframe Keyframe to add | ||
469 | /// @param keyName Name of the keyframe | ||
470 | void addKeyFrame(const Eigen::VectorXd & keyframe, const std::string & keyName); | ||
471 | |||
472 | /// @brief Fill geometry model with all the info taken from the mjcf model file | ||
473 | /// @param type Type of geometry to parse (COLLISION or VISUAL) | ||
474 | /// @param geomModel geometry model to fill | ||
475 | /// @param meshLoader mesh loader from hpp::fcl | ||
476 | void parseGeomTree( | ||
477 | const GeometryType & type, | ||
478 | GeometryModel & geomModel, | ||
479 | ::hpp::fcl::MeshLoaderPtr & meshLoader); | ||
480 | }; | ||
481 | namespace internal | ||
482 | { | ||
483 | 602 | inline std::istringstream getConfiguredStringStream(const std::string & str) | |
484 | { | ||
485 | 602 | std::istringstream posStream(str); | |
486 |
1/2✓ Branch 1 taken 602 times.
✗ Branch 2 not taken.
|
602 | posStream.exceptions(std::ios::failbit); |
487 | 602 | return posStream; | |
488 | } | ||
489 | |||
490 | template<int N> | ||
491 | 1190 | inline Eigen::Matrix<double, N, 1> getVectorFromStream(const std::string & str) | |
492 | { | ||
493 |
1/2✓ Branch 1 taken 595 times.
✗ Branch 2 not taken.
|
1190 | std::istringstream stream = getConfiguredStringStream(str); |
494 |
1/2✓ Branch 1 taken 595 times.
✗ Branch 2 not taken.
|
1190 | Eigen::Matrix<double, N, 1> vector; |
495 |
2/2✓ Branch 0 taken 1694 times.
✓ Branch 1 taken 595 times.
|
4578 | for (int i = 0; i < N; i++) |
496 |
2/4✓ Branch 1 taken 1694 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1694 times.
✗ Branch 5 not taken.
|
3388 | stream >> vector(i); |
497 | |||
498 | 2380 | return vector; | |
499 | 1190 | } | |
500 | |||
501 | 3 | inline Eigen::VectorXd getUnknownSizeVectorFromStream(const std::string & str) | |
502 | { | ||
503 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | std::istringstream stream = getConfiguredStringStream(str); |
504 | 3 | std::vector<double> vector; | |
505 | double elem; | ||
506 |
3/4✓ Branch 1 taken 19 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✓ Branch 4 taken 3 times.
|
19 | while (!stream.eof()) |
507 | { | ||
508 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | stream >> elem; |
509 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | vector.push_back(elem); |
510 | } | ||
511 | |||
512 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | Eigen::VectorXd returnVector(vector.size()); |
513 |
2/2✓ Branch 1 taken 16 times.
✓ Branch 2 taken 3 times.
|
19 | for (std::size_t i = 0; i < vector.size(); i++) |
514 |
1/2✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
|
16 | returnVector(static_cast<Eigen::Index>(i)) = vector[i]; |
515 | |||
516 | 6 | return returnVector; | |
517 | 3 | } | |
518 | } // namespace internal | ||
519 | } // namespace details | ||
520 | } // namespace mjcf | ||
521 | } // namespace pinocchio | ||
522 | |||
523 | #endif // __pinocchio_parsers_mjcf_graph_hpp__ | ||
524 |