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