Merge pull request #791 from erwincoumans/master

default contact erp (m_erp2) was too low in physics server

data/cube_gripper_left.urdf

@@ -6,10 +6,8 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="0.3"/>
+      <spinning_friction value="0.3"/>
       <inertia_scaling value="3.0"/>
-      <contact_cfm value="0.0"/>
-      <contact_erp value="1.0"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/cube_gripper_right.urdf

@@ -6,10 +6,8 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="0.3"/>
+      <spinning_friction value="0.3"/>
       <inertia_scaling value="3.0"/>
-      <contact_cfm value="0.0"/>
-      <contact_erp value="1.0"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/cube_small.urdf

@@ -3,7 +3,7 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="0.1"/>
+      <spinning_friction value="0.1"/>
       <inertia_scaling value="3.0"/>
     </contact>
     <inertial>

data/plane.urdf

@@ -1,9 +1,6 @@
 <?xml version="0.0" ?>
 <robot name="cube.urdf">
   <link name="baseLink">
-    <contact>
-      <rolling_friction value="0.3"/>
-    </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value=".0"/>

data/pr2_gripper.urdf

@@ -33,6 +33,7 @@
   <link name="left_gripper">
     <contact>
       <lateral_friction value="50.0"/>
+      <spinning_friction value="0.1"/>
     </contact>
     <visual>
       <origin rpy="0.0 0 0" xyz="0 0 0"/>

data/r2d2.urdf

@@ -388,6 +388,7 @@
     <child link="head"/>
     <axis xyz="0 0 1"/>
     <origin xyz="0 0 0.3"/>
+<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
   </joint>
 
   <link name="box">

examples/CommonInterfaces/CommonExampleInterface.h

@@ -46,7 +46,7 @@ public:
 	virtual bool	mouseButtonCallback(int button, int state, float x, float y)=0;
 	virtual bool	keyboardCallback(int key, int state)=0;
 
-	virtual void	vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4]) {}
+	virtual void	vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis) {}
 	virtual void	vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){}
 };
 

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -410,6 +410,10 @@ void ConvertURDF2BulletInternal(
 				{
 					col->setRollingFriction(contactInfo.m_rollingFriction);
 				}
+                if ((contactInfo.m_flags & URDF_CONTACT_HAS_SPINNING_FRICTION)!=0)
+                {
+                    col->setSpinningFriction(contactInfo.m_spinningFriction);
+                }
 				if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0)
 				{
 				    col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping);

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -16,17 +16,20 @@ enum UrdfJointTypes
 enum URDF_LinkContactFlags
 {
 	URDF_CONTACT_HAS_LATERAL_FRICTION=1,
-	URDF_CONTACT_HAS_ROLLING_FRICTION=2,
     URDF_CONTACT_HAS_INERTIA_SCALING=2,
     URDF_CONTACT_HAS_CONTACT_CFM=4,
 	URDF_CONTACT_HAS_CONTACT_ERP=8,
 	URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
+    URDF_CONTACT_HAS_ROLLING_FRICTION=32,
+    URDF_CONTACT_HAS_SPINNING_FRICTION=64,
+
 };
 
 struct URDFLinkContactInfo
 {
 	btScalar m_lateralFriction;
 	btScalar m_rollingFriction;
+    btScalar m_spinningFriction;
     btScalar m_inertiaScaling;
 	btScalar m_contactCfm;
 	btScalar m_contactErp;
@@ -38,6 +41,7 @@ struct URDFLinkContactInfo
 	URDFLinkContactInfo()
 		:m_lateralFriction(0.5),
 		m_rollingFriction(0),
+        m_spinningFriction(0),
         m_inertiaScaling(1),
 		m_contactCfm(0),
 		m_contactErp(0),

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -672,7 +672,28 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
                   }
               }
           }
