PyBullet/C-API: implement createVisualShapeArray, with multiple visual shapes (require 1 texture max, since visual shapes are merged)
This commit is contained in:
Erwin Coumans
File diff suppressed because one or more lines are too long
81
examples/pybullet/examples/createVisualShapeArray.py
Normal file
81
examples/pybullet/examples/createVisualShapeArray.py
Normal file
@@ -0,0 +1,81 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
def getRayFromTo(mouseX,mouseY):
|
||||
width, height, viewMat, projMat, cameraUp, camForward, horizon,vertical, _,_,dist, camTarget = p.getDebugVisualizerCamera()
|
||||
camPos = [camTarget[0] - dist*camForward[0],camTarget[1] - dist*camForward[1],camTarget[2] - dist*camForward[2]]
|
||||
farPlane = 10000
|
||||
rayForward = [(camTarget[0]-camPos[0]),(camTarget[1]-camPos[1]),(camTarget[2]-camPos[2])]
|
||||
invLen = farPlane*1./(math.sqrt(rayForward[0]*rayForward[0]+rayForward[1]*rayForward[1]+rayForward[2]*rayForward[2]))
|
||||
rayForward = [invLen*rayForward[0],invLen*rayForward[1],invLen*rayForward[2]]
|
||||
rayFrom = camPos
|
||||
oneOverWidth = float(1)/float(width)
|
||||
oneOverHeight = float(1)/float(height)
|
||||
dHor = [horizon[0] * oneOverWidth,horizon[1] * oneOverWidth,horizon[2] * oneOverWidth]
|
||||
dVer = [vertical[0] * oneOverHeight,vertical[1] * oneOverHeight,vertical[2] * oneOverHeight]
|
||||
rayToCenter=[rayFrom[0]+rayForward[0],rayFrom[1]+rayForward[1],rayFrom[2]+rayForward[2]]
|
||||
rayTo = [rayFrom[0]+rayForward[0] - 0.5 * horizon[0] + 0.5 * vertical[0]+float(mouseX)*dHor[0]-float(mouseY)*dVer[0],
|
||||
rayFrom[1]+rayForward[1] - 0.5 * horizon[1] + 0.5 * vertical[1]+float(mouseX)*dHor[1]-float(mouseY)*dVer[1],
|
||||
rayFrom[2]+rayForward[2] - 0.5 * horizon[2] + 0.5 * vertical[2]+float(mouseX)*dHor[2]-float(mouseY)*dVer[2]]
|
||||
return rayFrom,rayTo
|
||||
|
||||
cid = p.connect(p.SHARED_MEMORY)
|
||||
if (cid<0):
|
||||
p.connect(p.GUI)
|
||||
p.setPhysicsEngineParameter(numSolverIterations=10)
|
||||
p.setTimeStep(1./120.)
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
|
||||
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
|
||||
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
|
||||
#disable rendering during creation.
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
|
||||
#disable tinyrenderer, software (CPU) renderer, we don't use it here
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
|
||||
|
||||
shift = [0,-0.02,0]
|
||||
shift1 = [0,0.1,0]
|
||||
shift2 = [0,0,0]
|
||||
|
||||
meshScale=[0.1,0.1,0.1]
|
||||
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
|
||||
visualShapeId = p.createVisualShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck.obj",""], visualFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale])
|
||||
collisionShapeId = p.createCollisionShapeArray(shapeTypes=[p.GEOM_MESH, p.GEOM_BOX], halfExtents=[[0,0,0],[0.1,0.1,0.1]],fileNames=["duck_vhacd.obj",""], collisionFramePositions=[shift1,shift2,],meshScales=[meshScale,meshScale])
|
||||
|
||||
|
||||
rangex =2
|
||||
rangey = 2
|
||||
for i in range (rangex):
|
||||
for j in range (rangey ):
|
||||
mb = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition = [((-rangex/2)+i*2)*meshScale[0]*2,(-rangey/2+j)*meshScale[1]*4,1], useMaximalCoordinates=False)
|
||||
p.changeVisualShape(mb,-1,rgbaColor=[1,1,1,1])
|
||||
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
|
||||
p.stopStateLogging(logId)
|
||||
p.setGravity(0,0,-10)
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
colors = [[1,0,0,1],[0,1,0,1],[0,0,1,1],[1,1,1,1]]
|
||||
currentColor = 0
|
||||
|
||||
p.getCameraImage(64,64, renderer=p.ER_BULLET_HARDWARE_OPENGL)
|
||||
|
||||
while (1):
|
||||
|
||||
mouseEvents = p.getMouseEvents()
|
||||
for e in mouseEvents:
|
||||
if ((e[0] == 2) and (e[3]==0) and (e[4]& p.KEY_WAS_TRIGGERED)):
|
||||
mouseX = e[1]
|
||||
mouseY = e[2]
|
||||
rayFrom,rayTo=getRayFromTo(mouseX,mouseY)
|
||||
rayInfo = p.rayTest(rayFrom,rayTo)
|
||||
#p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3)
|
||||
for l in range(len(rayInfo)):
|
||||
hit = rayInfo[l]
|
||||
objectUid = hit[0]
|
||||
if (objectUid>=0):
|
||||
#p.removeBody(objectUid)
|
||||
p.changeVisualShape(objectUid,-1,rgbaColor=colors[currentColor])
|
||||
currentColor+=1
|
||||
if (currentColor>=len(colors)):
|
||||
currentColor=0
|
||||
@@ -6135,6 +6135,235 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_createVisualShapeArray(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
PyObject* shapeTypeArray = 0;
|
||||
PyObject* radiusArray = 0;
|
||||
PyObject* halfExtentsObjArray = 0;
|
||||
PyObject* lengthArray = 0;
|
||||
PyObject* fileNameArray = 0;
|
||||
PyObject* meshScaleObjArray = 0;
|
||||
PyObject* planeNormalObjArray = 0;
|
||||
PyObject* flagsArray = 0;
|
||||
PyObject* visualFramePositionObjArray = 0;
|
||||
PyObject* visualFrameOrientationObjArray = 0;
|
||||
|
||||
static char* kwlist[] = { "shapeTypes", "radii", "halfExtents", "lengths", "fileNames", "meshScales", "planeNormals",
|
||||
"flags", "visualFramePositions", "visualFrameOrientations", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|OOOOOOOOOi", kwlist,
|
||||
&shapeTypeArray, &radiusArray, &halfExtentsObjArray, &lengthArray, &fileNameArray, &meshScaleObjArray, &planeNormalObjArray, &flagsArray, &visualFramePositionObjArray, &visualFrameOrientationObjArray, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle = b3CreateVisualShapeCommandInit(sm);
|
||||
int numShapeTypes = 0;
|
||||
int numRadius = 0;
|
||||
int numHalfExtents = 0;
|
||||
int numLengths = 0;
|
||||
int numFileNames = 0;
|
||||
int numMeshScales = 0;
|
||||
int numPlaneNormals = 0;
|
||||
int numFlags = 0;
|
||||
int numPositions = 0;
|
||||
int numOrientations = 0;
|
||||
|
||||
int s;
|
||||
PyObject* shapeTypeArraySeq = shapeTypeArray ? PySequence_Fast(shapeTypeArray, "expected a sequence of shape types") : 0;
|
||||
PyObject* radiusArraySeq = radiusArray ? PySequence_Fast(radiusArray, "expected a sequence of radii") : 0;
|
||||
PyObject* halfExtentsArraySeq = halfExtentsObjArray ? PySequence_Fast(halfExtentsObjArray, "expected a sequence of half extents") : 0;
|
||||
PyObject* lengthArraySeq = lengthArray ? PySequence_Fast(lengthArray, "expected a sequence of lengths") : 0;
|
||||
PyObject* fileNameArraySeq = fileNameArray ? PySequence_Fast(fileNameArray, "expected a sequence of filename") : 0;
|
||||
PyObject* meshScaleArraySeq = meshScaleObjArray ? PySequence_Fast(meshScaleObjArray, "expected a sequence of mesh scale") : 0;
|
||||
PyObject* planeNormalArraySeq = planeNormalObjArray ? PySequence_Fast(planeNormalObjArray, "expected a sequence of plane normal") : 0;
|
||||
PyObject* flagsArraySeq = flagsArray ? PySequence_Fast(flagsArray, "expected a sequence of flags") : 0;
|
||||
PyObject* positionArraySeq = visualFramePositionObjArray ? PySequence_Fast(visualFramePositionObjArray, "expected a sequence of visual frame positions") : 0;
|
||||
PyObject* orientationArraySeq = visualFrameOrientationObjArray ? PySequence_Fast(visualFrameOrientationObjArray, "expected a sequence of visual frame orientations") : 0;
|
||||
|
||||
if (shapeTypeArraySeq == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "expected a sequence of shape types");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
numShapeTypes = shapeTypeArray ? PySequence_Size(shapeTypeArray) : 0;
|
||||
numRadius = radiusArraySeq ? PySequence_Size(radiusArraySeq) : 0;
|
||||
numHalfExtents = halfExtentsArraySeq ? PySequence_Size(halfExtentsArraySeq) : 0;
|
||||
numLengths = lengthArraySeq ? PySequence_Size(lengthArraySeq) : 0;
|
||||
numFileNames = fileNameArraySeq ? PySequence_Size(fileNameArraySeq) : 0;
|
||||
numMeshScales = meshScaleArraySeq ? PySequence_Size(meshScaleArraySeq) : 0;
|
||||
numPlaneNormals = planeNormalArraySeq ? PySequence_Size(planeNormalArraySeq) : 0;
|
||||
|
||||
for (s = 0; s<numShapeTypes; s++)
|
||||
{
|
||||
int shapeType = pybullet_internalGetIntFromSequence(shapeTypeArraySeq, s);
|
||||
if (shapeType >= GEOM_SPHERE)
|
||||
{
|
||||
|
||||
int shapeIndex = -1;
|
||||
|
||||
if (shapeType == GEOM_SPHERE && s <= numRadius)
|
||||
{
|
||||
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
|
||||
if (radius > 0)
|
||||
{
|
||||
shapeIndex = b3CreateVisualShapeAddSphere(commandHandle, radius);
|
||||
}
|
||||
}
|
||||
if (shapeType == GEOM_BOX)
|
||||
{
|
||||
PyObject* halfExtentsObj = 0;
|
||||
double halfExtents[3] = { 1, 1, 1 };
|
||||
|
||||
if (halfExtentsArraySeq && s <= numHalfExtents)
|
||||
{
|
||||
if (PyList_Check(halfExtentsArraySeq))
|
||||
{
|
||||
halfExtentsObj = PyList_GET_ITEM(halfExtentsArraySeq, s);
|
||||
}
|
||||
else
|
||||
{
|
||||
halfExtentsObj = PyTuple_GET_ITEM(halfExtentsArraySeq, s);
|
||||
}
|
||||
}
|
||||
pybullet_internalSetVectord(halfExtentsObj, halfExtents);
|
||||
shapeIndex = b3CreateVisualShapeAddBox(commandHandle, halfExtents);
|
||||
}
|
||||
if (shapeType == GEOM_CAPSULE && s <= numRadius)
|
||||
{
|
||||
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
|
||||
double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
|
||||
if (radius > 0 && height >= 0)
|
||||
{
|
||||
shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle, radius, height);
|
||||
}
|
||||
}
|
||||
if (shapeType == GEOM_CYLINDER && s <= numRadius && s<numLengths)
|
||||
{
|
||||
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
|
||||
double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
|
||||
if (radius > 0 && height >= 0)
|
||||
{
|
||||
shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle, radius, height);
|
||||
}
|
||||
}
|
||||
if (shapeType == GEOM_MESH)
|
||||
{
|
||||
double meshScale[3] = { 1, 1, 1 };
|
||||
|
||||
PyObject* meshScaleObj = meshScaleArraySeq ? PyList_GET_ITEM(meshScaleArraySeq, s) : 0;
|
||||
PyObject* fileNameObj = fileNameArraySeq ? PyList_GET_ITEM(fileNameArraySeq, s) : 0;
|
||||
const char* fileName = 0;
|
||||
|
||||
if (fileNameObj)
|
||||
{
|
||||
#if PY_MAJOR_VERSION >= 3
|
||||
PyObject* ob = PyUnicode_AsASCIIString(fileNameObj);
|
||||
fileName = PyBytes_AS_STRING(ob);
|
||||
#else
|
||||
fileName = PyString_AsString(fileNameObj);
|
||||
#endif
|
||||
}
|
||||
if (meshScaleObj)
|
||||
{
|
||||
pybullet_internalSetVectord(meshScaleObj, meshScale);
|
||||
}
|
||||
if (fileName)
|
||||
{
|
||||
shapeIndex = b3CreateVisualShapeAddMesh(commandHandle, fileName, meshScale);
|
||||
}
|
||||
|
||||
}
|
||||
if (shapeType == GEOM_PLANE)
|
||||
{
|
||||
PyObject* planeNormalObj = planeNormalArraySeq ? PyList_GET_ITEM(planeNormalArraySeq, s) : 0;
|
||||
double planeNormal[3];
|
||||
double planeConstant = 0;
|
||||
pybullet_internalSetVectord(planeNormalObj, planeNormal);
|
||||
shapeIndex = b3CreateVisualShapeAddPlane(commandHandle, planeNormal, planeConstant);
|
||||
}
|
||||
if (flagsArraySeq)
|
||||
{
|
||||
int flags = pybullet_internalGetIntFromSequence(flagsArraySeq, s);
|
||||
b3CreateVisualSetFlag(commandHandle, shapeIndex, flags);
|
||||
}
|
||||
if (positionArraySeq || orientationArraySeq)
|
||||
{
|
||||
PyObject* visualFramePositionObj = positionArraySeq ? PyList_GET_ITEM(positionArraySeq, s) : 0;
|
||||
PyObject* visualFrameOrientationObj = orientationArraySeq ? PyList_GET_ITEM(orientationArraySeq, s) : 0;
|
||||
double visualFramePosition[3] = { 0, 0, 0 };
|
||||
double visualFrameOrientation[4] = { 0, 0, 0, 1 };
|
||||
if (visualFramePositionObj)
|
||||
{
|
||||
pybullet_internalSetVectord(visualFramePositionObj, visualFramePosition);
|
||||
}
|
||||
|
||||
if (visualFrameOrientationObj)
|
||||
{
|
||||
pybullet_internalSetVector4d(visualFrameOrientationObj, visualFrameOrientation);
|
||||
}
|
||||
if (shapeIndex >= 0)
|
||||
{
|
||||
b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, visualFramePosition, visualFrameOrientation);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (shapeTypeArraySeq)
|
||||
Py_DECREF(shapeTypeArraySeq);
|
||||
if (radiusArraySeq)
|
||||
Py_DECREF(radiusArraySeq);
|
||||
if (halfExtentsArraySeq)
|
||||
Py_DECREF(halfExtentsArraySeq);
|
||||
if (lengthArraySeq)
|
||||
Py_DECREF(lengthArraySeq);
|
||||
if (fileNameArraySeq)
|
||||
Py_DECREF(fileNameArraySeq);
|
||||
if (meshScaleArraySeq)
|
||||
Py_DECREF(meshScaleArraySeq);
|
||||
if (planeNormalArraySeq)
|
||||
Py_DECREF(planeNormalArraySeq);
|
||||
if (flagsArraySeq)
|
||||
Py_DECREF(flagsArraySeq);
|
||||
if (positionArraySeq)
|
||||
Py_DECREF(positionArraySeq);
|
||||
if (orientationArraySeq)
|
||||
Py_DECREF(orientationArraySeq);
|
||||
|
||||
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, "createVisualShapeArray failed.");
|
||||
return NULL;
|
||||
}
|
||||
static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int physicsClientId = 0;
|
||||
@@ -8372,11 +8601,14 @@ static PyMethodDef SpamMethods[] = {
|
||||
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
|
||||
|
||||
{ "createCollisionShapeArray", (PyCFunction)pybullet_createCollisionShapeArray, METH_VARARGS | METH_KEYWORDS,
|
||||
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise." },
|
||||
"Create collision shapes. 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."},
|
||||
|
||||
{ "createVisualShapeArray", (PyCFunction)pybullet_createVisualShapeArray, METH_VARARGS | METH_KEYWORDS,
|
||||
"Create visual shapes. 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."},
|
||||
|
||||
|
||||
Reference in New Issue
Block a user