From a92a8f1135eb023b263005fcbdb7d0f72a2c4d81 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 16 Sep 2019 16:04:59 -0700 Subject: [PATCH] add demo for deformable contact --- examples/DeformableDemo/DeformableContact.cpp | 257 +++++++++++++++++- examples/DeformableDemo/DeformableContact.h | 26 +- examples/DeformableDemo/GraspDeformable.cpp | 84 +----- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 2 + 5 files changed, 284 insertions(+), 87 deletions(-) diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index b2a0c9efa..381bd130b 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -1,8 +1,251 @@ -// -// DeformableContact.cpp -// App_BulletExampleBrowser -// -// Created by Xuchen Han on 9/16/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "DeformableContact.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The DeformableContact shows the use of rolling friction. +///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. +///Generally it is best to leave the rolling friction coefficient zero (or close to zero). +class DeformableContact : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + DeformableContact(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableContact() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 12; + float pitch = -50; + float yaw = 120; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void DeformableContact::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150), btScalar(25.), btScalar(150))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -32, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(1); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + btScalar s = 4; + btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 20,20, + 1 + 2 + 4 + 8, true); + + psb->getCollisionShape()->setMargin(0.05); + psb->generateBendingConstraints(2); + 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; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(20,0.1, true); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + + + h = 2; + s = 2; + btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 10,10, + 0, true); + psb2->getCollisionShape()->setMargin(0.05); + psb2->generateBendingConstraints(2); + psb2->setTotalMass(1); + psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb2->m_cfg.kCHR = 1; // collision hardness with rigid body + psb2->m_cfg.kDF = 2; + psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->translate(btVector3(3,0,0)); + getDeformableDynamicsWorld()->addSoftBody(psb2); + + btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(20,0.1, true); + getDeformableDynamicsWorld()->addForce(psb2, mass_spring2); + m_forces.push_back(mass_spring2); + + btDeformableGravityForce* gravity_force2 = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb2, gravity_force2); + m_forces.push_back(gravity_force2); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableContact::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + +class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableContact(options.m_guiHelper); +} + -#include "DeformableContact.hpp" diff --git a/examples/DeformableDemo/DeformableContact.h b/examples/DeformableDemo/DeformableContact.h index 3183bbedc..1e1fe5676 100644 --- a/examples/DeformableDemo/DeformableContact.h +++ b/examples/DeformableDemo/DeformableContact.h @@ -1,13 +1,19 @@ -// -// DeformableContact.hpp -// App_BulletExampleBrowser -// -// Created by Xuchen Han on 9/16/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ -#ifndef DeformableContact_hpp -#define DeformableContact_hpp +#ifndef DEFORMABLE_CONTACT_H +#define DEFORMABLE_CONTACT_H -#include +class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options); -#endif /* DeformableContact_hpp */ +#endif //_DEFORMABLE_CONTACT_H diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 2b9905f9a..514ee5cc5 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -103,11 +103,6 @@ public: btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; if (motor) { -// if (dofIndex == 10 || dofIndex == 11) -// { -// motor->setVelocityTarget(fingerTargetVelocities[1], 1); -// motor->setMaxAppliedImpulse(1); -// } if (dofIndex == 6) { motor->setVelocityTarget(-fingerTargetVelocities[1], 1); @@ -118,7 +113,6 @@ public: motor->setVelocityTarget(fingerTargetVelocities[1], 1); motor->setMaxAppliedImpulse(2); } -// motor->setRhsClamp(SIMD_INFINITY); motor->setMaxAppliedImpulse(1); } } @@ -127,9 +121,8 @@ public: } //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 250.f; + float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); - btSoftBodyHelpers::writeObj("/Users/xuchenhan/Desktop/paper.obj", getDeformableDynamicsWorld()->getSoftBodyArray()[0]); } void createGrip() @@ -193,54 +186,11 @@ void GraspDeformable::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); -// deformableBodySolver->setWorld(getDeformableDynamicsWorld()); btVector3 gravity = btVector3(0, -9.81, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); -// // load a gripper -// { -// btTransform rootTrans; -// rootTrans.setIdentity(); -// BulletURDFImporter u2b(m_guiHelper,0,0,50,0); -// bool forceFixedBase = false; -// bool loadOk = u2b.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", forceFixedBase); -// if (loadOk) -// { -// for (int m = 0; m < u2b.getNumModels(); m++) -// { -// u2b.activateModel(m); -// -// btMultiBody* mb = 0; -// -// MyMultiBodyCreator creation(m_guiHelper); -// -// u2b.getRootTransformInWorld(rootTrans); -// ConvertURDF2Bullet(u2b, creation, rootTrans, getDeformableDynamicsWorld(), true, u2b.getPathPrefix(), CUF_USE_SDF+CUF_RESERVED); -// mb = creation.getBulletMultiBody(); -// -// int numLinks = mb->getNumLinks(); -// for (int i = 0; i < numLinks; i++) -// { -// int mbLinkIndex = i; -// float maxMotorImpulse = 1.f; -// -// if (supportsJointMotor(mb, mbLinkIndex)) -// { -// int dof = 0; -// btScalar desiredVelocity = 0.f; -// btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse); -// motor->setPositionTarget(0, 0); -// motor->setVelocityTarget(0, 1); -// mb->getLink(mbLinkIndex).m_userPtr = motor; -// getDeformableDynamicsWorld()->addMultiBodyConstraint(motor); -// motor->finalizeMultiDof(); -// } -// } -// } -// } -// } // build a gripper { bool damping = true; @@ -274,7 +224,6 @@ void GraspDeformable::initPhysics() } } - if (!damping) { mbC->setLinearDamping(0.0f); @@ -288,10 +237,9 @@ void GraspDeformable::initPhysics() btScalar q0 = 0.f * SIMD_PI / 180.f; if (numLinks > 0) mbC->setJointPosMultiDof(0, &q0); - /// addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents); - } + //create a ground { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); @@ -326,13 +274,13 @@ void GraspDeformable::initPhysics() { char relative_path[1024]; // b3FileUtils::findFile("banana.vtk", relative_path, 1024); -// b3FileUtils::findFile("ball.vtk", relative_path, 1024); - b3FileUtils::findFile("paper_collision.vtk", relative_path, 1024); + b3FileUtils::findFile("ball.vtk", relative_path, 1024); +// b3FileUtils::findFile("paper_collision.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); -// b3FileUtils::findFile("bread.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); +// b3FileUtils::findFile("bread.vtk", relative_path, 1024); // b3FileUtils::findFile("ditto.vtk", relative_path, 1024); // b3FileUtils::findFile("boot.vtk", relative_path, 1024); // btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), @@ -343,28 +291,24 @@ void GraspDeformable::initPhysics() btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path); // psb->scale(btVector3(30, 30, 30)); // for banana -// psb->scale(btVector3(.25, .25, .25)); -// psb->scale(btVector3(2, 2, 2)); + psb->scale(btVector3(.05, .05, .05)); +// psb->scale(btVector3(2, 2, 2)); // psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot // psb->scale(btVector3(1, 1, 1)); // for ditto -// psb->translate(btVector3(.25, 0, 0.4)); + psb->translate(btVector3(.25, 2, 0.4)); psb->getCollisionShape()->setMargin(0.02); - psb->setTotalMass(.1); + psb->setTotalMass(.01); 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.kDF = 20; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); -// btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, true); -// getDeformableDynamicsWorld()->addForce(psb, mass_spring); -// m_forces.push_back(mass_spring); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(250,500); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 87f47f162..d3e4b3f8c 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -353,6 +353,8 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/MultiBodyConstraintFeedback.cpp ../SoftDemo/SoftDemo.cpp ../SoftDemo/SoftDemo.h + ../DeformableDemo/DeformableContact.cpp + ../DeformableDemo/DeformableContact.h ../DeformableDemo/GraspDeformable.cpp ../DeformableDemo/GraspDeformable.h ../DeformableDemo/Pinch.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 1c8fa7921..ee04ece90 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -49,6 +49,7 @@ #include "../DeformableDemo/DeformableMultibody.h" #include "../DeformableDemo/VolumetricDeformable.h" #include "../DeformableDemo/GraspDeformable.h" +#include "../DeformableDemo/DeformableContact.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -186,6 +187,7 @@ static ExampleEntry gDefaultExamples[] = //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), ExampleEntry(0, "Deformabe Body"), + ExampleEntry(1, "Deformable-Deformable Contact", "Deformable contact", DeformableContactCreateFunc), ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc), ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),