Merge pull request #1350 from bingjeff/add_mass_matrix

[pybullet] Add calculateMassMatrix.

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -3397,6 +3397,50 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
     return true;
 }
 
+B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ)
+{
+    PhysicsClient* cl = (PhysicsClient*)physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    
+    command->m_type = CMD_CALCULATE_MASS_MATRIX;
+    command->m_updateFlags = 0;
+    int numJoints = cl->getNumJoints(bodyIndex);
+    for (int i = 0; i < numJoints; i++)
+    {
+        command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
+    }
+    
+    return (b3SharedMemoryCommandHandle)command;
+}
+
+
+B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
+{
+    const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+    btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
+    if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
+        return false;
+    
+    if (dofCount)
+    {
+    	*dofCount = status->m_massMatrixResultArgs.m_dofCount;
+    }
+    if (massMatrix)
+    {
+    	int numElements = status->m_massMatrixResultArgs.m_dofCount * status->m_massMatrixResultArgs.m_dofCount;
+        for (int i = 0; i < numElements; i++)
+        {
+            massMatrix[i] = status->m_massMatrixResultArgs.m_massMatrix[i];
+        }
+
+    }
+    
+    return true;
+}
+
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
 B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -311,19 +311,24 @@ B3_SHARED_API	void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
 	const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
-
 B3_SHARED_API	int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
 	int* bodyUniqueId,
 	int* dofCount,
 	double* jointForces);
 
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
-
 B3_SHARED_API	int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
 	int* dofCount,
 	double* linearJacobian,
 	double* angularJacobian);
 
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ);
+B3_SHARED_API	int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle,
+	int* dofCount,
+	double* massMatrix);
+
+
+
 ///compute the joint positions to move the end effector to a desired target using inverse kinematics
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
 B3_SHARED_API	void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6717,6 +6717,58 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     hasStatus = true;
                     break;
                 }
+                case CMD_CALCULATE_MASS_MATRIX:
+                {
+					BT_PROFILE("CMD_CALCULATE_MASS_MATRIX");
+
+                    SharedMemoryStatus& serverCmd = serverStatusOut;
+                    InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId);
+                    if (bodyHandle && bodyHandle->m_multiBody)
+                    {
+                        serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
+                        
+                        btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
+                        
+                        if (tree)
+                        {
+                            int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
+                            const int numDofs = bodyHandle->m_multiBody->getNumDofs();
+                            const int totDofs = numDofs + baseDofs;
+                            btInverseDynamics::vecx q(totDofs);
+                            btInverseDynamics::matxx massMatrix(totDofs, totDofs);
+                            for (int i = 0; i < numDofs; i++)
+                            {
+                                q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i];
+                            }
+                            if (-1 != tree->calculateMassMatrix(q, &massMatrix))
+                            {
+                                serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
+                                // Fill in the result into the shared memory.
+                                for (int i = 0; i < (totDofs); ++i)
+                                {
+                                    for (int j = 0; j < (totDofs); ++j)
+                                    {
+                                        int element = (totDofs) * i + j;
+                                        serverCmd.m_massMatrixResultArgs.m_massMatrix[element] = massMatrix(i,j);
+                                    }
+                                }
+                                serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
+                            }
+                            else
+                            {
+                                serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
+                            }
+                        }
+                        
+                    }
+                    else
+                    {
+                        serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
+                    }
+                    
+                    hasStatus = true;
+                    break;
+                }
                 case CMD_APPLY_EXTERNAL_FORCE:
                 {
 					BT_PROFILE("CMD_APPLY_EXTERNAL_FORCE");

examples/SharedMemory/SharedMemoryCommands.h

@@ -641,6 +641,18 @@ struct CalculateJacobianResultArgs
     double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
 };
 
