set up reduced grasping example

examples/ExampleBrowser/CMakeLists.txt

@@ -396,6 +396,8 @@ SET(BulletExampleBrowser_SRCS
 	../ReducedDeformableDemo/FreeFall.h
 	../ReducedDeformableDemo/ReducedCollide.cpp
 	../ReducedDeformableDemo/ReducedCollide.h
+	../ReducedDeformableDemo/ReducedGrasp.cpp
+	../ReducedDeformableDemo/ReducedGrasp.h
   ../Constraints/TestHingeTorque.cpp
   ../Constraints/TestHingeTorque.h
   ../Constraints/ConstraintDemo.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -76,6 +76,7 @@
 #include "../ReducedDeformableDemo/BasicTest.h"
 #include "../ReducedDeformableDemo/FreeFall.h"
 #include "../ReducedDeformableDemo/ReducedCollide.h"
+#include "../ReducedDeformableDemo/ReducedGrasp.h"
 #include "../InverseKinematics/InverseKinematicsExample.h"
 
 #ifdef B3_ENABLE_TINY_AUDIO
@@ -224,6 +225,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "BasicRD", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc),
 		ExampleEntry(1, "Reduced Free Fall", "Free fall ground contact test for the reduced deformable model", ReducedFreeFallCreateFunc),
 		ExampleEntry(1, "Reduced Collision Test", "Collision between a reduced block and the a rigid block", ReducedCollideCreateFunc),
+		ExampleEntry(1, "Reduced Grasp", "Grasp a reduced deformable block", ReducedGraspCreateFunc),
 		// ExampleEntry(1, "Simple Reduced Deformable Test", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc),
         
 #ifdef INCLUDE_CLOTH_DEMOS

examples/ReducedDeformableDemo/ReducedCollide.cpp

