modified benchmark example

3 files changed, 252 insertions, 376 deletions

diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp
index 1b4c44469..6370a4199 100644
--- a/examples/ReducedDeformableDemo/FreeFall.cpp
+++ b/examples/ReducedDeformableDemo/FreeFall.cpp
@@ -55,18 +55,18 @@ public:
 
     void resetCamera()
     {
-        float dist = 6;
-        float pitch = -10;
-        float yaw = 90;
+        // float dist = 6;
+        // float pitch = -10;
+        // float yaw = 90;
+        // float targetPos[3] = {0, 2, 0};
+        float dist = 10;
+        float pitch = -30;
+        float yaw = 125;
         float targetPos[3] = {0, 2, 0};
-        // float dist = 20;
-        // float pitch = -30;
-        // float yaw = 125;
-        // float targetPos[3] = {-2, 0, 2};
         m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
     }
     
-    void Ctor_RbUpStack()
+    void Ctor_RbUpStack(const btVector3& origin)
     {
         float mass = 10;
         btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
@@ -77,12 +77,41 @@ public:
         // btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
         // rb0->setLinearVelocity(btVector3(0, 0, 0));
 
-        startTransform.setOrigin(btVector3(0,10,0));
+        startTransform.setOrigin(origin);
         // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
         btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
         rb1->setActivationState(DISABLE_DEACTIVATION);
         // rb1->setLinearVelocity(btVector3(0, 0, 4));
     }