+struct CalculateMassMatrixArgs
+{
+	int m_bodyUniqueId;
+	double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateMassMatrixResultArgs
+{
+	int m_dofCount;
+	double m_massMatrix[MAX_DEGREE_OF_FREEDOM * MAX_DEGREE_OF_FREEDOM];
+};
+
 enum EnumCalculateInverseKinematicsFlags
 {
     IK_HAS_TARGET_POSITION=1,
@@ -972,6 +984,7 @@ struct SharedMemoryCommand
         struct ExternalForceArgs m_externalForceArguments;
 		struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
         struct CalculateJacobianArgs m_calculateJacobianArguments;
+        struct CalculateMassMatrixArgs m_calculateMassMatrixArguments;
         struct b3UserConstraint m_userConstraintArguments;
         struct RequestContactDataArgs m_requestContactPointArguments;
 		struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
@@ -1046,6 +1059,7 @@ struct SharedMemoryStatus
 		struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
 		struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
         struct CalculateJacobianResultArgs m_jacobianResultArgs;
+        struct CalculateMassMatrixResultArgs m_massMatrixResultArgs;
 		struct SendContactDataArgs m_sendContactPointArgs;
 		struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
 		struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;

examples/SharedMemory/SharedMemoryPublic.h

@@ -43,6 +43,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_CALCULATE_INVERSE_DYNAMICS,
     CMD_CALCULATE_INVERSE_KINEMATICS,
     CMD_CALCULATE_JACOBIAN,
+    CMD_CALCULATE_MASS_MATRIX,
     CMD_USER_CONSTRAINT,
     CMD_REQUEST_CONTACT_POINT_INFORMATION,
     CMD_REQUEST_RAY_CAST_INTERSECTIONS,
@@ -120,6 +121,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
         CMD_CALCULATED_JACOBIAN_COMPLETED,
         CMD_CALCULATED_JACOBIAN_FAILED,
+        CMD_CALCULATED_MASS_MATRIX_COMPLETED,
+        CMD_CALCULATED_MASS_MATRIX_FAILED,
 		CMD_CONTACT_POINT_INFORMATION_COMPLETED,
 		CMD_CONTACT_POINT_INFORMATION_FAILED,
 		CMD_REQUEST_AABB_OVERLAP_COMPLETED,
@@ -633,9 +636,7 @@ struct b3PluginArguments
 	int m_numInts;
 	int m_ints[B3_MAX_PLUGIN_ARG_SIZE];
 	int m_numFloats;
-	int m_floats[B3_MAX_PLUGIN_ARG_SIZE];
+	double m_floats[B3_MAX_PLUGIN_ARG_SIZE];
-
-
 };
 
 

examples/pybullet/pybullet.c

@@ -7168,8 +7168,8 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
 				double* jointPositions = (double*)malloc(byteSizeJoints);
 				double* jointVelocities = (double*)malloc(byteSizeJoints);
 				double* jointAccelerations = (double*)malloc(byteSizeJoints);
-				double* linearJacobian = (double*)malloc(3 * byteSizeJoints);
+				double* linearJacobian = NULL;
-				double* angularJacobian = (double*)malloc(3 * byteSizeJoints);
+				double* angularJacobian = NULL;
 
 				pybullet_internalSetVectord(localPosition, localPoint);
 				for (i = 0; i < numJoints; i++)
@@ -7267,6 +7267,100 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
 	return Py_None;
 }
 
