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