diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index a37e610edc..c898fbb65a 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1174,6 +1174,17 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, } deformable.m_repulsionStiffness = urdfLexicalCast(repulsion_xml->Attribute("value")); } + + XMLElement* grav_xml = config->FirstChildElement("gravity_factor"); + if (grav_xml) + { + if (!grav_xml->Attribute("value")) + { + logger->reportError("gravity_factor element must have value attribute"); + return false; + } + deformable.m_gravFactor = urdfLexicalCast(grav_xml->Attribute("value")); + } XMLElement* spring_xml = config->FirstChildElement("spring"); if (spring_xml) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 7cd87c49a8..b11578113c 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -228,6 +228,7 @@ struct UrdfDeformable double m_collisionMargin; double m_friction; double m_repulsionStiffness; + double m_gravFactor; SpringCoeffcients m_springCoefficients; LameCoefficients m_corotatedCoefficients; @@ -237,7 +238,7 @@ struct UrdfDeformable std::string m_simFileName; btHashMap m_userData; - UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_visualFileName(""), m_simFileName("") + UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_visualFileName(""), m_simFileName("") { } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d764f26e60..7e6ba5097f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -413,6 +413,15 @@ B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandl return 0; } +B3_SHARED_API int b3LoadSoftBodySetGravityFactor(b3SharedMemoryCommandHandle commandHandle, double gravFactor) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_gravFactor = gravFactor; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_GRAVITY_FACTOR; + return 0; +} + B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 13e6e3ab1f..16836ddd26 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8428,7 +8428,10 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe { deformable.m_repulsionStiffness = loadSoftBodyArgs.m_repulsionStiffness; } - + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_GRAVITY_FACTOR) + { + deformable.m_gravFactor = loadSoftBodyArgs.m_gravFactor; + } #endif } @@ -8641,14 +8644,17 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo // 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 + // turn on face contact for multibodies psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + /// turn on face contact for rigid body + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; // 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); psb->setSpringStiffness(deformable.m_repulsionStiffness); + psb->setGravityFactor(deformable.m_gravFactor); psb->initializeFaceTree(); } #endif //SKIP_DEFORMABLE_BODY diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 66a9d4494a..4df72e31a2 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -515,6 +515,7 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_SIM_MESH = 1<<15, LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16, LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE = 1<<17, + LOAD_SOFT_BODY_SET_GRAVITY_FACTOR = 1<<18, }; enum EnumSimParamInternalSimFlags @@ -549,6 +550,7 @@ struct LoadSoftBodyArgs int m_useFaceContact; char m_simFileName[MAX_FILENAME_LENGTH]; double m_repulsionStiffness; + double m_gravFactor; }; struct b3LoadSoftBodyResultArgs diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 13ee3eacb6..50a6584b5d 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -68,7 +68,7 @@ class btDeformableGravityForce : public btDeformableLagrangianForce btSoftBody::Node& n = psb->m_nodes[j]; size_t id = n.index; btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; - btVector3 scaled_force = scale * m_gravity * mass; + btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor; force[id] += scaled_force; } } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 49d1c9fd50..59b5365ca3 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -228,6 +228,7 @@ void btSoftBody::initDefaults() m_softSoftCollision = false; m_maxSpeedSquared = 0; m_repulsionStiffness = 0.5; + m_gravityFactor = 1; m_fdbvnt = 0; } @@ -3445,6 +3446,11 @@ void btSoftBody::setSpringStiffness(btScalar k) m_repulsionStiffness = k; } +void btSoftBody::setGravityFactor(btScalar gravFactor) +{ + m_gravityFactor = gravFactor; +} + void btSoftBody::initializeDmInverse() { btScalar unit_simplex_measure = 1./6.; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 1bd8334bcc..2248673058 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -821,6 +821,7 @@ class btSoftBody : public btCollisionObject btScalar m_maxSpeedSquared; btAlignedObjectArray m_quads; // quadrature points for collision detection btScalar m_repulsionStiffness; + btScalar m_gravityFactor; btAlignedObjectArray m_X; // initial positions btAlignedObjectArray m_renderNodesInterpolationWeights; @@ -1169,6 +1170,7 @@ class btSoftBody : public btCollisionObject void applyClusters(bool drift); void dampClusters(); void setSpringStiffness(btScalar k); + void setGravityFactor(btScalar gravFactor); void initializeDmInverse(); void updateDeformation(); void advanceDeformation();