From febbb92fb685e98034dad29c5ed882b75995ad1f Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Sat, 30 Sep 2017 13:13:46 -0700 Subject: [PATCH 1/5] Fix compile warnings for GwenOpenGLTest and Test_enet_chat_client. --- test/GwenOpenGLTest/OpenGLSample.cpp | 2 +- test/enet/chat/server/main.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/test/GwenOpenGLTest/OpenGLSample.cpp b/test/GwenOpenGLTest/OpenGLSample.cpp index 89a4d908c..95c77b55b 100644 --- a/test/GwenOpenGLTest/OpenGLSample.cpp +++ b/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); diff --git a/test/enet/chat/server/main.cpp b/test/enet/chat/server/main.cpp index ed708131b..d1f85bd99 100644 --- a/test/enet/chat/server/main.cpp +++ b/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; } } From 31ee8bf8bceb47652e3f94d0362cf85a3a309ce5 Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Sat, 30 Sep 2017 13:17:48 -0700 Subject: [PATCH 2/5] Fix small typo in b3PluginArguments. --- examples/SharedMemory/SharedMemoryPublic.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 28b7ec5b1..08fbea6be 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -633,9 +633,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]; }; From 7dfed652871c3f0882dd16d2105338148e95e21d Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Sat, 30 Sep 2017 13:39:11 -0700 Subject: [PATCH 3/5] Add calculateMass to shared memory interface. --- examples/SharedMemory/PhysicsClientC_API.cpp | 44 ++++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 9 +++- examples/SharedMemory/SharedMemoryCommands.h | 14 +++++++ examples/SharedMemory/SharedMemoryPublic.h | 3 ++ 4 files changed, 68 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d8c2fff7f..e38a1b745 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3377,6 +3377,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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d18a547fe..177f2c2cd 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -310,19 +310,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*/]); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b196d6c71..656ac66d8 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -638,6 +638,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, @@ -969,6 +981,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; @@ -1043,6 +1056,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; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 08fbea6be..9045f5db9 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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, From d9782f953ddb77618c308f8a13d5af311c8653a1 Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Sat, 30 Sep 2017 13:56:40 -0700 Subject: [PATCH 4/5] Add calculateMassMatrix to physics server command processor. --- .../PhysicsServerCommandProcessor.cpp | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f9e0cd6a9..83fce956a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6707,6 +6707,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"); From e04820af73d4d5101a4fa32e3ce0b55306516f02 Mon Sep 17 00:00:00 2001 From: Jeffrey Bingham Date: Sat, 30 Sep 2017 20:39:56 -0700 Subject: [PATCH 5/5] Add calculateMassMatrix to pybullet.c --- examples/pybullet/pybullet.c | 109 ++++++++++++++++++++++++++++++++++- 1 file changed, 107 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b332ad650..2b3f2eb8b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -7162,8 +7162,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* angularJacobian = (double*)malloc(3 * byteSizeJoints); + double* linearJacobian = NULL; + double* angularJacobian = NULL; pybullet_internalSetVectord(localPosition, localPoint); for (i = 0; i < numJoints; i++) @@ -7261,6 +7261,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, @@ -7566,6 +7660,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" @@ -7577,6 +7673,15 @@ static PyMethodDef SpamMethods[] = { "Returns:\n" " 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,