diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2022-03-07 16:35:07 -0800 |
---|---|---|
committer | GitHub <noreply@github.com> | 2022-03-07 16:35:07 -0800 |
commit | 4fbecfeddc448eba12da3b38b3449fa6c7a6ec19 (patch) | |
tree | 221e6968b52467a0af06695932fe666ad143e986 /examples/Importers | |
parent | aee1ab63fe459ad0d6c209cc3c9cffacab2b5854 (diff) | |
download | bullet3-4fbecfeddc448eba12da3b38b3449fa6c7a6ec19.tar.gz |
Revert "Revert "Reduced Deformable Model""revert-4194-revert-4069-master
Diffstat (limited to 'examples/Importers')
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); |