AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1190 from erwincoumans/master

Browse Source 
pybullet.getAABB and getAPIVersion
This commit is contained in:
erwincoumans 2017-06-15 21:53:41 -07:00 committed by GitHub
commit 37f5413a07
7 changed files with 200 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -1308,6 +1308,40 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
return bodyId;
}
int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3])
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
return 0;
if (linkIndex==-1)
{
aabbMin[0] = args.m_rootWorldAABBMin[0];
aabbMin[1] = args.m_rootWorldAABBMin[1];
aabbMin[2] = args.m_rootWorldAABBMin[2];
aabbMax[0] = args.m_rootWorldAABBMax[0];
aabbMax[1] = args.m_rootWorldAABBMax[1];
aabbMax[2] = args.m_rootWorldAABBMax[2];
return 1;
}
if (linkIndex >= 0 && linkIndex < args.m_numLinks)
{
aabbMin[0] = args.m_linkWorldAABBsMin[0];
aabbMin[1] = args.m_linkWorldAABBsMin[1];
aabbMin[2] = args.m_linkWorldAABBsMin[2];
aabbMax[0] = args.m_linkWorldAABBsMax[0];
aabbMax[1] = args.m_linkWorldAABBsMax[1];
aabbMax[2] = args.m_linkWorldAABBsMax[2];
return 1;
}
return 0;
}
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -57,6 +57,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[],
const double* jointReactionForces[]);
int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]);
///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);

49
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -4032,6 +4032,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
@ -4040,6 +4041,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks();
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
@ -4074,7 +4077,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] =
body->m_rootLocalInertialFrame.getRotation()[3];
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[0] = 0;
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[1] = 0;
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[2] = 0;
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[0] = -1;
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[1] = -1;
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[2] = -1;
if (body->m_multiBody->getBaseCollider())
{
btVector3 aabbMin,aabbMax;
body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr,aabbMin,aabbMax);
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[0] = aabbMin[0];
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[1] = aabbMin[1];
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMin[2] = aabbMin[2];
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[0] = aabbMax[0];
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[1] = aabbMax[1];
serverCmd.m_sendActualStateArgs.m_rootWorldAABBMax[2] = aabbMax[2];
}
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
@ -4176,6 +4200,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+0] = 0;
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+1] = 0;
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+2] = 0;
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+0] = -1;
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+1] = -1;
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+2] = -1;
if (body->m_multiBody->getLink(l).m_collider)
{
btVector3 aabbMin,aabbMax;
body->m_multiBody->getLinkCollider(l)->getCollisionShape()->getAabb(mb->getLink(l).m_cachedWorldTransform,aabbMin,aabbMax);
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+0] = aabbMin[0];
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+1] = aabbMin[1];
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMin[3*l+2] = aabbMin[2];
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+0] = aabbMax[0];
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+1] = aabbMax[1];
serverCmd.m_sendActualStateArgs.m_linkWorldAABBsMax[3*l+2] = aabbMax[2];
}
btVector3 worldLinVel(0,0,0);
btVector3 worldAngVel(0,0,0);
@ -4220,6 +4267,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -427,10 +427,13 @@ struct RequestActualStateArgs
struct SendActualStateArgs
{
int m_bodyUniqueId;
int m_numLinks;
int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU;
double m_rootLocalInertialFrame[7];
double m_rootWorldAABBMin[3];
double m_rootWorldAABBMax[3];
//actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
@ -444,6 +447,8 @@ struct SendActualStateArgs
double m_linkState[7*MAX_NUM_LINKS];
double m_linkWorldVelocities[6*MAX_NUM_LINKS];//linear velocity and angular velocity in world space (x/y/z each).
double m_linkLocalInertialFrames[7*MAX_NUM_LINKS];
double m_linkWorldAABBsMin[3*MAX_NUM_LINKS];
double m_linkWorldAABBsMax[3*MAX_NUM_LINKS];
};
enum EnumSensorTypes

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -4,7 +4,8 @@
#define SHARED_MEMORY_KEY 12347
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201706001
#define SHARED_MEMORY_MAGIC_NUMBER 201706015
//#define SHARED_MEMORY_MAGIC_NUMBER 201706001
//#define SHARED_MEMORY_MAGIC_NUMBER 201703024
@ -490,6 +491,8 @@ struct b3LinkState
double m_worldLinearVelocity[3]; //only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity)
double m_worldAngularVelocity[3]; //only valid when ACTUAL_STATE_COMPUTE_LINKVELOCITY is set (b3RequestActualStateCommandComputeLinkVelocity)
double m_worldAABBMin[3];//world space bounding minium and maximum box corners.
double m_worldAABBMax[3];
};
//todo: discuss and decide about control mode and combinations

