AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues 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
Show Stats 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())

4
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@ -783,8 +783,12 @@ public:
void setNumSolverIterations(int numIterations);
void setContactBreakingThreshold(double threshold);
int computeDofCount(int bodyUniqueId) const;
bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs &args, struct b3RobotSimulatorInverseKinematicsResults &results);
int calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags);
bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double *localPosition, const double *jointPositions, const double *jointVelocities, const double *jointAccelerations, double *linearJacobian, double *angularJacobian);
void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable);

36
examples/pybullet/pybullet.c
View File

@ -3945,6 +3945,37 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
}
}
static PyObject* pybullet_computeDofCount(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int bodyUniqueId = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "bodyUniqueId", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(dofCount);
#else
return PyInt_FromLong(dofCount);
#endif
}
}
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
@ -11794,7 +11825,7 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
else
{
PyErr_SetString(SpamError,
"Internal error in calculateJacobian");
"Internal error in calculateMassMatrix");
}
}
free(jointPositions);
@ -11954,6 +11985,9 @@ static PyMethodDef SpamMethods[] = {
{"getBodyInfo", (PyCFunction)pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS,
"Get the body info, given a body unique id."},
{ "computeDofCount", (PyCFunction)pybullet_computeDofCount, METH_VARARGS | METH_KEYWORDS,
"computeDofCount returns the number of degrees of freedom, including 7 degrees of freedom for the base in case of floating base" },
{"syncBodyInfo", (PyCFunction)pybullet_syncBodyInfo, METH_VARARGS | METH_KEYWORDS,
"syncBodyInfo(physicsClientId=0)\n"
"Update body and constraint/joint information, in case other clients made changes."},