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

pybullet.createCollisionShape, createVisualShape, createMultiBody, programmatic creation using ProgrammaticUrdfInterface

Browse Source 
(still preliminary, not ready for commit yet, see examples\pybullet\examples\createSphereMultiBodies.py)
This commit is contained in:
Erwin Coumans
2017-06-03 10:57:56 -07:00
parent ff695dd328
commit b23cb1dd2c
16 changed files with 1032 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
examples/pybullet/examples/createSphereMultiBodies.py Normal file
View File

@@ -0,0 +1,30 @@
import pybullet as p
import time
p.connect(p.GUI)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (10):
for j in range (10):
for k in range (10):
if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1])
else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1])
#p.changeDynamics(sphereUid,-1,spinningFriction=0.1, rollingFriction=0.1)
#p.loadSDF("stadium.sdf")
p.loadURDF("plane.urdf", useMaximalCoordinates=0)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while (1):
time.sleep(0.01)

165
examples/pybullet/pybullet.c
View File

@@ -4701,6 +4701,152 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject*
return NULL;
}
static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
int shapeType=-1;
double radius=-1;
PyObject* halfExtentsObj=0;
static char* kwlist[] = {"shapeType","radius","halfExtents", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOi", kwlist,
&shapeType, &radius,&halfExtentsObj, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (shapeType>=GEOM_SPHERE)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm);
if (shapeType==GEOM_SPHERE && radius>0)
{
b3CreateCollisionShapeAddSphere(commandHandle,radius);
}
if (shapeType==GEOM_BOX && halfExtentsObj)
{
double halfExtents[3] = {1,1,1};
pybullet_internalSetVectord(halfExtentsObj,halfExtents);
b3CreateCollisionShapeAddBox(commandHandle,halfExtents);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
{
int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createCollisionShape failed.");
return NULL;
}
#if 0
b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle);
#endif
static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
int test=-1;
static char* kwlist[] = {"test",NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &test,
&physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (test>=0)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CreateVisualShapeCommandInit(sm);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
{
int uid = b3GetStatusVisualShapeUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createVisualShape failed.");
return NULL;
}
static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
double baseMass = 0;
int baseCollisionShapeIndex=-1;
int baseVisualShapeIndex=-1;
PyObject* basePosObj=0;
PyObject* baseOrnObj=0;
static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation",
// "linkParentIndices", "linkJointTypes","linkMasses","linkCollisionShapeIndices",
NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOi", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex,
&basePosObj, &baseOrnObj,&physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm);
double basePosition[3]={0,0,0};
double baseOrientation[4]={0,0,0,1};
pybullet_internalSetVectord(basePosObj,basePosition);
pybullet_internalSetVector4d(baseOrnObj,baseOrientation);
int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED)
{
int uid = b3GetStatusMultiBodyUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createMultiBody failed.");
return NULL;
}
static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
{
b3SharedMemoryCommandHandle commandHandle;
@@ -6218,6 +6364,16 @@ static PyMethodDef SpamMethods[] = {
{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
"Load multibodies from an MJCF file."},
{"createCollisionShape", (PyCFunction)pybullet_createCollisionShape, METH_VARARGS | METH_KEYWORDS,
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,
"Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{"createMultiBody", (PyCFunction)pybullet_createMultiBody, METH_VARARGS | METH_KEYWORDS,
"Create a multi body. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS,
"Create a constraint between two bodies. Returns a (int) unique id, if successfull."},
@@ -6652,6 +6808,15 @@ initpybullet(void)
PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT);
PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN);
PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH);
PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);