unlock thread at exitPhysics

pybullet: don't crash in inverse kinematic if #dofs don't match due to free base
C-API: don't crash if status/statusHandle = 0

examples/ExampleBrowser/InProcessExampleBrowser.cpp

@@ -391,7 +391,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data)
                         } else
                         {
 //                              printf("polling..");
-							b3Clock::usleep(1000);
+							b3Clock::usleep(0);
                         }
                 };
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1654,6 +1654,9 @@ B3_SHARED_API	b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3Ph
 B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3])
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+	btAssert(status);
+	if (status==0)
+		return 0;
     const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs;
     btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED);
     if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED)
@@ -1695,6 +1698,9 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle
                            const double* actualStateQdot[],
                            const double* jointReactionForces[]) {
     const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
+	btAssert(status);
+	if (status==0)
+		return 0;
     const SendActualStateArgs &args = status->m_sendActualStateArgs;
     btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
     if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
@@ -1910,6 +1916,8 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat
 
 	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
 	b3Assert(status);
+	if (status==0)
+		return statusUniqueId;
 	b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
 	if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
 	{
@@ -1924,11 +1932,14 @@ B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHan
 
 	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
 	b3Assert(status);
+	if (status)
+	{
 		b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
 		if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
 		{
 			statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId;
 		}
+	}
 	return statusUniqueId;
 }
 
@@ -2317,12 +2328,14 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
 	b3Assert(status);
+	if (status)
+	{
 		b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
 		if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
 		{
 			return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
 		}
-
+	}
 	return -1;
 
 }
@@ -2635,12 +2648,15 @@ B3_SHARED_API	b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics
 B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue)
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	if (status)
+	{
 		btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
 		if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
 		{
 			*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
 			return 1;
 		}
+	}
 	return 0;
 }
 
@@ -3280,11 +3296,14 @@ B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHa
 {
 	int uid = -1;
 	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	if (status)
+	{
 		btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);	
 		if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
 		{
 			uid = status->m_loadTextureResultArguments.m_textureUniqueId;
 		}
+	}
 	return uid;
 }
 
@@ -3420,6 +3439,9 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
 	double* jointForces)
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	if (status==0)
+		return false;
+
 	btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
 	if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
 		return false;
@@ -3475,6 +3497,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
 B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian)
 {
     const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	if (status==0)
+		return false;
+
     btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
     if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
         return false;
@@ -3528,6 +3553,9 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
 	b3Assert(cl);
 
     const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	if (status==0)
+		return false;
+
     btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
     if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
         return false;
@@ -3661,9 +3689,12 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
 	double* jointPositions)
 {
 	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	btAssert(status);
+	if (status==0)
+		return 0;
 	btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
 	if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
-		return false;
+		return 0;
 
 
 	if (dofCount)
@@ -3682,7 +3713,7 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
 		}
 	}
 
-	return true;
+	return 1;
 }
 
 B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -7659,7 +7659,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 btAlignedObjectArray<double> q_current;
 								q_current.resize(numDofs);
                                 
-                                if (tree)
+								if (tree && (numDofs == tree->numDoFs()))
                                 {
                                     jacSize = jacobian_linear.size();
                                     // Set jacobian value
@@ -7697,7 +7697,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                             }
                                         }
                                     }
-                                }
+                                
                                 
                                 
 									btAlignedObjectArray<double> q_new;
@@ -7785,6 +7785,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
 								}
 							}
+						}
 						hasStatus = true;
 						break;
 					}

examples/SharedMemory/PhysicsServerExample.cpp

@@ -1789,6 +1789,7 @@ void    PhysicsServerExample::exitPhysics()
 {
 		for (int i=0;i<MAX_MOTION_NUM_THREADS;i++)
 		{
+			m_args[i].m_cs2->unlock();
 			m_args[i].m_cs->lock();
 			m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
 			m_args[i].m_cs->unlock();
@@ -1805,7 +1806,7 @@ void    PhysicsServerExample::exitPhysics()
 
                         } else
                         {
-							b3Clock::usleep(1000);
+							b3Clock::usleep(0);
                         }
 						//we need to call 'stepSimulation' to make sure that
 						//other threads get out of blocking state (workerThreadWait)