diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1fa562739..47be09e4c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1308,12 +1308,25 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) return bodyId; } +b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_REQUEST_COLLISION_INFO; + command->m_updateFlags = 0; + command->m_requestCollisionInfoArgs.m_bodyUniqueId = bodyUniqueId; + return (b3SharedMemoryCommandHandle) command; +} + 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) + const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs; + btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED); + if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED) return 0; if (linkIndex==-1) @@ -1330,13 +1343,13 @@ int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, doub 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]; + aabbMin[0] = args.m_linkWorldAABBsMin[linkIndex*3+0]; + aabbMin[1] = args.m_linkWorldAABBsMin[linkIndex*3+1]; + aabbMin[2] = args.m_linkWorldAABBsMin[linkIndex*3+2]; - aabbMax[0] = args.m_linkWorldAABBsMax[0]; - aabbMax[1] = args.m_linkWorldAABBsMax[1]; - aabbMax[2] = args.m_linkWorldAABBsMax[2]; + aabbMax[0] = args.m_linkWorldAABBsMax[linkIndex*3+0]; + aabbMax[1] = args.m_linkWorldAABBsMax[linkIndex*3+1]; + aabbMax[2] = args.m_linkWorldAABBsMax[linkIndex*3+2]; return 1; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 299cf1974..ee82cfbec 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -57,6 +57,8 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* actualStateQdot[], const double* jointReactionForces[]); + +b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); 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. diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index a2061b246..66ef8b435 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1079,6 +1079,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Request createVisualShape failed"); break; } + case CMD_REQUEST_COLLISION_INFO_COMPLETED: + { + break; + } + case CMD_REQUEST_COLLISION_INFO_FAILED: + { + b3Warning("Request getCollisionInfo failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index e49784d47..cafcd5449 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -914,7 +914,16 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Warning("createMultiBody failed"); break; } - + case CMD_REQUEST_COLLISION_INFO_COMPLETED: + { + break; + } + case CMD_REQUEST_COLLISION_INFO_FAILED: + { + b3Warning("Request getCollisionInfo failed"); + break; + } + default: { //b3Warning("Unknown server status type"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3ed5def06..6354e8fe0 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4023,6 +4023,105 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_REQUEST_COLLISION_INFO: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED; + hasStatus=true; + int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId; + InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + + + if (body && body->m_multiBody) + { + btMultiBody* mb = body->m_multiBody; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; + serverCmd.m_sendCollisionInfoArgs.m_numLinks = body->m_multiBody->getNumLinks(); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; + + if (body->m_multiBody->getBaseCollider()) + { + btTransform tr; + tr.setOrigin(mb->getBasePos()); + tr.setRotation(mb->getWorldToBaseRot().inverse()); + + btVector3 aabbMin,aabbMax; + body->m_multiBody->getBaseCollider()->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + } + for (int l=0;lgetNumLinks();l++) + { + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = -1; + serverCmd.m_sendCollisionInfoArgs.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_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMin[3*l+2] = aabbMin[2]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_linkWorldAABBsMax[3*l+2] = aabbMax[2]; + } + + } + } + else + { + if (body && body->m_rigidBody) + { + btRigidBody* rb = body->m_rigidBody; + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; + serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = 0; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = 0; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = -1; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = -1; + if (rb->getCollisionShape()) + { + btTransform tr = rb->getWorldTransform(); + + btVector3 aabbMin,aabbMax; + rb->getCollisionShape()->getAabb(tr,aabbMin,aabbMax); + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + } + } + } + break; + } + case CMD_REQUEST_ACTUAL_STATE: { BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); @@ -4077,28 +4176,7 @@ 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]; @@ -4200,29 +4278,7 @@ 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); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index f676f7c35..27ae33970 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -432,9 +432,7 @@ struct SendActualStateArgs 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]; double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; @@ -447,10 +445,24 @@ 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]; +}; + +struct b3SendCollisionInfoArgs +{ + int m_numLinks; + double m_rootWorldAABBMin[3]; + double m_rootWorldAABBMax[3]; + double m_linkWorldAABBsMin[3*MAX_NUM_LINKS]; double m_linkWorldAABBsMax[3*MAX_NUM_LINKS]; }; +struct b3RequestCollisionInfoArgs +{ + int m_bodyUniqueId; +}; + + enum EnumSensorTypes { SENSOR_FORCE_TORQUE=1, @@ -918,6 +930,7 @@ struct SharedMemoryCommand struct b3CreateCollisionShapeArgs m_createCollisionShapeArgs; struct b3CreateVisualShapeArgs m_createVisualShapeArgs; struct b3CreateMultiBodyArgs m_createMultiBodyArgs; + struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs; }; }; @@ -987,6 +1000,7 @@ struct SharedMemoryStatus struct b3CreateCollisionShapeResultArgs m_createCollisionShapeResultArgs; struct b3CreateVisualShapeResultArgs m_createVisualShapeResultArgs; struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs; + struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 04c06317e..9d1f92507 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -65,6 +65,7 @@ enum EnumSharedMemoryClientCommand CMD_CREATE_COLLISION_SHAPE, CMD_CREATE_VISUAL_SHAPE, CMD_CREATE_MULTI_BODY, + CMD_REQUEST_COLLISION_INFO, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -156,6 +157,8 @@ enum EnumSharedMemoryServerStatus CMD_CREATE_VISUAL_SHAPE_COMPLETED, CMD_CREATE_MULTI_BODY_FAILED, CMD_CREATE_MULTI_BODY_COMPLETED, + CMD_REQUEST_COLLISION_INFO_COMPLETED, + CMD_REQUEST_COLLISION_INFO_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/pybullet/examples/getAABB.py b/examples/pybullet/examples/getAABB.py new file mode 100644 index 000000000..59a2bfce0 --- /dev/null +++ b/examples/pybullet/examples/getAABB.py @@ -0,0 +1,56 @@ +import pybullet as p +p.connect(p.GUI) +r2d2 = p.loadURDF("r2d2.urdf") + + +aabb = p.getAABB(r2d2) +aabbMin = aabb[0] +aabbMax = aabb[1] + +f = [aabbMin[0],aabbMin[1],aabbMin[2]] +t = [aabbMax[0],aabbMin[1],aabbMin[2]] +p.addUserDebugLine(f,t,[1,1,1]) +f = [aabbMin[0],aabbMin[1],aabbMin[2]] +t = [aabbMin[0],aabbMax[1],aabbMin[2]] +p.addUserDebugLine(f,t,[1,1,1]) +f = [aabbMin[0],aabbMin[1],aabbMin[2]] +t = [aabbMin[0],aabbMin[1],aabbMax[2]] +p.addUserDebugLine(f,t,[1,1,1]) + +f = [aabbMax[0],aabbMax[1],aabbMax[2]] +t = [aabbMin[0],aabbMax[1],aabbMax[2]] +p.addUserDebugLine(f,t,[1,1,1]) +f = [aabbMax[0],aabbMax[1],aabbMax[2]] +t = [aabbMax[0],aabbMin[1],aabbMax[2]] +p.addUserDebugLine(f,t,[1,1,1]) +f = [aabbMax[0],aabbMax[1],aabbMax[2]] +t = [aabbMax[0],aabbMax[1],aabbMin[2]] +p.addUserDebugLine(f,t,[1,1,1]) + +for i in range (p.getNumJoints(r2d2)): + aabb = p.getAABB(r2d2,i) + aabbMin = aabb[0] + aabbMax = aabb[1] + + f = [aabbMin[0],aabbMin[1],aabbMin[2]] + t = [aabbMax[0],aabbMin[1],aabbMin[2]] + p.addUserDebugLine(f,t,[1,1,1]) + f = [aabbMin[0],aabbMin[1],aabbMin[2]] + t = [aabbMin[0],aabbMax[1],aabbMin[2]] + p.addUserDebugLine(f,t,[1,1,1]) + f = [aabbMin[0],aabbMin[1],aabbMin[2]] + t = [aabbMin[0],aabbMin[1],aabbMax[2]] + p.addUserDebugLine(f,t,[1,1,1]) + + f = [aabbMax[0],aabbMax[1],aabbMax[2]] + t = [aabbMin[0],aabbMax[1],aabbMax[2]] + p.addUserDebugLine(f,t,[1,1,1]) + f = [aabbMax[0],aabbMax[1],aabbMax[2]] + t = [aabbMax[0],aabbMin[1],aabbMax[2]] + p.addUserDebugLine(f,t,[1,1,1]) + f = [aabbMax[0],aabbMax[1],aabbMax[2]] + t = [aabbMax[0],aabbMax[1],aabbMin[2]] + p.addUserDebugLine(f,t,[1,1,1]) + +while(1): + a=0 \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d2cab391f..bbcc8c3cb 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2010,12 +2010,12 @@ static PyObject* pybullet_getAABB(PyObject* self, PyObject* args, PyObject* keyw } cmd_handle = - b3RequestActualStateCommandInit(sm, bodyUniqueId); + b3RequestCollisionInfoCommandInit(sm, bodyUniqueId); status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); status_type = b3GetStatusType(status_handle); - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + if (status_type != CMD_REQUEST_COLLISION_INFO_COMPLETED) { PyErr_SetString(SpamError, "getAABB failed."); return NULL;