From 7d8379bad45921ce0bf7c3feded6b2e6bd905016 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 14 Sep 2020 12:34:21 -0700 Subject: [PATCH 1/2] add configurable parameters to urdf --- .../Importers/ImportURDFDemo/UrdfParser.cpp | 11 ++++ .../Importers/ImportURDFDemo/UrdfParser.h | 24 +++++---- .../PhysicsServerCommandProcessor.cpp | 3 +- src/BulletSoftBody/btSoftBody.cpp | 51 ++++++++++--------- src/BulletSoftBody/btSoftBody.h | 2 + 5 files changed, 56 insertions(+), 35 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index c898fbb65..2765dff27 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1186,6 +1186,12 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, deformable.m_gravFactor = urdfLexicalCast(grav_xml->Attribute("value")); } + XMLElement* cache_barycenter = config->FirstChildElement("cache_barycenter"); + if (cache_barycenter) + { + deformable.m_cache_barycenter = true; + } + XMLElement* spring_xml = config->FirstChildElement("spring"); if (spring_xml) { @@ -1199,7 +1205,12 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, 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")); + + if (spring_xml->Attribute("bending_stride")) + deformable.m_springCoefficients.bending_stride = urdfLexicalCast(spring_xml->Attribute("bending_stride")); + } } XMLElement* corotated_xml = config->FirstChildElement("corotated"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index b11578113..efe51b704 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -200,17 +200,18 @@ struct UrdfJoint } }; - -struct SpringCoeffcients{ - double elastic_stiffness; - double damping_stiffness; - double bending_stiffness; +struct SpringCoeffcients +{ + double elastic_stiffness; + double damping_stiffness; + double bending_stiffness; int damp_all_directions; - SpringCoeffcients(): - elastic_stiffness(0.), - damping_stiffness(0.), - bending_stiffness(0.), - damp_all_directions(0){} + int bending_stride; + SpringCoeffcients() : elastic_stiffness(0.), + damping_stiffness(0.), + bending_stiffness(0.), + damp_all_directions(0), + bending_stride(2) {} }; struct LameCoefficients @@ -229,6 +230,7 @@ struct UrdfDeformable double m_friction; double m_repulsionStiffness; double m_gravFactor; + bool m_cache_barycenter; SpringCoeffcients m_springCoefficients; LameCoefficients m_corotatedCoefficients; @@ -238,7 +240,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_gravFactor(1.), m_visualFileName(""), m_simFileName("") + UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_cache_barycenter(false), m_visualFileName(""), m_simFileName("") { } }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6c933b190..603cb22d7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8783,7 +8783,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo psb->m_cfg.kDF = deformable.m_friction; if (deformable.m_springCoefficients.bending_stiffness) { - psb->generateBendingConstraints(2); + psb->generateBendingConstraints(deformable.m_springCoefficients.bending_stride); } btSoftBody::Material* pm = psb->appendMaterial(); pm->m_flags -= btSoftBody::fMaterial::DebugDraw; @@ -8802,6 +8802,7 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo psb->setSelfCollision(useSelfCollision); psb->setSpringStiffness(deformable.m_repulsionStiffness); psb->setGravityFactor(deformable.m_gravFactor); + psb->setCacheBarycenter(deformable.m_cache_barycenter); psb->initializeFaceTree(); } #endif //SKIP_DEFORMABLE_BODY diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index cc0c89ab4..39359f3ad 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -749,7 +749,7 @@ void btSoftBody::addAeroForceToNode(const btVector3& windVelocity, int nodeIndex fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack - // cos(10º) = 0.98480 + // cos(10º) = 0.98480 if (0 < n_dot_v && n_dot_v < 0.98480f) fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); @@ -835,7 +835,7 @@ void btSoftBody::addAeroForceToFace(const btVector3& windVelocity, int faceIndex fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); // Check angle of attack - // cos(10º) = 0.98480 + // cos(10º) = 0.98480 if (0 < n_dot_v && n_dot_v < 0.98480f) fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f - n_dot_v * n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); @@ -2821,7 +2821,6 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btGjkEpaSolver2::sResults results; // #define USE_QUADRATURE 1 -//#define CACHE_PREV_COLLISION // use collision quadrature point #ifdef USE_QUADRATURE @@ -2872,30 +2871,31 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO if (dst >= 0) return false; -// Use consistent barycenter to recalculate distance. -#ifdef CACHE_PREV_COLLISION - if (f.m_pcontact[3] != 0) + // Use consistent barycenter to recalculate distance. + if (this->m_cacheBarycenter) { - for (int i = 0; i < 3; ++i) - bary[i] = f.m_pcontact[i]; - contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); - const btConvexShape* csh = static_cast(shp); - btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results); - cti.m_colObj = colObjWrap->getCollisionObject(); - dst = results.distance; - cti.m_normal = results.normal; - cti.m_offset = dst; + if (f.m_pcontact[3] != 0) + { + for (int i = 0; i < 3; ++i) + bary[i] = f.m_pcontact[i]; + contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary); + const btConvexShape* csh = static_cast(shp); + btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results); + cti.m_colObj = colObjWrap->getCollisionObject(); + dst = results.distance; + cti.m_normal = results.normal; + cti.m_offset = dst; - //point-convex CD - wtr = colObjWrap->getWorldTransform(); - btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); - triangle_transform.setOrigin(f.m_n[0]->m_x); - btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); + //point-convex CD + wtr = colObjWrap->getWorldTransform(); + btTriangleShape triangle2(btVector3(0, 0, 0), f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); + triangle_transform.setOrigin(f.m_n[0]->m_x); + btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results); - dst = results.distance - csh->getMargin() - margin; - return true; + dst = results.distance - csh->getMargin() - margin; + return true; + } } -#endif // Use triangle-convex CD. wtr = colObjWrap->getWorldTransform(); @@ -3445,6 +3445,11 @@ void btSoftBody::setGravityFactor(btScalar gravFactor) m_gravityFactor = gravFactor; } +void btSoftBody::setCacheBarycenter(bool cacheBarycenter) +{ + m_cacheBarycenter = cacheBarycenter; +} + void btSoftBody::initializeDmInverse() { btScalar unit_simplex_measure = 1. / 6.; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 7ee30db9f..f578487b8 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -840,6 +840,7 @@ public: btAlignedObjectArray m_quads; // quadrature points for collision detection btScalar m_repulsionStiffness; btScalar m_gravityFactor; + bool m_cacheBarycenter; btAlignedObjectArray m_X; // initial positions btAlignedObjectArray m_renderNodesInterpolationWeights; @@ -1189,6 +1190,7 @@ public: void dampClusters(); void setSpringStiffness(btScalar k); void setGravityFactor(btScalar gravFactor); + void setCacheBarycenter(bool cacheBarycenter); void initializeDmInverse(); void updateDeformation(); void advanceDeformation(); From f51e92000a9ce610f634fa96975f577cf47cb8f8 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 17 Sep 2020 15:38:23 -0700 Subject: [PATCH 2/2] initialize default value --- src/BulletSoftBody/btSoftBody.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 39359f3ad..d1980ea6c 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -229,6 +229,7 @@ void btSoftBody::initDefaults() m_maxSpeedSquared = 0; m_repulsionStiffness = 0.5; m_gravityFactor = 1; + m_cacheBarycenter = false; m_fdbvnt = 0; }