diff options
Diffstat (limited to 'examples/Importers')
7 files changed, 129 insertions, 7 deletions
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h index 2c89fdbc9..2d8f01d06 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h @@ -34,7 +34,7 @@ public: virtual bool loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase = false); - virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0) { return false; } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 5e3cf4d56..7f96628f8 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -117,7 +117,7 @@ struct BulletErrorLogger : public ErrorLogger } }; -bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) +bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase, int flags) { if (strlen(fileName) == 0) return false; @@ -155,7 +155,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase) BulletErrorLogger loggie; m_data->m_urdfParser.setParseSDF(false); - bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase); + bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (flags & CUF_PARSE_SENSORS)); return result; } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index aa57abdae..0d58c06ae 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -23,7 +23,7 @@ public: virtual ~BulletURDFImporter(); - virtual bool loadURDF(const char* fileName, bool forceFixedBase = false); + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0); //warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path virtual bool loadSDF(const char* fileName, bool forceFixedBase = false); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.h b/examples/Importers/ImportURDFDemo/URDF2Bullet.h index 29d608b43..f503a103e 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.h +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.h @@ -31,6 +31,7 @@ enum ConvertURDFFlags CUF_ENABLE_SLEEPING = 2048, CUF_INITIALIZE_SAT_FEATURES = 4096, CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192, + CUF_PARSE_SENSORS = 16384, }; struct UrdfVisualShapeCache diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 4b03c86af..28924729f 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -12,7 +12,7 @@ class URDFImporterInterface public: virtual ~URDFImporterInterface() {} - virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) = 0; + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0) = 0; virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false; } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index d75cf1255..4c47e30a1 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1436,6 +1436,72 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* l return true; } +bool UrdfParser::parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, XMLElement* config, ErrorLogger* logger) +{ + // Sensors are mapped to Links with a Fixed Joints connecting to the parents. + // They has no extent or mass so they will work with the existing + // model without affecting the system. + logger->reportError("Adding Sensor "); + const char* sensorName = config->Attribute("name"); + if (!sensorName) + { + logger->reportError("Sensor with no name"); + return false; + } + + logger->reportError(sensorName); + link.m_name = sensorName; + link.m_linkTransformInWorld.setIdentity(); + 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; + + // Get Parent Link + XMLElement* parent_xml = config->FirstChildElement("parent"); + if (parent_xml) + { + if (m_parseSDF) + { + joint.m_parentLinkName = std::string(parent_xml->GetText()); + } + else + { + const char* pname = parent_xml->Attribute("link"); + if (!pname) + { + logger->reportError("no parent link name specified for sensor. this might be the root?"); + logger->reportError(joint.m_name.c_str()); + return false; + } + else + { + joint.m_parentLinkName = std::string(pname); + } + } + } + + joint.m_name = std::string(sensorName).append("_Joint"); + joint.m_childLinkName = sensorName; + joint.m_type = URDFFixedJoint; + joint.m_localJointAxis.setValue(0, 0, 0); + + // Get transform from Parent Link to Joint Frame + XMLElement* origin_xml = config->FirstChildElement("origin"); + if (origin_xml) + { + if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml, logger)) + { + logger->reportError("Malformed origin element for sensor:"); + logger->reportError(joint.m_name.c_str()); + return false; + } + } + + return true; +} + bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger) { // every link has children links and joints, but no parents, so we create a @@ -1516,7 +1582,7 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger) return true; } -bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase) +bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors) { XMLDocument xml_doc; xml_doc.Parse(urdfText); @@ -1647,6 +1713,54 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF } } + if (parseSensors) + { + // Get all Sensor Elements. + for (XMLElement* sensor_xml = robot_xml->FirstChildElement("sensor"); sensor_xml; sensor_xml = sensor_xml->NextSiblingElement("sensor")) + { + UrdfLink* sensor = new UrdfLink; + UrdfJoint* sensor_joint = new UrdfJoint; + + if (parseSensor(m_urdf2Model, *sensor, *sensor_joint, sensor_xml, logger)) + { + if (m_urdf2Model.m_links.find(sensor->m_name.c_str())) + { + logger->reportError("Sensor name is not unique, sensor and link names in the same model have to be unique"); + logger->reportError(sensor->m_name.c_str()); + delete sensor; + delete sensor_joint; + return false; + } + else if (m_urdf2Model.m_joints.find(sensor_joint->m_name.c_str())) + { + logger->reportError("Sensor Joint name is not unique, joint names in the same model have to be unique"); + logger->reportError(sensor_joint->m_name.c_str()); + delete sensor; + delete sensor_joint; + return false; + } + else + { + m_urdf2Model.m_links.insert(sensor->m_name.c_str(), sensor); + m_urdf2Model.m_joints.insert(sensor_joint->m_name.c_str(), sensor_joint); + } + } + else + { + logger->reportError("failed to parse sensor"); + delete sensor; + delete sensor_joint; + return false; + } + } + } + + if (m_urdf2Model.m_links.size() == 0) + { + logger->reportWarning("No links found in URDF file"); + return false; + } + bool ok(initTreeAndRoot(m_urdf2Model, logger)); if (!ok) { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index f44d7c95f..eff72d593 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -267,6 +267,7 @@ protected: bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger); bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger); bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger); + bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger); public: UrdfParser(); @@ -285,7 +286,13 @@ public: m_urdfScaling = scaling; } - bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase); + bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors); + + bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase) + { + return loadUrdf(urdfText, logger, forceFixedBase, false); + } + bool loadSDF(const char* sdfText, ErrorLogger* logger); int getNumModels() const |