104
examples/pybullet/pybullet.c
View File

@ -1972,6 +1972,89 @@ static int pybullet_internalGetBasePositionAndOrientation(
return 1;
}
static PyObject* pybullet_getAABB(PyObject* self, PyObject* args, PyObject* keywds)
{
int bodyUniqueId = -1;
int linkIndex = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
if (bodyUniqueId < 0)
{
PyErr_SetString(SpamError, "getAABB failed; invalid bodyUniqueId");
return NULL;
}
if (linkIndex < -1)
{
PyErr_SetString(SpamError, "getAABB failed; invalid linkIndex");
return NULL;
}
cmd_handle =
b3RequestActualStateCommandInit(sm, bodyUniqueId);
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getAABB failed.");
return NULL;
}
{
PyObject* pyListAabb=0;
PyObject* pyListAabbMin=0;
PyObject* pyListAabbMax=0;
double aabbMin[3];
double aabbMax[3];
int i=0;
if (b3GetStatusAABB(status_handle, linkIndex, aabbMin, aabbMax))
{
pyListAabb = PyTuple_New(2);
pyListAabbMin = PyTuple_New(3);
pyListAabbMax = PyTuple_New(3);
for (i=0;i<3;i++)
{
PyTuple_SetItem(pyListAabbMin, i, PyFloat_FromDouble(aabbMin[i]));
PyTuple_SetItem(pyListAabbMax, i, PyFloat_FromDouble(aabbMax[i]));
}
PyTuple_SetItem(pyListAabb, 0, pyListAabbMin);
PyTuple_SetItem(pyListAabb, 1, pyListAabbMax);
//PyFloat_FromDouble(basePosition[i]);
return pyListAabb;
}
}
}
PyErr_SetString(SpamError, "getAABB failed.");
return NULL;
}
// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion
// values for the base link of your object
// Object is retrieved based on body index, which is the order
@ -2365,6 +2448,18 @@ static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyOb
#endif
}
// Return the number of joints in an object based on
// body index; body index is based on order of sequence
// the object is loaded into simulation
static PyObject* pybullet_getAPIVersion(PyObject* self, PyObject* args, PyObject* keywds)
{
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(SHARED_MEMORY_MAGIC_NUMBER);
#else
return PyInt_FromLong(SHARED_MEMORY_MAGIC_NUMBER);
#endif
}
// Return the number of joints in an object based on
// body index; body index is based on order of sequence
// the object is loaded into simulation
@ -6470,6 +6565,10 @@ static PyMethodDef SpamMethods[] = {
"Get the world position and orientation of the base of the object. "
"(x,y,z) position vector and (x,y,z,w) quaternion orientation."},
{"getAABB", (PyCFunction)pybullet_getAABB,
METH_VARARGS | METH_KEYWORDS,
"Get the axis aligned bound box min and max coordinates in world space."},
{"resetBasePositionAndOrientation",
(PyCFunction)pybullet_resetBasePositionAndOrientation, METH_VARARGS | METH_KEYWORDS,
"Reset the world position and orientation of the base of the object "
@ -6679,6 +6778,11 @@ static PyMethodDef SpamMethods[] = {
{"setTimeOut", (PyCFunction)pybullet_setTimeOut, METH_VARARGS | METH_KEYWORDS,
"Set the timeOut in seconds, used for most of the API calls."},
{"getAPIVersion", (PyCFunction)pybullet_getAPIVersion,
METH_VARARGS | METH_KEYWORDS,
"Get version of the API. Compatibility exists for connections using the same API version. Make sure both client and server use the same number of bits (32-bit or 64bit)."},
// todo(erwincoumans)
// saveSnapshot
// loadSnapshot

5
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@ -142,9 +142,9 @@ public:
btMultiBodyLinkCollider* getLinkCollider(int index)
{
if (index >= 0 && index < m_colliders.size())
if (index >= 0 && index < getNumLinks())
{
return m_colliders[index];
return getLink(index).m_collider;
}
return 0;
}
@ -659,7 +659,6 @@ private:
btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;
//