+
+    void createReducedDeformableObject(const btVector3& origin, const btQuaternion& rotation)
+    {   
+        btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.01);
+        // rsb->scale(btVector3(1, 1, 0.5));
+
+        rsb->setTotalMass(10);
+
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(origin);
+        init_transform.setRotation(rotation);
+        rsb->transformTo(init_transform);
+
+        rsb->setStiffnessScale(5);
+        rsb->setDamping(damping_alpha, damping_beta);
+        // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
     
     void stepSimulation(float deltaTime)
     {
@@ -143,73 +172,24 @@ void FreeFall::initPhysics()
     m_dynamicsWorld->setGravity(gravity);
     m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
     // m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
-
-    // create volumetric reduced deformable body
-    {   
-        btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
-
-        getDeformableDynamicsWorld()->addSoftBody(rsb);
-        rsb->getCollisionShape()->setMargin(0.01);
-        // rsb->scale(btVector3(1, 1, 0.5));
-
-        rsb->setTotalMass(10);
-
-        btTransform init_transform;
-        init_transform.setIdentity();
-        init_transform.setOrigin(btVector3(0, 4, 0));
-        init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
-        rsb->transformTo(init_transform);
-
-        rsb->setStiffnessScale(5);
-        rsb->setDamping(damping_alpha, damping_beta);
-        // rsb->scale(btVector3(0.5, 0.5, 0.5));
-
-        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
-        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        rsb->m_cfg.kDF = 0;
-        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
-        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
-        rsb->m_sleepingThreshold = 0;
-        btSoftBodyHelpers::generateBoundaryFaces(rsb);
-        
-        // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
-        // rsb->setRigidVelocity(btVector3(0, 0, 1));
-        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
-    }
-
-    {   
-        btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
-
-        getDeformableDynamicsWorld()->addSoftBody(rsb);
-        rsb->getCollisionShape()->setMargin(0.01);
-        // rsb->scale(btVector3(1, 1, 0.5));
-
-        rsb->setTotalMass(10);
-
-        btTransform init_transform;
-        init_transform.setIdentity();
-        init_transform.setOrigin(btVector3(4, 4, 0));
-        init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
-        rsb->transformTo(init_transform);
-
-        rsb->setStiffnessScale(5);
-        rsb->setDamping(damping_alpha, damping_beta);
-        // rsb->scale(btVector3(0.5, 0.5, 0.5));
-
-        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
-        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        rsb->m_cfg.kDF = 0;
-        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
-        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
-        rsb->m_sleepingThreshold = 0;
-        btSoftBodyHelpers::generateBoundaryFaces(rsb);
-        
-        // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
-        // rsb->setRigidVelocity(btVector3(0, 0, 1));
-        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
-    }
-    // add a few rigid bodies
-    Ctor_RbUpStack();
+    
+    // 3x3 torus
+    createReducedDeformableObject(btVector3(4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(0, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(0, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(0, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(-4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(-4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createReducedDeformableObject(btVector3(-4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+
+    // Ctor_RbUpStack(btVector3(0, 10, 0));
+    // Ctor_RbUpStack(btVector3(0, 10, 4));
+    // Ctor_RbUpStack(btVector3(0, 10, 4));
+    // Ctor_RbUpStack(btVector3(-4, 10, 0));
+    // Ctor_RbUpStack(btVector3(4, 10, 0));
+    
     // create a static rigid box as the ground
     {
         // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
diff --git a/examples/ReducedDeformableDemo/ReducedGraspBenchmark.cpp b/examples/ReducedDeformableDemo/ReducedGraspBenchmark.cpp
index 2efc0c9e3..5c5ea07a4 100644
--- a/examples/ReducedDeformableDemo/ReducedGraspBenchmark.cpp
+++ b/examples/ReducedDeformableDemo/ReducedGraspBenchmark.cpp
@@ -28,11 +28,13 @@
 
 static btScalar damping_alpha = 0.0;
 static btScalar damping_beta = 0.0001;
+static btScalar COLLIDING_VELOCITY = 0;
 static int num_modes = 20;
-static bool run_reduced = false;
+static bool run_reduced = true;
 
 class ReducedGraspBenchmark : public CommonDeformableBodyBase
 {
+    btVector3 m_gravity;
 public:
     ReducedGraspBenchmark(struct GUIHelperInterface* helper)
         : CommonDeformableBodyBase(helper)
@@ -43,185 +45,166 @@ public:
     }
     void initPhysics();
 
-	void exitPhysics();
+    void exitPhysics();
 
-	void resetCamera()
-	{
+    // TODO: disable pick force, non-interactive for now.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        // float dist = 6;
+        // float pitch = -10;
+        // float yaw = 90;
+        // float targetPos[3] = {0, 2, 0};
         float dist = 10;
-        float pitch = -10;
-        float yaw = 90;
-        
-        float targetPos[3] = {0, 0, 0};
-		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
-	}
+        float pitch = -30;
+        float yaw = 125;
+        float targetPos[3] = {0, 2, 0};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
     
-    void stepSimulation(float deltaTime)
+    void Ctor_RbUpStack(const btVector3& origin)
     {
-        //use a smaller internal timestep for better stability
-        float internalTimeStep = 1. / 240.f;
-        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+        float mass = 10;
+        btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
+        // btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
+        btTransform startTransform;
+        startTransform.setIdentity();
+        // startTransform.setOrigin(btVector3(0, 12, 0));
+        // btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
+        // rb0->setLinearVelocity(btVector3(0, 0, 0));
+
+        startTransform.setOrigin(origin);
+        // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
+        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+        rb1->setActivationState(DISABLE_DEACTIVATION);
+        // rb1->setLinearVelocity(btVector3(0, 0, 4));
     }
-    
-    void createGrip()
+
+    void createDeform(const btVector3& origin, const btQuaternion& rotation)
     {
-        int count = 2;
-        float mass = 1e6;
-        btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 0.25));
-        {
-            btTransform startTransform;
-            startTransform.setIdentity();
-            startTransform.setOrigin(btVector3(0,1,0));
-            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
-            createRigidBody(mass, startTransform, shape);
-        }
-        {
-            btTransform startTransform;
-            startTransform.setIdentity();
-            startTransform.setOrigin(btVector3(0,1,-4));
-            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
-            createRigidBody(mass, startTransform, shape);
-        }
+
+        if (run_reduced)
+    {   
+        btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.01);
+        // rsb->scale(btVector3(1, 1, 0.5));
+
+        rsb->setTotalMass(10);
+
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(origin);
+        init_transform.setRotation(rotation);
+        rsb->transformTo(init_transform);
+
+        rsb->setStiffnessScale(5);
+        rsb->setDamping(damping_alpha, damping_beta);
+        // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+
+        std::cout << "Running reduced deformable\n";
+    }
+    else    // create full deformable cube
+    {
+        std::string filepath("../../../data/reduced_torus/");
+        std::string filename = filepath + "torus_mesh.vtk";
+        btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
+        
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(origin);
+        init_transform.setRotation(rotation);
+        psb->transform(init_transform);
+        psb->getCollisionShape()->setMargin(0.015);
+        psb->setTotalMass(10);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = .5;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        btSoftBodyHelpers::generateBoundaryFaces(psb);
+        
+        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(m_gravity);
+        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        m_forces.push_back(gravity_force);
         
+        btScalar E = 10000;
+        btScalar nu = 0.3;
+        btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
+        btScalar mu = E / (2 * (1 + nu));
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.01);
+        // neohookean->setPoissonRatio(0.3);
+        // neohookean->setYoungsModulus(25);
+        neohookean->setDamping(0.01);
+        psb->m_cfg.drag = 0.001;
+        getDeformableDynamicsWorld()->addForce(psb, neohookean);
+        m_forces.push_back(neohookean);
+
+        std::cout << "Running full deformable\n";
     }
 
-    void Ctor_RbUpStack()
-    {
-        float mass = 8;
-        btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
-        btTransform startTransform;
-        startTransform.setIdentity();
+        // btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
 
-        startTransform.setOrigin(btVector3(0,9.5,0));
-        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
-        rb1->setLinearVelocity(btVector3(0, 0, 0));
-        rb1->setFriction(0.7);
+        // getDeformableDynamicsWorld()->addSoftBody(rsb);
+        // rsb->getCollisionShape()->setMargin(0.01);
+        // // rsb->scale(btVector3(1, 1, 0.5));
+
+        // rsb->setTotalMass(10);
+
+        // btTransform init_transform;
+        // init_transform.setIdentity();
+        // init_transform.setOrigin(origin);
+        // init_transform.setRotation(rotation);
+        // rsb->transformTo(init_transform);
+
+        // rsb->setStiffnessScale(5);
+        // rsb->setDamping(damping_alpha, damping_beta);
+        // // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+        // rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        // rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        // rsb->m_cfg.kDF = 0;
+        // rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        // rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        // rsb->m_sleepingThreshold = 0;
+        // btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+      float internalTimeStep = 1. / 240.f;
+      m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
     }
     
     virtual void renderScene()
     {
         CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
-        
+
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
         {
-            btSoftBody* psb = static_cast<btSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
+            btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
             {
-                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
-                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); 
             }
         }
     }
-
-    static void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
 };
 
-void ReducedGraspBenchmark::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
-{
-    btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
-    if (rbs.size()<2)
-        return;
-    btRigidBody* rb0 = rbs[0];
-    // btScalar pressTime = 0.9;
-    // btScalar pressTime = 0.96;
-    btScalar pressTime = 1.26;
-    btScalar liftTime = 2.5;
-    btScalar shiftTime = 6;
-    btScalar holdTime = 7;
-    btScalar dropTime = 10;
-    // btScalar holdTime = 500;
-    // btScalar dropTime = 1000;
-    btTransform rbTransform;
-    rbTransform.setIdentity();
-    btVector3 translation;
-    btVector3 velocity;
-    
-    btVector3 initialTranslationLeft = btVector3(0,1,0);            // inner face has z=2
-    btVector3 initialTranslationRight = btVector3(0,1,-4);          // inner face has z=-2
-    btVector3 pinchVelocityLeft = btVector3(0,0,-1);
-    btVector3 pinchVelocityRight = btVector3(0,0,1);
-    btVector3 liftVelocity = btVector3(0,4,0);
-    btVector3 shiftVelocity = btVector3(0,0,2);
-    btVector3 holdVelocity = btVector3(0,0,0);
-    btVector3 openVelocityLeft = btVector3(0,0,4);
-    btVector3 openVelocityRight = btVector3(0,0,-4);
-    
-    if (time < pressTime)
-    {
-        velocity = pinchVelocityLeft;
-        translation = initialTranslationLeft + pinchVelocityLeft * time;
-    }
-    else if (time < liftTime)
-    {
-        velocity = liftVelocity;
-        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
-    }
-    else if (time < shiftTime)
-    {
-        velocity = shiftVelocity;
-        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
-    }
-    else if (time < holdTime)
-    {
-        velocity = btVector3(0,0,0);
-        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
-    }
-    else if (time < dropTime)
-    {
-        velocity = openVelocityLeft;
-        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
-    }
-    else
-    {
-        velocity = holdVelocity;
-        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
-    }
-    rbTransform.setOrigin(translation);
-    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
-    rb0->setCenterOfMassTransform(rbTransform);
-    rb0->setAngularVelocity(btVector3(0,0,0));
-    rb0->setLinearVelocity(velocity);
-    
-    btRigidBody* rb1 = rbs[1];
-    if (time < pressTime)
-    {
-        velocity = pinchVelocityRight;
-        translation = initialTranslationRight + pinchVelocityRight * time;
-    }
-    else if (time < liftTime)
-    {
-        velocity = liftVelocity;
-        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
-    }
-    else if (time < shiftTime)
-    {
-        velocity = shiftVelocity;
-        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
-    }
-    else if (time < holdTime)
-    {
-        velocity = btVector3(0,0,0);
-        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
-    }
-    else if (time < dropTime)
-    {
-        velocity = openVelocityRight;
-        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
-    }
-    else
-    {
-        velocity = holdVelocity;
-        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
-    }
-    rbTransform.setOrigin(translation);
-    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
-    rb1->setCenterOfMassTransform(rbTransform);
-    rb1->setAngularVelocity(btVector3(0,0,0));
-    rb1->setLinearVelocity(velocity);
-    
-    rb0->setFriction(20);
-    rb1->setFriction(20);
-}
-
 void ReducedGraspBenchmark::initPhysics()
 {
     m_guiHelper->setUpAxis(1);
@@ -252,85 +235,42 @@ void ReducedGraspBenchmark::initPhysics()
         m_solver = sol;
         m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
     }
-    // btDeformableBodySolver* solver = new btDeformableBodySolver();
-    // btReducedSoftBodySolver* solver = new btReducedSoftBodySolver();
-
-    // btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
-    // sol->setDeformableSolver(solver);
-    // m_solver = sol;
 
     // m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
     btVector3 gravity = btVector3(0, -10, 0);
+    m_gravity = gravity;
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
-	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
-    getDeformableDynamicsWorld()->setSolverCallback(GripperDynamics);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
-
-    // create volumetric reduced deformable body
-    if (run_reduced)
-    {   
-        btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
-
-        getDeformableDynamicsWorld()->addSoftBody(rsb);
-        rsb->getCollisionShape()->setMargin(0.015);
-        
-        btTransform init_transform;
-        init_transform.setIdentity();
-        init_transform.setOrigin(btVector3(0, 1, -2));
-        // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
-        // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
-        rsb->transform(init_transform);
-
-        rsb->setStiffnessScale(100);
-        rsb->setDamping(damping_alpha, damping_beta);
-        rsb->setTotalMass(10);
-
-        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
-        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        rsb->m_cfg.kDF = 0;
-        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
-        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
-        rsb->m_sleepingThreshold = 0;
-        btSoftBodyHelpers::generateBoundaryFaces(rsb);
-
-        std::cout << "Running reduced deformable\n";
-    }
-    else    // create full deformable cube
+    
+    // 3x3 torus
+    createDeform(btVector3(4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(0, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(0, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(0, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(-4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(-4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(-4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    
+    // create a static rigid box as the ground
     {
-        std::string filepath("../../../data/reduced_cube/");
-        std::string filename = filepath + "cube_mesh.vtk";
-        btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
-        
-        // psb->scale(btVector3(2, 2, 2));
-        psb->translate(btVector3(0, 1, -2));
-        psb->getCollisionShape()->setMargin(0.015);
-        psb->setTotalMass(10);
-        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
-        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = .5;
-        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
-        psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
-        getDeformableDynamicsWorld()->addSoftBody(psb);
-        btSoftBodyHelpers::generateBoundaryFaces(psb);
-        
-        btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
-        getDeformableDynamicsWorld()->addForce(psb, gravity_force);
-        m_forces.push_back(gravity_force);
-        
-        btScalar E = 10000;
-        btScalar nu = 0.3;
-        btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
-        btScalar mu = E / (2 * (1 + nu));
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.02);
-        // neohookean->setPoissonRatio(0.3);
-        // neohookean->setYoungsModulus(25);
-        neohookean->setDamping(0.01);
-        psb->m_cfg.drag = 0.001;
-        getDeformableDynamicsWorld()->addForce(psb, neohookean);
-        m_forces.push_back(neohookean);
+        // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
+        btBoxShape* groundShape = createBoxShape(btVector3(btScalar(20), btScalar(2), btScalar(20)));
+        m_collisionShapes.push_back(groundShape);
 
-        std::cout << "Running full deformable\n";
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
+        // groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
+        groundTransform.setOrigin(btVector3(0, 0, 0));
+        // groundTransform.setOrigin(btVector3(0, 0, 6));
+        // groundTransform.setOrigin(btVector3(0, -50, 0));
+        {
+            btScalar mass(0.);
+            createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
+        }
     }
 
     getDeformableDynamicsWorld()->setImplicit(false);
@@ -338,78 +278,33 @@ void ReducedGraspBenchmark::initPhysics()
     getDeformableDynamicsWorld()->setUseProjection(false);
     getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
     getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
-    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
+    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
     getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
     getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
     getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
     getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
-    
-    // grippers
-    createGrip();
-
-    // rigid block
-    // Ctor_RbUpStack();
-
-    // {
-    //     float mass = 10;
-    //     btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
-    //     btTransform startTransform;
-    //     startTransform.setIdentity();
-    //     startTransform.setOrigin(btVector3(0,4,0));
-    //     btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
-    //     rb1->setLinearVelocity(btVector3(0, 0, 0));
-    // }
-
-    //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, -25, 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(0.5);
-
-        //add the ground to the dynamics world
-        m_dynamicsWorld->addRigidBody(body);
-    }
 
     m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+    m_dynamicsWorld->setGravity(gravity);
 }
 
 void ReducedGraspBenchmark::exitPhysics()
 {
     //cleanup in the reverse order of creation/initialization
     removePickingConstraint();
-	//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;
-	}
+    //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++)
     {
@@ -417,23 +312,24 @@ void ReducedGraspBenchmark::exitPhysics()
         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 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_dynamicsWorld;
 
-	delete m_solver;
+    delete m_solver;
 
-	delete m_broadphase;
+    delete m_broadphase;
 
-	delete m_dispatcher;
+    delete m_dispatcher;
 
-	delete m_collisionConfiguration;
+    delete m_collisionConfiguration;
 }
 
 
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
index ee6d924ce..c283036aa 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
@@ -68,7 +68,7 @@ void btReducedSoftBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
   {
     btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
     rsb->m_nodeIndexOffset = sum;
-    rsb->m_nodeIndexOffset += rsb->m_nodes.size();
+    sum += rsb->m_nodes.size();
   }
 
 	btDeformableBodySolver::updateSoftBodies();