From 5c1eb2ec07d039d7c37c9bfd3d7cdc0e5e073942 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 30 Mar 2016 14:43:59 -0700 Subject: [PATCH] support 'world' tag for fixed objects in URDF --- .../Importers/ImportURDFDemo/UrdfParser.cpp | 27 +++++++++++++------ 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 27e8a156f..f15392929 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -379,14 +379,25 @@ bool UrdfParser::parseLink(UrdfLink& link, TiXmlElement *config, ErrorLogger* lo } } else { - logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame"); - link.m_inertia.m_mass = 1.f; - link.m_inertia.m_linkLocalFrame.setIdentity(); - link.m_inertia.m_ixx = 1.f; - link.m_inertia.m_iyy = 1.f; - link.m_inertia.m_izz= 1.f; - - logger->reportWarning(link.m_name.c_str()); + + if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0) + { + link.m_inertia.m_mass = 0.f; + link.m_inertia.m_linkLocalFrame.setIdentity(); + link.m_inertia.m_ixx = 0.f; + link.m_inertia.m_iyy = 0.f; + link.m_inertia.m_izz= 0.f; + } else + { + + logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame"); + link.m_inertia.m_mass = 1.f; + link.m_inertia.m_linkLocalFrame.setIdentity(); + link.m_inertia.m_ixx = 1.f; + link.m_inertia.m_iyy = 1.f; + link.m_inertia.m_izz= 1.f; + logger->reportWarning(link.m_name.c_str()); + } } // Multiple Visuals (optional)