Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add getClosestPoints.py example.

Browse Source 
allow to perform a getClosestPoints query with a collisionShape and world transform (position, orientation) that isn't part of the world.
(use createCollisionShape to create it)
This commit is contained in:
erwincoumans
2018-09-22 13:17:09 -07:00
parent 64a7a3d82f
commit cdf8c908ad
6 changed files with 270 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
examples/pybullet/examples/getClosestPoints.py Normal file
View File

@@ -0,0 +1,38 @@
import pybullet as p
import time
p.connect(p.GUI)
useCollisionShapeQuery = False
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2,0.2,0.2])
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom,basePosition=[0.5,0,1])
baseOrientationB = p.getQuaternionFromEuler([0,0.3,0])#[0,0.5,0.5,0]
basePositionB = [1.5,0,1]
obB = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geomBox,basePosition=basePositionB,baseOrientation=baseOrientationB )
lineWidth=3
colorRGB=[1,0,0]
lineId=p.addUserDebugLine(lineFromXYZ=[0,0,0],lineToXYZ=[0,0,0],lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0)
pitch=0
while (p.isConnected()):
pitch += 0.01
if (pitch>=3.1415*2.):
pitch=0
baseOrientationB = p.getQuaternionFromEuler([0,pitch,0])#[0,0.5,0.5,0]
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
if (useCollisionShapeQuery):
pts = p.getClosestPoints(bodyA=-1, bodyB=-1, distance=100, collisionShapeA=geom,collisionShapeB=geomBox, collisionShapePositionA=[0.5,0,1],collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
if len(pts)>0:
#print(pts)
distance = pts[0][8]
#print("distance=",distance)
ptA = pts[0][5]
ptB = pts[0][6]
p.addUserDebugLine(lineFromXYZ=ptA,lineToXYZ=ptB,lineColorRGB=colorRGB,lineWidth=lineWidth,lifeTime=0,replaceItemUniqueId=lineId);
time.sleep(1./240.)

59
examples/pybullet/pybullet.c
View File

@@ -6002,6 +6002,17 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
int bodyUniqueIdB = -1;
int linkIndexA = -2;
int linkIndexB = -2;
int collisionShapeA = -1;
int collisionShapeB = -1;
PyObject* collisionShapePositionAObj = 0;
PyObject* collisionShapeOrientationAObj = 0;
double collisionShapePositionA[3]={0,0,0};
double collisionShapeOrientationA[4]={0,0,0,1};
PyObject* collisionShapePositionBObj = 0;
PyObject* collisionShapeOrientationBObj = 0;
double collisionShapePositionB[3]={0,0,0};
double collisionShapeOrientationB[4]={0,0,0,1};
double distanceThreshold = 0.f;
@@ -6011,10 +6022,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
int statusType;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "physicsClientId", NULL};
static char* kwlist[] = {"bodyA", "bodyB", "distance", "linkIndexA", "linkIndexB", "collisionShapeA","collisionShapeB", "collisionShapePositionA","collisionShapePositionB","collisionShapeOrientationA","collisionShapeOrientationB","physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iiiiOOOOi", kwlist,
&bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,
&collisionShapeA, &collisionShapeB,
&collisionShapePositionAObj,&collisionShapePositionBObj,
&collisionShapeOrientationA,&collisionShapeOrientationBObj,
&physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -6025,8 +6040,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
}
commandHandle = b3InitClosestDistanceQuery(sm);
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
if (bodyUniqueIdA>=0)
{
b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
}
if (bodyUniqueIdB>=0)
{
b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB);
}
b3SetClosestDistanceThreshold(commandHandle, distanceThreshold);
if (linkIndexA >= -1)
{
@@ -6036,6 +6057,34 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
{
b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB);
}
if (collisionShapeA>=0)
{
b3SetClosestDistanceFilterCollisionShapeA(commandHandle, collisionShapeA);
}
if (collisionShapeB>=0)
{
b3SetClosestDistanceFilterCollisionShapeB(commandHandle, collisionShapeB);
}
if (collisionShapePositionAObj)
{
pybullet_internalSetVectord(collisionShapePositionAObj,collisionShapePositionA);
b3SetClosestDistanceFilterCollisionShapePositionA(commandHandle, collisionShapePositionA);
}
if (collisionShapePositionBObj)
{
pybullet_internalSetVectord(collisionShapePositionBObj,collisionShapePositionB);
b3SetClosestDistanceFilterCollisionShapePositionB(commandHandle, collisionShapePositionB);
}
if (collisionShapeOrientationAObj)
{
pybullet_internalSetVector4d(collisionShapeOrientationAObj,collisionShapeOrientationA);
b3SetClosestDistanceFilterCollisionShapeOrientationA(commandHandle, collisionShapeOrientationA);
}
if (collisionShapeOrientationBObj)
{
pybullet_internalSetVector4d(collisionShapeOrientationBObj,collisionShapeOrientationB);
b3SetClosestDistanceFilterCollisionShapeOrientationB(commandHandle, collisionShapeOrientationB);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);