-
+          {
+              TiXmlElement *spinning_xml = ci->FirstChildElement("spinning_friction");
+              if (spinning_xml)
+              {
+                  if (m_parseSDF)
+                  {
+                      link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->GetText());
+                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
+                  } else
+                  {
+                      if (!spinning_xml->Attribute("value"))
+                      {
+                          logger->reportError("Link/contact: spinning friction element must have value attribute");
+                          return false;
+                      }
+                      
+                      link.m_contactInfo.m_spinningFriction = urdfLexicalCast<double>(spinning_xml->Attribute("value"));
+                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_SPINNING_FRICTION;
+                      
+                  }
+              }
+          }
            {
            
               TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -27,6 +27,8 @@
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
 #include "SharedMemoryCommands.h"
 
+btVector3 gLastPickPos(0, 0, 0);
+
 struct UrdfLinkNameMapUtil
 {
 	btMultiBody* m_mb;
@@ -383,7 +385,7 @@ struct PhysicsServerCommandProcessorInternalData
 
 	btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
 	btMultiBody* m_gripperMultiBody;
-
+	int m_huskyId;
 	CommandLogger* m_commandLogger;
 	CommandLogPlayback* m_logPlayback;
 
@@ -435,6 +437,7 @@ struct PhysicsServerCommandProcessorInternalData
 		m_gripperRigidbodyFixed(0),
 		m_gripperMultiBody(0),
 		m_allowRealTimeSimulation(false),
+		m_huskyId(0),
 		m_commandLogger(0),
 		m_logPlayback(0),
 		m_physicsDeltaTime(1./240.),
@@ -593,7 +596,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
 
 	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
-	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.005;
+	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05;
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -1874,10 +1877,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     {
                         applyJointDamping(i);
                     }
-                    if (m_data->m_numSimulationSubSteps > 0)
+					
-                        m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,m_data->m_numSimulationSubSteps,m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps);
+					if (m_data->m_numSimulationSubSteps > 0)
-                    else
+					{
-                        m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0);
+						m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
+					}
+					else
+					{
+						m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, 0);
+					}
 
 					SharedMemoryStatus& serverCmd =serverStatusOut;
 					serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
@@ -2678,7 +2686,7 @@ void    PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
 	}
 }
 
-btVector3 gLastPickPos(0,0,0);
+
 
 bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
 {
@@ -2832,6 +2840,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
 
 btVector3 gVRGripperPos(0,0,0);
 btQuaternion gVRGripperOrn(0,0,0,1);
+btScalar gVRGripperAnalog = 0;
 bool gVRGripperClosed = false;
 
 
@@ -2860,7 +2869,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 			{
 				int bodyId = 0;
 
-				if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
+				if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
 				{
 					InteralBodyData* parentBody = m_data->getHandle(bodyId);
 					if (parentBody->m_multiBody)
@@ -2878,14 +2887,25 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 						m_data->m_gripperMultiBody = parentBody->m_multiBody;
 						if (m_data->m_gripperMultiBody->getNumLinks() > 2)
 						{
-							m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI);
+							m_data->m_gripperMultiBody->setJointPos(0, 0);
-							m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI);
+							m_data->m_gripperMultiBody->setJointPos(2, 0);
 						}
 						m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.);
 						btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
 						world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
 					}
 				}
+				
+				loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				
+				loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("husky/husky.urdf", btVector3(1, 1, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				m_data->m_huskyId = bodyId;
+				
 			}
 		}
 
@@ -2904,14 +2924,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 						{
 							motor->setErp(0.01);
 
-							if (gVRGripperClosed)
+							motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1);
-							{
+							
-								motor->setPositionTarget(0.2, 1);
+							
-							}
-							else
-							{
-								motor->setPositionTarget(SIMD_HALF_PI, 1);
-							}
 							motor->setVelocityTarget(0, 0.1);
 							btScalar maxImp = 1550.*m_data->m_physicsDeltaTime;
 							motor->setMaxAppliedImpulse(maxImp);

examples/SharedMemory/PhysicsServerExample.cpp

@@ -18,6 +18,8 @@
 
 extern btVector3 gLastPickPos;
 btVector3 gVRTeleportPos(0,0,0);
+btQuaternion gVRTeleportOrn(0, 0, 0,1);
+
 extern bool gVRGripperClosed;
 
 bool gDebugRenderToggle  = false;
@@ -556,7 +558,7 @@ public:
 	btVector3	getRayTo(int x,int y);
 
 	virtual void	vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]);
-	virtual void	vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4]);
+	virtual void	vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis);
 
 	virtual bool	mouseMoveCallback(float x,float y)
 	{
@@ -894,6 +896,7 @@ void PhysicsServerExample::renderScene()
 	///debug rendering
 	//m_args[0].m_cs->lock();
 	
+	//gVRTeleportPos[0] += 0.01;
 	vrOffset[12]=-gVRTeleportPos[0];
 	vrOffset[13]=-gVRTeleportPos[1];
 	vrOffset[14]=-gVRTeleportPos[2];
@@ -1101,7 +1104,7 @@ class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOpt
 }
 
 
-static int gGraspingController = 2;
+static int gGraspingController = -1;
 
 void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
 {
@@ -1110,6 +1113,8 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 	if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS)
 		return;
 
+	if (gGraspingController < 0)
+		gGraspingController = controllerId;
 
 	if (controllerId != gGraspingController)
 	{
@@ -1151,10 +1156,15 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 
 extern btVector3 gVRGripperPos;
 extern btQuaternion gVRGripperOrn;
+extern btScalar gVRGripperAnalog;
 
-void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4])
+
+
+void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
 {
 
+	
+
 	if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
 	{
 		printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
@@ -1162,6 +1172,7 @@ void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
 	}
 	if (controllerId == gGraspingController)
 	{
+		gVRGripperAnalog = analogAxis;
 		gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
 		btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
 		gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -713,14 +713,17 @@ bool CMainApplication::HandleInput()
 						}
 						else
 						{
+							
 //							printf("Device MOVED: %d\n", unDevice);
-							sExample->vrControllerMoveCallback(unDevice, pos, orn);
+							sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
 						}
 					}
 					else
 					{
 						if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
 						{
+							
+
 							b3Transform tr;
 							getControllerTransform(unDevice, tr);
 							float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] };
@@ -741,7 +744,7 @@ bool CMainApplication::HandleInput()
 							} else
 							{
 
-								sExample->vrControllerMoveCallback(unDevice, pos, orn);
+								sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
 							}
 						}
 					}

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -33,6 +33,7 @@ btCollisionObject::btCollisionObject()
 		m_friction(btScalar(0.5)),
 		m_restitution(btScalar(0.)),
 		m_rollingFriction(0.0f),
+        m_spinningFriction(0.f),
 		m_contactDamping(.1),
 		m_contactStiffness(1e4),
 		m_internalType(CO_COLLISION_OBJECT),

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -85,7 +85,8 @@ protected:
 
 	btScalar		m_friction;
 	btScalar		m_restitution;
-	btScalar		m_rollingFriction;
+	btScalar		m_rollingFriction;//torsional friction orthogonal to contact normal (useful to stop spheres rolling forever)
+    btScalar        m_spinningFriction; // torsional friction around the contact normal (useful for grasping)
 	btScalar		m_contactDamping;
 	btScalar		m_contactStiffness;
 	
@@ -323,7 +324,15 @@ public:
 	{
 		return m_rollingFriction;
 	}
-
+    void	setSpinningFriction(btScalar frict)
+    {
+        m_updateRevision++;
+        m_spinningFriction = frict;
+    }
+    btScalar	getSpinningFriction() const
+    {
+        return m_spinningFriction;
+    }
     void	setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
 	{
 		m_updateRevision++;

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -24,7 +24,6 @@ ContactAddedCallback		gContactAddedCallback=0;
 
 
 
-///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
 btScalar	btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
 {
 	btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction();
@@ -38,6 +37,17 @@ btScalar	btManifoldResult::calculateCombinedRollingFriction(const btCollisionObj
 
 }
 
+btScalar	btManifoldResult::calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    btScalar friction = body0->getSpinningFriction() * body1->getFriction() + body1->getSpinningFriction() * body0->getFriction();
+    
+    const btScalar MAX_FRICTION  = btScalar(10.);
+    if (friction < -MAX_FRICTION)
+        friction = -MAX_FRICTION;
+    if (friction > MAX_FRICTION)
+        friction = MAX_FRICTION;
+    return friction;
+}
 
 ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
 btScalar	btManifoldResult::calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
@@ -125,6 +135,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 	newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+    newPt.m_combinedSpinningFriction = calculateCombinedSpinningFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	
 	if (    (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
             (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))

src/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -146,7 +146,8 @@ public:
 	static btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
 	static btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
 	static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
-	static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
+    static btScalar calculateCombinedSpinningFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+    static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
 	static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
 };
 

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -72,7 +72,8 @@ class btManifoldPoint
 					m_distance1( distance ),
 					m_combinedFriction(btScalar(0.)),
 					m_combinedRollingFriction(btScalar(0.)),
-					m_combinedRestitution(btScalar(0.)),
+                    m_combinedSpinningFriction(btScalar(0.)),
+                    m_combinedRestitution(btScalar(0.)),
 					m_userPersistentData(0),
 					m_contactPointFlags(0),
 					m_appliedImpulse(0.f),
@@ -99,8 +100,9 @@ class btManifoldPoint
 		
 			btScalar	m_distance1;
 			btScalar	m_combinedFriction;
-			btScalar	m_combinedRollingFriction;
+			btScalar	m_combinedRollingFriction;//torsional friction orthogonal to contact normal, useful to make spheres stop rolling forever
-			btScalar	m_combinedRestitution;
+            btScalar	m_combinedSpinningFriction;//torsional friction around contact normal, useful for grasping objects
+            btScalar	m_combinedRestitution;
 
 			//BP mod, store contact triangles.
 			int			m_partId0;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -627,8 +627,8 @@ btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(c
 }
 
 
-void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
+void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
-									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+									btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 									btScalar desiredVelocity, btScalar cfmSlip)
 
@@ -647,8 +647,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 	solverConstraint.m_solverBodyIdA = solverBodyIdA;
 	solverConstraint.m_solverBodyIdB = solverBodyIdB;
 
