fix issue with btMultiBody friction in combination with soft contacts (friction should not re-use normal contact cfm/erp)

implement friction anchors, position friction correction, disabled by default. Use colObj->setCollisionFlag(flag | CF_HAS_FRICTION_ANCHOR); See test/RobotClientAPI/SlopeFrictionMain.cpp. In URDF or SDF, add <friction_anchor/> in <contact> section of <link> to enable.
PhysicsServer: properly restore old activation state after releasing picked object
btMultiBodyConstraintSolver: disable flip/flop of contact/friction constraint solving by default (it breaks some internal flaky unit tests)

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -166,6 +166,10 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
 	{
 		col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping);
 	}
+	if ((contactInfo.m_flags & URDF_CONTACT_HAS_FRICTION_ANCHOR) != 0)
+	{
+		col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
+	}
 }
 
 

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -23,7 +23,8 @@ enum URDF_LinkContactFlags
     URDF_CONTACT_HAS_ROLLING_FRICTION=32,
     URDF_CONTACT_HAS_SPINNING_FRICTION=64,
 	URDF_CONTACT_HAS_RESTITUTION=128,
-
+	URDF_CONTACT_HAS_FRICTION_ANCHOR=256,
+    
 };
 
 struct URDFLinkContactInfo

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -767,6 +767,13 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
                       
                   }
               }
+          }
+          {
+          	TiXmlElement *friction_anchor = ci->FirstChildElement("friction_anchor");
+            if (friction_anchor)
+            {
+            	link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_FRICTION_ANCHOR;
+            }
           }
            {
            

examples/RobotSimulator/RobotSimulatorMain.cpp

@@ -61,6 +61,7 @@ int main(int argc, char* argv[])
 	sim->setRealTimeSimulation(false);
 	int vidLogId = -1;
 	int minitaurLogId = -1;
+	int rotateCamera = 0;
 
 	while (sim->canSubmitCommand())
 	{
@@ -73,9 +74,11 @@ int main(int argc, char* argv[])
 			//m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased
 			for (int i=0;i<keyEvents.m_numKeyboardEvents;i++)
 			{
-				if (keyEvents.m_keyboardEvents[i].m_keyCode=='0')
+				b3KeyboardEvent& e = keyEvents.m_keyboardEvents[i];
+
+				if (e.m_keyCode=='0')
 				{
-					if ( keyEvents.m_keyboardEvents[i].m_keyState&eButtonTriggered)
+					if ( e.m_keyState&eButtonTriggered)
 					{
 						if (vidLogId < 0)
 						{
@@ -89,28 +92,40 @@ int main(int argc, char* argv[])
 					}
 				}
 
-				if (keyEvents.m_keyboardEvents[i].m_keyCode=='m')
+				if (e.m_keyCode=='m')
 				{
-					if ( minitaurLogId<0 && keyEvents.m_keyboardEvents[i].m_keyState&eButtonTriggered)
+					if ( minitaurLogId<0 && e.m_keyState&eButtonTriggered)
 					{
 						minitaurLogId = sim->startStateLogging(STATE_LOGGING_MINITAUR,"simlog.bin");
 					}
-					if (minitaurLogId>=0 && keyEvents.m_keyboardEvents[i].m_keyState&eButtonReleased)
+					if (minitaurLogId>=0 && e.m_keyState&eButtonReleased)
 					{
 						sim->stopStateLogging(minitaurLogId);
 						minitaurLogId=-1;
 					}
 				}
 
+				if (e.m_keyCode == 'r' && e.m_keyState&eButtonTriggered)
+				{
+					rotateCamera = 1-rotateCamera;
+				}
+
 
 				//printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState);
 			}
 		}
 		sim->stepSimulation();
-		static double yaw=0;
-		double distance = 10.5+9 * b3Sin(yaw);
-		yaw+=0.008;
-		sim->resetDebugVisualizerCamera(distance,yaw,20,b3MakeVector3(0,0,0.1));
+
+		if (rotateCamera)
+		{
+			static double yaw=0;
+			double distance = 1;
+			yaw+=0.1;
+			b3Vector3 basePos;
+			b3Quaternion baseOrn;
+			sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
+			sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
+		}
 		b3Clock::usleep(1000.*1000.*fixedTimeStep);
 	}
 

examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp

@@ -210,7 +210,7 @@ void b3RobotSimulatorClientAPI::stepSimulation()
 	{
 		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
 		statusType = b3GetStatusType(statusHandle);
-		b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
+		//b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
 	}
 }
 
@@ -226,7 +226,7 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
 	b3SharedMemoryStatusHandle statusHandle;
 	b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
+//	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
 }
 
 b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -914,7 +914,7 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
 int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
 {
     const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
-    b3Assert(status);
+    //b3Assert(status);
     if (status)
     {
         return status->m_type;
@@ -1063,7 +1063,7 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
 
 		b3SubmitClientCommand(physClient, commandHandle);
 
-		while ((statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
+		while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
 		{
 			statusHandle = b3ProcessServerStatus(physClient);
 		}

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -919,6 +919,7 @@ struct PhysicsServerCommandProcessorInternalData
 
 	//data for picking objects
 	class btRigidBody*	m_pickedBody;
+    int m_savedActivationState;
 	class btTypedConstraint* m_pickedConstraint;
 	class btMultiBodyPoint2Point*		m_pickingMultiBodyPoint2Point;
 	btVector3 m_oldPickingPos;
@@ -1146,7 +1147,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     
     m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
     m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
-    
+
+	m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
 	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
@@ -4978,6 +4980,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
 			if (!(body->isStaticObject() || body->isKinematicObject()))
 			{
 				m_data->m_pickedBody = body;
+                m_data->m_savedActivationState = body->getActivationState();
 				m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
 				//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
 				btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
@@ -5070,7 +5073,7 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
 		m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
 		delete m_data->m_pickedConstraint;
 		m_data->m_pickedConstraint = 0;
-		m_data->m_pickedBody->forceActivationState(ACTIVE_TAG);
+		m_data->m_pickedBody->forceActivationState(m_data->m_savedActivationState);
 		m_data->m_pickedBody = 0;
 	}
 	if (m_data->m_pickingMultiBodyPoint2Point)