From 7bce5d61f485d8f17c3c7d0400b550ff4e0e4822 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 27 Nov 2019 11:40:10 -0800 Subject: [PATCH] separate in-plane and bending stiffness for mass spring model for easier parameter tuning --- examples/SharedMemory/PhysicsClientC_API.cpp | 5 +-- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsServerCommandProcessor.cpp | 33 +++++++++++-------- examples/SharedMemory/SharedMemoryCommands.h | 25 +++++++------- examples/pybullet/pybullet.c | 7 ++-- .../btDeformableMassSpringForce.h | 13 +++++--- 6 files changed, 50 insertions(+), 35 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ee506c143..530634435 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -422,11 +422,12 @@ B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHand return 0; } -B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings) +B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); - command->m_loadSoftBodyArguments.m_useBendingSprings = useBendingSprings; + command->m_loadSoftBodyArguments.m_useBendingSprings = useBendingSprings; + command->m_loadSoftBodyArguments.m_springBendingStiffness = bendingStiffness; command->m_updateFlags |= LOAD_SOFT_BODY_ADD_BENDING_SPRINGS; return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index c67f79d9c..a6070ed95 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -643,7 +643,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyUseSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision); B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); - B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings); + B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings, double bendingStiffness); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3565c4d5..a321001cc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8070,7 +8070,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin; } - + btScalar spring_bending_stiffness = 0; { btSoftBody* psb = NULL; @@ -8137,7 +8137,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar } } #ifndef SKIP_DEFORMABLE_BODY - btScalar spring_elastic_stiffness, spring_damping_stiffness; + btScalar spring_elastic_stiffness, spring_damping_stiffness; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) { btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); @@ -8145,7 +8145,11 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false); + 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, false, spring_bending_stiffness); deformWorld->addForce(psb, springForce); m_data->m_lf.push_back(springForce); } @@ -8178,12 +8182,16 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar deformWorld->addForce(psb, neohookeanForce); m_data->m_lf.push_back(neohookeanForce); } - btScalar spring_elastic_stiffness, spring_damping_stiffness; + 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; - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true); + 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); } @@ -8258,19 +8266,18 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar friction_coeff = loadSoftBodyArgs.m_frictionCoeff; } psb->m_cfg.kDF = friction_coeff; - - bool use_bending_spring = true; + bool use_bending_spring = false; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) { - use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; + use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; + if (use_bending_spring) + { + psb->generateBendingConstraints(2); + } } btSoftBody::Material* pm = psb->appendMaterial(); pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - if (use_bending_spring) - { - psb->generateBendingConstraints(2,pm); - } - + // turn on the collision flag for deformable // collision between deformable and rigid psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ccb6357fe..958cf38a4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -523,18 +523,19 @@ struct LoadSoftBodyArgs double m_mass; double m_collisionMargin; double m_initialPosition[3]; - double m_initialOrientation[4]; - double m_springElasticStiffness; - double m_springDampingStiffness; - double m_corotatedMu; - double m_corotatedLambda; - int m_useBendingSprings; - double m_collisionHardness; - double m_useSelfCollision; - double m_frictionCoeff; - double m_NeoHookeanMu; - double m_NeoHookeanLambda; - double m_NeoHookeanDamping; + double m_initialOrientation[4]; + double m_springElasticStiffness; + double m_springDampingStiffness; + double m_springBendingStiffness; + double m_corotatedMu; + double m_corotatedLambda; + int m_useBendingSprings; + double m_collisionHardness; + double m_useSelfCollision; + double m_frictionCoeff; + double m_NeoHookeanMu; + double m_NeoHookeanLambda; + double m_NeoHookeanDamping; int m_useFaceContact; char m_simFileName[MAX_FILENAME_LENGTH]; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0857ea0da..53b119478 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1973,7 +1973,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* int physicsClientId = 0; int flags = 0; - static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", NULL}; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", NULL}; int bodyUniqueId = -1; const char* fileName = ""; @@ -1985,6 +1985,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* int useNeoHookean = 0; double springElasticStiffness = 1; double springDampingStiffness = 0.1; + double springBendingStiffness = 0.1; double NeoHookeanMu = 1; double NeoHookeanLambda = 1; double NeoHookeanDamping = 0.1; @@ -2002,7 +2003,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* PyObject* basePosObj = 0; PyObject* baseOrnObj = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiiddddddii", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiidddddddii", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision)) { return NULL; } @@ -2058,7 +2059,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* if (useMassSpring) { b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness); - b3LoadSoftBodyUseBendingSprings(command, useBendingSprings); + b3LoadSoftBodyUseBendingSprings(command, useBendingSprings, springBendingStiffness); } if (useNeoHookean) { diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 44ac91c62..54b4e4481 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -23,14 +23,18 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce // If true, the damping force will be in the direction of the spring // If false, the damping force will be in the direction of the velocity bool m_momentum_conserving; - btScalar m_elasticStiffness, m_dampingStiffness; + btScalar m_elasticStiffness, m_dampingStiffness, m_bendingStiffness; public: typedef btAlignedObjectArray TVStack; btDeformableMassSpringForce() : m_momentum_conserving(false), m_elasticStiffness(1), m_dampingStiffness(0.05) { } - btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d) + btDeformableMassSpringForce(btScalar k, btScalar d, bool conserve_angular = true, double bending_k = -1) : m_momentum_conserving(conserve_angular), m_elasticStiffness(k), m_dampingStiffness(d), m_bendingStiffness(bending_k) { + if (m_bendingStiffness < btScalar(0)) + { + m_bendingStiffness = m_elasticStiffness; + } } virtual void addScaledForces(btScalar scale, TVStack& force) @@ -103,7 +107,8 @@ public: // elastic force btVector3 dir = (node2->m_q - node1->m_q); btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); + btScalar scaled_stiffness = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); + btVector3 scaled_force = scaled_stiffness * (dir - dir_normalized * r); force[id1] += scaled_force; force[id2] -= scaled_force; } @@ -212,7 +217,6 @@ public: { continue; } - btScalar scaled_k = m_elasticStiffness * scale; for (int j = 0; j < psb->m_links.size(); ++j) { const btSoftBody::Link& link = psb->m_links[j]; @@ -227,6 +231,7 @@ public: btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); btVector3 dx_diff = dx[id1] - dx[id2]; btVector3 scaled_df = btVector3(0,0,0); + btScalar scaled_k = scale * (link.m_bbending ? m_bendingStiffness : m_elasticStiffness); if (dir_norm > SIMD_EPSILON) { scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized;