Revert "Revert "Reduced Deformable Model""revert-4194-revert-4069-master

4 files changed, 217 insertions, 2 deletions

diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
index cb7607b1a..e24437d7d 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
@@ -1541,3 +1541,8 @@ const struct UrdfDeformable& BulletURDFImporter::getDeformableModel() const
 {
 	return m_data->m_urdfParser.getDeformable();
 }
+
+const struct UrdfReducedDeformable& BulletURDFImporter::getReducedDeformableModel() const
+{
+	return m_data->m_urdfParser.getReducedDeformable();
+}
diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
index 66bb908de..5fe7f8c5f 100644
--- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
+++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
@@ -93,6 +93,7 @@ public:
 	virtual void setEnableTinyRenderer(bool enable);
 	void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
 	const struct UrdfDeformable& getDeformableModel() const;
+	const struct UrdfReducedDeformable& getReducedDeformableModel() const;
 };
 
 #endif  //BULLET_URDF_IMPORTER_H
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index 96f7538a2..aea391592 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -1282,6 +1282,171 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
 	return true;
 }
 
+bool UrdfParser::parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger)
+{
+	UrdfReducedDeformable& reduced_deformable = model.m_reducedDeformable;
+	const char* name = config->Attribute("name");
+	if (!name)
+	{
+		logger->reportError("Reduced deformable with no name");
+		return false;
+	}
+	reduced_deformable.m_name = name;
+
+	{
+		XMLElement* numModes_xml = config->FirstChildElement("num_modes");
+		if (numModes_xml)
+		{
+			if (!numModes_xml->Attribute("value"))
+			{
+				logger->reportError("numModes_xml element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_numModes = urdfLexicalCast<double>(numModes_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* mass_xml = config->FirstChildElement("mass");
+		if (mass_xml)
+		{
+			if (!mass_xml->Attribute("value"))
+			{
+				logger->reportError("mass_xml element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* stiffnessScale_xml = config->FirstChildElement("stiffness_scale");
+		if (stiffnessScale_xml)
+		{
+			if (!stiffnessScale_xml->Attribute("value"))
+			{
+				logger->reportError("stiffnessScale_xml element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_stiffnessScale = urdfLexicalCast<double>(stiffnessScale_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* collisionMargin_xml = config->FirstChildElement("collision_margin");
+		if (collisionMargin_xml)
+		{
+			if (!collisionMargin_xml->Attribute("value"))
+			{
+				logger->reportError("collision_margin element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_collisionMargin = urdfLexicalCast<double>(collisionMargin_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* erp_xml = config->FirstChildElement("erp");
+		if (erp_xml)
+		{
+			if (!erp_xml->Attribute("value"))
+			{
+				logger->reportError("friction element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_erp = urdfLexicalCast<double>(erp_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* cfm_xml = config->FirstChildElement("cfm");
+		if (cfm_xml)
+		{
+			if (!cfm_xml->Attribute("value"))
+			{
+				logger->reportError("cfm element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_cfm = urdfLexicalCast<double>(cfm_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* damping_xml = config->FirstChildElement("damping_coefficient");
+		if (damping_xml)
+		{
+			if (!damping_xml->Attribute("value"))
+			{
+				logger->reportError("damping_coefficient element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_damping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* friction_xml = config->FirstChildElement("friction");
+		if (friction_xml)
+		{
+			if (!friction_xml->Attribute("value"))
+			{
+				logger->reportError("friction element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
+		}
+	}
+
+	XMLElement* vis_xml = config->FirstChildElement("visual");
+	if (!vis_xml)
+	{
+		logger->reportError("expected an visual element");
+		return false;
+	}
+	if (!vis_xml->Attribute("filename"))
+	{
+		logger->reportError("expected a filename for visual geometry");
+		return false;
+	}
+	std::string fn = vis_xml->Attribute("filename");
+	reduced_deformable.m_visualFileName = fn;
+
+	int out_type(0);
+	bool success = UrdfFindMeshFile(m_fileIO,
+									model.m_sourceFile, fn, sourceFileLocation(vis_xml),
+									&reduced_deformable.m_visualFileName, &out_type);
+
+	if (!success)
+	{
+		// warning already printed
+		return false;
+	}
+
+	// collision mesh is optional
+	XMLElement* col_xml = config->FirstChildElement("collision");
+	if (col_xml)
+	{
+		if (!col_xml->Attribute("filename"))
+		{
+			logger->reportError("expected a filename for collision geoemtry");
+			return false;
+		}
+		fn = col_xml->Attribute("filename");
+		success = UrdfFindMeshFile(m_fileIO,
+								   model.m_sourceFile, fn, sourceFileLocation(col_xml),
+								   &reduced_deformable.m_simFileName, &out_type);
+
+		if (!success)
+		{
+			// warning already printed
+			return false;
+		}
+	}
+
+	ParseUserData(config, reduced_deformable.m_userData, logger);
+	return true;
+}
+
 bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
 {
 	joint.m_lowerLimit = 0.f;
@@ -2126,9 +2291,16 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 	//	logger->printMessage(msg);
 
 
+  
+	XMLElement* reduced_deformable_xml = robot_xml->FirstChildElement("reduced_deformable");
+  if (reduced_deformable_xml) {
+    return parseReducedDeformable(m_urdf2Model, reduced_deformable_xml, logger);
+	}
+	
   XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable");
-  if(deformable_xml)
+  if (deformable_xml) {
     return parseDeformable(m_urdf2Model, deformable_xml, logger);
+	}
 
 	for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
 	{
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index b234fa66b..8256099a5 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -247,6 +247,37 @@ struct UrdfDeformable
 	}
 };
 
+struct UrdfReducedDeformable
+{
+	std::string m_name;
+	int m_numModes;
+
+	double m_mass;
+	double m_stiffnessScale;
+	double m_erp;
+	double m_cfm;
+	double m_friction;
+	double m_collisionMargin;
+	double m_damping;
+
+	std::string m_visualFileName;
+	std::string m_simFileName;
+	btHashMap<btHashString, std::string> m_userData;
+
+	UrdfReducedDeformable() 
+		:	m_numModes(1),
+			m_mass(1),
+			m_stiffnessScale(100),
+			m_erp(0.2),					// generally, 0.2 is a good value for erp and cfm
+			m_cfm(0.2),
+			m_friction(0),
+			m_collisionMargin(0.02),
+			m_damping(0),
+			m_visualFileName(""),
+			m_simFileName("")
+	{}
+};
+
 struct UrdfModel
 {
 	std::string m_name;
@@ -256,6 +287,7 @@ struct UrdfModel
 	btHashMap<btHashString, UrdfLink*> m_links;
 	btHashMap<btHashString, UrdfJoint*> m_joints;
 	UrdfDeformable m_deformable;
+	UrdfReducedDeformable m_reducedDeformable;
 	// Maps user data keys to user data values.
 	btHashMap<btHashString, std::string> m_userData;
 
@@ -333,7 +365,7 @@ protected:
 	bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
   bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger);
 	bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
-
+	bool parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
 
 public:
 	UrdfParser(struct CommonFileIOInterface* fileIO);
@@ -413,6 +445,11 @@ public:
 		return m_urdf2Model.m_deformable;
 	}
 
+	const UrdfReducedDeformable& getReducedDeformable() const
+	{
+		return m_urdf2Model.m_reducedDeformable;
+	}
+
 	bool mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level);
 	bool printTree(UrdfLink* link, ErrorLogger* logger, int level);
 	bool recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger);