Merge pull request #1900 from crewmatt/master

Update URDF Importer to process Sensor elements.

10 files changed, 134 insertions, 11 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
diff --git a/examples/SharedMemory/PhysicsClientGRPC.cpp b/examples/SharedMemory/PhysicsClientGRPC.cpp
index 6cc8498e5..94a73369d 100644
--- a/examples/SharedMemory/PhysicsClientGRPC.cpp
+++ b/examples/SharedMemory/PhysicsClientGRPC.cpp
@@ -14,7 +14,7 @@ using grpc::Channel;
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "SharedMemory/grpc/ConvertGRPCBullet.h"
 
-using pybullet_grpc::PyBulletAPI;
+using pybullet_grpc::grpc::PyBulletAPI;
 
 static unsigned int b3DeserializeInt2(const unsigned char* input)
 {
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 835e3c59f..1c8088db2 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1990,7 +1990,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 	{
 	}
 
-	virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
+	virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0)
 	{
 		b3Assert(0);
 		return false;
@@ -3014,7 +3014,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 
 	BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
 	u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
-	bool loadOk = u2b.loadURDF(fileName, useFixedBase);
+	bool loadOk = u2b.loadURDF(fileName, useFixedBase, flags);
 
 	if (loadOk)
 	{
@@ -5676,7 +5676,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 							}
 						}
 					}  //fi
-					//break;
+					   //break;
 				}
 			}
 		}  //if (body && body->m_rigidBody)
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index b51d73560..bdda027fa 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/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