diff --git a/data/torus_deform.urdf b/data/torus_deform.urdf new file mode 100644 index 000000000..c5d661993 --- /dev/null +++ b/data/torus_deform.urdf @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 72113b88c..ec0cba28f 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1533,3 +1533,8 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn const struct UrdfModel* BulletURDFImporter::getUrdfModel() const { return &m_data->m_urdfParser.getModel(); }; + +const struct UrdfDeformable& BulletURDFImporter::getDeformableModel() const +{ + return m_data->m_urdfParser.getDeformable(); +} diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 8c1906d2b..eb890c016 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -2,8 +2,8 @@ #define BULLET_URDF_IMPORTER_H #include "URDFImporterInterface.h" - #include "UrdfRenderingInterface.h" +#include "UrdfParser.h" struct BulletURDFTexture { @@ -91,6 +91,7 @@ public: virtual void setEnableTinyRenderer(bool enable); void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const; + const struct UrdfDeformable& getDeformableModel() const; }; #endif //BULLET_URDF_IMPORTER_H diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 23a29383a..f656e4fde 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1098,6 +1098,155 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement* config, return true; } +bool UrdfParser::parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger) +{ + const char* mu = config->Attribute("mu"); + const char* lambda = config->Attribute("lambda"); + const char* damping = config->Attribute("damping"); + if (!mu || !lambda) + { + logger->reportError("expected mu lambda for LameCoefficients."); + return false; + } + lameCoefficients.mu = urdfLexicalCast(mu); + lameCoefficients.lambda = urdfLexicalCast(lambda); + if (damping) + lameCoefficients.damping = urdfLexicalCast(damping); + else + lameCoefficients.damping = 0; + return true; +} + +bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger) +{ + UrdfDeformable& deformable = model.m_deformable; + const char* deformableName = config->Attribute("name"); + if (!deformableName) + { + logger->reportError("Deformable with no name"); + return false; + } + deformable.m_name = deformableName; + + XMLElement* i = config->FirstChildElement("inertial"); + if (!i) + { + logger->reportError("expected an inertial element"); + } + UrdfInertia inertia; + if (!parseInertia(inertia, i, logger)) + { + logger->reportError("Could not parse inertial element for deformable:"); + logger->reportError(deformable.m_name.c_str()); + return false; + } + deformable.m_mass = inertia.m_mass; + + 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; + } + deformable.m_collisionMargin = urdfLexicalCast(collisionMargin_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; + } + deformable.m_friction = urdfLexicalCast(friction_xml->Attribute("value")); + } + + XMLElement* spring_xml = config->FirstChildElement("spring"); + if (spring_xml) + { + if (!spring_xml->Attribute("elastic_stiffness") || !spring_xml->Attribute("damping_stiffness")) + { + logger->reportError("spring element expect elastic and damping stiffness"); + return false; + } + + deformable.m_springCoefficients.elastic_stiffness = urdfLexicalCast(spring_xml->Attribute("elastic_stiffness")); + deformable.m_springCoefficients.damping_stiffness = urdfLexicalCast(spring_xml->Attribute("damping_stiffness")); + + if (spring_xml->Attribute("bending_stiffness")) + deformable.m_springCoefficients.bending_stiffness = urdfLexicalCast(spring_xml->Attribute("bending_stiffness")); + } + + XMLElement* corotated_xml = config->FirstChildElement("corotated"); + if (corotated_xml) + { + if (!parseLameCoefficients(deformable.m_corotatedCoefficients, corotated_xml, logger)) + { + return false; + } + } + + XMLElement* neohookean_xml = config->FirstChildElement("neohookean"); + if (neohookean_xml) + { + if (!parseLameCoefficients(deformable.m_neohookeanCoefficients, neohookean_xml, logger)) + { + return false; + } + } + + 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"); + deformable.m_visualFileName = fn; + + int out_type(0); + bool success = UrdfFindMeshFile(m_fileIO, + model.m_sourceFile, fn, sourceFileLocation(vis_xml), + &deformable.m_visualFileName, &out_type); + + if (!success) + { + // warning already printed + return false; + } + + 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 = vis_xml->Attribute("filename"); + success = UrdfFindMeshFile(m_fileIO, + model.m_sourceFile, fn, sourceFileLocation(vis_xml), + &deformable.m_simFileName, &out_type); + + if (!success) + { + // warning already printed + return false; + } + } + + ParseUserData(config, deformable.m_userData, logger); + return true; +} + bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger) { joint.m_lowerLimit = 0.f; @@ -1898,6 +2047,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF } m_urdf2Model.m_name = name; + ParseUserData(robot_xml, m_urdf2Model.m_userData, logger); + // Get all Material elements for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { @@ -1921,6 +2072,11 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF // sprintf(msg,"Num materials=%d", m_model.m_materials.size()); // logger->printMessage(msg); + + XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable"); + 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")) { UrdfLink* link = new UrdfLink; @@ -2040,8 +2196,6 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF } } - ParseUserData(robot_xml, m_urdf2Model.m_userData, logger); - if (m_urdf2Model.m_links.size() == 0) { logger->reportWarning("No links found in URDF file"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index dff15c472..9b99bc36f 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -200,6 +200,45 @@ struct UrdfJoint } }; + +struct SpringCoeffcients{ + double elastic_stiffness; + double damping_stiffness; + double bending_stiffness; + SpringCoeffcients(): + elastic_stiffness(0.), + damping_stiffness(0.), + bending_stiffness(0.){} +}; + +struct LameCoefficients +{ + double mu; + double lambda; + double damping; + LameCoefficients() : mu(0.), lambda(0.), damping(0.) {} +}; + +struct UrdfDeformable +{ + std::string m_name; + double m_mass; + double m_collisionMargin; + double m_friction; + + SpringCoeffcients m_springCoefficients; + LameCoefficients m_corotatedCoefficients; + LameCoefficients m_neohookeanCoefficients; + + std::string m_visualFileName; + std::string m_simFileName; + btHashMap m_userData; + + UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_visualFileName(""), m_simFileName("") + { + } +}; + struct UrdfModel { std::string m_name; @@ -208,6 +247,7 @@ struct UrdfModel btHashMap m_materials; btHashMap m_links; btHashMap m_joints; + UrdfDeformable m_deformable; // Maps user data keys to user data values. btHashMap m_userData; @@ -283,6 +323,9 @@ protected: 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); + bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger); + bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger); + public: UrdfParser(struct CommonFileIOInterface* fileIO); @@ -357,11 +400,14 @@ public: return m_urdf2Model; } + const UrdfDeformable& getDeformable() const + { + return m_urdf2Model.m_deformable; + } 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); - std::string sourceFileLocation(tinyxml2::XMLElement* e); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0c611326f..b540f50d1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3489,6 +3489,12 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto rootTrans.setOrigin(pos); rootTrans.setRotation(orn); u2b.setRootTransformInWorld(rootTrans); + if (!(u2b.getDeformableModel().m_visualFileName.empty())) + { + bool use_self_collision = false; + use_self_collision = (flags & CUF_USE_SELF_COLLISION); + return processDeformable(u2b.getDeformableModel(), pos, orn, bodyUniqueIdPtr, bufferServerToClient, bufferSizeInBytes, globalScaling, use_self_collision); + } bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); if (ok) { @@ -8169,274 +8175,261 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe return hasStatus; } -bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDeformable& deformable, bool verbose) { - serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; - bool hasStatus = true; -#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - double scale = 1; - double mass = 1; - double collisionMargin = 0.02; const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments; - if (m_data->m_verboseOutput) + if (verbose) { b3Printf("Processed CMD_LOAD_SOFT_BODY:%s", loadSoftBodyArgs.m_fileName); } btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0); btAssert(loadSoftBodyArgs.m_fileName); - btVector3 initialPos(0, 0, 0); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION) - { - initialPos[0] = loadSoftBodyArgs.m_initialPosition[0]; - initialPos[1] = loadSoftBodyArgs.m_initialPosition[1]; - initialPos[2] = loadSoftBodyArgs.m_initialPosition[2]; - } - btQuaternion initialOrn(0, 0, 0, 1); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION) - { - initialOrn[0] = loadSoftBodyArgs.m_initialOrientation[0]; - initialOrn[1] = loadSoftBodyArgs.m_initialOrientation[1]; - initialOrn[2] = loadSoftBodyArgs.m_initialOrientation[2]; - initialOrn[3] = loadSoftBodyArgs.m_initialOrientation[3]; - } - - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE) - { - scale = clientCmd.m_loadSoftBodyArguments.m_scale; - } if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS) { - mass = clientCmd.m_loadSoftBodyArguments.m_mass; + deformable.m_mass = loadSoftBodyArgs.m_mass; } if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN) { - collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin; + deformable.m_collisionMargin = loadSoftBodyArgs.m_collisionMargin; } - btScalar spring_bending_stiffness = 0; + deformable.m_visualFileName = loadSoftBodyArgs.m_fileName; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH) { - btSoftBody* psb = NULL; - CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); - char relativeFileName[1024]; - char pathPrefix[1024]; - pathPrefix[0] = 0; - if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024)) - { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } - const std::string& error_message_prefix = ""; - std::string out_found_filename, out_found_sim_filename; - int out_type(0), out_sim_type(0); + deformable.m_simFileName = loadSoftBodyArgs.m_simFileName; + } + else + { + deformable.m_simFileName = ""; + } +#ifndef SKIP_DEFORMABLE_BODY + deformable.m_springCoefficients.elastic_stiffness = loadSoftBodyArgs.m_springElasticStiffness; + deformable.m_springCoefficients.damping_stiffness = loadSoftBodyArgs.m_springDampingStiffness; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) + { + deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness; + } + + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) + { + deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu; + deformable.m_corotatedCoefficients.lambda = loadSoftBodyArgs.m_corotatedLambda; + } + + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) + { + deformable.m_neohookeanCoefficients.mu = loadSoftBodyArgs.m_NeoHookeanMu; + deformable.m_neohookeanCoefficients.lambda = loadSoftBodyArgs.m_NeoHookeanLambda; + deformable.m_neohookeanCoefficients.damping = loadSoftBodyArgs.m_NeoHookeanDamping; + } + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) + { + deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff; + } - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH) - { - bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); - } - else - { - out_sim_type = out_type; - out_found_sim_filename = out_found_filename; - } - if (out_sim_type == UrdfGeometry::FILE_OBJ) - { - std::vector shapes; - tinyobj::attrib_t attribute; - std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO); - if (!shapes.empty()) - { - const tinyobj::shape_t& shape = shapes[0]; - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - for (int i = 0; i < attribute.vertices.size(); i++) - { - vertices.push_back(attribute.vertices[i]); - } - for (int i = 0; i < shape.mesh.indices.size(); i++) - { - indices.push_back(shape.mesh.indices[i].vertex_index); - } - int numTris = shape.mesh.indices.size() / 3; - if (numTris > 0) - { - { - btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); - if (softWorld) - { - psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); - } - } - { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) - { - psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); - } - } - } - } -#ifndef SKIP_DEFORMABLE_BODY - btScalar spring_elastic_stiffness, spring_damping_stiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) - { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) - { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) - { - spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness; - } - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); - deformWorld->addForce(psb, springForce); - m_data->m_lf.push_back(springForce); - } - } #endif - } - else if (out_sim_type == UrdfGeometry::FILE_VTK) +} + +bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision) +{ + btSoftBody* psb = NULL; + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (fileIO->findResourcePath(deformable.m_visualFileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + const std::string& error_message_prefix = ""; + std::string out_found_filename, out_found_sim_filename; + int out_type(0), out_sim_type(0); + + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + if (!deformable.m_simFileName.empty()) + { + bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, deformable.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); + } + else + { + out_sim_type = out_type; + out_found_sim_filename = out_found_filename; + } + if (out_sim_type == UrdfGeometry::FILE_OBJ) + { + std::vector shapes; + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO); + if (!shapes.empty()) { -#ifndef SKIP_DEFORMABLE_BODY - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) + const tinyobj::shape_t& shape = shapes[0]; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + for (int i = 0; i < attribute.vertices.size(); i++) + { + vertices.push_back(attribute.vertices[i]); + } + for (int i = 0; i < shape.mesh.indices.size(); i++) + { + indices.push_back(shape.mesh.indices[i].vertex_index); + } + int numTris = shape.mesh.indices.size() / 3; + if (numTris > 0) { - psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str()); - btScalar corotated_mu, corotated_lambda; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) { - corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; - corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; - btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); - deformWorld->addForce(psb, corotatedForce); - m_data->m_lf.push_back(corotatedForce); - } - btScalar neohookean_mu, neohookean_lambda, neohookean_damping; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) - { - neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; - neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; - neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping; - btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); - deformWorld->addForce(psb, neohookeanForce); - m_data->m_lf.push_back(neohookeanForce); - } - btScalar spring_elastic_stiffness, spring_damping_stiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) - { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) { - spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness; + psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + } + } + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); } - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); - deformWorld->addForce(psb, springForce); - m_data->m_lf.push_back(springForce); } } -#endif } - if (psb != NULL) - { #ifndef SKIP_DEFORMABLE_BODY - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld && deformable.m_springCoefficients.elastic_stiffness > 0.) + { + btDeformableLagrangianForce* springForce = + new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness, + deformable.m_springCoefficients.damping_stiffness, + true, deformable.m_springCoefficients.bending_stiffness); + deformWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); + } +#endif + } + else if (out_sim_type == UrdfGeometry::FILE_VTK) + { +#ifndef SKIP_DEFORMABLE_BODY + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str()); + btScalar corotated_mu(0.), corotated_lambda(0.); + corotated_mu = deformable.m_corotatedCoefficients.mu; + corotated_lambda = deformable.m_corotatedCoefficients.lambda; + if (corotated_mu > 0 || corotated_lambda > 0) + { + btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); + deformWorld->addForce(psb, corotatedForce); + m_data->m_lf.push_back(corotatedForce); + } + btScalar neohookean_mu, neohookean_lambda, neohookean_damping; + neohookean_mu = deformable.m_neohookeanCoefficients.mu; + neohookean_lambda = deformable.m_neohookeanCoefficients.lambda; + neohookean_damping = deformable.m_neohookeanCoefficients.damping; + if (neohookean_mu > 0 || neohookean_lambda > 0) + { + btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); + deformWorld->addForce(psb, neohookeanForce); + m_data->m_lf.push_back(neohookeanForce); + } + + btScalar spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness; + spring_elastic_stiffness = deformable.m_springCoefficients.elastic_stiffness; + spring_damping_stiffness = deformable.m_springCoefficients.damping_stiffness; + spring_bending_stiffness = deformable.m_springCoefficients.bending_stiffness; + if (spring_elastic_stiffness > 0.) + { + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); + deformWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); + } + } +#endif + } + if (psb != NULL) + { +#ifndef SKIP_DEFORMABLE_BODY + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + // load render mesh + if (out_found_sim_filename != out_found_filename) { // load render mesh - if (out_found_sim_filename != out_found_filename) { - // load render mesh + tinyobj::attrib_t attribute; + std::vector shapes; + + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface()); + + for (int s = 0; s < (int)shapes.size(); s++) { - tinyobj::attrib_t attribute; - std::vector shapes; - - std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface()); - - for (int s = 0; s < (int)shapes.size(); s++) + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + int vertexCount = attribute.vertices.size() / 3; + for (int v = 0; v < vertexCount; v++) { - tinyobj::shape_t& shape = shapes[s]; - int faceCount = shape.mesh.indices.size(); - int vertexCount = attribute.vertices.size()/3; - for (int v=0;vm_renderNodes.push_back(n); + } + for (int f = 0; f < faceCount; f += 3) + { + if (f < 0 && f >= int(shape.mesh.indices.size())) { - btSoftBody::Node n; - n.m_x = btVector3(attribute.vertices[3*v],attribute.vertices[3*v+1],attribute.vertices[3*v+2]); - psb->m_renderNodes.push_back(n); - } - for (int f = 0; f < faceCount; f += 3) - { - if (f < 0 && f >= int(shape.mesh.indices.size())) - { - continue; - } - tinyobj::index_t v_0 = shape.mesh.indices[f]; - tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; - tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; - btSoftBody::Face ff; - ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index]; - ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index]; - ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index]; - psb->m_renderFaces.push_back(ff); + continue; } + tinyobj::index_t v_0 = shape.mesh.indices[f]; + tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; + tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; + btSoftBody::Face ff; + ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index]; + ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index]; + ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index]; + psb->m_renderFaces.push_back(ff); } } - if (out_sim_type == UrdfGeometry::FILE_VTK) - { - btSoftBodyHelpers::interpolateBarycentricWeights(psb); - } - else if (out_sim_type == UrdfGeometry::FILE_OBJ) - { - btSoftBodyHelpers::extrapolateBarycentricWeights(psb); - } } - else + if (out_sim_type == UrdfGeometry::FILE_VTK) { - psb->m_renderNodes.resize(0); + btSoftBodyHelpers::interpolateBarycentricWeights(psb); } - btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); - btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); - deformWorld->addForce(psb, gravityForce); - m_data->m_lf.push_back(gravityForce); - btScalar collision_hardness = 1; - psb->m_cfg.kKHR = collision_hardness; - psb->m_cfg.kCHR = collision_hardness; - - btScalar friction_coeff = 0; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) + else if (out_sim_type == UrdfGeometry::FILE_OBJ) { - friction_coeff = loadSoftBodyArgs.m_frictionCoeff; + btSoftBodyHelpers::extrapolateBarycentricWeights(psb); } - psb->m_cfg.kDF = friction_coeff; - bool use_bending_spring = false; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) - { - use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; - if (use_bending_spring) - { - psb->generateBendingConstraints(2); - } - } - btSoftBody::Material* pm = psb->appendMaterial(); - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - - // turn on the collision flag for deformable - // collision between deformable and rigid - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - // turn on face contact only for multibodies - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; - // collion between deformable and deformable and self-collision - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; - psb->setCollisionFlags(0); - psb->setTotalMass(mass); - bool use_self_collision = false; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION) - { - use_self_collision = loadSoftBodyArgs.m_useSelfCollision; - } - psb->setSelfCollision(use_self_collision); } -#endif//SKIP_DEFORMABLE_BODY + else + { + psb->m_renderNodes.resize(0); + } + btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); + btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); + deformWorld->addForce(psb, gravityForce); + m_data->m_lf.push_back(gravityForce); + btScalar collision_hardness = 1; + psb->m_cfg.kKHR = collision_hardness; + psb->m_cfg.kCHR = collision_hardness; + + psb->m_cfg.kDF = deformable.m_friction; + if (deformable.m_springCoefficients.bending_stiffness) + { + psb->generateBendingConstraints(2); + } + btSoftBody::Material* pm = psb->appendMaterial(); + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + + // turn on the collision flag for deformable + // collision between deformable and rigid + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + // turn on face contact only for multibodies + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + // collion between deformable and deformable and self-collision + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->setCollisionFlags(0); + psb->setTotalMass(deformable.m_mass); + psb->setSelfCollision(useSelfCollision); + } +#endif //SKIP_DEFORMABLE_BODY #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); if (softWorld) @@ -8450,21 +8443,22 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar //turn on softbody vs softbody collision psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->randomizeConstraints(); - psb->setTotalMass(mass, true); + psb->setTotalMass(deformable.m_mass, true); } -#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - psb->scale(btVector3(scale, scale, scale)); - psb->rotate(initialOrn); - psb->translate(initialPos); +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + psb->scale(btVector3(scale, scale, scale)); + psb->rotate(orn); + psb->translate(pos); - psb->getCollisionShape()->setMargin(collisionMargin); - psb->getCollisionShape()->setUserPointer(psb); + psb->getCollisionShape()->setMargin(deformable.m_collisionMargin); + psb->getCollisionShape()->setUserPointer(psb); #ifndef SKIP_DEFORMABLE_BODY if (deformWorld) { deformWorld->addSoftBody(psb); - } else -#endif//SKIP_DEFORMABLE_BODY + } + else +#endif //SKIP_DEFORMABLE_BODY { btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); if (softWorld) @@ -8472,71 +8466,123 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar softWorld->addSoftBody(psb); } } - m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - bodyHandle->m_softBody = psb; + m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); + *bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId); + bodyHandle->m_softBody = psb; - b3VisualShapeData visualShape; + b3VisualShapeData visualShape; - visualShape.m_objectUniqueId = bodyUniqueId; - visualShape.m_linkIndex = -1; - visualShape.m_visualGeometryType = URDF_GEOM_MESH; - //dimensions just contains the scale - visualShape.m_dimensions[0] = scale; - visualShape.m_dimensions[1] = scale; - visualShape.m_dimensions[2] = scale; - //filename - strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); - visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; - //position and orientation - visualShape.m_localVisualFrame[0] = initialPos[0]; - visualShape.m_localVisualFrame[1] = initialPos[1]; - visualShape.m_localVisualFrame[2] = initialPos[2]; - visualShape.m_localVisualFrame[3] = initialOrn[0]; - visualShape.m_localVisualFrame[4] = initialOrn[1]; - visualShape.m_localVisualFrame[5] = initialOrn[2]; - visualShape.m_localVisualFrame[6] = initialOrn[3]; - //color and ids to be set by the renderer - visualShape.m_rgbaColor[0] = 0; - visualShape.m_rgbaColor[1] = 0; - visualShape.m_rgbaColor[2] = 0; - visualShape.m_rgbaColor[3] = 1; - visualShape.m_tinyRendererTextureId = -1; - visualShape.m_textureUniqueId =-1; - visualShape.m_openglTextureId = -1; + visualShape.m_objectUniqueId = *bodyUniqueId; + visualShape.m_linkIndex = -1; + visualShape.m_visualGeometryType = URDF_GEOM_MESH; + //dimensions just contains the scale + visualShape.m_dimensions[0] = scale; + visualShape.m_dimensions[1] = scale; + visualShape.m_dimensions[2] = scale; + //filename + strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); + visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; + //position and orientation + visualShape.m_localVisualFrame[0] = pos[0]; + visualShape.m_localVisualFrame[1] = pos[1]; + visualShape.m_localVisualFrame[2] = pos[2]; + visualShape.m_localVisualFrame[3] = orn[0]; + visualShape.m_localVisualFrame[4] = orn[1]; + visualShape.m_localVisualFrame[5] = orn[2]; + visualShape.m_localVisualFrame[6] = orn[3]; + //color and ids to be set by the renderer + visualShape.m_rgbaColor[0] = 0; + visualShape.m_rgbaColor[1] = 0; + visualShape.m_rgbaColor[2] = 0; + visualShape.m_rgbaColor[3] = 1; + visualShape.m_tinyRendererTextureId = -1; + visualShape.m_textureUniqueId = -1; + visualShape.m_openglTextureId = -1; - m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO); + m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO); - serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; - serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; - int pos = strlen(relativeFileName)-1; - while(pos>=0 && relativeFileName[pos]!='/') { pos--;} - btAssert(strlen(relativeFileName)-pos-5>0); - std::string object_name (std::string(relativeFileName).substr(pos+1, strlen(relativeFileName)- 5 - pos)); + if (!deformable.m_name.empty()) + { + bodyHandle->m_bodyName = deformable.m_name; + } + else + { + int pos = strlen(relativeFileName) - 1; + while (pos >= 0 && relativeFileName[pos] != '/') + { + pos--; + } + btAssert(strlen(relativeFileName) - pos - 5 > 0); + std::string object_name(std::string(relativeFileName).substr(pos + 1, strlen(relativeFileName) - 5 - pos)); bodyHandle->m_bodyName = object_name; + } + b3Notification notification; + notification.m_notificationType = BODY_ADDED; + notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId; + m_data->m_pluginManager.addNotification(notification); + } + return true; +} - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; +bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; + bool hasStatus = true; +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + UrdfDeformable deformable; + + constructUrdfDeformable(clientCmd, deformable, m_data->m_verboseOutput); + // const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments; + btVector3 initialPos(0, 0, 0); + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION) + { + initialPos[0] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[0]; + initialPos[1] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[1]; + initialPos[2] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[2]; + } + btQuaternion initialOrn(0, 0, 0, 1); + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION) + { + initialOrn[0] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[0]; + initialOrn[1] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[1]; + initialOrn[2] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[2]; + initialOrn[3] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[3]; + } + + double scale = 1; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE) + { + scale = clientCmd.m_loadSoftBodyArguments.m_scale; + } + bool use_self_collision = false; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION) + { + use_self_collision = clientCmd.m_loadSoftBodyArguments.m_useSelfCollision; + } + + int bodyUniqueId = -1; + bool completedOk = processDeformable(deformable, initialPos, initialOrn, &bodyUniqueId, bufferServerToClient, bufferSizeInBytes, scale, use_self_collision); + if (completedOk && bodyUniqueId > 0) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); + serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; #ifdef ENABLE_LINK_MAPPER - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize(); - } -#endif - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); - - b3Notification notification; - notification.m_notificationType = BODY_ADDED; - notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; - m_data->m_pluginManager.addNotification(notification); + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize(); } - } #endif + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; +#endif + } return hasStatus; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index ef972a400..18db2cdd7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -5,6 +5,7 @@ #include "LinearMath/btVector3.h" #include "PhysicsCommandProcessorInterface.h" +#include "../Importers/ImportURDFDemo/UrdfParser.h" struct SharedMemLines { @@ -104,6 +105,7 @@ protected: bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); bool processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, class URDFImporterInterface& u2b); + bool processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision); bool supportsJointMotor(class btMultiBody* body, int linkIndex); diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index ff02237d6..eedc1ea83 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/examples/pybullet/examples/deformable_torus.py @@ -12,6 +12,9 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2]) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5) + +bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION) + p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) p.setRealTimeSimulation(1)