From 7d1b93cc1766c64cc4a510a3ce29598f6614676a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 28 Aug 2019 10:01:14 -0700 Subject: [PATCH] contact solve for newton --- data/single_tet.vtk | 15 ++++++++++ examples/DeformableDemo/DeformableRigid.cpp | 4 +-- examples/DeformableDemo/GraspDeformable.cpp | 24 +++++++-------- .../btDeformableBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 19 ++++++++++-- src/BulletSoftBody/btDeformableBodySolver.h | 1 + .../btDeformableMultiBodyDynamicsWorld.cpp | 29 +++++++++---------- .../btDeformableMultiBodyDynamicsWorld.h | 4 +-- src/BulletSoftBody/btSoftBody.h | 3 +- 9 files changed, 63 insertions(+), 38 deletions(-) create mode 100644 data/single_tet.vtk diff --git a/data/single_tet.vtk b/data/single_tet.vtk new file mode 100644 index 000000000..94cf9fe64 --- /dev/null +++ b/data/single_tet.vtk @@ -0,0 +1,15 @@ +# vtk DataFile Version 2.0 +ball_, Created by Gmsh +ASCII +DATASET UNSTRUCTURED_GRID +POINTS 4 double +0 0 0 +1 0 0 +0 1 0 +0 0 1 + +CELLS 1 1 +4 0 1 2 3 + +CELL_TYPES 1 +10 diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 3cbfba451..850f72600 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -234,11 +234,11 @@ void DeformableRigid::initPhysics() psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 1; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.01, false); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index c97aa0d3c..34127774d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -325,7 +325,8 @@ void GraspDeformable::initPhysics() { char relative_path[1024]; // b3FileUtils::findFile("banana.vtk", relative_path, 1024); - b3FileUtils::findFile("ball.vtk", relative_path, 1024); + b3FileUtils::findFile("ball.vtk", relative_path, 1024); +// b3FileUtils::findFile("single_tet.vtk", relative_path, 1024); // b3FileUtils::findFile("tube.vtk", relative_path, 1024); // b3FileUtils::findFile("torus.vtk", relative_path, 1024); // b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); @@ -343,21 +344,16 @@ void GraspDeformable::initPhysics() psb->scale(btVector3(.25, .25, .25)); // psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot // psb->scale(btVector3(1, 1, 1)); // for ditto -// psb->translate(btVector3(0, 0, 2)); for boot - psb->getCollisionShape()->setMargin(0.01); + psb->translate(btVector3(.25, 0, 0.4)); + psb->getCollisionShape()->setMargin(0.02); psb->setTotalMass(.1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); - psb->getWorldInfo()->m_maxDisplacement = .1f; - // nonlinear damping -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.04, true)); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(0,6)); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,0.04, true); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); @@ -365,7 +361,7 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,10); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } @@ -415,8 +411,8 @@ void GraspDeformable::initPhysics() { SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity); - slider.m_minVal = -.1; - slider.m_maxVal = .1; + slider.m_minVal = -.3; + slider.m_maxVal = .3; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } @@ -470,8 +466,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor { //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 1.f; - float linkMass = 1.f; + float baseMass = 100.f; + float linkMass = 100.f; int numLinks = 2; if (baseMass) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 2a2ff5c63..b66a02aff 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -83,7 +83,7 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { - // only the velocity of the constrained nodes needs to be updated during CG solve + // only the velocity of the constrained nodes needs to be updated during contact solve for (int i = 0; i < projection.m_constraints.size(); ++i) { int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 30424e7c3..600dbafa4 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -67,7 +67,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { break; } -// m_objective->applyDynamicFriction(m_residual); + m_objective->applyDynamicFriction(m_residual); computeStep(m_ddv, m_residual); updateDv(); for (int j = 0; j < m_numNodes; ++j) @@ -189,6 +189,20 @@ void btDeformableBodySolver::backupVelocity() } } +void btDeformableBodySolver::backupVn() +{ + int counter = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn; + m_backupVelocity[counter++] = psb->m_nodes[j].m_vn; + } + } +} + void btDeformableBodySolver::revertVelocity() { int counter = 0; @@ -246,8 +260,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; - n.m_q = n.m_x; - n.m_q += n.m_v * dt; + n.m_q = n.m_x + n.m_v * dt; } /* Bounds */ psb->updateBounds(); diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index a1107de03..fed15b2b2 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -75,6 +75,7 @@ public: void predictDeformableMotion(btSoftBody* psb, btScalar dt); void backupVelocity(); + void backupVn(); void revertVelocity(); void updateVelocity(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 7ddecdcd5..3fff883c9 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -168,6 +168,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } node.m_x = node.m_x + timeStep * node.m_v; node.m_q = node.m_x; + node.m_vn = node.m_v; } } m_deformableBodySolver->revertVelocity(); @@ -175,11 +176,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - if (!m_implicit) - { - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); - } + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); @@ -187,11 +185,13 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) if (m_implicit) { - // revert to v_n after collisions are registered - m_deformableBodySolver->revertVelocity(); - // todo @xuchenhan : think about whether contact solve should be done with v_n velocity or v_{n+1}^* velocity. It's using v_n for implicit and v_{n+1}^* for non-implicit now. + // at this point dv = v_{n+1} - v_{n+1}^* + // modify dv such that dv = v_{n+1} - v_n + // modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton + m_deformableBodySolver->backupVn(); } - // At this point, dv is golden for nodes in contact + + // At this point, dv should be golden for nodes in contact m_deformableBodySolver->solveDeformableConstraints(timeStep); } @@ -211,7 +211,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints() // build islands m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); - // write the constraint information of each island to the callback, and also setup the constraints in solver } } @@ -317,11 +316,11 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) m_internalTime += timeStep; m_deformableBodySolver->setImplicit(m_implicit); m_deformableBodySolver->reinitialize(m_softBodies, timeStep); - if (m_implicit) - { - // backup v_n velocity - m_deformableBodySolver->backupVelocity(); - } +// if (m_implicit) +// { +// // todo: backup v_n velocity somewhere else +// m_deformableBodySolver->backupVelocity(); +// } btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 219cbfb27..972f05664 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -73,7 +73,7 @@ public: m_sbi.m_broadphase = pairCache; m_sbi.m_dispatcher = dispatcher; m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025); + m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025); m_sbi.m_sparsesdf.Reset(); m_sbi.air_density = (btScalar)1.2; @@ -82,7 +82,7 @@ public: m_sbi.water_normal = btVector3(0, 0, 0); m_sbi.m_gravity.setValue(0, -10, 0); m_internalTime = 0.0; - m_implicit = false; + m_implicit = true; } void setSolverCallback(btSolverCallback cb) diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index cd235c29d..5371e172d 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -251,8 +251,9 @@ public: struct Node : Feature { btVector3 m_x; // Position - btVector3 m_q; // Previous step position + btVector3 m_q; // Previous step position/Test position btVector3 m_v; // Velocity + btVector3 m_vn; // Previous step velocity btVector3 m_f; // Force accumulator btVector3 m_n; // Normal btScalar m_im; // 1/mass