1 #include "UrdfParser.h"
2 #include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
3 #include "urdfStringSplit.h"
4 #include "urdfLexicalCast.h"
5 #include "UrdfFindMeshFile.h"
6
7 using namespace tinyxml2;
8
UrdfParser(CommonFileIOInterface * fileIO)9 UrdfParser::UrdfParser(CommonFileIOInterface* fileIO)
10 : m_parseSDF(false),
11 m_activeSdfModel(-1),
12 m_urdfScaling(1),
13 m_fileIO(fileIO)
14 {
15 m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
16 }
17
~UrdfParser()18 UrdfParser::~UrdfParser()
19 {
20 for (int i = 0; i < m_tmpModels.size(); i++)
21 {
22 delete m_tmpModels[i];
23 }
24 }
25
parseVector4(btVector4 & vec4,const std::string & vector_str)26 static bool parseVector4(btVector4& vec4, const std::string& vector_str)
27 {
28 vec4.setZero();
29 btArray<std::string> pieces;
30 btArray<double> rgba;
31 btAlignedObjectArray<std::string> strArray;
32 urdfIsAnyOf(" ", strArray);
33 urdfStringSplit(pieces, vector_str, strArray);
34 for (int i = 0; i < pieces.size(); ++i)
35 {
36 if (!pieces[i].empty())
37 {
38 rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
39 }
40 }
41 if (rgba.size() != 4)
42 {
43 return false;
44 }
45 vec4.setValue(rgba[0], rgba[1], rgba[2], rgba[3]);
46 return true;
47 }
48
parseVector3(btVector3 & vec3,const std::string & vector_str,ErrorLogger * logger,bool lastThree=false)49 static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false)
50 {
51 vec3.setZero();
52 btArray<std::string> pieces;
53 btArray<double> rgba;
54 btAlignedObjectArray<std::string> strArray;
55 urdfIsAnyOf(" ", strArray);
56 urdfStringSplit(pieces, vector_str, strArray);
57 for (int i = 0; i < pieces.size(); ++i)
58 {
59 if (!pieces[i].empty())
60 {
61 rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
62 }
63 }
64 if (rgba.size() < 3)
65 {
66 logger->reportWarning("Couldn't parse vector3");
67 return false;
68 }
69 if (lastThree)
70 {
71 vec3.setValue(rgba[rgba.size() - 3], rgba[rgba.size() - 2], rgba[rgba.size() - 1]);
72 }
73 else
74 {
75 vec3.setValue(rgba[0], rgba[1], rgba[2]);
76 }
77 return true;
78 }
79
80 // Parses user data from an xml element and stores it in a hashmap. User data is
81 // expected to reside in a <user-data> tag that is nested inside a <bullet> tag.
82 // Example:
83 // <bullet>
84 // <user-data key="label">...</user-data>
85 // </bullet>
ParseUserData(const XMLElement * element,btHashMap<btHashString,std::string> & user_data,ErrorLogger * logger)86 static void ParseUserData(const XMLElement* element, btHashMap<btHashString,
87 std::string>& user_data, ErrorLogger* logger) {
88 // Parse any custom Bullet-specific info.
89 for (const XMLElement* bullet_xml = element->FirstChildElement("bullet");
90 bullet_xml; bullet_xml = bullet_xml->NextSiblingElement("bullet")) {
91 for (const XMLElement* user_data_xml = bullet_xml->FirstChildElement("user-data");
92 user_data_xml; user_data_xml = user_data_xml->NextSiblingElement("user-data")) {
93 const char* key_attr = user_data_xml->Attribute("key");
94 if (!key_attr) {
95 logger->reportError("User data tag must have a key attribute.");
96 }
97 const char* text = user_data_xml->GetText();
98 user_data.insert(key_attr, text ? text : "");
99 }
100 }
101 }
102
parseMaterial(UrdfMaterial & material,XMLElement * config,ErrorLogger * logger)103 bool UrdfParser::parseMaterial(UrdfMaterial& material, XMLElement* config, ErrorLogger* logger)
104 {
105 if (!config->Attribute("name"))
106 {
107 logger->reportError("Material must contain a name attribute");
108 return false;
109 }
110
111 material.m_name = config->Attribute("name");
112
113 // texture
114 XMLElement* t = config->FirstChildElement("texture");
115 if (t)
116 {
117 if (t->Attribute("filename"))
118 {
119 material.m_textureFilename = t->Attribute("filename");
120 }
121 }
122
123 if (material.m_textureFilename.length() == 0)
124 {
125 //logger->reportWarning("material has no texture file name");
126 }
127
128 // color
129 {
130 XMLElement* c = config->FirstChildElement("color");
131 if (c)
132 {
133 if (c->Attribute("rgba"))
134 {
135 if (!parseVector4(material.m_matColor.m_rgbaColor, c->Attribute("rgba")))
136 {
137 std::string msg = material.m_name + " has no rgba";
138 logger->reportWarning(msg.c_str());
139 }
140 }
141 }
142 }
143
144 {
145 // specular (non-standard)
146 XMLElement* s = config->FirstChildElement("specular");
147 if (s)
148 {
149 if (s->Attribute("rgb"))
150 {
151 if (!parseVector3(material.m_matColor.m_specularColor, s->Attribute("rgb"), logger))
152 {
153 }
154 }
155 }
156 }
157 return true;
158 }
159
parseTransform(btTransform & tr,XMLElement * xml,ErrorLogger * logger,bool parseSDF)160 bool UrdfParser::parseTransform(btTransform& tr, XMLElement* xml, ErrorLogger* logger, bool parseSDF)
161 {
162 tr.setIdentity();
163
164 btVector3 vec(0, 0, 0);
165 if (parseSDF)
166 {
167 parseVector3(vec, std::string(xml->GetText()), logger);
168 }
169 else
170 {
171 const char* xyz_str = xml->Attribute("xyz");
172 if (xyz_str)
173 {
174 parseVector3(vec, std::string(xyz_str), logger);
175 }
176 }
177 tr.setOrigin(vec * m_urdfScaling);
178
179 if (parseSDF)
180 {
181 btVector3 rpy;
182 if (parseVector3(rpy, std::string(xml->GetText()), logger, true))
183 {
184 double phi, the, psi;
185 double roll = rpy[0];
186 double pitch = rpy[1];
187 double yaw = rpy[2];
188
189 phi = roll / 2.0;
190 the = pitch / 2.0;
191 psi = yaw / 2.0;
192
193 btQuaternion orn(
194 sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
195 cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
196 cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
197 cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
198
199 orn.normalize();
200 tr.setRotation(orn);
201 }
202 }
203 else
204 {
205 const char* rpy_str = xml->Attribute("rpy");
206 if (rpy_str != NULL)
207 {
208 btVector3 rpy;
209 if (parseVector3(rpy, std::string(rpy_str), logger))
210 {
211 double phi, the, psi;
212 double roll = rpy[0];
213 double pitch = rpy[1];
214 double yaw = rpy[2];
215
216 phi = roll / 2.0;
217 the = pitch / 2.0;
218 psi = yaw / 2.0;
219
220 btQuaternion orn(
221 sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
222 cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
223 cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
224 cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
225
226 orn.normalize();
227 tr.setRotation(orn);
228 }
229 }
230 }
231 return true;
232 }
233
parseInertia(UrdfInertia & inertia,XMLElement * config,ErrorLogger * logger)234 bool UrdfParser::parseInertia(UrdfInertia& inertia, XMLElement* config, ErrorLogger* logger)
235 {
236 inertia.m_linkLocalFrame.setIdentity();
237 inertia.m_mass = 0.f;
238 if (m_parseSDF)
239 {
240 XMLElement* pose = config->FirstChildElement("pose");
241 if (pose)
242 {
243 parseTransform(inertia.m_linkLocalFrame, pose, logger, m_parseSDF);
244 }
245 }
246
247 // Origin
248 XMLElement* o = config->FirstChildElement("origin");
249 if (o)
250 {
251 if (!parseTransform(inertia.m_linkLocalFrame, o, logger))
252 {
253 return false;
254 }
255 }
256
257 XMLElement* mass_xml = config->FirstChildElement("mass");
258 if (!mass_xml)
259 {
260 logger->reportError("Inertial element must have a mass element");
261 return false;
262 }
263 if (m_parseSDF)
264 {
265 inertia.m_mass = urdfLexicalCast<double>(mass_xml->GetText());
266 }
267 else
268 {
269 if (!mass_xml->Attribute("value"))
270 {
271 logger->reportError("Inertial: mass element must have value attribute");
272 return false;
273 }
274
275 inertia.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
276 }
277
278 XMLElement* inertia_xml = config->FirstChildElement("inertia");
279 if (!inertia_xml)
280 {
281 logger->reportError("Inertial element must have inertia element");
282 return false;
283 }
284 if (m_parseSDF)
285 {
286 XMLElement* ixx = inertia_xml->FirstChildElement("ixx");
287 XMLElement* ixy = inertia_xml->FirstChildElement("ixy");
288 XMLElement* ixz = inertia_xml->FirstChildElement("ixz");
289 XMLElement* iyy = inertia_xml->FirstChildElement("iyy");
290 XMLElement* iyz = inertia_xml->FirstChildElement("iyz");
291 XMLElement* izz = inertia_xml->FirstChildElement("izz");
292 if (ixx && ixy && ixz && iyy && iyz && izz)
293 {
294 inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
295 inertia.m_ixy = urdfLexicalCast<double>(ixy->GetText());
296 inertia.m_ixz = urdfLexicalCast<double>(ixz->GetText());
297 inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
298 inertia.m_iyz = urdfLexicalCast<double>(iyz->GetText());
299 inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
300 }
301 else
302 {
303 if (ixx && iyy && izz)
304 {
305 inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
306 inertia.m_ixy = 0;
307 inertia.m_ixz = 0;
308 inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
309 inertia.m_iyz = 0;
310 inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
311 }
312 else
313 {
314 logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
315 return false;
316 }
317 }
318 }
319 else
320 {
321 if (!(inertia_xml->Attribute("ixx") && inertia_xml->Attribute("ixy") && inertia_xml->Attribute("ixz") &&
322 inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
323 inertia_xml->Attribute("izz")))
324 {
325 if ((inertia_xml->Attribute("ixx") && inertia_xml->Attribute("iyy") &&
326 inertia_xml->Attribute("izz")))
327 {
328 inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
329 inertia.m_ixy = 0;
330 inertia.m_ixz = 0;
331 inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
332 inertia.m_iyz = 0;
333 inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
334 }
335 else
336 {
337 logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
338 return false;
339 }
340 }
341 else
342 {
343 inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
344 inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
345 inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
346 inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
347 inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
348 inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
349 }
350 }
351 return true;
352 }
353
parseGeometry(UrdfGeometry & geom,XMLElement * g,ErrorLogger * logger)354 bool UrdfParser::parseGeometry(UrdfGeometry& geom, XMLElement* g, ErrorLogger* logger)
355 {
356 // btAssert(g);
357 if (g == 0)
358 return false;
359
360 XMLElement* shape = g->FirstChildElement();
361 if (!shape)
362 {
363 logger->reportError("Geometry tag contains no child element.");
364 return false;
365 }
366
367 //const std::string type_name = shape->ValueTStr().c_str();
368 const std::string type_name = shape->Value();
369 if (type_name == "sphere")
370 {
371 geom.m_type = URDF_GEOM_SPHERE;
372 if (m_parseSDF)
373 {
374 XMLElement* size = shape->FirstChildElement("radius");
375 if (0 == size)
376 {
377 logger->reportError("sphere requires a radius child element");
378 return false;
379 }
380 geom.m_sphereRadius = urdfLexicalCast<double>(size->GetText());
381 }
382 else
383 {
384 if (!shape->Attribute("radius"))
385 {
386 logger->reportError("Sphere shape must have a radius attribute");
387 return false;
388 }
389 else
390 {
391 geom.m_sphereRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
392 }
393 }
394 }
395 else if (type_name == "box")
396 {
397 geom.m_type = URDF_GEOM_BOX;
398 if (m_parseSDF)
399 {
400 XMLElement* size = shape->FirstChildElement("size");
401 if (0 == size)
402 {
403 logger->reportError("box requires a size child element");
404 return false;
405 }
406 parseVector3(geom.m_boxSize, size->GetText(), logger);
407 geom.m_boxSize *= m_urdfScaling;
408 }
409 else
410 {
411 if (!shape->Attribute("size"))
412 {
413 logger->reportError("box requires a size attribute");
414 return false;
415 }
416 else
417 {
418 parseVector3(geom.m_boxSize, shape->Attribute("size"), logger);
419 geom.m_boxSize *= m_urdfScaling;
420 }
421 }
422 }
423 else if (type_name == "cylinder")
424 {
425 geom.m_type = URDF_GEOM_CYLINDER;
426 geom.m_hasFromTo = false;
427 geom.m_capsuleRadius = 0.1;
428 geom.m_capsuleHeight = 0.1;
429
430 if (m_parseSDF)
431 {
432 if (XMLElement* scale = shape->FirstChildElement("radius"))
433 {
434 parseVector3(geom.m_meshScale, scale->GetText(), logger);
435 geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
436 }
437 if (XMLElement* scale = shape->FirstChildElement("length"))
438 {
439 parseVector3(geom.m_meshScale, scale->GetText(), logger);
440 geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
441 }
442 }
443 else
444 {
445 if (!shape->Attribute("length") || !shape->Attribute("radius"))
446 {
447 logger->reportError("Cylinder shape must have both length and radius attributes");
448 return false;
449 }
450 geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
451 geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
452 }
453 }
454 else if (type_name == "capsule")
455 {
456 geom.m_type = URDF_GEOM_CAPSULE;
457 geom.m_hasFromTo = false;
458 if (m_parseSDF)
459 {
460 if (XMLElement* scale = shape->FirstChildElement("radius"))
461 {
462 parseVector3(geom.m_meshScale, scale->GetText(), logger);
463 geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
464 }
465 if (XMLElement* scale = shape->FirstChildElement("length"))
466 {
467 parseVector3(geom.m_meshScale, scale->GetText(), logger);
468 geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(scale->GetText());
469 }
470 }
471 else
472 {
473 if (!shape->Attribute("length") || !shape->Attribute("radius"))
474 {
475 logger->reportError("Capsule shape must have both length and radius attributes");
476 return false;
477 }
478 geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
479 geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
480 }
481 }
482 else if ((type_name == "mesh") || (type_name == "cdf"))
483 {
484 if ((type_name == "cdf"))
485 {
486 geom.m_type = URDF_GEOM_CDF;
487 }
488 else
489 {
490 geom.m_type = URDF_GEOM_MESH;
491 }
492 geom.m_meshScale.setValue(1, 1, 1);
493 std::string fn;
494
495 if (m_parseSDF)
496 {
497 if (XMLElement* scale = shape->FirstChildElement("scale"))
498 {
499 parseVector3(geom.m_meshScale, scale->GetText(), logger);
500 }
501 if (XMLElement* filename = shape->FirstChildElement("uri"))
502 {
503 fn = filename->GetText();
504 }
505 }
506 else
507 {
508 // URDF
509 if (shape->Attribute("filename"))
510 {
511 fn = shape->Attribute("filename");
512 }
513 if (shape->Attribute("scale"))
514 {
515 if (!parseVector3(geom.m_meshScale, shape->Attribute("scale"), logger))
516 {
517 logger->reportWarning("Scale should be a vector3, not single scalar. Workaround activated.\n");
518 std::string scalar_str = shape->Attribute("scale");
519 double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
520 if (scaleFactor)
521 {
522 geom.m_meshScale.setValue(scaleFactor, scaleFactor, scaleFactor);
523 }
524 }
525 }
526 }
527
528 geom.m_meshScale *= m_urdfScaling;
529
530 if (fn.empty())
531 {
532 logger->reportError("Mesh filename is empty");
533 return false;
534 }
535
536 geom.m_meshFileName = fn;
537 bool success = UrdfFindMeshFile(m_fileIO,
538 m_urdf2Model.m_sourceFile, fn, sourceFileLocation(shape),
539 &geom.m_meshFileName, &geom.m_meshFileType);
540 if (!success)
541 {
542 // warning already printed
543 return false;
544 }
545 }
546 else
547 {
548 if (type_name == "plane")
549 {
550 geom.m_type = URDF_GEOM_PLANE;
551 if (this->m_parseSDF)
552 {
553 XMLElement* n = shape->FirstChildElement("normal");
554 XMLElement* s = shape->FirstChildElement("size");
555
556 if ((0 == n) || (0 == s))
557 {
558 logger->reportError("Plane shape must have both normal and size attributes");
559 return false;
560 }
561
562 parseVector3(geom.m_planeNormal, n->GetText(), logger);
563 }
564 else
565 {
566 if (!shape->Attribute("normal"))
567 {
568 logger->reportError("plane requires a normal attribute");
569 return false;
570 }
571 else
572 {
573 parseVector3(geom.m_planeNormal, shape->Attribute("normal"), logger);
574 }
575 }
576 }
577 else
578 {
579 logger->reportError("Unknown geometry type:");
580 logger->reportError(type_name.c_str());
581 return false;
582 }
583 }
584
585 return true;
586 }
587
parseCollision(UrdfCollision & collision,XMLElement * config,ErrorLogger * logger)588 bool UrdfParser::parseCollision(UrdfCollision& collision, XMLElement* config, ErrorLogger* logger)
589 {
590 collision.m_linkLocalFrame.setIdentity();
591
592 if (m_parseSDF)
593 {
594 XMLElement* pose = config->FirstChildElement("pose");
595 if (pose)
596 {
597 parseTransform(collision.m_linkLocalFrame, pose, logger, m_parseSDF);
598 }
599 }
600
601 // Origin
602 XMLElement* o = config->FirstChildElement("origin");
603 if (o)
604 {
605 if (!parseTransform(collision.m_linkLocalFrame, o, logger))
606 return false;
607 }
608 // Geometry
609 XMLElement* geom = config->FirstChildElement("geometry");
610 if (!parseGeometry(collision.m_geometry, geom, logger))
611 {
612 return false;
613 }
614
615 {
616 const char* group_char = config->Attribute("group");
617 if (group_char)
618 {
619 collision.m_flags |= URDF_HAS_COLLISION_GROUP;
620 collision.m_collisionGroup = urdfLexicalCast<int>(group_char);
621 }
622 }
623
624 {
625 const char* mask_char = config->Attribute("mask");
626 if (mask_char)
627 {
628 collision.m_flags |= URDF_HAS_COLLISION_MASK;
629 collision.m_collisionMask = urdfLexicalCast<int>(mask_char);
630 }
631 }
632
633 const char* name_char = config->Attribute("name");
634 if (name_char)
635 collision.m_name = name_char;
636
637 const char* concave_char = config->Attribute("concave");
638 if (concave_char)
639 collision.m_flags |= URDF_FORCE_CONCAVE_TRIMESH;
640
641 return true;
642 }
643
parseVisual(UrdfModel & model,UrdfVisual & visual,XMLElement * config,ErrorLogger * logger)644 bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, XMLElement* config, ErrorLogger* logger)
645 {
646 visual.m_linkLocalFrame.setIdentity();
647 if (m_parseSDF)
648 {
649 XMLElement* pose = config->FirstChildElement("pose");
650 if (pose)
651 {
652 parseTransform(visual.m_linkLocalFrame, pose, logger, m_parseSDF);
653 }
654 }
655
656 // Origin
657 XMLElement* o = config->FirstChildElement("origin");
658 if (o)
659 {
660 if (!parseTransform(visual.m_linkLocalFrame, o, logger))
661 return false;
662 }
663 // Geometry
664 XMLElement* geom = config->FirstChildElement("geometry");
665 if (!parseGeometry(visual.m_geometry, geom, logger))
666 {
667 return false;
668 }
669
670 const char* name_char = config->Attribute("name");
671 if (name_char)
672 visual.m_name = name_char;
673
674 visual.m_geometry.m_hasLocalMaterial = false;
675
676 // Material
677 XMLElement* mat = config->FirstChildElement("material");
678 //todo(erwincoumans) skip materials in SDF for now (due to complexity)
679 if (mat)
680 {
681 if (m_parseSDF)
682 {
683 UrdfMaterial* matPtr = new UrdfMaterial;
684 matPtr->m_name = "mat";
685 if (name_char)
686 matPtr->m_name = name_char;
687
688 UrdfMaterial** oldMatPtrPtr = model.m_materials[matPtr->m_name.c_str()];
689 if (oldMatPtrPtr)
690 {
691 UrdfMaterial* oldMatPtr = *oldMatPtrPtr;
692 model.m_materials.remove(matPtr->m_name.c_str());
693 if (oldMatPtr)
694 delete oldMatPtr;
695 }
696 model.m_materials.insert(matPtr->m_name.c_str(), matPtr);
697 {
698 XMLElement* diffuse = mat->FirstChildElement("diffuse");
699 if (diffuse)
700 {
701 std::string diffuseText = diffuse->GetText();
702 btVector4 rgba(1, 0, 0, 1);
703 parseVector4(rgba, diffuseText);
704 matPtr->m_matColor.m_rgbaColor = rgba;
705
706 visual.m_materialName = matPtr->m_name;
707 visual.m_geometry.m_hasLocalMaterial = true;
708 }
709 }
710 {
711 XMLElement* specular = mat->FirstChildElement("specular");
712 if (specular)
713 {
714 std::string specularText = specular->GetText();
715 btVector3 rgba(1, 1, 1);
716 parseVector3(rgba, specularText, logger);
717 matPtr->m_matColor.m_specularColor = rgba;
718 visual.m_materialName = matPtr->m_name;
719 visual.m_geometry.m_hasLocalMaterial = true;
720 }
721 }
722 }
723 else
724 {
725 // get material name
726 if (!mat->Attribute("name"))
727 {
728 logger->reportError("Visual material must contain a name attribute");
729 return false;
730 }
731 visual.m_materialName = mat->Attribute("name");
732
733 // try to parse material element in place
734
735 XMLElement* t = mat->FirstChildElement("texture");
736 XMLElement* c = mat->FirstChildElement("color");
737 XMLElement* s = mat->FirstChildElement("specular");
738 if (t || c || s)
739 {
740 if (parseMaterial(visual.m_geometry.m_localMaterial, mat, logger))
741 {
742 UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial);
743
744 UrdfMaterial** oldMatPtrPtr = model.m_materials[matPtr->m_name.c_str()];
745 if (oldMatPtrPtr)
746 {
747 UrdfMaterial* oldMatPtr = *oldMatPtrPtr;
748 model.m_materials.remove(matPtr->m_name.c_str());
749 if (oldMatPtr)
750 delete oldMatPtr;
751 }
752 model.m_materials.insert(matPtr->m_name.c_str(), matPtr);
753 visual.m_geometry.m_hasLocalMaterial = true;
754 }
755 }
756 }
757 }
758 ParseUserData(config, visual.m_userData, logger);
759
760 return true;
761 }
762
parseLink(UrdfModel & model,UrdfLink & link,XMLElement * config,ErrorLogger * logger)763 bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement* config, ErrorLogger* logger)
764 {
765 const char* linkName = config->Attribute("name");
766 if (!linkName)
767 {
768 logger->reportError("Link with no name");
769 return false;
770 }
771 link.m_name = linkName;
772
773 if (m_parseSDF)
774 {
775 XMLElement* pose = config->FirstChildElement("pose");
776 if (0 == pose)
777 {
778 link.m_linkTransformInWorld.setIdentity();
779 }
780 else
781 {
782 parseTransform(link.m_linkTransformInWorld, pose, logger, m_parseSDF);
783 }
784 }
785
786 {
787 //optional 'audio_source' parameters
788 //modified version of SDF audio_source specification in //http://sdformat.org/spec?ver=1.6&elem=link
789 #if 0
790 <audio_source>
791 <uri>file://media/audio/cheer.mp3</uri>
792 <pitch>2.0</pitch>
793 <gain>1.0</gain>
794 <loop>false</loop>
795 <contact>
796 <collision>collision</collision>
797 </contact>
798 </audio_source>
799 #endif
800 XMLElement* ci = config->FirstChildElement("audio_source");
801 if (ci)
802 {
803 link.m_audioSource.m_flags |= SDFAudioSource::SDFAudioSourceValid;
804
805 const char* fn = ci->Attribute("filename");
806 if (fn)
807 {
808 link.m_audioSource.m_uri = fn;
809 }
810 else
811 {
812 if (XMLElement* filename_xml = ci->FirstChildElement("uri"))
813 {
814 link.m_audioSource.m_uri = filename_xml->GetText();
815 }
816 }
817 if (XMLElement* pitch_xml = ci->FirstChildElement("pitch"))
818 {
819 link.m_audioSource.m_pitch = urdfLexicalCast<double>(pitch_xml->GetText());
820 }
821 if (XMLElement* gain_xml = ci->FirstChildElement("gain"))
822 {
823 link.m_audioSource.m_gain = urdfLexicalCast<double>(gain_xml->GetText());
824 }
825
826 if (XMLElement* attack_rate_xml = ci->FirstChildElement("attack_rate"))
827 {
828 link.m_audioSource.m_attackRate = urdfLexicalCast<double>(attack_rate_xml->GetText());
829 }
830
831 if (XMLElement* decay_rate_xml = ci->FirstChildElement("decay_rate"))
832 {
833 link.m_audioSource.m_decayRate = urdfLexicalCast<double>(decay_rate_xml->GetText());
834 }
835
836 if (XMLElement* sustain_level_xml = ci->FirstChildElement("sustain_level"))
837 {
838 link.m_audioSource.m_sustainLevel = urdfLexicalCast<double>(sustain_level_xml->GetText());
839 }
840
841 if (XMLElement* release_rate_xml = ci->FirstChildElement("release_rate"))
842 {
843 link.m_audioSource.m_releaseRate = urdfLexicalCast<double>(release_rate_xml->GetText());
844 }
845
846 if (XMLElement* loop_xml = ci->FirstChildElement("loop"))
847 {
848 std::string looptxt = loop_xml->GetText();
849 if (looptxt == "true")
850 {
851 link.m_audioSource.m_flags |= SDFAudioSource::SDFAudioSourceLooping;
852 }
853 }
854 if (XMLElement* forceThreshold_xml = ci->FirstChildElement("collision_force_threshold"))
855 {
856 link.m_audioSource.m_collisionForceThreshold = urdfLexicalCast<double>(forceThreshold_xml->GetText());
857 }
858 //todo: see other audio_source children
859 //pitch, gain, loop, contact
860 }
861 }
862 {
863 //optional 'contact' parameters
864 XMLElement* ci = config->FirstChildElement("contact");
865 if (ci)
866 {
867 XMLElement* damping_xml = ci->FirstChildElement("inertia_scaling");
868 if (damping_xml)
869 {
870 if (m_parseSDF)
871 {
872 link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
873 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
874 }
875 else
876 {
877 if (!damping_xml->Attribute("value"))
878 {
879 logger->reportError("Link/contact: damping element must have value attribute");
880 return false;
881 }
882
883 link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
884 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
885 }
886 }
887 {
888 XMLElement* friction_xml = ci->FirstChildElement("lateral_friction");
889 if (friction_xml)
890 {
891 if (m_parseSDF)
892 {
893 link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
894 }
895 else
896 {
897 if (!friction_xml->Attribute("value"))
898 {
899 logger->reportError("Link/contact: lateral_friction element must have value attribute");
900 return false;
901 }
902
903 link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
904 }
905 }
906 }
907
908 {
909 XMLElement* rolling_xml = ci->FirstChildElement("rolling_friction");
910 if (rolling_xml)
911 {
912 if (m_parseSDF)
913 {
914 link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
915 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
916 }
917 else
918 {
919 if (!rolling_xml->Attribute("value"))
920 {
921 logger->reportError("Link/contact: rolling friction element must have value attribute");
922 return false;
923 }
924
925 link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
926 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
927 }
928 }
929 }
930
931 {
932 XMLElement* restitution_xml = ci->FirstChildElement("restitution");
933 if (restitution_xml)
934 {
935 if (m_parseSDF)
936 {
937 link.m_contactInfo.m_restitution = urdfLexicalCast<double>(restitution_xml->GetText());
938 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION;
939 }
940 else
941 {
942 if (!restitution_xml->Attribute("value"))
943 {
944 logger->reportError("Link/contact: restitution element must have value attribute");
945 return false;
946 }
947
948 link.m_contactInfo.m_restitution = urdfLexicalCast<double>(restitution_xml->Attribute("value"));
949 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_RESTITUTION;
950 }
951 }
952 }
953
954 {
955 XMLElement* spinning_xml = ci->FirstChildElement("spinning_friction");
956 if (spinning_xml)
957 {
958 if (m_parseSDF)
959 {
960 link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->GetText());
961 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
962 }
963 else
964 {
965 if (!spinning_xml->Attribute("value"))
966 {
967 logger->reportError("Link/contact: spinning friction element must have value attribute");
968 return false;
969 }
970
971 link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->Attribute("value"));
972 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
973 }
974 }
975 }
976 {
977 XMLElement* friction_anchor = ci->FirstChildElement("friction_anchor");
978 if (friction_anchor)
979 {
980 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_FRICTION_ANCHOR;
981 }
982 }
983 {
984 XMLElement* stiffness_xml = ci->FirstChildElement("stiffness");
985 if (stiffness_xml)
986 {
987 if (m_parseSDF)
988 {
989 link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->GetText());
990 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
991 }
992 else
993 {
994 if (!stiffness_xml->Attribute("value"))
995 {
996 logger->reportError("Link/contact: stiffness element must have value attribute");
997 return false;
998 }
999
1000 link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->Attribute("value"));
1001 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
1002 }
1003 }
1004 }
1005 {
1006 XMLElement* damping_xml = ci->FirstChildElement("damping");
1007 if (damping_xml)
1008 {
1009 if (m_parseSDF)
1010 {
1011 link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->GetText());
1012 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
1013 }
1014 else
1015 {
1016 if (!damping_xml->Attribute("value"))
1017 {
1018 logger->reportError("Link/contact: damping element must have value attribute");
1019 return false;
1020 }
1021
1022 link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
1023 link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
1024 }
1025 }
1026 }
1027 }
1028 }
1029
1030 // Inertial (optional)
1031 XMLElement* i = config->FirstChildElement("inertial");
1032 if (i)
1033 {
1034 if (!parseInertia(link.m_inertia, i, logger))
1035 {
1036 logger->reportError("Could not parse inertial element for Link:");
1037 logger->reportError(link.m_name.c_str());
1038 return false;
1039 }
1040 }
1041 else
1042 {
1043 if ((strlen(linkName) == 5) && (strncmp(linkName, "world", 5)) == 0)
1044 {
1045 link.m_inertia.m_mass = 0.f;
1046 link.m_inertia.m_linkLocalFrame.setIdentity();
1047 link.m_inertia.m_ixx = 0.f;
1048 link.m_inertia.m_iyy = 0.f;
1049 link.m_inertia.m_izz = 0.f;
1050 }
1051 else
1052 {
1053 logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
1054 link.m_inertia.m_mass = 1.f;
1055 link.m_inertia.m_linkLocalFrame.setIdentity();
1056 link.m_inertia.m_ixx = 1.f;
1057 link.m_inertia.m_iyy = 1.f;
1058 link.m_inertia.m_izz = 1.f;
1059 logger->reportWarning(link.m_name.c_str());
1060 }
1061 }
1062
1063 // Multiple Visuals (optional)
1064 for (XMLElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
1065 {
1066 UrdfVisual visual;
1067 visual.m_sourceFileLocation = sourceFileLocation(vis_xml);
1068
1069 if (parseVisual(model, visual, vis_xml, logger))
1070 {
1071 link.m_visualArray.push_back(visual);
1072 }
1073 else
1074 {
1075 logger->reportError("Could not parse visual element for Link:");
1076 logger->reportError(link.m_name.c_str());
1077 return false;
1078 }
1079 }
1080
1081 // Multiple Collisions (optional)
1082 for (XMLElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
1083 {
1084 UrdfCollision col;
1085 col.m_sourceFileLocation = sourceFileLocation(col_xml);
1086
1087 if (parseCollision(col, col_xml, logger))
1088 {
1089 link.m_collisionArray.push_back(col);
1090 }
1091 else
1092 {
1093 logger->reportError("Could not parse collision element for Link:");
1094 logger->reportError(link.m_name.c_str());
1095 return false;
1096 }
1097 }
1098 ParseUserData(config, link.m_userData, logger);
1099 return true;
1100 }
1101
parseLameCoefficients(LameCoefficients & lameCoefficients,tinyxml2::XMLElement * config,ErrorLogger * logger)1102 bool UrdfParser::parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger)
1103 {
1104 const char* mu = config->Attribute("mu");
1105 const char* lambda = config->Attribute("lambda");
1106 const char* damping = config->Attribute("damping");
1107 if (!mu || !lambda)
1108 {
1109 logger->reportError("expected mu lambda for LameCoefficients.");
1110 return false;
1111 }
1112 lameCoefficients.mu = urdfLexicalCast<double>(mu);
1113 lameCoefficients.lambda = urdfLexicalCast<double>(lambda);
1114 if (damping)
1115 lameCoefficients.damping = urdfLexicalCast<double>(damping);
1116 else
1117 lameCoefficients.damping = 0;
1118 return true;
1119 }
1120
parseDeformable(UrdfModel & model,tinyxml2::XMLElement * config,ErrorLogger * logger)1121 bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger)
1122 {
1123 UrdfDeformable& deformable = model.m_deformable;
1124 const char* deformableName = config->Attribute("name");
1125 if (!deformableName)
1126 {
1127 logger->reportError("Deformable with no name");
1128 return false;
1129 }
1130 deformable.m_name = deformableName;
1131
1132 XMLElement* i = config->FirstChildElement("inertial");
1133 if (!i)
1134 {
1135 logger->reportError("expected an inertial element");
1136 return false;
1137 }
1138 UrdfInertia inertia;
1139 if (!parseInertia(inertia, i, logger))
1140 {
1141 logger->reportError("Could not parse inertial element for deformable:");
1142 logger->reportError(deformable.m_name.c_str());
1143 return false;
1144 }
1145 deformable.m_mass = inertia.m_mass;
1146
1147 XMLElement* collisionMargin_xml = config->FirstChildElement("collision_margin");
1148 if (collisionMargin_xml)
1149 {
1150 if (!collisionMargin_xml->Attribute("value"))
1151 {
1152 logger->reportError("collision_margin element must have value attribute");
1153 return false;
1154 }
1155 deformable.m_collisionMargin = urdfLexicalCast<double>(collisionMargin_xml->Attribute("value"));
1156 }
1157
1158 XMLElement* friction_xml = config->FirstChildElement("friction");
1159 if (friction_xml)
1160 {
1161 if (!friction_xml->Attribute("value"))
1162 {
1163 logger->reportError("friction element must have value attribute");
1164 return false;
1165 }
1166 deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
1167 }
1168
1169 XMLElement* repulsion_xml = config->FirstChildElement("repulsion_stiffness");
1170 if (repulsion_xml)
1171 {
1172 if (!repulsion_xml->Attribute("value"))
1173 {
1174 logger->reportError("repulsion_stiffness element must have value attribute");
1175 return false;
1176 }
1177 deformable.m_repulsionStiffness = urdfLexicalCast<double>(repulsion_xml->Attribute("value"));
1178 }
1179
1180 XMLElement* grav_xml = config->FirstChildElement("gravity_factor");
1181 if (grav_xml)
1182 {
1183 if (!grav_xml->Attribute("value"))
1184 {
1185 logger->reportError("gravity_factor element must have value attribute");
1186 return false;
1187 }
1188 deformable.m_gravFactor = urdfLexicalCast<double>(grav_xml->Attribute("value"));
1189 }
1190
1191 XMLElement* cache_barycenter = config->FirstChildElement("cache_barycenter");
1192 if (cache_barycenter)
1193 {
1194 deformable.m_cache_barycenter = true;
1195 }
1196
1197 XMLElement* spring_xml = config->FirstChildElement("spring");
1198 if (spring_xml)
1199 {
1200 if (!spring_xml->Attribute("elastic_stiffness") || !spring_xml->Attribute("damping_stiffness"))
1201 {
1202 logger->reportError("spring element expect elastic and damping stiffness");
1203 return false;
1204 }
1205
1206 deformable.m_springCoefficients.elastic_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("elastic_stiffness"));
1207 deformable.m_springCoefficients.damping_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("damping_stiffness"));
1208
1209 if (spring_xml->Attribute("bending_stiffness"))
1210 {
1211 deformable.m_springCoefficients.bending_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("bending_stiffness"));
1212
1213 if (spring_xml->Attribute("bending_stride"))
1214 deformable.m_springCoefficients.bending_stride = urdfLexicalCast<int>(spring_xml->Attribute("bending_stride"));
1215 }
1216 }
1217
1218 XMLElement* corotated_xml = config->FirstChildElement("corotated");
1219 if (corotated_xml)
1220 {
1221 if (!parseLameCoefficients(deformable.m_corotatedCoefficients, corotated_xml, logger))
1222 {
1223 return false;
1224 }
1225 }
1226
1227 XMLElement* neohookean_xml = config->FirstChildElement("neohookean");
1228 if (neohookean_xml)
1229 {
1230 if (!parseLameCoefficients(deformable.m_neohookeanCoefficients, neohookean_xml, logger))
1231 {
1232 return false;
1233 }
1234 }
1235
1236 XMLElement* vis_xml = config->FirstChildElement("visual");
1237 if (!vis_xml)
1238 {
1239 logger->reportError("expected an visual element");
1240 return false;
1241 }
1242 if (!vis_xml->Attribute("filename"))
1243 {
1244 logger->reportError("expected a filename for visual geometry");
1245 return false;
1246 }
1247 std::string fn = vis_xml->Attribute("filename");
1248 deformable.m_visualFileName = fn;
1249
1250 int out_type(0);
1251 bool success = UrdfFindMeshFile(m_fileIO,
1252 model.m_sourceFile, fn, sourceFileLocation(vis_xml),
1253 &deformable.m_visualFileName, &out_type);
1254
1255 if (!success)
1256 {
1257 // warning already printed
1258 return false;
1259 }
1260
1261 XMLElement* col_xml = config->FirstChildElement("collision");
1262 if (col_xml)
1263 {
1264 if (!col_xml->Attribute("filename"))
1265 {
1266 logger->reportError("expected a filename for collision geoemtry");
1267 return false;
1268 }
1269 fn = vis_xml->Attribute("filename");
1270 success = UrdfFindMeshFile(m_fileIO,
1271 model.m_sourceFile, fn, sourceFileLocation(vis_xml),
1272 &deformable.m_simFileName, &out_type);
1273
1274 if (!success)
1275 {
1276 // warning already printed
1277 return false;
1278 }
1279 }
1280
1281 ParseUserData(config, deformable.m_userData, logger);
1282 return true;
1283 }
1284
parseJointLimits(UrdfJoint & joint,XMLElement * config,ErrorLogger * logger)1285 bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
1286 {
1287 joint.m_lowerLimit = 0.f;
1288 joint.m_upperLimit = -1.f;
1289 joint.m_effortLimit = 0.f;
1290 joint.m_velocityLimit = 0.f;
1291 joint.m_jointDamping = 0.f;
1292 joint.m_jointFriction = 0.f;
1293
1294 if (m_parseSDF)
1295 {
1296 XMLElement* lower_xml = config->FirstChildElement("lower");
1297 if (lower_xml)
1298 {
1299 joint.m_lowerLimit = urdfLexicalCast<double>(lower_xml->GetText());
1300 }
1301
1302 XMLElement* upper_xml = config->FirstChildElement("upper");
1303 if (upper_xml)
1304 {
1305 joint.m_upperLimit = urdfLexicalCast<double>(upper_xml->GetText());
1306 }
1307
1308 XMLElement* effort_xml = config->FirstChildElement("effort");
1309 if (effort_xml)
1310 {
1311 joint.m_effortLimit = urdfLexicalCast<double>(effort_xml->GetText());
1312 }
1313
1314 XMLElement* velocity_xml = config->FirstChildElement("velocity");
1315 if (velocity_xml)
1316 {
1317 joint.m_velocityLimit = urdfLexicalCast<double>(velocity_xml->GetText());
1318 }
1319 }
1320 else
1321 {
1322 const char* lower_str = config->Attribute("lower");
1323 if (lower_str)
1324 {
1325 joint.m_lowerLimit = urdfLexicalCast<double>(lower_str);
1326 }
1327
1328 const char* upper_str = config->Attribute("upper");
1329 if (upper_str)
1330 {
1331 joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
1332 }
1333
1334 if (joint.m_type == URDFPrismaticJoint)
1335 {
1336 joint.m_lowerLimit *= m_urdfScaling;
1337 joint.m_upperLimit *= m_urdfScaling;
1338 }
1339
1340 // Get joint effort limit
1341 const char* effort_str = config->Attribute("effort");
1342 if (effort_str)
1343 {
1344 joint.m_effortLimit = urdfLexicalCast<double>(effort_str);
1345 }
1346
1347 // Get joint velocity limit
1348 const char* velocity_str = config->Attribute("velocity");
1349 if (velocity_str)
1350 {
1351 joint.m_velocityLimit = urdfLexicalCast<double>(velocity_str);
1352 }
1353 }
1354
1355 return true;
1356 }
1357
parseJointDynamics(UrdfJoint & joint,XMLElement * config,ErrorLogger * logger)1358 bool UrdfParser::parseJointDynamics(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
1359 {
1360 joint.m_jointDamping = 0;
1361 joint.m_jointFriction = 0;
1362
1363 if (m_parseSDF)
1364 {
1365 XMLElement* damping_xml = config->FirstChildElement("damping");
1366 if (damping_xml)
1367 {
1368 joint.m_jointDamping = urdfLexicalCast<double>(damping_xml->GetText());
1369 }
1370
1371 XMLElement* friction_xml = config->FirstChildElement("friction");
1372 if (friction_xml)
1373 {
1374 joint.m_jointFriction = urdfLexicalCast<double>(friction_xml->GetText());
1375 }
1376
1377 if (damping_xml == NULL && friction_xml == NULL)
1378 {
1379 logger->reportError("joint dynamics element specified with no damping and no friction");
1380 return false;
1381 }
1382 }
1383 else
1384 {
1385 // Get joint damping
1386 const char* damping_str = config->Attribute("damping");
1387 if (damping_str)
1388 {
1389 joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
1390 }
1391
1392 // Get joint friction
1393 const char* friction_str = config->Attribute("friction");
1394 if (friction_str)
1395 {
1396 joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
1397 }
1398
1399 if (damping_str == NULL && friction_str == NULL)
1400 {
1401 logger->reportError("joint dynamics element specified with no damping and no friction");
1402 return false;
1403 }
1404 }
1405
1406 return true;
1407 }
1408
parseJoint(UrdfJoint & joint,XMLElement * config,ErrorLogger * logger)1409 bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
1410 {
1411 // Get Joint Name
1412 const char* name = config->Attribute("name");
1413 if (!name)
1414 {
1415 logger->reportError("unnamed joint found");
1416 return false;
1417 }
1418 joint.m_name = name;
1419 joint.m_parentLinkToJointTransform.setIdentity();
1420
1421 // Get transform from Parent Link to Joint Frame
1422 XMLElement* origin_xml = config->FirstChildElement("origin");
1423 if (origin_xml)
1424 {
1425 if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml, logger))
1426 {
1427 logger->reportError("Malformed parent origin element for joint:");
1428 logger->reportError(joint.m_name.c_str());
1429 return false;
1430 }
1431 }
1432
1433 // Get Parent Link
1434 XMLElement* parent_xml = config->FirstChildElement("parent");
1435 if (parent_xml)
1436 {
1437 if (m_parseSDF)
1438 {
1439 joint.m_parentLinkName = std::string(parent_xml->GetText());
1440 }
1441 else
1442 {
1443 const char* pname = parent_xml->Attribute("link");
1444 if (!pname)
1445 {
1446 logger->reportError("no parent link name specified for Joint link. this might be the root?");
1447 logger->reportError(joint.m_name.c_str());
1448 return false;
1449 }
1450 else
1451 {
1452 joint.m_parentLinkName = std::string(pname);
1453 }
1454 }
1455 }
1456
1457 // Get Child Link
1458 XMLElement* child_xml = config->FirstChildElement("child");
1459 if (child_xml)
1460 {
1461 if (m_parseSDF)
1462 {
1463 joint.m_childLinkName = std::string(child_xml->GetText());
1464 }
1465 else
1466 {
1467 const char* pname = child_xml->Attribute("link");
1468 if (!pname)
1469 {
1470 logger->reportError("no child link name specified for Joint link [%s].");
1471 logger->reportError(joint.m_name.c_str());
1472 return false;
1473 }
1474 else
1475 {
1476 joint.m_childLinkName = std::string(pname);
1477 }
1478 }
1479 }
1480
1481 // Get Joint type
1482 const char* type_char = config->Attribute("type");
1483 if (!type_char)
1484 {
1485 logger->reportError("joint [%s] has no type, check to see if it's a reference.");
1486 logger->reportError(joint.m_name.c_str());
1487 return false;
1488 }
1489
1490 std::string type_str = type_char;
1491 if (type_str == "spherical")
1492 joint.m_type = URDFSphericalJoint;
1493 else if (type_str == "planar")
1494 joint.m_type = URDFPlanarJoint;
1495 else if (type_str == "floating")
1496 joint.m_type = URDFFloatingJoint;
1497 else if (type_str == "revolute")
1498 joint.m_type = URDFRevoluteJoint;
1499 else if (type_str == "continuous")
1500 joint.m_type = URDFContinuousJoint;
1501 else if (type_str == "prismatic")
1502 joint.m_type = URDFPrismaticJoint;
1503 else if (type_str == "fixed")
1504 joint.m_type = URDFFixedJoint;
1505 else
1506 {
1507 logger->reportError("Joint ");
1508 logger->reportError(joint.m_name.c_str());
1509 logger->reportError("has unknown type:");
1510 logger->reportError(type_str.c_str());
1511 return false;
1512 }
1513
1514 if (m_parseSDF)
1515 {
1516 if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
1517 {
1518 // axis
1519 XMLElement* axis_xml = config->FirstChildElement("axis");
1520 if (!axis_xml)
1521 {
1522 std::string msg("urdfdom: no axis element for Joint, defaulting to (1,0,0) axis");
1523 msg = msg + " " + joint.m_name + "\n";
1524 logger->reportWarning(msg.c_str());
1525 joint.m_localJointAxis.setValue(1, 0, 0);
1526 }
1527 else
1528 {
1529 XMLElement* xyz_xml = axis_xml->FirstChildElement("xyz");
1530 if (xyz_xml)
1531 {
1532 if (!parseVector3(joint.m_localJointAxis, std::string(xyz_xml->GetText()), logger))
1533 {
1534 logger->reportError("Malformed axis element:");
1535 logger->reportError(joint.m_name.c_str());
1536 logger->reportError(" for joint:");
1537 logger->reportError(xyz_xml->GetText());
1538 return false;
1539 }
1540 }
1541
1542 XMLElement* limit_xml = axis_xml->FirstChildElement("limit");
1543 if (limit_xml)
1544 {
1545 if (joint.m_type != URDFContinuousJoint)
1546 {
1547 if (!parseJointLimits(joint, limit_xml, logger))
1548 {
1549 logger->reportError("Could not parse limit element for joint:");
1550 logger->reportError(joint.m_name.c_str());
1551 return false;
1552 }
1553 }
1554 }
1555 else if (joint.m_type == URDFRevoluteJoint)
1556 {
1557 logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
1558 logger->reportError(joint.m_name.c_str());
1559 return false;
1560 }
1561 else if (joint.m_type == URDFPrismaticJoint)
1562 {
1563 logger->reportError("Joint is of type PRISMATIC without limits");
1564 logger->reportError(joint.m_name.c_str());
1565 return false;
1566 }
1567
1568 XMLElement* prop_xml = axis_xml->FirstChildElement("dynamics");
1569 if (prop_xml)
1570 {
1571 if (!parseJointDynamics(joint, prop_xml, logger))
1572 {
1573 logger->reportError("Could not parse dynamics element for joint:");
1574 logger->reportError(joint.m_name.c_str());
1575 return false;
1576 }
1577 }
1578 }
1579 }
1580 }
1581 else
1582 {
1583 // Get Joint Axis
1584 if (joint.m_type != URDFFloatingJoint && joint.m_type != URDFFixedJoint)
1585 {
1586 // axis
1587 XMLElement* axis_xml = config->FirstChildElement("axis");
1588 if (!axis_xml)
1589 {
1590 std::string msg("urdfdom: no axis element for Joint, defaulting to (1,0,0) axis");
1591 msg = msg + " " + joint.m_name + "\n";
1592 logger->reportWarning(msg.c_str());
1593 joint.m_localJointAxis.setValue(1, 0, 0);
1594 }
1595 else
1596 {
1597 if (axis_xml->Attribute("xyz"))
1598 {
1599 if (!parseVector3(joint.m_localJointAxis, axis_xml->Attribute("xyz"), logger))
1600 {
1601 logger->reportError("Malformed axis element:");
1602 logger->reportError(joint.m_name.c_str());
1603 logger->reportError(" for joint:");
1604 logger->reportError(axis_xml->Attribute("xyz"));
1605 return false;
1606 }
1607 }
1608 }
1609 }
1610
1611 // Get limit
1612 XMLElement* limit_xml = config->FirstChildElement("limit");
1613 if (limit_xml)
1614 {
1615 if (!parseJointLimits(joint, limit_xml, logger))
1616 {
1617 logger->reportError("Could not parse limit element for joint:");
1618 logger->reportError(joint.m_name.c_str());
1619 return false;
1620 }
1621 }
1622 else if (joint.m_type == URDFRevoluteJoint)
1623 {
1624 logger->reportError("Joint is of type REVOLUTE but it does not specify limits");
1625 logger->reportError(joint.m_name.c_str());
1626 return false;
1627 }
1628 else if (joint.m_type == URDFPrismaticJoint)
1629 {
1630 logger->reportError("Joint is of type PRISMATIC without limits");
1631 logger->reportError(joint.m_name.c_str());
1632 return false;
1633 }
1634
1635 joint.m_jointDamping = 0;
1636 joint.m_jointFriction = 0;
1637
1638 // Get Dynamics
1639 XMLElement* prop_xml = config->FirstChildElement("dynamics");
1640 if (prop_xml)
1641 {
1642 // Get joint damping
1643 const char* damping_str = prop_xml->Attribute("damping");
1644 if (damping_str)
1645 {
1646 joint.m_jointDamping = urdfLexicalCast<double>(damping_str);
1647 }
1648
1649 // Get joint friction
1650 const char* friction_str = prop_xml->Attribute("friction");
1651 if (friction_str)
1652 {
1653 joint.m_jointFriction = urdfLexicalCast<double>(friction_str);
1654 }
1655
1656 if (damping_str == NULL && friction_str == NULL)
1657 {
1658 logger->reportError("joint dynamics element specified with no damping and no friction");
1659 return false;
1660 }
1661 }
1662 }
1663
1664 return true;
1665 }
1666
parseSensor(UrdfModel & model,UrdfLink & link,UrdfJoint & joint,XMLElement * config,ErrorLogger * logger)1667 bool UrdfParser::parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
1668 {
1669 // Sensors are mapped to Links with a Fixed Joints connecting to the parents.
1670 // They has no extent or mass so they will work with the existing
1671 // model without affecting the system.
1672 logger->reportError("Adding Sensor ");
1673 const char* sensorName = config->Attribute("name");
1674 if (!sensorName)
1675 {
1676 logger->reportError("Sensor with no name");
1677 return false;
1678 }
1679
1680 logger->reportError(sensorName);
1681 link.m_name = sensorName;
1682 link.m_linkTransformInWorld.setIdentity();
1683 link.m_inertia.m_mass = 0.f;
1684 link.m_inertia.m_linkLocalFrame.setIdentity();
1685 link.m_inertia.m_ixx = 0.f;
1686 link.m_inertia.m_iyy = 0.f;
1687 link.m_inertia.m_izz = 0.f;
1688
1689 // Get Parent Link
1690 XMLElement* parent_xml = config->FirstChildElement("parent");
1691 if (parent_xml)
1692 {
1693 if (m_parseSDF)
1694 {
1695 joint.m_parentLinkName = std::string(parent_xml->GetText());
1696 }
1697 else
1698 {
1699 const char* pname = parent_xml->Attribute("link");
1700 if (!pname)
1701 {
1702 logger->reportError("no parent link name specified for sensor. this might be the root?");
1703 logger->reportError(joint.m_name.c_str());
1704 return false;
1705 }
1706 else
1707 {
1708 joint.m_parentLinkName = std::string(pname);
1709 }
1710 }
1711 }
1712
1713 joint.m_name = std::string(sensorName).append("_Joint");
1714 joint.m_childLinkName = sensorName;
1715 joint.m_type = URDFFixedJoint;
1716 joint.m_localJointAxis.setValue(0, 0, 0);
1717
1718 // Get transform from Parent Link to Joint Frame
1719 XMLElement* origin_xml = config->FirstChildElement("origin");
1720 if (origin_xml)
1721 {
1722 if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml, logger))
1723 {
1724 logger->reportError("Malformed origin element for sensor:");
1725 logger->reportError(joint.m_name.c_str());
1726 return false;
1727 }
1728 }
1729
1730 return true;
1731 }
1732
CalculatePrincipalAxisTransform(btScalar * masses,const btTransform * transforms,btMatrix3x3 * inertiasIn,btTransform & principal,btVector3 & inertiaOut)1733 static void CalculatePrincipalAxisTransform(btScalar* masses, const btTransform* transforms, btMatrix3x3* inertiasIn, btTransform& principal, btVector3& inertiaOut)
1734 {
1735 int n = 2;
1736
1737 btScalar totalMass = 0;
1738 btVector3 center(0, 0, 0);
1739 int k;
1740
1741 for (k = 0; k < n; k++)
1742 {
1743 btAssert(masses[k] > 0);
1744 center += transforms[k].getOrigin() * masses[k];
1745 totalMass += masses[k];
1746 }
1747
1748 btAssert(totalMass > 0);
1749
1750 center /= totalMass;
1751 principal.setOrigin(center);
1752
1753 btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
1754 for (k = 0; k < n; k++)
1755 {
1756
1757
1758 const btTransform& t = transforms[k];
1759 btVector3 o = t.getOrigin() - center;
1760
1761 //compute inertia tensor in coordinate system of parent
1762 btMatrix3x3 j = t.getBasis().transpose();
1763 j *= inertiasIn[k];
1764 j = t.getBasis() * j;
1765
1766 //add inertia tensor
1767 tensor[0] += j[0];
1768 tensor[1] += j[1];
1769 tensor[2] += j[2];
1770
1771 //compute inertia tensor of pointmass at o
1772 btScalar o2 = o.length2();
1773 j[0].setValue(o2, 0, 0);
1774 j[1].setValue(0, o2, 0);
1775 j[2].setValue(0, 0, o2);
1776 j[0] += o * -o.x();
1777 j[1] += o * -o.y();
1778 j[2] += o * -o.z();
1779
1780 //add inertia tensor of pointmass
1781 tensor[0] += masses[k] * j[0];
1782 tensor[1] += masses[k] * j[1];
1783 tensor[2] += masses[k] * j[2];
1784 }
1785
1786 tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
1787 inertiaOut.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
1788 }
1789
mergeFixedLinks(UrdfModel & model,UrdfLink * link,ErrorLogger * logger,bool forceFixedBase,int level)1790 bool UrdfParser::mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level)
1791 {
1792 for (int l = 0; l < level; l++)
1793 {
1794 printf("\t");
1795 }
1796 printf("processing %s\n", link->m_name.c_str());
1797
1798 for (int i = 0; i < link->m_childJoints.size();)
1799 {
1800 if (link->m_childJoints[i]->m_type == URDFFixedJoint)
1801 {
1802 UrdfLink* childLink = link->m_childLinks[i];
1803 UrdfJoint* childJoint = link->m_childJoints[i];
1804 for (int l = 0; l < level+1; l++)
1805 {
1806 printf("\t");
1807 }
1808 //mergeChildLink
1809 printf("merge %s into %s!\n", childLink->m_name.c_str(), link->m_name.c_str());
1810 for (int c = 0; c < childLink->m_collisionArray.size(); c++)
1811 {
1812 UrdfCollision col = childLink->m_collisionArray[c];
1813 col.m_linkLocalFrame = childJoint->m_parentLinkToJointTransform * col.m_linkLocalFrame;
1814 link->m_collisionArray.push_back(col);
1815 }
1816
1817 for (int c = 0; c < childLink->m_visualArray.size(); c++)
1818 {
1819 UrdfVisual viz = childLink->m_visualArray[c];
1820 viz.m_linkLocalFrame = childJoint->m_parentLinkToJointTransform * viz.m_linkLocalFrame;
1821 link->m_visualArray.push_back(viz);
1822 }
1823
1824 if (!link->m_inertia.m_hasLinkLocalFrame)
1825 {
1826 link->m_inertia.m_linkLocalFrame.setIdentity();
1827 }
1828 if (!childLink->m_inertia.m_hasLinkLocalFrame)
1829 {
1830 childLink->m_inertia.m_linkLocalFrame.setIdentity();
1831 }
1832 //for a 'forceFixedBase' don't merge
1833 bool isStaticBase = false;
1834 if (forceFixedBase && link->m_parentJoint == 0)
1835 isStaticBase = true;
1836 if (link->m_inertia.m_mass==0 && link->m_parentJoint == 0)
1837 isStaticBase = true;
1838
1839 //skip the mass and inertia merge for a fixed base link
1840 if (!isStaticBase)
1841 {
1842 btScalar masses[2] = { (btScalar)link->m_inertia.m_mass, (btScalar)childLink->m_inertia.m_mass };
1843 btTransform transforms[2] = { link->m_inertia.m_linkLocalFrame, childJoint->m_parentLinkToJointTransform * childLink->m_inertia.m_linkLocalFrame };
1844 btMatrix3x3 inertiaLink(
1845 link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
1846 link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
1847 link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
1848 btMatrix3x3 inertiaChild(
1849 childLink->m_inertia.m_ixx, childLink->m_inertia.m_ixy, childLink->m_inertia.m_ixz,
1850 childLink->m_inertia.m_ixy, childLink->m_inertia.m_iyy, childLink->m_inertia.m_iyz,
1851 childLink->m_inertia.m_ixz, childLink->m_inertia.m_iyz, childLink->m_inertia.m_izz);
1852 btMatrix3x3 inertiasIn[2] = { inertiaLink, inertiaChild };
1853 btVector3 inertiaOut;
1854 btTransform principal;
1855 CalculatePrincipalAxisTransform(masses, transforms, inertiasIn, principal, inertiaOut);
1856 link->m_inertia.m_hasLinkLocalFrame = true;
1857 link->m_inertia.m_linkLocalFrame.setIdentity();
1858 //link->m_inertia.m_linkLocalFrame = principal;
1859 link->m_inertia.m_linkLocalFrame.setOrigin(principal.getOrigin());
1860
1861 link->m_inertia.m_ixx = inertiaOut[0];
1862 link->m_inertia.m_ixy = 0;
1863 link->m_inertia.m_ixz = 0;
1864 link->m_inertia.m_iyy = inertiaOut[1];
1865 link->m_inertia.m_iyz = 0;
1866 link->m_inertia.m_izz = inertiaOut[2];
1867 link->m_inertia.m_mass += childLink->m_inertia.m_mass;
1868 }
1869 link->m_childJoints.removeAtIndex(i);
1870 link->m_childLinks.removeAtIndex(i);
1871
1872 for (int g = 0; g < childLink->m_childJoints.size(); g++)
1873 {
1874 UrdfLink* grandChildLink = childLink->m_childLinks[g];
1875 UrdfJoint* grandChildJoint = childLink->m_childJoints[g];
1876 for (int l = 0; l < level+2; l++)
1877 {
1878 printf("\t");
1879 }
1880 printf("relink %s from %s to %s!\n", grandChildLink->m_name.c_str(), childLink->m_name.c_str(), link->m_name.c_str());
1881
1882 grandChildJoint->m_parentLinkName = link->m_name;
1883 grandChildJoint->m_parentLinkToJointTransform =
1884 childJoint->m_parentLinkToJointTransform * grandChildJoint->m_parentLinkToJointTransform;
1885
1886 grandChildLink->m_parentLink = link;
1887 grandChildLink->m_parentJoint->m_parentLinkName = link->m_name;
1888
1889 link->m_childJoints.push_back(grandChildJoint);
1890 link->m_childLinks.push_back(grandChildLink);
1891 }
1892 model.m_links.remove(childLink->m_name.c_str());
1893 model.m_joints.remove(childJoint->m_name.c_str());
1894
1895 }
1896 else
1897 {
1898 //keep this link and recurse
1899 mergeFixedLinks(model, link->m_childLinks[i], logger, forceFixedBase, level+1);
1900 i++;
1901 }
1902 }
1903 return true;
1904 }
1905
1906
1907 const std::string sJointNames[]={"unused",
1908 "URDFRevoluteJoint",
1909 "URDFPrismaticJoint",
1910 "URDFContinuousJoint",
1911 "URDFFloatingJoint",
1912 "URDFPlanarJoint",
1913 "URDFFixedJoint",
1914 "URDFSphericalJoint",
1915 };
1916
printTree(UrdfLink * link,ErrorLogger * logger,int level)1917 bool UrdfParser::printTree(UrdfLink* link, ErrorLogger* logger, int level)
1918 {
1919 printf("\n");
1920 for (int l = 0; l < level; l++)
1921 {
1922 printf("\t");
1923 }
1924 printf("%s (mass=%f) ", link->m_name.c_str(), link->m_inertia.m_mass);
1925 if (link->m_parentJoint)
1926 {
1927 printf("(joint %s, joint type=%s\n", link->m_parentJoint->m_name.c_str(), sJointNames[link->m_parentJoint->m_type].c_str());
1928 }
1929 else
1930 {
1931 printf("\n");
1932 }
1933
1934 for (int i = 0; i<link->m_childJoints.size(); i++)
1935 {
1936 printTree(link->m_childLinks[i], logger, level + 1);
1937 btAssert(link->m_childJoints[i]->m_parentLinkName == link->m_name);
1938 }
1939 return true;
1940 }
1941
recreateModel(UrdfModel & model,UrdfLink * link,ErrorLogger * logger)1942 bool UrdfParser::recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger)
1943 {
1944 if (!link->m_parentJoint)
1945 {
1946 link->m_linkIndex = model.m_links.size();
1947 model.m_links.insert(link->m_name.c_str(), link);
1948 }
1949 for (int i = 0; i < link->m_childJoints.size(); i++)
1950 {
1951 link->m_childLinks[i]->m_linkIndex = model.m_links.size();
1952 const char* childName = link->m_childLinks[i]->m_name.c_str();
1953 UrdfLink* childLink = link->m_childLinks[i];
1954 model.m_links.insert(childName, childLink);
1955 const char* jointName = link->m_childLinks[i]->m_parentJoint->m_name.c_str();
1956 UrdfJoint* joint = link->m_childLinks[i]->m_parentJoint;
1957 model.m_joints.insert(jointName, joint);
1958 }
1959 for (int i = 0; i < link->m_childJoints.size(); i++)
1960 {
1961 recreateModel(model, link->m_childLinks[i], logger);
1962 }
1963 return true;
1964 }
initTreeAndRoot(UrdfModel & model,ErrorLogger * logger)1965 bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
1966 {
1967 // every link has children links and joints, but no parents, so we create a
1968 // local convenience data structure for keeping child->parent relations
1969 btHashMap<btHashString, btHashString> parentLinkTree;
1970
1971 // loop through all joints, for every link, assign children links and children joints
1972 for (int i = 0; i < model.m_joints.size(); i++)
1973 {
1974 UrdfJoint** jointPtr = model.m_joints.getAtIndex(i);
1975 if (jointPtr)
1976 {
1977 UrdfJoint* joint = *jointPtr;
1978 std::string parent_link_name = joint->m_parentLinkName;
1979 std::string child_link_name = joint->m_childLinkName;
1980 if (parent_link_name.empty() || child_link_name.empty())
1981 {
1982 logger->reportError("parent link or child link is empty for joint");
1983 logger->reportError(joint->m_name.c_str());
1984 return false;
1985 }
1986
1987 UrdfLink** childLinkPtr = model.m_links.find(joint->m_childLinkName.c_str());
1988 if (!childLinkPtr)
1989 {
1990 logger->reportError("Cannot find child link for joint ");
1991 logger->reportError(joint->m_name.c_str());
1992
1993 return false;
1994 }
1995 UrdfLink* childLink = *childLinkPtr;
1996
1997 UrdfLink** parentLinkPtr = model.m_links.find(joint->m_parentLinkName.c_str());
1998 if (!parentLinkPtr)
1999 {
2000 logger->reportError("Cannot find parent link for a joint");
2001 logger->reportError(joint->m_name.c_str());
2002 return false;
2003 }
2004 UrdfLink* parentLink = *parentLinkPtr;
2005
2006 childLink->m_parentLink = parentLink;
2007
2008 childLink->m_parentJoint = joint;
2009 parentLink->m_childJoints.push_back(joint);
2010 parentLink->m_childLinks.push_back(childLink);
2011 parentLinkTree.insert(childLink->m_name.c_str(), parentLink->m_name.c_str());
2012 }
2013 }
2014
2015 //search for children that have no parent, those are 'root'
2016 for (int i = 0; i < model.m_links.size(); i++)
2017 {
2018 UrdfLink** linkPtr = model.m_links.getAtIndex(i);
2019 btAssert(linkPtr);
2020 if (linkPtr)
2021 {
2022 UrdfLink* link = *linkPtr;
2023 link->m_linkIndex = i;
2024
2025 if (!link->m_parentLink)
2026 {
2027 model.m_rootLinks.push_back(link);
2028 }
2029 }
2030 }
2031
2032 if (model.m_rootLinks.size() > 1)
2033 {
2034 std::string multipleRootMessage =
2035 "URDF file with multiple root links found:";
2036
2037 for (int i = 0; i < model.m_rootLinks.size(); i++)
2038 {
2039 multipleRootMessage += " ";
2040 multipleRootMessage += model.m_rootLinks[i]->m_name.c_str();
2041 }
2042 logger->reportWarning(multipleRootMessage.c_str());
2043 }
2044
2045 if (model.m_rootLinks.size() == 0)
2046 {
2047 logger->reportError("URDF without root link found");
2048 return false;
2049 }
2050
2051
2052
2053 return true;
2054 }
2055
loadUrdf(const char * urdfText,ErrorLogger * logger,bool forceFixedBase,bool parseSensors)2056 bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors)
2057 {
2058 XMLDocument xml_doc;
2059 xml_doc.Parse(urdfText);
2060 if (xml_doc.Error())
2061 {
2062 #ifdef G3_TINYXML2
2063 logger->reportError("xml reading error");
2064 #else
2065 logger->reportError(xml_doc.ErrorStr());
2066 xml_doc.ClearError();
2067 #endif
2068 return false;
2069 }
2070
2071 XMLElement* robot_xml = xml_doc.FirstChildElement("robot");
2072 if (!robot_xml)
2073 {
2074 logger->reportError("expected a robot element");
2075 return false;
2076 }
2077
2078 // Get robot name
2079 const char* name = robot_xml->Attribute("name");
2080 if (!name)
2081 {
2082 logger->reportError("Expected a name for robot");
2083 return false;
2084 }
2085 m_urdf2Model.m_name = name;
2086
2087 ParseUserData(robot_xml, m_urdf2Model.m_userData, logger);
2088
2089 // Get all Material elements
2090 for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
2091 {
2092 UrdfMaterial* material = new UrdfMaterial;
2093
2094 parseMaterial(*material, material_xml, logger);
2095
2096 UrdfMaterial** mat = m_urdf2Model.m_materials.find(material->m_name.c_str());
2097 if (mat)
2098 {
2099 delete material;
2100 logger->reportWarning("Duplicate material");
2101 }
2102 else
2103 {
2104 m_urdf2Model.m_materials.insert(material->m_name.c_str(), material);
2105 }
2106 }
2107
2108 // char msg[1024];
2109 // sprintf(msg,"Num materials=%d", m_model.m_materials.size());
2110 // logger->printMessage(msg);
2111
2112
2113 XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable");
2114 if(deformable_xml)
2115 return parseDeformable(m_urdf2Model, deformable_xml, logger);
2116
2117 for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
2118 {
2119 UrdfLink* link = new UrdfLink;
2120
2121 if (parseLink(m_urdf2Model, *link, link_xml, logger))
2122 {
2123 if (m_urdf2Model.m_links.find(link->m_name.c_str()))
2124 {
2125 logger->reportError("Link name is not unique, link names in the same model have to be unique");
2126 logger->reportError(link->m_name.c_str());
2127 delete link;
2128 return false;
2129 }
2130 else
2131 {
2132 //copy model material into link material, if link has no local material
2133 for (int i = 0; i < link->m_visualArray.size(); i++)
2134 {
2135 UrdfVisual& vis = link->m_visualArray.at(i);
2136 if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
2137 {
2138 UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
2139 if (mat && *mat)
2140 {
2141 vis.m_geometry.m_localMaterial = **mat;
2142 }
2143 else
2144 {
2145 //logger->reportError("Cannot find material with name:");
2146 //logger->reportError(vis.m_materialName.c_str());
2147 }
2148 }
2149 }
2150
2151 m_urdf2Model.m_links.insert(link->m_name.c_str(), link);
2152 }
2153 }
2154 else
2155 {
2156 logger->reportError("failed to parse link");
2157 delete link;
2158 return false;
2159 }
2160 }
2161 if (m_urdf2Model.m_links.size() == 0)
2162 {
2163 logger->reportWarning("No links found in URDF file");
2164 return false;
2165 }
2166
2167 // Get all Joint elements
2168 for (XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
2169 {
2170 UrdfJoint* joint = new UrdfJoint;
2171
2172 if (parseJoint(*joint, joint_xml, logger))
2173 {
2174 if (m_urdf2Model.m_joints.find(joint->m_name.c_str()))
2175 {
2176 logger->reportError("joint '%s' is not unique.");
2177 logger->reportError(joint->m_name.c_str());
2178 delete joint;
2179 return false;
2180 }
2181 else
2182 {
2183 m_urdf2Model.m_joints.insert(joint->m_name.c_str(), joint);
2184 }
2185 }
2186 else
2187 {
2188 logger->reportError("joint xml is not initialized correctly");
2189 delete joint;
2190 return false;
2191 }
2192 }
2193
2194 if (parseSensors)
2195 {
2196 // Get all Sensor Elements.
2197 for (XMLElement* sensor_xml = robot_xml->FirstChildElement("sensor"); sensor_xml; sensor_xml = sensor_xml->NextSiblingElement("sensor"))
2198 {
2199 UrdfLink* sensor = new UrdfLink;
2200 UrdfJoint* sensor_joint = new UrdfJoint;
2201
2202 if (parseSensor(m_urdf2Model, *sensor, *sensor_joint, sensor_xml, logger))
2203 {
2204 if (m_urdf2Model.m_links.find(sensor->m_name.c_str()))
2205 {
2206 logger->reportError("Sensor name is not unique, sensor and link names in the same model have to be unique");
2207 logger->reportError(sensor->m_name.c_str());
2208 delete sensor;
2209 delete sensor_joint;
2210 return false;
2211 }
2212 else if (m_urdf2Model.m_joints.find(sensor_joint->m_name.c_str()))
2213 {
2214 logger->reportError("Sensor Joint name is not unique, joint names in the same model have to be unique");
2215 logger->reportError(sensor_joint->m_name.c_str());
2216 delete sensor;
2217 delete sensor_joint;
2218 return false;
2219 }
2220 else
2221 {
2222 m_urdf2Model.m_links.insert(sensor->m_name.c_str(), sensor);
2223 m_urdf2Model.m_joints.insert(sensor_joint->m_name.c_str(), sensor_joint);
2224 }
2225 }
2226 else
2227 {
2228 logger->reportError("failed to parse sensor");
2229 delete sensor;
2230 delete sensor_joint;
2231 return false;
2232 }
2233 }
2234 }
2235
2236 if (m_urdf2Model.m_links.size() == 0)
2237 {
2238 logger->reportWarning("No links found in URDF file");
2239 return false;
2240 }
2241
2242 bool ok(initTreeAndRoot(m_urdf2Model, logger));
2243 if (!ok)
2244 {
2245 return false;
2246 }
2247
2248 if (forceFixedBase)
2249 {
2250 for (int i = 0; i < m_urdf2Model.m_rootLinks.size(); i++)
2251 {
2252 UrdfLink* link(m_urdf2Model.m_rootLinks.at(i));
2253 link->m_inertia.m_mass = 0.0;
2254 link->m_inertia.m_ixx = 0.0;
2255 link->m_inertia.m_ixy = 0.0;
2256 link->m_inertia.m_ixz = 0.0;
2257 link->m_inertia.m_iyy = 0.0;
2258 link->m_inertia.m_iyz = 0.0;
2259 link->m_inertia.m_izz = 0.0;
2260 }
2261 }
2262
2263 return true;
2264 }
2265
activateModel(int modelIndex)2266 void UrdfParser::activateModel(int modelIndex)
2267 {
2268 m_activeSdfModel = modelIndex;
2269 }
2270
loadSDF(const char * sdfText,ErrorLogger * logger)2271 bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
2272 {
2273 XMLDocument xml_doc;
2274 xml_doc.Parse(sdfText);
2275 if (xml_doc.Error())
2276 {
2277 #ifdef G3_TINYXML2
2278 logger->reportError("xml reading error");
2279 #else
2280 logger->reportError(xml_doc.ErrorStr());
2281 xml_doc.ClearError();
2282 #endif
2283 return false;
2284 }
2285
2286 XMLElement* sdf_xml = xml_doc.FirstChildElement("sdf");
2287 if (!sdf_xml)
2288 {
2289 logger->reportError("expected an sdf element");
2290 return false;
2291 }
2292
2293 //apparently, SDF doesn't require a "world" element, optional? URDF does.
2294 XMLElement* world_xml = sdf_xml->FirstChildElement("world");
2295
2296 XMLElement* robot_xml = 0;
2297
2298 if (!world_xml)
2299 {
2300 logger->reportWarning("expected a world element, continuing without it.");
2301 robot_xml = sdf_xml->FirstChildElement("model");
2302 }
2303 else
2304 {
2305 robot_xml = world_xml->FirstChildElement("model");
2306 }
2307
2308 // Get all model (robot) elements
2309 for (; robot_xml; robot_xml = robot_xml->NextSiblingElement("model"))
2310 {
2311 UrdfModel* localModel = new UrdfModel;
2312 m_tmpModels.push_back(localModel);
2313
2314 XMLElement* stat = robot_xml->FirstChildElement("static");
2315 if (0 != stat)
2316 {
2317 int val = int(atof(stat->GetText()));
2318 if (val == 1)
2319 {
2320 localModel->m_overrideFixedBase = true;
2321 }
2322 }
2323
2324 // Get robot name
2325 const char* name = robot_xml->Attribute("name");
2326 if (!name)
2327 {
2328 logger->reportError("Expected a name for robot");
2329 return false;
2330 }
2331 localModel->m_name = name;
2332
2333 XMLElement* pose_xml = robot_xml->FirstChildElement("pose");
2334 if (0 == pose_xml)
2335 {
2336 localModel->m_rootTransformInWorld.setIdentity();
2337 }
2338 else
2339 {
2340 parseTransform(localModel->m_rootTransformInWorld, pose_xml, logger, m_parseSDF);
2341 }
2342
2343 // Get all Material elements
2344 for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
2345 {
2346 UrdfMaterial* material = new UrdfMaterial;
2347
2348 parseMaterial(*material, material_xml, logger);
2349
2350 UrdfMaterial** mat = localModel->m_materials.find(material->m_name.c_str());
2351 if (mat)
2352 {
2353 logger->reportWarning("Duplicate material");
2354 delete material;
2355 }
2356 else
2357 {
2358 localModel->m_materials.insert(material->m_name.c_str(), material);
2359 }
2360 }
2361
2362 // char msg[1024];
2363 // sprintf(msg,"Num materials=%d", m_model.m_materials.size());
2364 // logger->printMessage(msg);
2365
2366 for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
2367 {
2368 UrdfLink* link = new UrdfLink;
2369
2370 if (parseLink(*localModel, *link, link_xml, logger))
2371 {
2372 if (localModel->m_links.find(link->m_name.c_str()))
2373 {
2374 logger->reportError("Link name is not unique, link names in the same model have to be unique");
2375 logger->reportError(link->m_name.c_str());
2376 delete link;
2377 return false;
2378 }
2379 else
2380 {
2381 //copy model material into link material, if link has no local material
2382 for (int i = 0; i < link->m_visualArray.size(); i++)
2383 {
2384 UrdfVisual& vis = link->m_visualArray.at(i);
2385 if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
2386 {
2387 UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
2388 if (mat && *mat)
2389 {
2390 vis.m_geometry.m_localMaterial = **mat;
2391 }
2392 else
2393 {
2394 //logger->reportError("Cannot find material with name:");
2395 //logger->reportError(vis.m_materialName.c_str());
2396 }
2397 }
2398 }
2399
2400 localModel->m_links.insert(link->m_name.c_str(), link);
2401 }
2402 }
2403 else
2404 {
2405 logger->reportError("failed to parse link");
2406 delete link;
2407 return false;
2408 }
2409 }
2410 if (localModel->m_links.size() == 0)
2411 {
2412 logger->reportWarning("No links found in URDF file");
2413 return false;
2414 }
2415
2416 // Get all Joint elements
2417 for (XMLElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
2418 {
2419 UrdfJoint* joint = new UrdfJoint;
2420
2421 if (parseJoint(*joint, joint_xml, logger))
2422 {
2423 if (localModel->m_joints.find(joint->m_name.c_str()))
2424 {
2425 logger->reportError("joint '%s' is not unique.");
2426 logger->reportError(joint->m_name.c_str());
2427 delete joint;
2428 return false;
2429 }
2430 else
2431 {
2432 localModel->m_joints.insert(joint->m_name.c_str(), joint);
2433 }
2434 }
2435 else
2436 {
2437 logger->reportError("joint xml is not initialized correctly");
2438 delete joint;
2439 return false;
2440 }
2441 }
2442
2443 bool ok(initTreeAndRoot(*localModel, logger));
2444 if (!ok)
2445 {
2446 return false;
2447 }
2448 m_sdfModels.push_back(localModel);
2449 }
2450
2451 return true;
2452 }
2453
sourceFileLocation(XMLElement * e)2454 std::string UrdfParser::sourceFileLocation(XMLElement* e)
2455 {
2456 #if 0
2457 //no C++11 etc, no snprintf
2458
2459 char buf[1024];
2460 snprintf(buf, sizeof(buf), "%s:%i", m_urdf2Model.m_sourceFile.c_str(), e->Row());
2461 return buf;
2462 #else
2463 char row[1024];
2464 #ifdef G3_TINYXML2
2465 sprintf(row, "unknown line");
2466 #else
2467 sprintf(row, "%d", e->GetLineNum());
2468 #endif
2469 std::string str = m_urdf2Model.m_sourceFile.c_str() + std::string(":") + std::string(row);
2470 return str;
2471 #endif
2472 }
2473