@@ -65,7 +65,7 @@ public:
     
     void Ctor_RbUpStack()
     {
-        float mass = 10;
+        float mass = 8;
         btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
         btTransform startTransform;
         startTransform.setIdentity();
@@ -148,6 +148,7 @@ void ReducedCollide::initPhysics()
         getDeformableDynamicsWorld()->addSoftBody(rsb);
         rsb->getCollisionShape()->setMargin(0.1);
         // rsb->scale(btVector3(1, 1, 1));
+        rsb->rotate(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
         rsb->translate(btVector3(0, 2, 0));  //TODO: add back translate and scale
         rsb->setStiffnessScale(10);
         rsb->setDamping(damping_alpha, damping_beta);

examples/ReducedDeformableDemo/ReducedGrasp.cpp

@@ -0,0 +1,361 @@
+/*
+ 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 "ReducedGrasp.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h"
+#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
+// static btScalar E = 50;
+// static btScalar nu = 0.3;
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0;
+static btScalar COLLIDING_VELOCITY = 4;
+static int start_mode = 6;
+static int num_modes = 1;
+
+class ReducedGrasp : public CommonDeformableBodyBase
+{
+public:
+    ReducedGrasp(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+    }
+    virtual ~ReducedGrasp()
+    {
+    }
+    void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 25;
+        float pitch = -30;
+        float yaw = 100;
+        float targetPos[3] = {0, -0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
+    
+    void createGrip()
+    {
+        int count = 2;
+        float mass = 1e6;
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(3, 3, 0.5)),
+        };
+        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+        for (int i = 0; i < count; ++i)
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(10, 0, 0));
+            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+            createRigidBody(mass, startTransform, shape[i % nshapes]);
+        }
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::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 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 = 1.5;
+    btScalar liftTime = 2.5;
+    btScalar shiftTime = 3.5;
+    btScalar holdTime = 4.5*1000;
+    btScalar dropTime = 5.3*1000;
+    btTransform rbTransform;
+    rbTransform.setIdentity();
+    btVector3 translation;
+    btVector3 velocity;
+    
+    btVector3 initialTranslationLeft = btVector3(0.5,3,4);
+    btVector3 initialTranslationRight = btVector3(0.5,3,-4);
+    btVector3 pinchVelocityLeft = btVector3(0,0,-2);
+    btVector3 pinchVelocityRight = btVector3(0,0,2);
+    btVector3 liftVelocity = btVector3(0,5,0);
+    btVector3 shiftVelocity = btVector3(0,0,5);
+    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 ReducedGrasp::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();
+    btReducedSoftBodySolver* reducedSoftBodySolver = new btReducedSoftBodySolver();
+    btVector3 gravity = btVector3(0, -10, 0);
+    reducedSoftBodySolver->setGravity(gravity);
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+	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
+    {   
+        std::string filepath("../../../examples/SoftDemo/");
+        std::string filename = filepath + "mesh.vtk";
+        btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
+        
+        rsb->setReducedModes(start_mode, num_modes, rsb->m_nodes.size());
+        btReducedSoftBodyHelpers::readReducedDeformableInfoFromFiles(rsb, filepath.c_str());
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.1);
+        // rsb->scale(btVector3(1, 1, 1));
+        rsb->rotate(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
+        rsb->translate(btVector3(0, 2, 0));  //TODO: add back translate and scale
+        rsb->setStiffnessScale(10);
+        rsb->setDamping(damping_alpha, damping_beta);
+
+        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->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
+        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
+        
+        // btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
+        // getDeformableDynamicsWorld()->addForce(rsb, gravity_force);
+        // m_forces.push_back(gravity_force);
+    }
+
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(true);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+    
+    createGrip();
+
+    //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);
+}
+
+void ReducedGrasp::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;
+	}
+    // 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* ReducedGraspCreateFunc(struct CommonExampleOptions& options)
+{
+    return new ReducedGrasp(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/ReducedGrasp.h

@@ -0,0 +1,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 _REDUCED_GRASP_H
+#define _REDUCED_GRASP_H
+
+class CommonExampleInterface* ReducedGraspCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_GRASP_H

src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp

@@ -69,8 +69,8 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
 void btReducedDeformableRigidContactConstraint::setSolverBody(btSolverBody& solver_body)
 {
 	m_solverBody = &solver_body;
-	m_linearComponentNormal = m_contactNormalA * m_solverBody->internalGetInvMass();
-	btVector3	torqueAxis = -m_relPosA.cross(m_contactNormalA);
+	m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
+	btVector3	torqueAxis = m_relPosA.cross(m_contactNormalA);
 	m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
 }
 
@@ -202,33 +202,34 @@ btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btCont
 	// apply impulse to the rigid/multibodies involved and change their velocities
 	if (!m_collideStatic)
 	{
+		std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t'
+																			<< m_linearComponentNormal[1] << '\t'
+																			<< m_linearComponentNormal[2] << '\n';
+		std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t'
+																			<< m_angularComponentNormal[1] << '\t'
+																			<< m_angularComponentNormal[2] << '\n';
+
 		const btSoftBody::sCti& cti = m_contact->m_cti;
 		if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 		{
-			// if (!m_collideStatic)
-			// {
-			// 	std::cout << "rigid impulse applied!!\n";
-			// 	std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
-			// 	<< m_solverBody->getDeltaLinearVelocity()[1] << '\t'
-			// 		<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
-			// 	std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
-			// 	<< m_solverBody->getDeltaAngularVelocity()[1] << '\t'
-			// 		<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
-			// }
+			std::cout << "rigid impulse applied!!\n";
+			std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
+			<< m_solverBody->getDeltaLinearVelocity()[1] << '\t'
+				<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
+			std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
+			<< m_solverBody->getDeltaAngularVelocity()[1] << '\t'
+				<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
 
-			m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -deltaImpulse);
+			m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
 
-			if (!m_collideStatic)
-			{
-				std::cout << "after\n";
-				std::cout << "rigid impulse applied!!\n";
-				std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
-				<< m_solverBody->getDeltaLinearVelocity()[1] << '\t'
-					<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
-				std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
-				<< m_solverBody->getDeltaAngularVelocity()[1] << '\t'
-					<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
-			}
+			std::cout << "after\n";
+			std::cout << "rigid impulse applied!!\n";
+			std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
+			<< m_solverBody->getDeltaLinearVelocity()[1] << '\t'
+				<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
+			std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
+			<< m_solverBody->getDeltaAngularVelocity()[1] << '\t'
+				<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
 		}
 		else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
 		{
@@ -290,6 +291,12 @@ void btReducedDeformableNodeRigidContactConstraint::warmStarting()
 	m_bufferVelocityA = va;
 	m_bufferVelocityB = vb;
 
+	// add the external impulse force (TODO: add external torque impulse)
+	// if (!m_collideStatic)
+	// {
+	// 	va += m_solverBody->m_originalBody->getTotalForce() * m_solverBody->m_originalBody->getInvMass() * m_dt;
+	// }
+
 	// we define the (+) direction of errors to be the outward surface normal of the rigid object
 	btVector3 v_rel = vb - va;
 	// get tangent direction of the relative velocity

src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h

@@ -47,8 +47,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
   btScalar m_erp;
   btScalar m_friction;
 
-  btVector3 m_contactNormalA;     // for rigid body
-  btVector3 m_contactNormalB;     // for reduced deformable body
+  btVector3 m_contactNormalA;     // surface normal for rigid body (opposite direction as impulse)
+  btVector3 m_contactNormalB;     // surface normal for reduced deformable body (opposite direction as impulse)
   btVector3 m_contactTangent;     // tangential direction of the relative velocity
   btVector3 m_relPosA;            // relative position of the contact point for A (rigid)
   btVector3 m_relPosB;            // relative position of the contact point for B

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp

@@ -376,6 +376,18 @@ void btReducedSoftBody::translate(const btVector3& trs)
   updateInertiaTensor();
 }
 
+void btReducedSoftBody::rotate(const btQuaternion& rot)
+{
+  // translate mesh
+	btSoftBody::rotate(rot);
+  updateRestNodalPositions();
+  
+  // update rigid frame
+  m_rigidTransformWorld.setRotation(rot);
+  m_interpolationWorldTransform = m_rigidTransformWorld;
+  updateInertiaTensor();
+}
+
 void btReducedSoftBody::updateRestNodalPositions()
 {
   // update reset nodal position

src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h

@@ -123,6 +123,8 @@ class btReducedSoftBody : public btSoftBody
   //
   virtual void translate(const btVector3& trs);
 
+  virtual void rotate(const btQuaternion& rot);
+
   void updateRestNodalPositions();
 
   void updateInertiaTensor();

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -67,7 +67,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 			}
 		}
 		std::cout << "======next step=========\n";
-		// std::this_thread::sleep_for (std::chrono::milliseconds(300));
+		// std::this_thread::sleep_for(std::chrono::milliseconds(100));
 	}
 	return 0.f;
 }