Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1350 from bingjeff/add_mass_matrix

Browse Source 
[pybullet] Add calculateMassMatrix.
This commit is contained in:
erwincoumans
2017-10-05 07:45:30 -07:00
committed by GitHub
parent 3821f85019 e04820af73
commit a2d6a2e822
8 changed files with 232 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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)
{

9
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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*/]);

52
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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");

14
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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;

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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];
};

109
examples/pybullet/pybullet.c
View File

@@ -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* angularJacobian = (double*)malloc(3 * byteSizeJoints);
double* linearJacobian = NULL;
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"
@@ -7583,6 +7679,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,

2
test/GwenOpenGLTest/OpenGLSample.cpp
View File

@@ -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);

4
test/enet/chat/server/main.cpp
View File

@@ -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;
}
}