-	solverConstraint.m_friction = cp.m_combinedRollingFriction;
+    solverConstraint.m_friction = combinedTorsionalFriction;
-	solverConstraint.m_originalContactPoint = 0;
+    solverConstraint.m_originalContactPoint = 0;
 
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
@@ -704,11 +704,11 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolv
 
 
 
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+btSolverConstraint&	btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
+	setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2,
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
@@ -1069,28 +1069,11 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 
-			btVector3 angVelA(0,0,0),angVelB(0,0,0);
-			if (rb0)
-				angVelA = rb0->getAngularVelocity();
-			if (rb1)
-				angVelB = rb1->getAngularVelocity();
-			btVector3 relAngVel = angVelB-angVelA;
-
 			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
 			{
-                //only a single rollingFriction per manifold
+               
-				//rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
 				{
-					relAngVel.normalize();
+					addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (relAngVel.length()>0.001)
-						addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-
-				} else
-				{
-					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					btVector3 axis0,axis1;
 					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
 					axis0.normalize();
@@ -1101,9 +1084,11 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
+                                                       cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+						addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,
+                                                       cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 
 				}
 			}

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -54,13 +54,13 @@ protected:
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
-	void setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
+	void setupTorsionalFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
-									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+									btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
 	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
-	btSolverConstraint&	addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
+	btSolverConstraint&	addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
 
 	
 	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -79,6 +79,8 @@ void	btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo&
 	//moved to btCollisionObject
 	m_friction = constructionInfo.m_friction;
 	m_rollingFriction = constructionInfo.m_rollingFriction;
+    m_spinningFriction = constructionInfo.m_spinningFriction;
+    
 	m_restitution = constructionInfo.m_restitution;
 
 	setCollisionShape( constructionInfo.m_collisionShape );

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -135,6 +135,8 @@ public:
 		///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
 		///See Bullet/Demos/RollingFrictionDemo for usage
 		btScalar			m_rollingFriction;
+        btScalar			m_spinningFriction;//torsional friction around contact normal
+        
 		///best simulation results using zero restitution.
 		btScalar			m_restitution;
 
@@ -158,6 +160,7 @@ public:
 			m_angularDamping(btScalar(0.)),
 			m_friction(btScalar(0.5)),
 			m_rollingFriction(btScalar(0)),
+            m_spinningFriction(btScalar(0)),
 			m_restitution(btScalar(0.)),
 			m_linearSleepingThreshold(btScalar(0.8)),
 			m_angularSleepingThreshold(btScalar(1.f)),

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -557,9 +557,11 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 }
 
-void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
+void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
                                                                   const btVector3& constraintNormal,
-                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+                                                                  btManifoldPoint& cp,
+                                                                    btScalar combinedTorsionalFriction,
+                                                                    const btContactSolverInfo& infoGlobal,
                                                                   btScalar& relaxation,
                                                                   bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
@@ -784,7 +786,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
             }
         }
 
-        solverConstraint.m_friction = cp.m_combinedRollingFriction;
+        solverConstraint.m_friction =combinedTorsionalFriction;
         
         if(!isFriction)
         {
@@ -860,7 +862,9 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 	return solverConstraint;
 }
 
-btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
+btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
+                                                                btScalar combinedTorsionalFriction,
+                                                                                                     btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
     BT_PROFILE("addMultiBodyRollingFrictionConstraint");
     btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
@@ -891,7 +895,7 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyRollingFri
     
     solverConstraint.m_originalContactPoint = &cp;
     
-    setupMultiBodyRollingFrictionConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
+    setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
     return solverConstraint;
 }
 
@@ -1010,9 +1014,9 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
                     
                     if (rollingFriction > 0)
                     {
-                        addMultiBodyRollingFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+                        addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
-                        //addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+                        addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
-                        //addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
+                        addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
 
                         rollingFriction--;
                     }

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -50,7 +50,9 @@ protected:
     
     btMultiBodySolverConstraint&	addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
 
-    btMultiBodySolverConstraint&	addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+    btMultiBodySolverConstraint&	addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
+                                                            btScalar combinedTorsionalFriction,
+                                                            btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0);
 
 	void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
 																 btScalar* jacA,btScalar* jacB,
@@ -63,9 +65,12 @@ protected:
 																 btScalar& relaxation,
 																 bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
     
-    void setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
+    //either rolling or spinning friction
+    void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
                                          const btVector3& contactNormal,
-                                         btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+                                         btManifoldPoint& cp,
+                                        btScalar combinedTorsionalFriction,
+                                        const btContactSolverInfo& infoGlobal,
                                          btScalar& relaxation,
                                          bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);