Update URDF Importer to process sensor elements.

Update the URDF Importer to add sensors as 0 mass 0 inertia elements
attached by a fixed joint. This way their states can be read as links.

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

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -1,5 +1,5 @@
 #include "UrdfParser.h"
-#include "../../ThirdPartyLibs/tinyxml2/tinyxml2.h"
+#include "third_party/tinyxml2/tinyxml2.h"
 #include "urdfStringSplit.h"
 #include "urdfLexicalCast.h"
 using namespace tinyxml2;
@@ -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,53 @@ 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)
 	{

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,12 @@ 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

examples/SharedMemory/SharedMemoryPublic.h

@@ -820,6 +820,7 @@ enum eURDF_Flags
 	URDF_ENABLE_SLEEPING = 2048,
 	URDF_INITIALIZE_SAT_FEATURES = 4096,
 	URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
+  URDF_PARSE_SENSORS = 16384,
 };
 
 enum eUrdfGeomTypes  //sync with UrdfParser UrdfGeomTypes