Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-07-22 05:59:32 +00:00
Code Issues Packages Projects Releases Wiki Activity

expose computeDofCount to pybullet

Browse Source 
expose computeDofCount and calculateMassMatrix to BulletRobotics C++ API
(all untested)
This commit is contained in:
Erwin Coumans
2020-02-14 19:58:32 -08:00
parent 56e6893ed9
commit 0617f4f3cd
3 changed files with 78 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -988,6 +988,45 @@ bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseKinematics(const struct
return result;
}
int b3RobotSimulatorClientAPI_NoDirect::computeDofCount(int bodyUniqueId) const
{
if (!isConnected())
{
b3Warning("Not connected");
return 0;
}
return b3ComputeDofCount(m_data->m_physicsClientHandle, bodyUniqueId);
}
int b3RobotSimulatorClientAPI_NoDirect::calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags)
{
if (!isConnected())
{
b3Warning("Not connected");
return 0;
}
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle =
b3CalculateMassMatrixCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, numJointPositions);
b3CalculateMassMatrixSetFlags(commandHandle, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
{
int dofCount;
b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, &dofCount, NULL);
if (dofCount)
{
b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, NULL, massMatrix);
return dofCount;
}
}
return 0;
}
bool b3RobotSimulatorClientAPI_NoDirect::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
{
if (!isConnected())