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/SkelParser.hpp"
34 
35 #include <algorithm>
36 #include <cstddef>
37 #include <vector>
38 
39 #include <Eigen/Dense>
40 #include <Eigen/StdVector>
41 
42 #include "dart/collision/CollisionObject.hpp"
43 #include "dart/collision/dart/DARTCollisionDetector.hpp"
44 #include "dart/collision/fcl/FCLCollisionDetector.hpp"
45 #include "dart/common/Console.hpp"
46 #include "dart/config.hpp"
47 #include "dart/constraint/ConstraintSolver.hpp"
48 #include "dart/dynamics/BallJoint.hpp"
49 #include "dart/dynamics/BodyNode.hpp"
50 #include "dart/dynamics/BoxShape.hpp"
51 #include "dart/dynamics/CapsuleShape.hpp"
52 #include "dart/dynamics/ConeShape.hpp"
53 #include "dart/dynamics/CylinderShape.hpp"
54 #include "dart/dynamics/EllipsoidShape.hpp"
55 #include "dart/dynamics/EulerJoint.hpp"
56 #include "dart/dynamics/FreeJoint.hpp"
57 #include "dart/dynamics/Joint.hpp"
58 #include "dart/dynamics/Marker.hpp"
59 #include "dart/dynamics/MeshShape.hpp"
60 #include "dart/dynamics/MultiSphereConvexHullShape.hpp"
61 #include "dart/dynamics/PlanarJoint.hpp"
62 #include "dart/dynamics/PlaneShape.hpp"
63 #include "dart/dynamics/PrismaticJoint.hpp"
64 #include "dart/dynamics/PyramidShape.hpp"
65 #include "dart/dynamics/RevoluteJoint.hpp"
66 #include "dart/dynamics/ScrewJoint.hpp"
67 #include "dart/dynamics/ShapeNode.hpp"
68 #include "dart/dynamics/Skeleton.hpp"
69 #include "dart/dynamics/SoftBodyNode.hpp"
70 #include "dart/dynamics/SoftMeshShape.hpp"
71 #include "dart/dynamics/SphereShape.hpp"
72 #include "dart/dynamics/TranslationalJoint.hpp"
73 #include "dart/dynamics/TranslationalJoint2D.hpp"
74 #include "dart/dynamics/UniversalJoint.hpp"
75 #include "dart/dynamics/WeldJoint.hpp"
76 #include "dart/utils/CompositeResourceRetriever.hpp"
77 #include "dart/utils/DartResourceRetriever.hpp"
78 #include "dart/utils/XmlHelpers.hpp"
79 
80 namespace dart {
81 namespace utils {
82 
83 namespace {
84 
85 enum NextResult
86 {
87   VALID,
88   CONTINUE,
89   BREAK,
90   CREATE_FREEJOINT_ROOT
91 };
92 
93 using BodyPropPtr = std::shared_ptr<dynamics::BodyNode::Properties>;
94 using JointPropPtr = std::shared_ptr<dynamics::Joint::Properties>;
95 
96 struct SkelBodyNode
97 {
98   BodyPropPtr properties;
99   Eigen::Isometry3d initTransform;
100   std::vector<dynamics::Marker::BasicProperties> markers;
101   std::string type;
102   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
103 };
104 
105 struct SkelJoint
106 {
107   JointPropPtr properties;
108   Eigen::VectorXd position;
109   Eigen::VectorXd velocity;
110   Eigen::VectorXd acceleration;
111   Eigen::VectorXd force;
112   std::string parentName;
113   std::string childName;
114   std::string type;
115 };
116 
117 // first: BodyNode name | second: BodyNode information
118 using BodyMap = common::aligned_map<std::string, SkelBodyNode>;
119 
120 // first: Child BodyNode name | second: Joint information
121 using JointMap = std::map<std::string, SkelJoint>;
122 
123 // first: Order that Joint appears in file | second: Child BodyNode name
124 using IndexToJoint = std::map<std::size_t, std::string>;
125 
126 // first: Child BodyNode name | second: Order that Joint appears in file
127 using JointToIndex = std::map<std::string, std::size_t>;
128 
129 simulation::WorldPtr readWorld(
130     tinyxml2::XMLElement* _worldElement,
131     const common::Uri& _baseUri,
132     const common::ResourceRetrieverPtr& _retriever);
133 
134 dart::dynamics::SkeletonPtr readSkeleton(
135     tinyxml2::XMLElement* _skeletonElement,
136     const common::Uri& _baseUri,
137     const common::ResourceRetrieverPtr& _retriever);
138 
139 SkelBodyNode readBodyNode(
140     tinyxml2::XMLElement* _bodyElement,
141     const Eigen::Isometry3d& _skeletonFrame,
142     const common::Uri& _baseUri,
143     const common::ResourceRetrieverPtr& _retriever);
144 
145 SkelBodyNode readSoftBodyNode(
146     tinyxml2::XMLElement* _softBodyNodeElement,
147     const Eigen::Isometry3d& _skeletonFrame,
148     const common::Uri& _baseUri,
149     const common::ResourceRetrieverPtr& _retriever);
150 
151 dynamics::ShapePtr readShape(
152     tinyxml2::XMLElement* shapeElement,
153     const std::string& bodyName,
154     const common::Uri& baseUri,
155     const common::ResourceRetrieverPtr& retriever);
156 
157 /// Read marker
158 dynamics::Marker::BasicProperties readMarker(
159     tinyxml2::XMLElement* _markerElement);
160 
161 void readJoint(
162     tinyxml2::XMLElement* _jointElement,
163     const BodyMap& _bodyNodes,
164     JointMap& _joints,
165     IndexToJoint& _order,
166     JointToIndex& _lookup);
167 
168 JointPropPtr readRevoluteJoint(
169     tinyxml2::XMLElement* _jointElement,
170     SkelJoint& _joint,
171     const std::string& _name);
172 
173 JointPropPtr readPrismaticJoint(
174     tinyxml2::XMLElement* _jointElement,
175     SkelJoint& _joint,
176     const std::string& _name);
177 
178 JointPropPtr readScrewJoint(
179     tinyxml2::XMLElement* _jointElement,
180     SkelJoint& _joint,
181     const std::string& _name);
182 
183 JointPropPtr readUniversalJoint(
184     tinyxml2::XMLElement* _universalJointElement,
185     SkelJoint& _joint,
186     const std::string& _name);
187 
188 JointPropPtr readBallJoint(
189     tinyxml2::XMLElement* _jointElement,
190     SkelJoint& _joint,
191     const std::string& _name);
192 
193 JointPropPtr readEulerJoint(
194     tinyxml2::XMLElement* _jointElement,
195     SkelJoint& _joint,
196     const std::string& _name);
197 
198 JointPropPtr readTranslationalJoint(
199     tinyxml2::XMLElement* _jointElement,
200     SkelJoint& _joint,
201     const std::string& _name);
202 
203 JointPropPtr readTranslationalJoint2D(
204     tinyxml2::XMLElement* _jointElement,
205     SkelJoint& _joint,
206     const std::string& _name);
207 
208 JointPropPtr readPlanarJoint(
209     tinyxml2::XMLElement* _jointElement,
210     SkelJoint& _joint,
211     const std::string& _name);
212 
213 JointPropPtr readFreeJoint(
214     tinyxml2::XMLElement* _jointElement,
215     SkelJoint& _joint,
216     const std::string& _name);
217 
218 JointPropPtr readWeldJoint(
219     tinyxml2::XMLElement* _jointElement,
220     SkelJoint& _joint,
221     const std::string& _name);
222 
223 common::ResourceRetrieverPtr getRetriever(
224     const common::ResourceRetrieverPtr& _retriever);
225 
226 dynamics::ShapeNode* readShapeNode(
227     dynamics::BodyNode* bodyNode,
228     tinyxml2::XMLElement* shapeNodeEle,
229     const std::string& shapeNodeName,
230     const common::Uri& baseUri,
231     const common::ResourceRetrieverPtr& retriever);
232 
233 void readVisualizationShapeNode(
234     dynamics::BodyNode* bodyNode,
235     tinyxml2::XMLElement* vizShapeNodeEle,
236     const common::Uri& baseUri,
237     const common::ResourceRetrieverPtr& retriever);
238 
239 void readCollisionShapeNode(
240     dynamics::BodyNode* bodyNode,
241     tinyxml2::XMLElement* collShapeNodeEle,
242     const common::Uri& baseUri,
243     const common::ResourceRetrieverPtr& retriever);
244 
245 void readAspects(
246     const dynamics::SkeletonPtr& skeleton,
247     tinyxml2::XMLElement* skeletonElement,
248     const common::Uri& baseUri,
249     const common::ResourceRetrieverPtr& retriever);
250 
251 tinyxml2::XMLElement* checkFormatAndGetWorldElement(
252     tinyxml2::XMLDocument& _document);
253 
254 simulation::WorldPtr readWorld(
255     tinyxml2::XMLElement* _worldElement,
256     const common::Uri& _baseUri,
257     const common::ResourceRetrieverPtr& _retriever);
258 
259 NextResult getNextJointAndNodePair(
260     JointMap::iterator& it,
261     BodyMap::const_iterator& child,
262     dynamics::BodyNode*& parent,
263     const dynamics::SkeletonPtr skeleton,
264     JointMap& joints,
265     const BodyMap& bodyNodes);
266 
267 template <typename BodyType>
268 std::pair<dynamics::Joint*, dynamics::BodyNode*> createJointAndNodePair(
269     dynamics::SkeletonPtr skeleton,
270     dynamics::BodyNode* parent,
271     const SkelJoint& joint,
272     const typename BodyType::Properties& body);
273 
274 bool createJointAndNodePair(
275     dynamics::SkeletonPtr skeleton,
276     dynamics::BodyNode* parent,
277     const SkelJoint& joint,
278     const SkelBodyNode& body);
279 
280 dynamics::SkeletonPtr readSkeleton(
281     tinyxml2::XMLElement* _skeletonElement,
282     const common::Uri& _baseUri,
283     const common::ResourceRetrieverPtr& _retriever);
284 
285 SkelBodyNode readBodyNode(
286     tinyxml2::XMLElement* _bodyNodeElement,
287     const Eigen::Isometry3d& _skeletonFrame,
288     const common::Uri& _baseUri,
289     const common::ResourceRetrieverPtr& _retriever);
290 
291 SkelBodyNode readSoftBodyNode(
292     tinyxml2::XMLElement* _softBodyNodeElement,
293     const Eigen::Isometry3d& _skeletonFrame,
294     const common::Uri& _baseUri,
295     const common::ResourceRetrieverPtr& _retriever);
296 
297 dynamics::ShapePtr readShape(
298     tinyxml2::XMLElement* vizEle,
299     const std::string& bodyName,
300     const common::Uri& baseUri,
301     const common::ResourceRetrieverPtr& retriever);
302 
303 dynamics::Marker::BasicProperties readMarker(
304     tinyxml2::XMLElement* _markerElement);
305 
306 void readJoint(
307     tinyxml2::XMLElement* _jointElement,
308     const BodyMap& _bodyNodes,
309     JointMap& _joints,
310     IndexToJoint& _order,
311     JointToIndex& _lookup);
312 
313 void getDofAttributeIfItExists(
314     const std::string& _attribute,
315     double* _value,
316     const std::string& _element_type,
317     const tinyxml2::XMLElement* _xmlElement,
318     const std::string& _jointName,
319     std::size_t _index);
320 
321 void setDofLimitAttributes(
322     tinyxml2::XMLElement* _dofElement,
323     const std::string& _element_type,
324     const std::string& _jointName,
325     std::size_t _index,
326     double* lower,
327     double* upper,
328     double* initial);
329 
330 template <typename PropertyType>
331 void readAllDegreesOfFreedom(
332     tinyxml2::XMLElement* _jointElement,
333     PropertyType& _properties,
334     SkelJoint& _joint,
335     const std::string& _jointName,
336     std::size_t _numDofs);
337 
338 template <typename PropertyType>
339 void readDegreeOfFreedom(
340     tinyxml2::XMLElement* _dofElement,
341     PropertyType& properties,
342     SkelJoint& joint,
343     const std::string& jointName,
344     std::size_t numDofs);
345 
346 template <typename PropertyType>
347 void readJointDynamicsAndLimit(
348     tinyxml2::XMLElement* _jointElement,
349     PropertyType& _properties,
350     SkelJoint& _joint,
351     const std::string& _name,
352     std::size_t _numAxis);
353 
354 JointPropPtr readWeldJoint(
355     tinyxml2::XMLElement* _jointElement, SkelJoint& _joint, const std::string&);
356 
357 JointPropPtr readRevoluteJoint(
358     tinyxml2::XMLElement* _jointElement,
359     SkelJoint& _joint,
360     const std::string& _name);
361 
362 JointPropPtr readPrismaticJoint(
363     tinyxml2::XMLElement* _jointElement,
364     SkelJoint& _joint,
365     const std::string& _name);
366 
367 JointPropPtr readScrewJoint(
368     tinyxml2::XMLElement* _jointElement,
369     SkelJoint& _joint,
370     const std::string& _name);
371 
372 JointPropPtr readUniversalJoint(
373     tinyxml2::XMLElement* _jointElement,
374     SkelJoint& _joint,
375     const std::string& _name);
376 
377 JointPropPtr readBallJoint(
378     tinyxml2::XMLElement* _jointElement,
379     SkelJoint& _joint,
380     const std::string& _name);
381 
382 JointPropPtr readEulerJoint(
383     tinyxml2::XMLElement* _jointElement,
384     SkelJoint& _joint,
385     const std::string& _name);
386 
387 JointPropPtr readTranslationalJoint(
388     tinyxml2::XMLElement* _jointElement,
389     SkelJoint& _joint,
390     const std::string& _name);
391 
392 JointPropPtr readPlanarJoint(
393     tinyxml2::XMLElement* _jointElement,
394     SkelJoint& _joint,
395     const std::string& _name);
396 
397 JointPropPtr readFreeJoint(
398     tinyxml2::XMLElement* _jointElement,
399     SkelJoint& _joint,
400     const std::string& _name);
401 
402 common::ResourceRetrieverPtr getRetriever(
403     const common::ResourceRetrieverPtr& _retriever);
404 
405 } // anonymous namespace
406 
407 //==============================================================================
readWorld(const common::Uri & _uri,const common::ResourceRetrieverPtr & _retriever)408 simulation::WorldPtr SkelParser::readWorld(
409     const common::Uri& _uri, const common::ResourceRetrieverPtr& _retriever)
410 {
411   const common::ResourceRetrieverPtr retriever = getRetriever(_retriever);
412 
413   //--------------------------------------------------------------------------
414   // Load xml and create Document
415   tinyxml2::XMLDocument _dartFile;
416   try
417   {
418     openXMLFile(_dartFile, _uri, retriever);
419   }
420   catch (std::exception const& e)
421   {
422     dterr << "[readWorld] LoadFile [" << _uri.toString()
423           << "] Failed: " << e.what() << "\n";
424     return nullptr;
425   }
426 
427   tinyxml2::XMLElement* worldElement = checkFormatAndGetWorldElement(_dartFile);
428   if (!worldElement)
429   {
430     dterr << "[readWorld] File named [" << _uri.toString()
431           << "] could not be parsed!\n";
432     return nullptr;
433   }
434 
435   return ::dart::utils::readWorld(worldElement, _uri, retriever);
436 }
437 
438 //==============================================================================
readWorldXML(const std::string & _xmlString,const common::Uri & _baseUri,const common::ResourceRetrieverPtr & _retriever)439 simulation::WorldPtr SkelParser::readWorldXML(
440     const std::string& _xmlString,
441     const common::Uri& _baseUri,
442     const common::ResourceRetrieverPtr& _retriever)
443 {
444   const common::ResourceRetrieverPtr retriever = getRetriever(_retriever);
445 
446   tinyxml2::XMLDocument _dartXML;
447   if (_dartXML.Parse(_xmlString.c_str()) != tinyxml2::XML_SUCCESS)
448   {
449     _dartXML.PrintError();
450     return nullptr;
451   }
452 
453   tinyxml2::XMLElement* worldElement = checkFormatAndGetWorldElement(_dartXML);
454   if (!worldElement)
455   {
456     dterr << "[readWorldXML] XML String could not be parsed!\n";
457     return nullptr;
458   }
459 
460   return ::dart::utils::readWorld(worldElement, _baseUri, retriever);
461 }
462 
463 //==============================================================================
readSkeleton(const common::Uri & uri,const common::ResourceRetrieverPtr & nullOrRetriever)464 dynamics::SkeletonPtr SkelParser::readSkeleton(
465     const common::Uri& uri, const common::ResourceRetrieverPtr& nullOrRetriever)
466 {
467   const common::ResourceRetrieverPtr retriever = getRetriever(nullOrRetriever);
468 
469   //--------------------------------------------------------------------------
470   // Load xml and create Document
471   tinyxml2::XMLDocument dartFile;
472   try
473   {
474     openXMLFile(dartFile, uri, retriever);
475   }
476   catch (std::exception const& e)
477   {
478     std::cout << "LoadFile [" << uri.toString() << "] Fails: " << e.what()
479               << std::endl;
480     return nullptr;
481   }
482 
483   //--------------------------------------------------------------------------
484   // Load DART
485   tinyxml2::XMLElement* skelElement = nullptr;
486   skelElement = dartFile.FirstChildElement("skel");
487   if (skelElement == nullptr)
488   {
489     dterr << "Skel file[" << uri.toString()
490           << "] does not contain <skel> as the element.\n";
491     return nullptr;
492   }
493 
494   //--------------------------------------------------------------------------
495   // Load World
496   tinyxml2::XMLElement* skeletonElement = nullptr;
497   skeletonElement = skelElement->FirstChildElement("skeleton");
498   if (skeletonElement == nullptr)
499   {
500     dterr << "Skel file[" << uri.toString()
501           << "] does not contain <skeleton> element "
502           << "under <skel> element.\n";
503     return nullptr;
504   }
505 
506   dynamics::SkeletonPtr newSkeleton
507       = ::dart::utils::readSkeleton(skeletonElement, uri, retriever);
508 
509   return newSkeleton;
510 }
511 
512 namespace {
513 
514 //==============================================================================
readShapeNode(dynamics::BodyNode * bodyNode,tinyxml2::XMLElement * shapeNodeEle,const std::string & shapeNodeName,const common::Uri & baseUri,const common::ResourceRetrieverPtr & retriever)515 dynamics::ShapeNode* readShapeNode(
516     dynamics::BodyNode* bodyNode,
517     tinyxml2::XMLElement* shapeNodeEle,
518     const std::string& shapeNodeName,
519     const common::Uri& baseUri,
520     const common::ResourceRetrieverPtr& retriever)
521 {
522   assert(bodyNode);
523 
524   auto shape = readShape(shapeNodeEle, bodyNode->getName(), baseUri, retriever);
525   auto shapeNode = bodyNode->createShapeNode(shape, shapeNodeName);
526 
527   // Transformation
528   if (hasElement(shapeNodeEle, "transformation"))
529   {
530     Eigen::Isometry3d W = getValueIsometry3d(shapeNodeEle, "transformation");
531     shapeNode->setRelativeTransform(W);
532   }
533 
534   return shapeNode;
535 }
536 
537 //==============================================================================
readVisualizationShapeNode(dynamics::BodyNode * bodyNode,tinyxml2::XMLElement * vizShapeNodeEle,const common::Uri & baseUri,const common::ResourceRetrieverPtr & retriever)538 void readVisualizationShapeNode(
539     dynamics::BodyNode* bodyNode,
540     tinyxml2::XMLElement* vizShapeNodeEle,
541     const common::Uri& baseUri,
542     const common::ResourceRetrieverPtr& retriever)
543 {
544   dynamics::ShapeNode* newShapeNode = readShapeNode(
545       bodyNode,
546       vizShapeNodeEle,
547       bodyNode->getName() + " - visual shape",
548       baseUri,
549       retriever);
550 
551   auto visualAspect = newShapeNode->getVisualAspect(true);
552 
553   // color
554   if (hasElement(vizShapeNodeEle, "color"))
555   {
556     Eigen::VectorXd color = getValueVectorXd(vizShapeNodeEle, "color");
557 
558     if (color.size() == 3)
559     {
560       visualAspect->setColor(static_cast<Eigen::Vector3d>(color));
561     }
562     else if (color.size() == 4)
563     {
564       visualAspect->setColor(static_cast<Eigen::Vector4d>(color));
565     }
566     else
567     {
568       dtwarn << "[readVisualizationShapeNode] Invalid format for <color> "
569              << "element; " << color.size() << "d vector is given. It should "
570              << "be either 3d vector or 4d vector (the 4th element is for "
571              << "alpha). Ignoring the given color value.\n";
572     }
573   }
574 }
575 
576 //==============================================================================
readCollisionShapeNode(dynamics::BodyNode * bodyNode,tinyxml2::XMLElement * collShapeNodeEle,const common::Uri & baseUri,const common::ResourceRetrieverPtr & retriever)577 void readCollisionShapeNode(
578     dynamics::BodyNode* bodyNode,
579     tinyxml2::XMLElement* collShapeNodeEle,
580     const common::Uri& baseUri,
581     const common::ResourceRetrieverPtr& retriever)
582 {
583   dynamics::ShapeNode* newShapeNode = readShapeNode(
584       bodyNode,
585       collShapeNodeEle,
586       bodyNode->getName() + " - collision shape",
587       baseUri,
588       retriever);
589 
590   auto collisionAspect = newShapeNode->getCollisionAspect(true);
591   newShapeNode->createDynamicsAspect();
592 
593   // collidable
594   if (hasElement(collShapeNodeEle, "collidable"))
595   {
596     const bool collidable = getValueDouble(collShapeNodeEle, "collidable");
597     collisionAspect->setCollidable(collidable);
598   }
599 }
600 
601 //==============================================================================
readAspects(const dynamics::SkeletonPtr & skeleton,tinyxml2::XMLElement * skeletonElement,const common::Uri & baseUri,const common::ResourceRetrieverPtr & retriever)602 void readAspects(
603     const dynamics::SkeletonPtr& skeleton,
604     tinyxml2::XMLElement* skeletonElement,
605     const common::Uri& baseUri,
606     const common::ResourceRetrieverPtr& retriever)
607 {
608   ElementEnumerator xmlBodies(skeletonElement, "body");
609   while (xmlBodies.next())
610   {
611     auto bodyElement = xmlBodies.get();
612     auto bodyNodeName = getAttributeString(bodyElement, "name");
613     auto bodyNode = skeleton->getBodyNode(bodyNodeName);
614 
615     // visualization_shape
616     ElementEnumerator vizShapes(bodyElement, "visualization_shape");
617     while (vizShapes.next())
618       readVisualizationShapeNode(bodyNode, vizShapes.get(), baseUri, retriever);
619 
620     // collision_shape
621     ElementEnumerator collShapes(bodyElement, "collision_shape");
622     while (collShapes.next())
623       readCollisionShapeNode(bodyNode, collShapes.get(), baseUri, retriever);
624 
625     // Update inertia if unspecified
626     if (hasElement(bodyElement, "inertia"))
627     {
628       tinyxml2::XMLElement* inertiaElement = getElement(bodyElement, "inertia");
629 
630       if (!hasElement(inertiaElement, "moment_of_inertia"))
631       {
632         for (auto& shapeNode : bodyNode->getShapeNodes())
633         {
634           const auto& shapeType = shapeNode->getShape()->getType();
635           if (dynamics::SoftMeshShape::getStaticType() == shapeType)
636             continue;
637 
638           auto mass = bodyNode->getMass();
639           Eigen::Matrix3d Ic = shapeNode->getShape()->computeInertia(mass);
640           auto inertia = bodyNode->getInertia();
641           inertia.setMoment(Ic);
642           bodyNode->setInertia(inertia);
643 
644           // TODO(JS): We use the inertia of the first non-soft mesh shape in
645           // a body for the body's inertia when not specified. We might want to
646           // use the summation of all the shapes' inertia instead.
647 
648           break;
649         }
650       }
651     }
652   }
653 }
654 
655 //==============================================================================
checkFormatAndGetWorldElement(tinyxml2::XMLDocument & _document)656 tinyxml2::XMLElement* checkFormatAndGetWorldElement(
657     tinyxml2::XMLDocument& _document)
658 {
659   //--------------------------------------------------------------------------
660   // Check xml tag
661   tinyxml2::XMLElement* skelElement = nullptr;
662   skelElement = _document.FirstChildElement("skel");
663   if (skelElement == nullptr)
664   {
665     dterr << "XML Document does not contain <skel> as the root element.\n";
666     return nullptr;
667   }
668 
669   //--------------------------------------------------------------------------
670   // Load World
671   tinyxml2::XMLElement* worldElement = nullptr;
672   worldElement = skelElement->FirstChildElement("world");
673   if (worldElement == nullptr)
674   {
675     dterr << "XML Document does not contain a <world> element under the <skel> "
676           << "element.\n";
677     return nullptr;
678   }
679 
680   return worldElement;
681 }
682 
683 //==============================================================================
684 static std::shared_ptr<collision::CollisionDetector>
createFclMeshCollisionDetector()685 createFclMeshCollisionDetector()
686 {
687   auto cd = collision::CollisionDetector::getFactory()->create("fcl");
688   auto fcl = std::static_pointer_cast<collision::FCLCollisionDetector>(cd);
689   fcl->setPrimitiveShapeType(collision::FCLCollisionDetector::MESH);
690   fcl->setContactPointComputationMethod(collision::FCLCollisionDetector::DART);
691 
692   return fcl;
693 }
694 
695 //==============================================================================
readWorld(tinyxml2::XMLElement * _worldElement,const common::Uri & _baseUri,const common::ResourceRetrieverPtr & _retriever)696 simulation::WorldPtr readWorld(
697     tinyxml2::XMLElement* _worldElement,
698     const common::Uri& _baseUri,
699     const common::ResourceRetrieverPtr& _retriever)
700 {
701   assert(_worldElement != nullptr);
702 
703   // Create a world
704   simulation::WorldPtr newWorld = simulation::World::create();
705 
706   //--------------------------------------------------------------------------
707   // Load physics
708   tinyxml2::XMLElement* physicsElement
709       = _worldElement->FirstChildElement("physics");
710   if (physicsElement != nullptr)
711   {
712     // Time step
713     tinyxml2::XMLElement* timeStepElement = nullptr;
714     timeStepElement = physicsElement->FirstChildElement("time_step");
715     if (timeStepElement != nullptr)
716     {
717       std::string strTimeStep = timeStepElement->GetText();
718       double timeStep = toDouble(strTimeStep);
719       newWorld->setTimeStep(timeStep);
720     }
721 
722     // Gravity
723     tinyxml2::XMLElement* gravityElement = nullptr;
724     gravityElement = physicsElement->FirstChildElement("gravity");
725     if (gravityElement != nullptr)
726     {
727       std::string strGravity = gravityElement->GetText();
728       Eigen::Vector3d gravity = toVector3d(strGravity);
729       newWorld->setGravity(gravity);
730     }
731 
732     // Collision detector
733     std::shared_ptr<collision::CollisionDetector> collision_detector;
734 
735     if (hasElement(physicsElement, "collision_detector"))
736     {
737       const auto cdType = getValueString(physicsElement, "collision_detector");
738 
739       if (cdType == "fcl_mesh")
740       {
741         collision_detector = createFclMeshCollisionDetector();
742       }
743       else if (cdType == "fcl")
744       {
745         collision_detector
746             = collision::CollisionDetector::getFactory()->create("fcl");
747         auto cd = std::static_pointer_cast<collision::FCLCollisionDetector>(
748             collision_detector);
749         cd->setPrimitiveShapeType(collision::FCLCollisionDetector::PRIMITIVE);
750         cd->setContactPointComputationMethod(
751             collision::FCLCollisionDetector::DART);
752       }
753       else
754       {
755         collision_detector
756             = collision::CollisionDetector::getFactory()->create(cdType);
757       }
758 
759       if (!collision_detector)
760       {
761         dtwarn << "Unknown collision detector[" << cdType << "]. "
762                << "Default collision detector[fcl_mesh] will be loaded.\n";
763       }
764     }
765 
766     if (!collision_detector)
767       collision_detector = createFclMeshCollisionDetector();
768 
769     newWorld->getConstraintSolver()->setCollisionDetector(collision_detector);
770   }
771 
772   //--------------------------------------------------------------------------
773   // Load soft skeletons
774   ElementEnumerator SkeletonElements(_worldElement, "skeleton");
775   while (SkeletonElements.next())
776   {
777     dynamics::SkeletonPtr newSkeleton = ::dart::utils::readSkeleton(
778         SkeletonElements.get(), _baseUri, _retriever);
779 
780     newWorld->addSkeleton(newSkeleton);
781   }
782 
783   return newWorld;
784 }
785 
786 //==============================================================================
getNextJointAndNodePair(JointMap::iterator & it,BodyMap::const_iterator & child,dynamics::BodyNode * & parent,const dynamics::SkeletonPtr skeleton,JointMap & joints,const BodyMap & bodyNodes)787 NextResult getNextJointAndNodePair(
788     JointMap::iterator& it,
789     BodyMap::const_iterator& child,
790     dynamics::BodyNode*& parent,
791     const dynamics::SkeletonPtr skeleton,
792     JointMap& joints,
793     const BodyMap& bodyNodes)
794 {
795   NextResult result = VALID;
796   const SkelJoint& joint = it->second;
797   parent = skeleton->getBodyNode(joint.parentName);
798   if (nullptr == parent && !joint.parentName.empty())
799   {
800     // Find the properties of the parent Joint of the current Joint, because it
801     // does not seem to be created yet.
802     JointMap::iterator check_parent_joint = joints.find(joint.parentName);
803     if (check_parent_joint == joints.end())
804     {
805       BodyMap::const_iterator check_parent_node
806           = bodyNodes.find(joint.parentName);
807       if (check_parent_node == bodyNodes.end())
808       {
809         dterr << "[getNextJointAndNodePair] Could not find BodyNode "
810               << "named [" << joint.parentName << "] requested as parent of "
811               << "the Joint named [" << joint.properties->mName << "]. We will "
812               << "now quit parsing.\n";
813         return BREAK;
814       }
815 
816       // If the current Joint has a parent BodyNode but does not have a parent
817       // Joint, then we need to create a FreeJoint for the parent BodyNode.
818       result = CREATE_FREEJOINT_ROOT;
819     }
820     else
821     {
822       it = check_parent_joint;
823       return CONTINUE; // Create the parent before creating the current Joint
824     }
825   }
826 
827   // Find the child node of this Joint, so we can create them together
828   child = bodyNodes.find(joint.childName);
829   if (child == bodyNodes.end())
830   {
831     dterr << "[getNextJointAndNodePair] Could not find BodyNode "
832           << "named [" << joint.childName << "] requested as child of Joint ["
833           << joint.properties->mName << "]. This should not be possible! "
834           << "We will now quit parsing. Please report this bug!\n";
835     return BREAK;
836   }
837 
838   return result;
839 }
840 
841 //==============================================================================
842 template <typename BodyType>
createJointAndNodePair(dynamics::SkeletonPtr skeleton,dynamics::BodyNode * parent,const SkelJoint & joint,const typename BodyType::Properties & body)843 std::pair<dynamics::Joint*, dynamics::BodyNode*> createJointAndNodePair(
844     dynamics::SkeletonPtr skeleton,
845     dynamics::BodyNode* parent,
846     const SkelJoint& joint,
847     const typename BodyType::Properties& body)
848 {
849   if (std::string("weld") == joint.type)
850     return skeleton->createJointAndBodyNodePair<dynamics::WeldJoint, BodyType>(
851         parent,
852         static_cast<const dynamics::WeldJoint::Properties&>(*joint.properties),
853         body);
854 
855   else if (std::string("prismatic") == joint.type)
856     return skeleton
857         ->createJointAndBodyNodePair<dynamics::PrismaticJoint, BodyType>(
858             parent,
859             static_cast<const dynamics::PrismaticJoint::Properties&>(
860                 *joint.properties),
861             body);
862 
863   else if (std::string("revolute") == joint.type)
864     return skeleton
865         ->createJointAndBodyNodePair<dynamics::RevoluteJoint, BodyType>(
866             parent,
867             static_cast<const dynamics::RevoluteJoint::Properties&>(
868                 *joint.properties),
869             body);
870 
871   else if (std::string("universal") == joint.type)
872     return skeleton
873         ->createJointAndBodyNodePair<dynamics::UniversalJoint, BodyType>(
874             parent,
875             static_cast<const dynamics::UniversalJoint::Properties&>(
876                 *joint.properties),
877             body);
878 
879   else if (std::string("ball") == joint.type)
880     return skeleton->createJointAndBodyNodePair<dynamics::BallJoint, BodyType>(
881         parent,
882         static_cast<const dynamics::BallJoint::Properties&>(*joint.properties),
883         body);
884 
885   else if (std::string("euler") == joint.type)
886     return skeleton->createJointAndBodyNodePair<dynamics::EulerJoint, BodyType>(
887         parent,
888         static_cast<const dynamics::EulerJoint::Properties&>(*joint.properties),
889         body);
890 
891   else if (std::string("translational") == joint.type)
892     return skeleton
893         ->createJointAndBodyNodePair<dynamics::TranslationalJoint, BodyType>(
894             parent,
895             static_cast<const dynamics::TranslationalJoint::Properties&>(
896                 *joint.properties),
897             body);
898 
899   else if (std::string("planar") == joint.type)
900     return skeleton
901         ->createJointAndBodyNodePair<dynamics::PlanarJoint, BodyType>(
902             parent,
903             static_cast<const dynamics::PlanarJoint::Properties&>(
904                 *joint.properties),
905             body);
906 
907   else if (std::string("free") == joint.type)
908     return skeleton->createJointAndBodyNodePair<dynamics::FreeJoint, BodyType>(
909         parent,
910         static_cast<const dynamics::FreeJoint::Properties&>(*joint.properties),
911         body);
912 
913   else
914   {
915     dterr << "[createJointAndNodePair] Unsupported Joint type (" << joint.type
916           << ") for Joint named [" << joint.properties->mName
917           << "]! It will be discarded.\n";
918     return std::pair<dynamics::Joint*, dynamics::BodyNode*>(nullptr, nullptr);
919   }
920 }
921 
922 //==============================================================================
createJointAndNodePair(dynamics::SkeletonPtr skeleton,dynamics::BodyNode * parent,const SkelJoint & joint,const SkelBodyNode & body)923 bool createJointAndNodePair(
924     dynamics::SkeletonPtr skeleton,
925     dynamics::BodyNode* parent,
926     const SkelJoint& joint,
927     const SkelBodyNode& body)
928 {
929   std::pair<dynamics::Joint*, dynamics::BodyNode*> pair;
930   if (body.type.empty())
931     pair = createJointAndNodePair<dynamics::BodyNode>(
932         skeleton,
933         parent,
934         joint,
935         static_cast<const dynamics::BodyNode::Properties&>(*body.properties));
936   else if (std::string("soft") == body.type)
937     pair = createJointAndNodePair<dynamics::SoftBodyNode>(
938         skeleton,
939         parent,
940         joint,
941         static_cast<const dynamics::SoftBodyNode::Properties&>(
942             *body.properties));
943   else
944   {
945     dterr << "[createJointAndNodePair] Invalid type (" << body.type
946           << ") for BodyNode named [" << body.properties->mName << "]\n";
947     return false;
948   }
949 
950   if (pair.first == nullptr || pair.second == nullptr)
951     return false;
952 
953   dynamics::Joint* newJoint = pair.first;
954   newJoint->setPositions(joint.position);
955   newJoint->setVelocities(joint.velocity);
956   newJoint->setAccelerations(joint.acceleration);
957   newJoint->setForces(joint.force);
958 
959   dynamics::BodyNode* bn = pair.second;
960   for (std::size_t i = 0; i < body.markers.size(); ++i)
961     bn->createNode<dynamics::Marker>(body.markers[i]);
962 
963   return true;
964 }
965 
966 //==============================================================================
readSkeleton(tinyxml2::XMLElement * _skeletonElement,const common::Uri & _baseUri,const common::ResourceRetrieverPtr & _retriever)967 dynamics::SkeletonPtr readSkeleton(
968     tinyxml2::XMLElement* _skeletonElement,
969     const common::Uri& _baseUri,
970     const common::ResourceRetrieverPtr& _retriever)
971 {
972   assert(_skeletonElement != nullptr);
973 
974   dynamics::SkeletonPtr newSkeleton = dynamics::Skeleton::create();
975   Eigen::Isometry3d skeletonFrame = Eigen::Isometry3d::Identity();
976 
977   //--------------------------------------------------------------------------
978   // Name attribute
979   std::string name = getAttributeString(_skeletonElement, "name");
980   newSkeleton->setName(name);
981 
982   //--------------------------------------------------------------------------
983   // transformation
984   if (hasElement(_skeletonElement, "transformation"))
985   {
986     Eigen::Isometry3d W
987         = getValueIsometry3d(_skeletonElement, "transformation");
988     skeletonFrame = W;
989   }
990 
991   //--------------------------------------------------------------------------
992   // immobile attribute
993   tinyxml2::XMLElement* mobileElement = nullptr;
994   mobileElement = _skeletonElement->FirstChildElement("mobile");
995   if (mobileElement != nullptr)
996   {
997     newSkeleton->setMobile(toBool(mobileElement->GetText()));
998   }
999 
1000   //--------------------------------------------------------------------------
1001   // Bodies
1002   BodyMap bodyNodes;
1003   ElementEnumerator xmlBodies(_skeletonElement, "body");
1004   while (xmlBodies.next())
1005   {
1006     SkelBodyNode newBodyNode = readSoftBodyNode(
1007         xmlBodies.get(), skeletonFrame, _baseUri, _retriever);
1008 
1009     BodyMap::const_iterator it = bodyNodes.find(newBodyNode.properties->mName);
1010     if (it != bodyNodes.end())
1011     {
1012       dterr << "[readSkeleton] Skeleton named [" << name << "] has "
1013             << "multiple BodyNodes with the name ["
1014             << newBodyNode.properties->mName << "], but BodyNode names must be "
1015             << "unique! We will discard all BodyNodes with a repeated name.\n";
1016       continue;
1017     }
1018 
1019     bodyNodes[newBodyNode.properties->mName] = newBodyNode;
1020   }
1021 
1022   //--------------------------------------------------------------------------
1023   // Joints
1024   JointMap joints;
1025   IndexToJoint order;
1026   JointToIndex lookup;
1027   ElementEnumerator xmlJoints(_skeletonElement, "joint");
1028   while (xmlJoints.next())
1029     readJoint(xmlJoints.get(), bodyNodes, joints, order, lookup);
1030 
1031   //--------------------------------------------------------------------------
1032   // Assemble skeleton
1033   JointMap::iterator it = joints.find(order.begin()->second);
1034   BodyMap::const_iterator child;
1035   dynamics::BodyNode* parent;
1036   while (it != joints.end())
1037   {
1038     NextResult result = getNextJointAndNodePair(
1039         it, child, parent, newSkeleton, joints, bodyNodes);
1040 
1041     if (BREAK == result)
1042       break;
1043     else if (CONTINUE == result)
1044       continue;
1045     else if (CREATE_FREEJOINT_ROOT == result)
1046     {
1047       // If a root FreeJoint is needed for the parent of the current joint, then
1048       // create it
1049       BodyMap::const_iterator rootNode = bodyNodes.find(it->second.parentName);
1050       SkelJoint rootJoint;
1051       rootJoint.properties = dynamics::FreeJoint::Properties::createShared(
1052           dynamics::Joint::Properties("root", rootNode->second.initTransform));
1053       rootJoint.type = "free";
1054 
1055       if (!createJointAndNodePair(
1056               newSkeleton, nullptr, rootJoint, rootNode->second))
1057         break;
1058 
1059       continue;
1060     }
1061 
1062     if (!createJointAndNodePair(newSkeleton, parent, it->second, child->second))
1063       break;
1064 
1065     JointToIndex::iterator index = lookup.find(it->first);
1066     order.erase(index->second);
1067     lookup.erase(index);
1068     joints.erase(it);
1069 
1070     IndexToJoint::iterator nextJoint = order.begin();
1071     if (nextJoint == order.end())
1072       break;
1073 
1074     it = joints.find(nextJoint->second);
1075   }
1076 
1077   // Read aspects here since aspects cannot be added if the BodyNodes haven't
1078   // created yet.
1079   readAspects(newSkeleton, _skeletonElement, _baseUri, _retriever);
1080 
1081   newSkeleton->resetPositions();
1082   newSkeleton->resetVelocities();
1083 
1084   return newSkeleton;
1085 }
1086 
1087 //==============================================================================
readBodyNode(tinyxml2::XMLElement * _bodyNodeElement,const Eigen::Isometry3d & _skeletonFrame,const common::Uri &,const common::ResourceRetrieverPtr &)1088 SkelBodyNode readBodyNode(
1089     tinyxml2::XMLElement* _bodyNodeElement,
1090     const Eigen::Isometry3d& _skeletonFrame,
1091     const common::Uri& /*_baseUri*/,
1092     const common::ResourceRetrieverPtr& /*_retriever*/)
1093 {
1094   assert(_bodyNodeElement != nullptr);
1095 
1096   BodyPropPtr newBodyNode(new dynamics::BodyNode::Properties);
1097   Eigen::Isometry3d initTransform = Eigen::Isometry3d::Identity();
1098 
1099   // Name attribute
1100   newBodyNode->mName = getAttributeString(_bodyNodeElement, "name");
1101 
1102   //--------------------------------------------------------------------------
1103   // gravity
1104   if (hasElement(_bodyNodeElement, "gravity"))
1105   {
1106     newBodyNode->mGravityMode = getValueBool(_bodyNodeElement, "gravity");
1107   }
1108 
1109   //--------------------------------------------------------------------------
1110   // self_collide
1111   //    if (hasElement(_bodyElement, "self_collide"))
1112   //    {
1113   //        bool gravityMode = getValueBool(_bodyElement, "self_collide");
1114   //    }
1115 
1116   //--------------------------------------------------------------------------
1117   // transformation
1118   if (hasElement(_bodyNodeElement, "transformation"))
1119   {
1120     Eigen::Isometry3d W
1121         = getValueIsometry3d(_bodyNodeElement, "transformation");
1122     initTransform = _skeletonFrame * W;
1123   }
1124   else
1125   {
1126     initTransform = _skeletonFrame;
1127   }
1128 
1129   //--------------------------------------------------------------------------
1130   // inertia
1131   if (hasElement(_bodyNodeElement, "inertia"))
1132   {
1133     tinyxml2::XMLElement* inertiaElement
1134         = getElement(_bodyNodeElement, "inertia");
1135 
1136     // mass
1137     double mass = getValueDouble(inertiaElement, "mass");
1138     newBodyNode->mInertia.setMass(mass);
1139 
1140     // moment of inertia
1141     if (hasElement(inertiaElement, "moment_of_inertia"))
1142     {
1143       tinyxml2::XMLElement* moiElement
1144           = getElement(inertiaElement, "moment_of_inertia");
1145 
1146       double ixx = getValueDouble(moiElement, "ixx");
1147       double iyy = getValueDouble(moiElement, "iyy");
1148       double izz = getValueDouble(moiElement, "izz");
1149 
1150       double ixy = getValueDouble(moiElement, "ixy");
1151       double ixz = getValueDouble(moiElement, "ixz");
1152       double iyz = getValueDouble(moiElement, "iyz");
1153 
1154       newBodyNode->mInertia.setMoment(ixx, iyy, izz, ixy, ixz, iyz);
1155     }
1156 
1157     // offset
1158     if (hasElement(inertiaElement, "offset"))
1159     {
1160       Eigen::Vector3d offset = getValueVector3d(inertiaElement, "offset");
1161       newBodyNode->mInertia.setLocalCOM(offset);
1162     }
1163   }
1164 
1165   SkelBodyNode skelBodyNode;
1166   skelBodyNode.properties = newBodyNode;
1167   skelBodyNode.initTransform = initTransform;
1168 
1169   //--------------------------------------------------------------------------
1170   // marker
1171   ElementEnumerator markers(_bodyNodeElement, "marker");
1172   while (markers.next())
1173   {
1174     skelBodyNode.markers.push_back(readMarker(markers.get()));
1175   }
1176 
1177   return skelBodyNode;
1178 }
1179 
1180 //==============================================================================
readSoftBodyNode(tinyxml2::XMLElement * _softBodyNodeElement,const Eigen::Isometry3d & _skeletonFrame,const common::Uri & _baseUri,const common::ResourceRetrieverPtr & _retriever)1181 SkelBodyNode readSoftBodyNode(
1182     tinyxml2::XMLElement* _softBodyNodeElement,
1183     const Eigen::Isometry3d& _skeletonFrame,
1184     const common::Uri& _baseUri,
1185     const common::ResourceRetrieverPtr& _retriever)
1186 {
1187   //---------------------------------- Note ------------------------------------
1188   // SoftBodyNode is created if _softBodyNodeElement has <soft_shape>.
1189   // Otherwise, BodyNode is created.
1190 
1191   //----------------------------------------------------------------------------
1192   assert(_softBodyNodeElement != nullptr);
1193 
1194   SkelBodyNode standardBodyNode = readBodyNode(
1195       _softBodyNodeElement, _skeletonFrame, _baseUri, _retriever);
1196 
1197   // If _softBodyNodeElement has no <soft_shape>, return rigid body node
1198   if (!hasElement(_softBodyNodeElement, "soft_shape"))
1199     return standardBodyNode;
1200 
1201   //----------------------------------------------------------------------------
1202   // Soft properties
1203   dynamics::SoftBodyNode::UniqueProperties newSoftBodyNode;
1204 
1205   if (hasElement(_softBodyNodeElement, "soft_shape"))
1206   {
1207     tinyxml2::XMLElement* softShapeEle
1208         = getElement(_softBodyNodeElement, "soft_shape");
1209 
1210     // mass
1211     double totalMass = getValueDouble(softShapeEle, "total_mass");
1212 
1213     // transformation
1214     Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
1215     if (hasElement(softShapeEle, "transformation"))
1216       T = getValueIsometry3d(softShapeEle, "transformation");
1217 
1218     // geometry
1219     tinyxml2::XMLElement* geometryEle = getElement(softShapeEle, "geometry");
1220     if (hasElement(geometryEle, "sphere"))
1221     {
1222       tinyxml2::XMLElement* sphereEle = getElement(geometryEle, "sphere");
1223       const auto radius = getValueDouble(sphereEle, "radius");
1224       const auto nSlices = getValueUInt(sphereEle, "num_slices");
1225       const auto nStacks = getValueUInt(sphereEle, "num_stacks");
1226       newSoftBodyNode = dynamics::SoftBodyNodeHelper::makeSphereProperties(
1227           radius, nSlices, nStacks, totalMass);
1228     }
1229     else if (hasElement(geometryEle, "box"))
1230     {
1231       tinyxml2::XMLElement* boxEle = getElement(geometryEle, "box");
1232       Eigen::Vector3d size = getValueVector3d(boxEle, "size");
1233       Eigen::Vector3i frags = getValueVector3i(boxEle, "frags");
1234       newSoftBodyNode = dynamics::SoftBodyNodeHelper::makeBoxProperties(
1235           size, T, frags, totalMass);
1236     }
1237     else if (hasElement(geometryEle, "ellipsoid"))
1238     {
1239       tinyxml2::XMLElement* ellipsoidEle = getElement(geometryEle, "ellipsoid");
1240       Eigen::Vector3d size = getValueVector3d(ellipsoidEle, "size");
1241       const auto nSlices = getValueUInt(ellipsoidEle, "num_slices");
1242       const auto nStacks = getValueUInt(ellipsoidEle, "num_stacks");
1243       newSoftBodyNode = dynamics::SoftBodyNodeHelper::makeEllipsoidProperties(
1244           size, nSlices, nStacks, totalMass);
1245     }
1246     else if (hasElement(geometryEle, "cylinder"))
1247     {
1248       tinyxml2::XMLElement* ellipsoidEle = getElement(geometryEle, "cylinder");
1249       double radius = getValueDouble(ellipsoidEle, "radius");
1250       double height = getValueDouble(ellipsoidEle, "height");
1251       double nSlices = getValueDouble(ellipsoidEle, "num_slices");
1252       double nStacks = getValueDouble(ellipsoidEle, "num_stacks");
1253       double nRings = getValueDouble(ellipsoidEle, "num_rings");
1254       newSoftBodyNode = dynamics::SoftBodyNodeHelper::makeCylinderProperties(
1255           radius, height, nSlices, nStacks, nRings, totalMass);
1256     }
1257     else
1258     {
1259       dterr << "[readSoftBodyNode] Unknown soft shape in "
1260             << "SoftBodyNode named [" << standardBodyNode.properties->mName
1261             << "]\n";
1262     }
1263 
1264     // kv
1265     if (hasElement(softShapeEle, "kv"))
1266     {
1267       newSoftBodyNode.mKv = getValueDouble(softShapeEle, "kv");
1268     }
1269 
1270     // ke
1271     if (hasElement(softShapeEle, "ke"))
1272     {
1273       newSoftBodyNode.mKe = getValueDouble(softShapeEle, "ke");
1274     }
1275 
1276     // damp
1277     if (hasElement(softShapeEle, "damp"))
1278     {
1279       newSoftBodyNode.mDampCoeff = getValueDouble(softShapeEle, "damp");
1280     }
1281   }
1282 
1283   SkelBodyNode softBodyNode;
1284   softBodyNode.properties = dynamics::SoftBodyNode::Properties::createShared(
1285       *standardBodyNode.properties, newSoftBodyNode);
1286 
1287   softBodyNode.initTransform = standardBodyNode.initTransform;
1288   softBodyNode.type = "soft";
1289 
1290   return softBodyNode;
1291 }
1292 
1293 //==============================================================================
readShape(tinyxml2::XMLElement * vizEle,const std::string & bodyName,const common::Uri & baseUri,const common::ResourceRetrieverPtr & retriever)1294 dynamics::ShapePtr readShape(
1295     tinyxml2::XMLElement* vizEle,
1296     const std::string& bodyName,
1297     const common::Uri& baseUri,
1298     const common::ResourceRetrieverPtr& retriever)
1299 {
1300   dynamics::ShapePtr newShape;
1301 
1302   // Geometry
1303   assert(hasElement(vizEle, "geometry"));
1304   tinyxml2::XMLElement* geometryEle = getElement(vizEle, "geometry");
1305 
1306   if (hasElement(geometryEle, "sphere"))
1307   {
1308     tinyxml2::XMLElement* sphereEle = getElement(geometryEle, "sphere");
1309     const auto radius = getValueDouble(sphereEle, "radius");
1310     newShape = dynamics::ShapePtr(new dynamics::SphereShape(radius));
1311   }
1312   else if (hasElement(geometryEle, "box"))
1313   {
1314     tinyxml2::XMLElement* boxEle = getElement(geometryEle, "box");
1315     Eigen::Vector3d size = getValueVector3d(boxEle, "size");
1316     newShape = dynamics::ShapePtr(new dynamics::BoxShape(size));
1317   }
1318   else if (hasElement(geometryEle, "ellipsoid"))
1319   {
1320     tinyxml2::XMLElement* ellipsoidEle = getElement(geometryEle, "ellipsoid");
1321     Eigen::Vector3d size = getValueVector3d(ellipsoidEle, "size");
1322     newShape = dynamics::ShapePtr(new dynamics::EllipsoidShape(size));
1323   }
1324   else if (hasElement(geometryEle, "cylinder"))
1325   {
1326     tinyxml2::XMLElement* cylinderEle = getElement(geometryEle, "cylinder");
1327     double radius = getValueDouble(cylinderEle, "radius");
1328     double height = getValueDouble(cylinderEle, "height");
1329     newShape = dynamics::ShapePtr(new dynamics::CylinderShape(radius, height));
1330   }
1331   else if (hasElement(geometryEle, "capsule"))
1332   {
1333     tinyxml2::XMLElement* capsuleEle = getElement(geometryEle, "capsule");
1334     double radius = getValueDouble(capsuleEle, "radius");
1335     double height = getValueDouble(capsuleEle, "height");
1336     newShape = dynamics::ShapePtr(new dynamics::CapsuleShape(radius, height));
1337   }
1338   else if (hasElement(geometryEle, "cone"))
1339   {
1340     tinyxml2::XMLElement* coneEle = getElement(geometryEle, "cone");
1341     double radius = getValueDouble(coneEle, "radius");
1342     double height = getValueDouble(coneEle, "height");
1343     newShape = dynamics::ShapePtr(new dynamics::ConeShape(radius, height));
1344   }
1345   else if (hasElement(geometryEle, "pyramid"))
1346   {
1347     tinyxml2::XMLElement* coneEle = getElement(geometryEle, "pyramid");
1348     double base_width = getValueDouble(coneEle, "base_width");
1349     double base_depth = getValueDouble(coneEle, "base_depth");
1350     double height = getValueDouble(coneEle, "height");
1351     newShape = dynamics::ShapePtr(
1352         new dynamics::PyramidShape(base_width, base_depth, height));
1353   }
1354   else if (hasElement(geometryEle, "plane"))
1355   {
1356     tinyxml2::XMLElement* planeEle = getElement(geometryEle, "plane");
1357     Eigen::Vector3d normal = getValueVector3d(planeEle, "normal");
1358     if (hasElement(planeEle, "offset"))
1359     {
1360       double offset = getValueDouble(planeEle, "offset");
1361       newShape = dynamics::ShapePtr(new dynamics::PlaneShape(normal, offset));
1362     }
1363     else if (hasElement(planeEle, "point"))
1364     {
1365       dtwarn << "[readShape] <point> element of <plane> is "
1366              << "deprecated as of DART 4.3. Please use <offset> element "
1367              << "instead." << std::endl;
1368       Eigen::Vector3d point = getValueVector3d(planeEle, "point");
1369       newShape = dynamics::ShapePtr(new dynamics::PlaneShape(normal, point));
1370     }
1371     else
1372     {
1373       dtwarn << "[readShape] <offset> element is not specified for "
1374              << "plane shape. DART will use 0.0." << std::endl;
1375       newShape = dynamics::ShapePtr(new dynamics::PlaneShape(normal, 0.0));
1376     }
1377   }
1378   else if (hasElement(geometryEle, "multi_sphere"))
1379   {
1380     tinyxml2::XMLElement* multiSphereEle
1381         = getElement(geometryEle, "multi_sphere");
1382 
1383     ElementEnumerator xmlSpheres(multiSphereEle, "sphere");
1384     dynamics::MultiSphereConvexHullShape::Spheres spheres;
1385     while (xmlSpheres.next())
1386     {
1387       const double radius = getValueDouble(xmlSpheres.get(), "radius");
1388       const Eigen::Vector3d position
1389           = getValueVector3d(xmlSpheres.get(), "position");
1390 
1391       spheres.emplace_back(radius, position);
1392     }
1393 
1394     newShape
1395         = dynamics::ShapePtr(new dynamics::MultiSphereConvexHullShape(spheres));
1396   }
1397   else if (hasElement(geometryEle, "mesh"))
1398   {
1399     tinyxml2::XMLElement* meshEle = getElement(geometryEle, "mesh");
1400     std::string filename = getValueString(meshEle, "file_name");
1401     Eigen::Vector3d scale = getValueVector3d(meshEle, "scale");
1402 
1403     const common::Uri meshUri
1404         = common::Uri::createFromRelativeUri(baseUri, filename);
1405     const aiScene* model = dynamics::MeshShape::loadMesh(meshUri, retriever);
1406     if (model)
1407     {
1408       newShape = std::make_shared<dynamics::MeshShape>(
1409           scale, model, meshUri, retriever);
1410     }
1411     else
1412     {
1413       dterr << "Fail to load model[" << filename << "]." << std::endl;
1414     }
1415   }
1416   else
1417   {
1418     dterr << "[readShape] Unknown visualization shape in BodyNode "
1419           << "named [" << bodyName << "]\n";
1420     assert(0);
1421     return nullptr;
1422   }
1423 
1424   return newShape;
1425 }
1426 
1427 //==============================================================================
readMarker(tinyxml2::XMLElement * _markerElement)1428 dynamics::Marker::BasicProperties readMarker(
1429     tinyxml2::XMLElement* _markerElement)
1430 {
1431   // Name attribute
1432   std::string name = getAttributeString(_markerElement, "name");
1433 
1434   // offset
1435   Eigen::Vector3d offset = Eigen::Vector3d::Zero();
1436   if (hasElement(_markerElement, "offset"))
1437     offset = getValueVector3d(_markerElement, "offset");
1438 
1439   dynamics::Marker::BasicProperties newMarker;
1440   newMarker.mName = name;
1441   newMarker.mRelativeTf.translation() = offset;
1442 
1443   return newMarker;
1444 }
1445 
1446 //==============================================================================
readJoint(tinyxml2::XMLElement * _jointElement,const BodyMap & _bodyNodes,JointMap & _joints,IndexToJoint & _order,JointToIndex & _lookup)1447 void readJoint(
1448     tinyxml2::XMLElement* _jointElement,
1449     const BodyMap& _bodyNodes,
1450     JointMap& _joints,
1451     IndexToJoint& _order,
1452     JointToIndex& _lookup)
1453 {
1454   assert(_jointElement != nullptr);
1455 
1456   //--------------------------------------------------------------------------
1457   // Name attribute
1458   std::string name = getAttributeString(_jointElement, "name");
1459 
1460   SkelJoint joint;
1461 
1462   //--------------------------------------------------------------------------
1463   // Type attribute
1464   joint.type = getAttributeString(_jointElement, "type");
1465   assert(!joint.type.empty());
1466   if (joint.type == std::string("weld"))
1467     joint.properties = readWeldJoint(_jointElement, joint, name);
1468   else if (joint.type == std::string("prismatic"))
1469     joint.properties = readPrismaticJoint(_jointElement, joint, name);
1470   else if (joint.type == std::string("revolute"))
1471     joint.properties = readRevoluteJoint(_jointElement, joint, name);
1472   else if (joint.type == std::string("screw"))
1473     joint.properties = readScrewJoint(_jointElement, joint, name);
1474   else if (joint.type == std::string("universal"))
1475     joint.properties = readUniversalJoint(_jointElement, joint, name);
1476   else if (joint.type == std::string("ball"))
1477     joint.properties = readBallJoint(_jointElement, joint, name);
1478   else if (joint.type == std::string("euler"))
1479     joint.properties = readEulerJoint(_jointElement, joint, name);
1480   else if (joint.type == std::string("translational"))
1481     joint.properties = readTranslationalJoint(_jointElement, joint, name);
1482   else if (joint.type == std::string("translational2d"))
1483     joint.properties = readTranslationalJoint2D(_jointElement, joint, name);
1484   else if (joint.type == std::string("planar"))
1485     joint.properties = readPlanarJoint(_jointElement, joint, name);
1486   else if (joint.type == std::string("free"))
1487     joint.properties = readFreeJoint(_jointElement, joint, name);
1488   else
1489   {
1490     dterr << "[readJoint] Unsupported joint type [" << joint.type
1491           << "] requested by Joint named [" << name << "]. This Joint will be "
1492           << "discarded.\n";
1493     return;
1494   }
1495   assert(joint.properties != nullptr);
1496 
1497   joint.properties->mName = name;
1498 
1499   //--------------------------------------------------------------------------
1500   // Actuator attribute
1501   if (hasAttribute(_jointElement, "actuator"))
1502   {
1503     const std::string actuator = getAttributeString(_jointElement, "actuator");
1504 
1505     if (actuator == "force")
1506       joint.properties->mActuatorType = dynamics::Joint::FORCE;
1507     else if (actuator == "passive")
1508       joint.properties->mActuatorType = dynamics::Joint::PASSIVE;
1509     else if (actuator == "servo")
1510       joint.properties->mActuatorType = dynamics::Joint::SERVO;
1511     else if (actuator == "acceleration")
1512       joint.properties->mActuatorType = dynamics::Joint::ACCELERATION;
1513     else if (actuator == "velocity")
1514       joint.properties->mActuatorType = dynamics::Joint::VELOCITY;
1515     else if (actuator == "locked")
1516       joint.properties->mActuatorType = dynamics::Joint::LOCKED;
1517     else
1518       dterr << "Joint named [" << name
1519             << "] contains invalid actuator attribute [" << actuator << "].\n";
1520   }
1521   else
1522   {
1523     joint.properties->mActuatorType = dynamics::Joint::DefaultActuatorType;
1524   }
1525 
1526   //--------------------------------------------------------------------------
1527   // parent
1528   BodyMap::const_iterator parent = _bodyNodes.end();
1529   if (hasElement(_jointElement, "parent"))
1530   {
1531     joint.parentName = getValueString(_jointElement, "parent");
1532     parent = _bodyNodes.find(joint.parentName);
1533   }
1534   else
1535   {
1536     dterr << "[readJoint] Joint named [" << name << "] is missing "
1537           << "a parent BodyNode!\n";
1538     assert(0);
1539   }
1540 
1541   // Use an empty string (rather than "world") to indicate that the joint has no
1542   // parent
1543   if (joint.parentName == std::string("world")
1544       && _bodyNodes.find("world") == _bodyNodes.end())
1545     joint.parentName.clear();
1546 
1547   if (parent == _bodyNodes.end() && !joint.parentName.empty())
1548   {
1549     dterr << "[readJoint] Could not find a BodyNode named [" << joint.parentName
1550           << "] requested as the parent of Joint named [" << name << "]!\n";
1551     return;
1552   }
1553 
1554   //--------------------------------------------------------------------------
1555   // child
1556   BodyMap::const_iterator child = _bodyNodes.end();
1557   if (hasElement(_jointElement, "child"))
1558   {
1559     joint.childName = getValueString(_jointElement, "child");
1560     child = _bodyNodes.find(joint.childName);
1561   }
1562   else
1563   {
1564     dterr << "[readJoint] Joint named [" << name << "] is missing "
1565           << "a child BodyNode!\n";
1566     assert(0);
1567   }
1568 
1569   if (child == _bodyNodes.end())
1570   {
1571     dterr << "[readJoint] Could not find a BodyNode named [" << joint.childName
1572           << "] requested as the child of Joint named [" << name << "]!\n";
1573     return;
1574   }
1575 
1576   //--------------------------------------------------------------------------
1577   // transformation
1578   Eigen::Isometry3d parentWorld = Eigen::Isometry3d::Identity();
1579   Eigen::Isometry3d childToJoint = Eigen::Isometry3d::Identity();
1580   Eigen::Isometry3d childWorld = child->second.initTransform;
1581 
1582   if (parent != _bodyNodes.end())
1583     parentWorld = parent->second.initTransform;
1584 
1585   if (hasElement(_jointElement, "transformation"))
1586     childToJoint = getValueIsometry3d(_jointElement, "transformation");
1587 
1588   Eigen::Isometry3d parentToJoint
1589       = parentWorld.inverse() * childWorld * childToJoint;
1590 
1591   joint.properties->mT_ParentBodyToJoint = parentToJoint;
1592   if (!math::verifyTransform(joint.properties->mT_ParentBodyToJoint))
1593     dterr << "[readJoint] Invalid parent to Joint transform for "
1594           << "Joint named [" << name << "]:\n"
1595           << joint.properties->mT_ParentBodyToJoint.matrix() << "\n";
1596 
1597   joint.properties->mT_ChildBodyToJoint = childToJoint;
1598   if (!math::verifyTransform(joint.properties->mT_ChildBodyToJoint))
1599     dterr << "[readJoint] Invalid child to Joint transform for "
1600           << "Joint named [" << name << "]:\n"
1601           << joint.properties->mT_ChildBodyToJoint.matrix() << "\n";
1602 
1603   JointMap::iterator it = _joints.find(joint.childName);
1604   if (it != _joints.end())
1605   {
1606     dterr << "[readJoint] BodyNode named [" << joint.childName
1607           << "] has been assigned two parent Joints: ["
1608           << it->second.properties->mName << "] and [" << name << "]. A "
1609           << "BodyNode must have exactly one parent Joint. [" << name << "] "
1610           << "will be discarded!\n";
1611     return;
1612   }
1613 
1614   _joints[joint.childName] = joint;
1615 
1616   // Keep track of when each joint appeared in the file
1617   std::size_t nextIndex;
1618   IndexToJoint::reverse_iterator lastIndex = _order.rbegin();
1619   if (lastIndex == _order.rend())
1620     nextIndex = 0;
1621   else
1622     nextIndex = lastIndex->first + 1;
1623 
1624   _order[nextIndex] = joint.childName;
1625   _lookup[joint.childName] = nextIndex;
1626 }
1627 
1628 //==============================================================================
getDofAttributeIfItExists(const std::string & _attribute,double * _value,const std::string & _element_type,const tinyxml2::XMLElement * _xmlElement,const std::string & _jointName,std::size_t _index)1629 void getDofAttributeIfItExists(
1630     const std::string& _attribute,
1631     double* _value,
1632     const std::string& _element_type,
1633     const tinyxml2::XMLElement* _xmlElement,
1634     const std::string& _jointName,
1635     std::size_t _index)
1636 {
1637   if (_xmlElement->QueryDoubleAttribute(_attribute.c_str(), _value)
1638       == tinyxml2::XML_WRONG_ATTRIBUTE_TYPE)
1639   {
1640     dterr << "[getDofAttributeIfItExists] Invalid type for [" << _attribute
1641           << "] attribute of [" << _element_type << "] element in the ["
1642           << _index << "] dof of Joint [" << _jointName << "].\n";
1643   }
1644 }
1645 
1646 //==============================================================================
setDofLimitAttributes(tinyxml2::XMLElement * _dofElement,const std::string & _element_type,const std::string & _jointName,std::size_t _index,double * lower,double * upper,double * initial)1647 void setDofLimitAttributes(
1648     tinyxml2::XMLElement* _dofElement,
1649     const std::string& _element_type,
1650     const std::string& _jointName,
1651     std::size_t _index,
1652     double* lower,
1653     double* upper,
1654     double* initial)
1655 {
1656   const tinyxml2::XMLElement* xmlElement
1657       = getElement(_dofElement, _element_type);
1658 
1659   getDofAttributeIfItExists(
1660       "lower", lower, _element_type, xmlElement, _jointName, _index);
1661   getDofAttributeIfItExists(
1662       "upper", upper, _element_type, xmlElement, _jointName, _index);
1663   getDofAttributeIfItExists(
1664       "initial", initial, _element_type, xmlElement, _jointName, _index);
1665 }
1666 
1667 //==============================================================================
1668 // This structure exists to allow a common interface for setting values in
1669 // GenericJoint::Properties
1670 struct DofProxy
1671 {
1672   std::size_t index;
1673   bool valid;
1674 
1675   double* lowerPosition;
1676   double* upperPosition;
1677   double* initalPosition;
1678 
1679   double* lowerVelocity;
1680   double* upperVelocity;
1681   double* initialVelocity;
1682 
1683   double* lowerAcceleration;
1684   double* upperAcceleration;
1685   double* initialAcceleration;
1686 
1687   double* lowerForce;
1688   double* upperForce;
1689   double* initialForce;
1690 
1691   double* springStiffness;
1692   double* restPosition;
1693   double* dampingCoefficient;
1694   double* friction;
1695 
1696   bool* preserveName;
1697   std::string* name;
1698 
1699   template <typename PropertyType>
DofProxydart::utils::__anon085b569b0211::DofProxy1700   DofProxy(
1701       PropertyType& properties,
1702       SkelJoint& joint,
1703       std::size_t _index,
1704       const std::string& jointName)
1705     : index(_index),
1706       valid(true),
1707 
1708       lowerPosition(&properties.mPositionLowerLimits.data()[index]),
1709       upperPosition(&properties.mPositionUpperLimits.data()[index]),
1710       initalPosition(&properties.mInitialPositions.data()[index]),
1711 
1712       lowerVelocity(&properties.mVelocityLowerLimits.data()[index]),
1713       upperVelocity(&properties.mVelocityUpperLimits.data()[index]),
1714       initialVelocity(&properties.mInitialVelocities.data()[index]),
1715 
1716       lowerAcceleration(&properties.mAccelerationLowerLimits.data()[index]),
1717       upperAcceleration(&properties.mAccelerationUpperLimits.data()[index]),
1718       initialAcceleration(&joint.acceleration.data()[index]),
1719 
1720       lowerForce(&properties.mForceLowerLimits.data()[index]),
1721       upperForce(&properties.mForceUpperLimits.data()[index]),
1722       initialForce(&joint.force.data()[index]),
1723 
1724       springStiffness(&properties.mSpringStiffnesses.data()[index]),
1725       restPosition(&properties.mRestPositions.data()[index]),
1726       dampingCoefficient(&properties.mDampingCoefficients.data()[index]),
1727       friction(&properties.mFrictions.data()[index]),
1728 
1729       preserveName(&properties.mPreserveDofNames[index]),
1730       name(&properties.mDofNames[index])
1731   {
1732     if ((int)index >= properties.mPositionLowerLimits.size())
1733     {
1734       dterr << "[SkelParser] Joint named [" << jointName << "] has a dof "
1735             << "element (" << index << ") which is out of bounds (max "
1736             << properties.mPositionLowerLimits.size() - 1 << ")\n";
1737       valid = false;
1738     }
1739   }
1740 };
1741 
1742 //==============================================================================
1743 template <typename PropertyType>
readAllDegreesOfFreedom(tinyxml2::XMLElement * _jointElement,PropertyType & _properties,SkelJoint & _joint,const std::string & _jointName,std::size_t _numDofs)1744 void readAllDegreesOfFreedom(
1745     tinyxml2::XMLElement* _jointElement,
1746     PropertyType& _properties,
1747     SkelJoint& _joint,
1748     const std::string& _jointName,
1749     std::size_t _numDofs)
1750 {
1751   if (_joint.position.size() < (int)_numDofs)
1752   {
1753     _joint.position.resize(_numDofs);
1754     _joint.position.setZero();
1755   }
1756 
1757   if (_joint.velocity.size() < (int)_numDofs)
1758   {
1759     _joint.velocity.resize(_numDofs);
1760     _joint.velocity.setZero();
1761   }
1762 
1763   if (_joint.acceleration.size() < (int)_numDofs)
1764   {
1765     _joint.acceleration.resize((int)_numDofs);
1766     _joint.acceleration.setZero();
1767   }
1768 
1769   if (_joint.force.size() < (int)_numDofs)
1770   {
1771     _joint.force.resize(_numDofs);
1772     _joint.force.setZero();
1773   }
1774 
1775   ElementEnumerator DofElements(_jointElement, "dof");
1776   while (DofElements.next())
1777     readDegreeOfFreedom(
1778         DofElements.get(), _properties, _joint, _jointName, _numDofs);
1779 }
1780 
1781 //==============================================================================
1782 template <typename PropertyType>
readDegreeOfFreedom(tinyxml2::XMLElement * _dofElement,PropertyType & properties,SkelJoint & joint,const std::string & jointName,std::size_t numDofs)1783 void readDegreeOfFreedom(
1784     tinyxml2::XMLElement* _dofElement,
1785     PropertyType& properties,
1786     SkelJoint& joint,
1787     const std::string& jointName,
1788     std::size_t numDofs)
1789 {
1790   int localIndex = -1;
1791   int xml_err = _dofElement->QueryIntAttribute("local_index", &localIndex);
1792 
1793   // If the localIndex is out of bounds, quit
1794   if (localIndex >= (int)numDofs)
1795   {
1796     dterr << "[readDegreeOfFreedom] Joint named '" << jointName
1797           << "' contains dof element with invalid "
1798           << "number attribute [" << localIndex << "]. It must be less than "
1799           << numDofs << ".\n";
1800     return;
1801   }
1802 
1803   // If no localIndex was found, report an error and quit
1804   if (localIndex == -1 && numDofs > 1)
1805   {
1806     if (tinyxml2::XML_NO_ATTRIBUTE == xml_err)
1807     {
1808       dterr << "[readDegreeOfFreedom] Joint named [" << jointName << "] has ["
1809             << numDofs
1810             << "] DOFs, but the xml contains a dof element without its "
1811             << "local_index specified. For Joints with multiple DOFs, all dof "
1812             << "elements must specify their local_index attribute.\n";
1813     }
1814     else if (tinyxml2::XML_WRONG_ATTRIBUTE_TYPE == xml_err)
1815     {
1816       dterr << "[readDegreeOfFreedom] Joint named [" << jointName
1817             << "] has a dof element with a wrongly "
1818             << "formatted local_index attribute.\n";
1819     }
1820 
1821     return;
1822   }
1823   // Unless the joint is a single-dof joint
1824   else if (localIndex == -1 && numDofs == 1)
1825     localIndex = 0;
1826 
1827   DofProxy proxy(properties, joint, localIndex, jointName);
1828 
1829   const char* name = _dofElement->Attribute("name");
1830   if (name)
1831   {
1832     *proxy.name = std::string(name);
1833     *proxy.preserveName = true;
1834   }
1835 
1836   if (hasElement(_dofElement, "position"))
1837   {
1838     setDofLimitAttributes(
1839         _dofElement,
1840         "position",
1841         jointName,
1842         localIndex,
1843         proxy.lowerPosition,
1844         proxy.upperPosition,
1845         proxy.initalPosition);
1846   }
1847 
1848   if (hasElement(_dofElement, "velocity"))
1849   {
1850     setDofLimitAttributes(
1851         _dofElement,
1852         "velocity",
1853         jointName,
1854         localIndex,
1855         proxy.lowerVelocity,
1856         proxy.upperVelocity,
1857         proxy.initialVelocity);
1858   }
1859 
1860   if (hasElement(_dofElement, "acceleration"))
1861   {
1862     setDofLimitAttributes(
1863         _dofElement,
1864         "acceleration",
1865         jointName,
1866         localIndex,
1867         proxy.lowerAcceleration,
1868         proxy.upperAcceleration,
1869         proxy.initialAcceleration);
1870   }
1871 
1872   if (hasElement(_dofElement, "force"))
1873   {
1874     setDofLimitAttributes(
1875         _dofElement,
1876         "force",
1877         jointName,
1878         localIndex,
1879         proxy.lowerForce,
1880         proxy.upperForce,
1881         proxy.initialForce);
1882   }
1883 
1884   if (hasElement(_dofElement, "damping"))
1885     *proxy.dampingCoefficient = getValueDouble(_dofElement, "damping");
1886 
1887   if (hasElement(_dofElement, "friction"))
1888     *proxy.friction = getValueDouble(_dofElement, "friction");
1889 
1890   if (hasElement(_dofElement, "spring_rest_position"))
1891     *proxy.restPosition = getValueDouble(_dofElement, "spring_rest_position");
1892 
1893   if (hasElement(_dofElement, "spring_stiffness"))
1894     *proxy.springStiffness = getValueDouble(_dofElement, "spring_stiffness");
1895 }
1896 
1897 //==============================================================================
1898 template <typename PropertyType>
readJointDynamicsAndLimit(tinyxml2::XMLElement * _jointElement,PropertyType & _properties,SkelJoint & _joint,const std::string & _name,std::size_t _numAxis)1899 void readJointDynamicsAndLimit(
1900     tinyxml2::XMLElement* _jointElement,
1901     PropertyType& _properties,
1902     SkelJoint& _joint,
1903     const std::string& _name,
1904     std::size_t _numAxis)
1905 {
1906   // TODO(MXG): Consider printing warnings for these tags that recommends using
1907   // the dof tag instead, because all functionality of these tags have been
1908   // moved to the dof tag
1909 
1910   assert(_jointElement != nullptr);
1911   assert(_numAxis <= 6);
1912 
1913   std::string axisName = "axis";
1914 
1915   // axis
1916   for (std::size_t i = 0; i < _numAxis; ++i)
1917   {
1918     if (i != 0)
1919       axisName = "axis" + std::to_string(i + 1);
1920 
1921     if (hasElement(_jointElement, axisName))
1922     {
1923       DofProxy proxy(_properties, _joint, i, _name);
1924 
1925       tinyxml2::XMLElement* axisElement = getElement(_jointElement, axisName);
1926 
1927       // damping
1928       if (hasElement(axisElement, "damping"))
1929       {
1930         dtwarn << "[SkelParser] <damping> tag is now an element under the "
1931                << "<dynamics> tag. Please see "
1932                << "(https://github.com/dartsim/dart/wiki/) for more details.\n";
1933         double damping = getValueDouble(axisElement, "damping");
1934         *proxy.dampingCoefficient = damping;
1935       }
1936 
1937       // dynamics
1938       if (hasElement(axisElement, "dynamics"))
1939       {
1940         tinyxml2::XMLElement* dynamicsElement
1941             = getElement(axisElement, "dynamics");
1942 
1943         // damping
1944         if (hasElement(dynamicsElement, "damping"))
1945         {
1946           double val = getValueDouble(dynamicsElement, "damping");
1947           *proxy.dampingCoefficient = val;
1948         }
1949 
1950         // friction
1951         if (hasElement(dynamicsElement, "friction"))
1952         {
1953           double val = getValueDouble(dynamicsElement, "friction");
1954           *proxy.friction = val;
1955         }
1956 
1957         // spring_rest_position
1958         if (hasElement(dynamicsElement, "spring_rest_position"))
1959         {
1960           double val = getValueDouble(dynamicsElement, "spring_rest_position");
1961           *proxy.restPosition = val;
1962         }
1963 
1964         // spring_stiffness
1965         if (hasElement(dynamicsElement, "spring_stiffness"))
1966         {
1967           double val = getValueDouble(dynamicsElement, "spring_stiffness");
1968           *proxy.springStiffness = val;
1969         }
1970       }
1971 
1972       // limit
1973       if (hasElement(axisElement, "limit"))
1974       {
1975         tinyxml2::XMLElement* limitElement = getElement(axisElement, "limit");
1976 
1977         // lower
1978         if (hasElement(limitElement, "lower"))
1979         {
1980           double lower = getValueDouble(limitElement, "lower");
1981           *proxy.lowerPosition = lower;
1982         }
1983 
1984         // upper
1985         if (hasElement(limitElement, "upper"))
1986         {
1987           double upper = getValueDouble(limitElement, "upper");
1988           *proxy.upperPosition = upper;
1989         }
1990       }
1991     }
1992   }
1993 }
1994 
1995 //==============================================================================
readWeldJoint(tinyxml2::XMLElement *,SkelJoint &,const std::string &)1996 JointPropPtr readWeldJoint(
1997     tinyxml2::XMLElement* /*_jointElement*/,
1998     SkelJoint& /*_joint*/,
1999     const std::string&)
2000 {
2001   return dynamics::WeldJoint::Properties::createShared();
2002 }
2003 
2004 //==============================================================================
readRevoluteJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2005 JointPropPtr readRevoluteJoint(
2006     tinyxml2::XMLElement* _jointElement,
2007     SkelJoint& _joint,
2008     const std::string& _name)
2009 {
2010   assert(_jointElement != nullptr);
2011 
2012   dynamics::RevoluteJoint::Properties properties;
2013 
2014   //--------------------------------------------------------------------------
2015   // axis
2016   if (hasElement(_jointElement, "axis"))
2017   {
2018     tinyxml2::XMLElement* axisElement = getElement(_jointElement, "axis");
2019 
2020     // xyz
2021     Eigen::Vector3d xyz = getValueVector3d(axisElement, "xyz");
2022     properties.mAxis = xyz;
2023   }
2024   else
2025   {
2026     dterr << "[readRevoluteJoint] Revolute Joint named [" << _name
2027           << "] is missing axis information!\n";
2028     assert(0);
2029   }
2030 
2031   readJointDynamicsAndLimit<dynamics::GenericJoint<math::R1Space>::Properties>(
2032       _jointElement, properties, _joint, _name, 1);
2033 
2034   //--------------------------------------------------------------------------
2035   // init_pos
2036   if (hasElement(_jointElement, "init_pos"))
2037   {
2038     double init_pos = getValueDouble(_jointElement, "init_pos");
2039     Eigen::VectorXd ipos = Eigen::VectorXd(1);
2040     ipos << init_pos;
2041     _joint.position = ipos;
2042     properties.mInitialPositions[0] = ipos[0];
2043   }
2044 
2045   //--------------------------------------------------------------------------
2046   // init_vel
2047   if (hasElement(_jointElement, "init_vel"))
2048   {
2049     double init_vel = getValueDouble(_jointElement, "init_vel");
2050     Eigen::VectorXd ivel = Eigen::VectorXd(1);
2051     ivel << init_vel;
2052     _joint.velocity = ivel;
2053     properties.mInitialVelocities[0] = ivel[0];
2054   }
2055 
2056   readAllDegreesOfFreedom<dynamics::GenericJoint<math::R1Space>::Properties>(
2057       _jointElement, properties, _joint, _name, 1);
2058 
2059   return dynamics::RevoluteJoint::Properties::createShared(properties);
2060 }
2061 
2062 //==============================================================================
readPrismaticJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2063 JointPropPtr readPrismaticJoint(
2064     tinyxml2::XMLElement* _jointElement,
2065     SkelJoint& _joint,
2066     const std::string& _name)
2067 {
2068   assert(_jointElement != nullptr);
2069 
2070   dynamics::PrismaticJoint::Properties properties;
2071 
2072   //--------------------------------------------------------------------------
2073   // axis
2074   if (hasElement(_jointElement, "axis"))
2075   {
2076     tinyxml2::XMLElement* axisElement = getElement(_jointElement, "axis");
2077 
2078     // xyz
2079     Eigen::Vector3d xyz = getValueVector3d(axisElement, "xyz");
2080     properties.mAxis = xyz;
2081   }
2082   else
2083   {
2084     dterr << "[readPrismaticJoint] Prismatic Joint named [" << _name
2085           << "] is missing axis information!\n";
2086     assert(0);
2087   }
2088 
2089   readJointDynamicsAndLimit<dynamics::GenericJoint<math::R1Space>::Properties>(
2090       _jointElement, properties, _joint, _name, 1);
2091 
2092   //--------------------------------------------------------------------------
2093   // init_pos
2094   if (hasElement(_jointElement, "init_pos"))
2095   {
2096     double init_pos = getValueDouble(_jointElement, "init_pos");
2097     Eigen::VectorXd ipos = Eigen::VectorXd(1);
2098     ipos << init_pos;
2099     _joint.position = ipos;
2100     properties.mInitialPositions[0] = ipos[0];
2101   }
2102 
2103   //--------------------------------------------------------------------------
2104   // init_vel
2105   if (hasElement(_jointElement, "init_vel"))
2106   {
2107     double init_vel = getValueDouble(_jointElement, "init_vel");
2108     Eigen::VectorXd ivel = Eigen::VectorXd(1);
2109     ivel << init_vel;
2110     _joint.velocity = ivel;
2111     properties.mInitialVelocities[0] = ivel[0];
2112   }
2113 
2114   readAllDegreesOfFreedom<dynamics::GenericJoint<math::R1Space>::Properties>(
2115       _jointElement, properties, _joint, _name, 1);
2116 
2117   return dynamics::PrismaticJoint::Properties::createShared(properties);
2118 }
2119 
2120 //==============================================================================
readScrewJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2121 JointPropPtr readScrewJoint(
2122     tinyxml2::XMLElement* _jointElement,
2123     SkelJoint& _joint,
2124     const std::string& _name)
2125 {
2126   assert(_jointElement != nullptr);
2127 
2128   dynamics::ScrewJoint::Properties properties;
2129 
2130   //--------------------------------------------------------------------------
2131   // axis
2132   if (hasElement(_jointElement, "axis"))
2133   {
2134     tinyxml2::XMLElement* axisElement = getElement(_jointElement, "axis");
2135 
2136     // xyz
2137     Eigen::Vector3d xyz = getValueVector3d(axisElement, "xyz");
2138     properties.mAxis = xyz;
2139 
2140     // pitch
2141     if (hasElement(axisElement, "pitch"))
2142     {
2143       double pitch = getValueDouble(axisElement, "pitch");
2144       properties.mPitch = pitch;
2145     }
2146   }
2147   else
2148   {
2149     dterr << "[readScrewJoint] Screw Joint named [" << _name
2150           << "] is missing axis information!\n";
2151     assert(0);
2152   }
2153 
2154   readJointDynamicsAndLimit<dynamics::GenericJoint<math::R1Space>::Properties>(
2155       _jointElement, properties, _joint, _name, 1);
2156 
2157   //--------------------------------------------------------------------------
2158   // init_pos
2159   if (hasElement(_jointElement, "init_pos"))
2160   {
2161     double init_pos = getValueDouble(_jointElement, "init_pos");
2162     Eigen::VectorXd ipos = Eigen::VectorXd(1);
2163     ipos << init_pos;
2164     _joint.position = ipos;
2165     properties.mInitialPositions[0] = ipos[0];
2166   }
2167 
2168   //--------------------------------------------------------------------------
2169   // init_vel
2170   if (hasElement(_jointElement, "init_vel"))
2171   {
2172     double init_vel = getValueDouble(_jointElement, "init_vel");
2173     Eigen::VectorXd ivel = Eigen::VectorXd(1);
2174     ivel << init_vel;
2175     _joint.velocity = ivel;
2176     properties.mInitialVelocities[0] = ivel[0];
2177   }
2178 
2179   readAllDegreesOfFreedom<dynamics::GenericJoint<math::R1Space>::Properties>(
2180       _jointElement, properties, _joint, _name, 1);
2181 
2182   return dynamics::ScrewJoint::Properties::createShared(properties);
2183 }
2184 
2185 //==============================================================================
readUniversalJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2186 JointPropPtr readUniversalJoint(
2187     tinyxml2::XMLElement* _jointElement,
2188     SkelJoint& _joint,
2189     const std::string& _name)
2190 {
2191   assert(_jointElement != nullptr);
2192 
2193   dynamics::UniversalJoint::Properties properties;
2194 
2195   //--------------------------------------------------------------------------
2196   // axis
2197   if (hasElement(_jointElement, "axis"))
2198   {
2199     tinyxml2::XMLElement* axisElement = getElement(_jointElement, "axis");
2200 
2201     // xyz
2202     Eigen::Vector3d xyz = getValueVector3d(axisElement, "xyz");
2203     properties.mAxis[0] = xyz;
2204   }
2205   else
2206   {
2207     dterr << "[readUniversalJoint] Universal Joint named [" << _name
2208           << "] is missing axis information!\n";
2209     assert(0);
2210   }
2211 
2212   //--------------------------------------------------------------------------
2213   // axis2
2214   if (hasElement(_jointElement, "axis2"))
2215   {
2216     tinyxml2::XMLElement* axis2Element = getElement(_jointElement, "axis2");
2217 
2218     // xyz
2219     Eigen::Vector3d xyz = getValueVector3d(axis2Element, "xyz");
2220     properties.mAxis[1] = xyz;
2221   }
2222   else
2223   {
2224     dterr << "[readUniversalJoint] Universal Joint named [" << _name
2225           << "] is missing axis2 information!\n";
2226     assert(0);
2227   }
2228 
2229   readJointDynamicsAndLimit(_jointElement, properties, _joint, _name, 2);
2230 
2231   //--------------------------------------------------------------------------
2232   // init_pos
2233   if (hasElement(_jointElement, "init_pos"))
2234   {
2235     Eigen::Vector2d init_pos = getValueVector2d(_jointElement, "init_pos");
2236     _joint.position = init_pos;
2237     properties.mInitialPositions = init_pos;
2238   }
2239 
2240   //--------------------------------------------------------------------------
2241   // init_vel
2242   if (hasElement(_jointElement, "init_vel"))
2243   {
2244     Eigen::Vector2d init_vel = getValueVector2d(_jointElement, "init_vel");
2245     _joint.velocity = init_vel;
2246     properties.mInitialVelocities = init_vel;
2247   }
2248 
2249   readAllDegreesOfFreedom(_jointElement, properties, _joint, _name, 2);
2250 
2251   return dynamics::UniversalJoint::Properties::createShared(properties);
2252 }
2253 
2254 //==============================================================================
readBallJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2255 JointPropPtr readBallJoint(
2256     tinyxml2::XMLElement* _jointElement,
2257     SkelJoint& _joint,
2258     const std::string& _name)
2259 {
2260   assert(_jointElement != nullptr);
2261 
2262   dynamics::BallJoint::Properties properties;
2263 
2264   //--------------------------------------------------------------------------
2265   // init_pos
2266   if (hasElement(_jointElement, "init_pos"))
2267   {
2268     Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos");
2269     _joint.position = init_pos;
2270     properties.mInitialPositions = init_pos;
2271   }
2272 
2273   //--------------------------------------------------------------------------
2274   // init_vel
2275   if (hasElement(_jointElement, "init_vel"))
2276   {
2277     Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel");
2278     _joint.velocity = init_vel;
2279     properties.mInitialVelocities = init_vel;
2280   }
2281 
2282   readAllDegreesOfFreedom(_jointElement, properties, _joint, _name, 3);
2283 
2284   return dynamics::BallJoint::Properties::createShared(properties);
2285 }
2286 
2287 //==============================================================================
readEulerJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2288 JointPropPtr readEulerJoint(
2289     tinyxml2::XMLElement* _jointElement,
2290     SkelJoint& _joint,
2291     const std::string& _name)
2292 {
2293   assert(_jointElement != nullptr);
2294 
2295   dynamics::EulerJoint::Properties properties;
2296 
2297   //--------------------------------------------------------------------------
2298   // axis order
2299   std::string order = getValueString(_jointElement, "axis_order");
2300   if (order == "xyz")
2301   {
2302     properties.mAxisOrder = dynamics::EulerJoint::AxisOrder::XYZ;
2303   }
2304   else if (order == "zyx")
2305   {
2306     properties.mAxisOrder = dynamics::EulerJoint::AxisOrder::ZYX;
2307   }
2308   else
2309   {
2310     dterr << "[readEulerJoint] Undefined Euler axis order for "
2311           << "Euler Joint named [" << _name << "]\n";
2312     assert(0);
2313   }
2314 
2315   //--------------------------------------------------------------------------
2316   // axis
2317   readJointDynamicsAndLimit(_jointElement, properties, _joint, _name, 3);
2318 
2319   //--------------------------------------------------------------------------
2320   // init_pos
2321   if (hasElement(_jointElement, "init_pos"))
2322   {
2323     Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos");
2324     _joint.position = init_pos;
2325     properties.mInitialPositions = init_pos;
2326   }
2327 
2328   //--------------------------------------------------------------------------
2329   // init_vel
2330   if (hasElement(_jointElement, "init_vel"))
2331   {
2332     Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel");
2333     _joint.velocity = init_vel;
2334     properties.mInitialVelocities = init_vel;
2335   }
2336 
2337   readAllDegreesOfFreedom(_jointElement, properties, _joint, _name, 3);
2338 
2339   return dynamics::EulerJoint::Properties::createShared(properties);
2340 }
2341 
2342 //==============================================================================
readTranslationalJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2343 JointPropPtr readTranslationalJoint(
2344     tinyxml2::XMLElement* _jointElement,
2345     SkelJoint& _joint,
2346     const std::string& _name)
2347 {
2348   assert(_jointElement != nullptr);
2349 
2350   dynamics::TranslationalJoint::Properties properties;
2351 
2352   //--------------------------------------------------------------------------
2353   // axis
2354   readJointDynamicsAndLimit(_jointElement, properties, _joint, _name, 3);
2355 
2356   //--------------------------------------------------------------------------
2357   // init_pos
2358   if (hasElement(_jointElement, "init_pos"))
2359   {
2360     Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos");
2361     _joint.position = init_pos;
2362     properties.mInitialPositions = init_pos;
2363   }
2364 
2365   //--------------------------------------------------------------------------
2366   // init_vel
2367   if (hasElement(_jointElement, "init_vel"))
2368   {
2369     Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel");
2370     _joint.velocity = init_vel;
2371     properties.mInitialVelocities = init_vel;
2372   }
2373 
2374   readAllDegreesOfFreedom(_jointElement, properties, _joint, _name, 3);
2375 
2376   return dynamics::TranslationalJoint::Properties::createShared(properties);
2377 }
2378 
2379 //==============================================================================
readTranslationalJoint2D(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2380 JointPropPtr readTranslationalJoint2D(
2381     tinyxml2::XMLElement* _jointElement,
2382     SkelJoint& _joint,
2383     const std::string& _name)
2384 {
2385   assert(_jointElement != nullptr);
2386 
2387   dynamics::TranslationalJoint2D::Properties properties;
2388 
2389   //--------------------------------------------------------------------------
2390   // Plane
2391   if (hasElement(_jointElement, "plane"))
2392   {
2393     tinyxml2::XMLElement* planeElement = getElement(_jointElement, "plane");
2394 
2395     // Type attribute
2396     std::string type = getAttributeString(planeElement, "type");
2397 
2398     if (type == "xy")
2399     {
2400       properties.setXYPlane();
2401     }
2402     else if (type == "yz")
2403     {
2404       properties.setYZPlane();
2405     }
2406     else if (type == "zx")
2407     {
2408       properties.setZXPlane();
2409     }
2410     else if (type == "arbitrary")
2411     {
2412       const auto* transAxis1Element
2413           = getElement(planeElement, "translation_axis1");
2414 
2415       const auto* transAxis2Element
2416           = getElement(planeElement, "translation_axis2");
2417 
2418       properties.setArbitraryPlane(
2419           getValueVector3d(transAxis1Element, "xyz"),
2420           getValueVector3d(transAxis2Element, "xyz"));
2421     }
2422     else
2423     {
2424       dterr << "[readTranslationalJoint2D] TranslationalJoint2D named ["
2425             << _name << "] contains unsupported plane type. "
2426             << "Defaulting to XY-Plane.\n";
2427       properties.setXYPlane();
2428     }
2429   }
2430   else
2431   {
2432     dtwarn << "[readTranslationalJoint2D] TranslationalJoint2D named [" << _name
2433            << "] doesn't contain plane element. "
2434            << "Defaulting to XY-Plane.\n";
2435     properties.setXYPlane();
2436   }
2437 
2438   //--------------------------------------------------------------------------
2439   // axis
2440   readJointDynamicsAndLimit(_jointElement, properties, _joint, _name, 2);
2441 
2442   //--------------------------------------------------------------------------
2443   // init_pos
2444   if (hasElement(_jointElement, "init_pos"))
2445   {
2446     Eigen::Vector2d init_pos = getValueVector2d(_jointElement, "init_pos");
2447     _joint.position = init_pos;
2448     properties.mInitialPositions = init_pos;
2449   }
2450 
2451   //--------------------------------------------------------------------------
2452   // init_vel
2453   if (hasElement(_jointElement, "init_vel"))
2454   {
2455     Eigen::Vector2d init_vel = getValueVector2d(_jointElement, "init_vel");
2456     _joint.velocity = init_vel;
2457     properties.mInitialVelocities = init_vel;
2458   }
2459 
2460   readAllDegreesOfFreedom(_jointElement, properties, _joint, _name, 2);
2461 
2462   return dynamics::TranslationalJoint2D::Properties::createShared(properties);
2463 }
2464 
2465 //==============================================================================
readPlanarJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2466 JointPropPtr readPlanarJoint(
2467     tinyxml2::XMLElement* _jointElement,
2468     SkelJoint& _joint,
2469     const std::string& _name)
2470 {
2471   assert(_jointElement != nullptr);
2472 
2473   dynamics::PlanarJoint::Properties properties;
2474 
2475   //--------------------------------------------------------------------------
2476   // Plane
2477   if (hasElement(_jointElement, "plane"))
2478   {
2479     tinyxml2::XMLElement* planeElement = getElement(_jointElement, "plane");
2480 
2481     // Type attribute
2482     std::string type = getAttributeString(planeElement, "type");
2483 
2484     if (type == "xy")
2485     {
2486       properties.mPlaneType = dynamics::PlanarJoint::PlaneType::XY;
2487     }
2488     else if (type == "yz")
2489     {
2490       properties.mPlaneType = dynamics::PlanarJoint::PlaneType::YZ;
2491     }
2492     else if (type == "zx")
2493     {
2494       properties.mPlaneType = dynamics::PlanarJoint::PlaneType::ZX;
2495     }
2496     else if (type == "arbitrary")
2497     {
2498       properties.mPlaneType = dynamics::PlanarJoint::PlaneType::ARBITRARY;
2499 
2500       tinyxml2::XMLElement* transAxis1Element
2501           = getElement(planeElement, "translation_axis1");
2502 
2503       properties.mTransAxis1 = getValueVector3d(transAxis1Element, "xyz");
2504 
2505       tinyxml2::XMLElement* transAxis2Element
2506           = getElement(planeElement, "translation_axis2");
2507 
2508       properties.mTransAxis2 = getValueVector3d(transAxis2Element, "xyz");
2509     }
2510     else
2511     {
2512       dterr << "[readPlanarJoint] Planar Joint named [" << _name
2513             << "] is missing plane type information. Defaulting to XY-Plane.\n";
2514       properties.mPlaneType = dynamics::PlanarJoint::PlaneType::XY;
2515     }
2516   }
2517   else
2518   {
2519     dtwarn << "[readPlanarJoint] Planar Joint named [" << _name
2520            << "] is missing plane type information. Defaulting to XY-Plane.\n";
2521     properties.mPlaneType = dynamics::PlanarJoint::PlaneType::XY;
2522   }
2523 
2524   //--------------------------------------------------------------------------
2525   // axis
2526   readJointDynamicsAndLimit(_jointElement, properties, _joint, _name, 3);
2527 
2528   //--------------------------------------------------------------------------
2529   // init_pos
2530   if (hasElement(_jointElement, "init_pos"))
2531   {
2532     Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos");
2533     _joint.position = init_pos;
2534     properties.mInitialPositions = init_pos;
2535   }
2536 
2537   //--------------------------------------------------------------------------
2538   // init_vel
2539   if (hasElement(_jointElement, "init_vel"))
2540   {
2541     Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel");
2542     _joint.velocity = init_vel;
2543     properties.mInitialVelocities = init_vel;
2544   }
2545 
2546   readAllDegreesOfFreedom(_jointElement, properties, _joint, _name, 3);
2547 
2548   return dynamics::PlanarJoint::Properties::createShared(properties);
2549 }
2550 
2551 //==============================================================================
readFreeJoint(tinyxml2::XMLElement * _jointElement,SkelJoint & _joint,const std::string & _name)2552 JointPropPtr readFreeJoint(
2553     tinyxml2::XMLElement* _jointElement,
2554     SkelJoint& _joint,
2555     const std::string& _name)
2556 {
2557   assert(_jointElement != nullptr);
2558 
2559   dynamics::FreeJoint::Properties properties;
2560 
2561   //--------------------------------------------------------------------------
2562   // init_pos
2563   if (hasElement(_jointElement, "init_pos"))
2564   {
2565     Eigen::Vector6d init_pos = getValueVector6d(_jointElement, "init_pos");
2566     _joint.position = init_pos;
2567     properties.mInitialPositions = init_pos;
2568   }
2569 
2570   //--------------------------------------------------------------------------
2571   // init_vel
2572   if (hasElement(_jointElement, "init_vel"))
2573   {
2574     Eigen::Vector6d init_vel = getValueVector6d(_jointElement, "init_vel");
2575     _joint.velocity = init_vel;
2576     properties.mInitialVelocities = init_vel;
2577   }
2578 
2579   readAllDegreesOfFreedom(_jointElement, properties, _joint, _name, 6);
2580 
2581   return dynamics::FreeJoint::Properties::createShared(properties);
2582 }
2583 
2584 //==============================================================================
getRetriever(const common::ResourceRetrieverPtr & _retriever)2585 common::ResourceRetrieverPtr getRetriever(
2586     const common::ResourceRetrieverPtr& _retriever)
2587 {
2588   if (_retriever)
2589   {
2590     return _retriever;
2591   }
2592   else
2593   {
2594     auto newRetriever = std::make_shared<utils::CompositeResourceRetriever>();
2595     newRetriever->addSchemaRetriever(
2596         "file", std::make_shared<common::LocalResourceRetriever>());
2597     newRetriever->addSchemaRetriever("dart", DartResourceRetriever::create());
2598     return newRetriever;
2599   }
2600 }
2601 
2602 } // anonymous namespace
2603 
2604 } // namespace utils
2605 } // namespace dart
2606