+/// Given an object id, joint positions, joint velocities and joint
+/// accelerations, compute the Jacobian
+static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	{
+		int bodyUniqueId;
+		PyObject* objPositions;
+		int physicsClientId = 0;
+		b3PhysicsClientHandle sm = 0;
+		static char* kwlist[] = {"bodyUniqueId", "objPositions", "physicsClientId", NULL};
+		if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist,
+				&bodyUniqueId, &objPositions, &physicsClientId))
+		{
+			return NULL;
+		}
+		sm = getPhysicsClient(physicsClientId);
+		if (sm == 0)
+		{
+			PyErr_SetString(SpamError, "Not connected to physics server.");
+			return NULL;
+		}
+
+		{
+			int szObPos = PySequence_Size(objPositions);
+			int numJoints = b3GetNumJoints(sm, bodyUniqueId);
+			if (numJoints && (szObPos == numJoints))
+			{
+				int byteSizeJoints = sizeof(double) * numJoints;
+				PyObject* pyResultList;
+				double* jointPositions = (double*)malloc(byteSizeJoints);
+				double* massMatrix = NULL;
+				int i;
+				for (i = 0; i < numJoints; i++)
+				{
+					jointPositions[i] =
+						pybullet_internalGetFloatFromSequence(objPositions, i);
+				}
+				{
+					b3SharedMemoryStatusHandle statusHandle;	
+					int statusType;
+					b3SharedMemoryCommandHandle commandHandle =
+						b3CalculateMassMatrixCommandInit(sm, bodyUniqueId, jointPositions);
+					statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+					statusType = b3GetStatusType(statusHandle);
+					if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
+					{
+						int dofCount;
+						b3GetStatusMassMatrix(statusHandle, &dofCount, NULL);
+						if (dofCount)
+						{
+							pyResultList = PyTuple_New(dofCount);
+							int byteSizeDofCount = sizeof(double) * dofCount;
+							massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
+							b3GetStatusMassMatrix(statusHandle, NULL, massMatrix);
+							if (massMatrix)
+							{
+								int r;
+								for (r = 0; r < dofCount; ++r) {
+									int c;
+									PyObject* pyrow = PyTuple_New(dofCount);
+									for (c = 0; c < dofCount; ++c) {
+										int element = r * dofCount + c;
+										PyTuple_SetItem(pyrow, c,
+													PyFloat_FromDouble(massMatrix[element]));
+									}
+									PyTuple_SetItem(pyResultList, r, pyrow);
+								}
+							}
+						}
+					}
+					else
+					{
+						PyErr_SetString(SpamError,
+										"Internal error in calculateJacobian");
+					}
+				}
+				free(jointPositions);
+				free(massMatrix);
+				if (pyResultList) return pyResultList;
+			}
+			else
+			{
+				PyErr_SetString(SpamError,
+								"calculateMassMatrix [numJoints] needs to be "
+								"positive and [joint positions] "
+								"need to match the number of joints.");
+				return NULL;
+			}
+		}
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
 static PyMethodDef SpamMethods[] = {
 
 	{"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS,
@@ -7572,6 +7666,8 @@ static PyMethodDef SpamMethods[] = {
 	 "accelerations, compute the joint forces using Inverse Dynamics"},
 
 	 {"calculateJacobian", (PyCFunction)pybullet_calculateJacobian, METH_VARARGS | METH_KEYWORDS,
+	 "linearJacobian, angularJacobian = calculateJacobian(bodyUniqueId, "
+	 "linkIndex, localPosition, objPositions, objVelocities, objAccelerations, physicsClientId=0)\n"
 	 "Compute the jacobian for a specified local position on a body and its kinematics.\n"
 	 "Args:\n"
 	 "  bodyIndex - a scalar defining the unique object id.\n"
@@ -7584,6 +7680,15 @@ static PyMethodDef SpamMethods[] = {
 	 "  linearJacobian - a list of the partial linear velocities of the jacobian.\n"
 	 "  angularJacobian - a list of the partial angular velocities of the jacobian.\n"},
 	
+	{"calculateMassMatrix", (PyCFunction)pybullet_calculateMassMatrix, METH_VARARGS | METH_KEYWORDS,
+	"massMatrix = calculateMassMatrix(bodyUniqueId, objPositions, physicsClientId=0)\n"
+	 "Compute the mass matrix for an object and its chain of bodies.\n"
+	 "Args:\n"
+	 "  bodyIndex - a scalar defining the unique object id.\n"
+	 "  objPositions - a list of the joint positions.\n"
+	 "Returns:\n"
+	 "  massMatrix - a list of lists of the mass matrix components.\n"},
+
 	{"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics,
 	 METH_VARARGS | METH_KEYWORDS,
 	 "Inverse Kinematics bindings: Given an object id, "

test/GwenOpenGLTest/OpenGLSample.cpp

@@ -331,7 +331,7 @@ int main()
 
     int majorGlVersion, minorGlVersion;
 
-    if (!sscanf((const char*)glGetString(GL_VERSION), "%d.%d", &majorGlVersion, &minorGlVersion)==2)
+    if (!(sscanf((const char*)glGetString(GL_VERSION), "%d.%d", &majorGlVersion, &minorGlVersion)==2))
     {
         printf("Exit: Error cannot extract OpenGL version from GL_VERSION string\n");
         exit(0);

test/enet/chat/server/main.cpp

@@ -64,7 +64,7 @@ int main(int argc, char *argv[])
 					break;
 
 				case ENET_EVENT_TYPE_RECEIVE:
-					printf("A packet of length %u containing '%s' was "
+					printf("A packet of length %lu containing '%s' was "
 						"received from %s on channel %u.\n",
 						event.packet->dataLength,
 						event.packet->data,
@@ -83,6 +83,8 @@ int main(int argc, char *argv[])
 
 					event.peer->data = NULL;
 
+					break;
+				case ENET_EVENT_TYPE_NONE:
 					break;
 				}
 			}