Merge pull request #2357 from fuchuyuan/setWarmStartFactor

API and pybullet bindings for setting warmstarting factor

6 files changed, 40 insertions, 15 deletions

diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 5f4a7a2e5..3fe405ba1 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -668,6 +668,15 @@ B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHand
 	return 0;
 }
 
+B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_physSimParamArgs.m_warmStartingFactor = warmStartingFactor;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_WARM_STARTING_FACTOR;
+	return 0;
+}
+
 B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 2c097119a..f11f7a872 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -338,6 +338,7 @@ extern "C"
 	B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
 	B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
 	B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
+	B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
 	B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
 	B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
 	B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 32853ddb9..cf2dc2328 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -9070,6 +9070,10 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 		m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = clientCmd.m_physSimParamArgs.m_reportSolverAnalytics;
 	}
 
+	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_WARM_STARTING_FACTOR)
+	{
+		m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = clientCmd.m_physSimParamArgs.m_warmStartingFactor;
+	}
 	SharedMemoryStatus& serverCmd = serverStatusOut;
 	serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
 	return hasStatus;
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index cca091d3a..9aa7a74a0 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -482,7 +482,8 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_CONSTRAINT_SOLVER_TYPE = 1 << 24,
 	SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25,
 	SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26,
-
+	SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27,
+        
 };
 
 enum EnumLoadSoftBodyUpdateFlags
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index babf06d8f..680de9481 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -7,8 +7,9 @@
 //Please don't replace an existing magic number:
 //instead, only ADD a new one at the top, comment-out previous one
 
-#define SHARED_MEMORY_MAGIC_NUMBER 2019060190
-// #define SHARED_MEMORY_MAGIC_NUMBER 201904030
+#define SHARED_MEMORY_MAGIC_NUMBER 201908050
+//#define SHARED_MEMORY_MAGIC_NUMBER 2019060190
+//#define SHARED_MEMORY_MAGIC_NUMBER 201904030
 //#define SHARED_MEMORY_MAGIC_NUMBER 201902120
 //#define SHARED_MEMORY_MAGIC_NUMBER 201811260
 //#define SHARED_MEMORY_MAGIC_NUMBER 201810250
@@ -935,6 +936,7 @@ struct b3PhysicsSimulationParameters
 	double m_gravityAcceleration[3];
 	int m_numSimulationSubSteps;
 	int m_numSolverIterations;
+	double m_warmStartingFactor;
 	int m_useRealTimeSimulation;
 	int m_useSplitImpulse;
 	double m_splitImpulsePenetrationThreshold;
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 4364cbbab..419048d71 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -336,19 +336,19 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
 				struct b3ForwardDynamicsAnalyticsArgs analyticsData;
 				int numIslands = 0;
 				int i;
-				PyObject* pyAnalyticsData = PyTuple_New(numIslands);
-
+                                PyObject* val = 0;
+                                
 				numIslands = b3GetStatusForwardDynamicsAnalyticsData(statusHandle, &analyticsData);
-
+				PyObject* pyAnalyticsData = PyTuple_New(numIslands);
+                                
 				for (i=0;i<numIslands;i++)
 				{
-					int numFields = 4;
-					PyObject* pyIslandData = PyTuple_New(numFields);
-					PyTuple_SetItem(pyIslandData, 0, PyLong_FromLong(analyticsData.m_islandData[i].m_islandId));
-					PyTuple_SetItem(pyIslandData, 1, PyLong_FromLong(analyticsData.m_islandData[i].m_numBodies));
-					PyTuple_SetItem(pyIslandData, 2, PyLong_FromLong(analyticsData.m_islandData[i].m_numIterationsUsed));
-					PyTuple_SetItem(pyIslandData, 3, PyFloat_FromDouble(analyticsData.m_islandData[i].m_remainingLeastSquaresResidual));
-					PyTuple_SetItem(pyAnalyticsData, i, pyIslandData);
+                                        val = Py_BuildValue("{s:i, s:i, s:i, s:d}",
+                                                            "islandId", analyticsData.m_islandData[i].m_islandId,
+                                                            "numBodies", analyticsData.m_islandData[i].m_numBodies,
+                                                            "numIterationsUsed", analyticsData.m_islandData[i].m_numIterationsUsed,
+                                                            "remainingResidual", analyticsData.m_islandData[i].m_remainingLeastSquaresResidual);
+					PyTuple_SetItem(pyAnalyticsData, i, val);
 				}
 
 				return pyAnalyticsData;
@@ -1577,6 +1577,9 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 
 	int minimumSolverIslandSize = -1;
 	int reportSolverAnalytics = -1;
+
+	double warmStartingFactor = -1;
+
 	int physicsClientId = 0;
 
 	static char* kwlist[] = {"fixedTimeStep",
@@ -1603,11 +1606,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 							 "globalCFM",
 							 "minimumSolverIslandSize",
 							 "reportSolverAnalytics",
+							 "warmStartingFactor",
 							 "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
 									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, 
-									&reportSolverAnalytics, &physicsClientId))
+									&reportSolverAnalytics, &warmStartingFactor, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1728,6 +1732,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		{
 			b3PhysicsParamSetSolverAnalytics(command, reportSolverAnalytics);
 		}
+		if (warmStartingFactor >= 0)
+		{
+			b3PhysicsParamSetWarmStartingFactor(command, warmStartingFactor);
+		}
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}