Merge pull request #979 from erwincoumans/master
fix lack of collision scaling in pybullet URDF/SDF files (only graphi…
This commit is contained in:
@@ -606,14 +606,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
||||
int numSubSteps = -1;
|
||||
int collisionFilterMode = -1;
|
||||
double contactBreakingThreshold = -1;
|
||||
|
||||
int maxNumCmdPer1ms = -1;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "physicsClientId", NULL };
|
||||
static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "physicsClientId", NULL };
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
|
||||
&collisionFilterMode, &contactBreakingThreshold, &physicsClientId))
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,
|
||||
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
@@ -658,7 +658,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
|
||||
{
|
||||
b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold);
|
||||
}
|
||||
|
||||
if (maxNumCmdPer1ms>=0)
|
||||
{
|
||||
b3PhysicsParamSetMaxNumCommandsPer1ms(command,maxNumCmdPer1ms);
|
||||
}
|
||||
|
||||
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
|
||||
|
||||
@@ -3004,6 +3007,43 @@ static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* arg
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_resetDebugVisualizerCamera(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
float cameraDistance = -1;
|
||||
float cameraYaw = -1;
|
||||
float cameraPitch = -1;
|
||||
PyObject* cameraTargetPosObj=0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm=0;
|
||||
static char *kwlist[] = {"cameraDistance", "cameraYaw", "cameraPitch", "cameraTargetPosition", "physicsClientId", NULL };
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "fffO|i", kwlist,
|
||||
&cameraDistance, &cameraYaw, &cameraPitch, &cameraTargetPosObj, &physicsClientId))
|
||||
return NULL;
|
||||
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(sm);
|
||||
if ((cameraDistance>=0) && (cameraYaw>=0) && (cameraPitch>=0))
|
||||
{
|
||||
float cameraTargetPosition[3];
|
||||
if (pybullet_internalSetVector(cameraTargetPosObj,cameraTargetPosition))
|
||||
{
|
||||
b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle,cameraDistance,cameraPitch, cameraYaw, cameraTargetPosition);
|
||||
}
|
||||
}
|
||||
b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
}
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
@@ -5098,9 +5138,14 @@ static PyMethodDef SpamMethods[] = {
|
||||
},
|
||||
|
||||
{ "configureDebugVisualizer", (PyCFunction)pybullet_configureDebugVisualizer, METH_VARARGS | METH_KEYWORDS,
|
||||
"For the 3D OpenGL Visualizer, enable/disable GUI, shadows. Set the camera parameters."
|
||||
"For the 3D OpenGL Visualizer, enable/disable GUI, shadows."
|
||||
},
|
||||
|
||||
{ "resetDebugVisualizerCamera", (PyCFunction)pybullet_resetDebugVisualizerCamera, METH_VARARGS | METH_KEYWORDS,
|
||||
"For the 3D OpenGL Visualizer, set the camera distance, yaw, pitch and target position."
|
||||
},
|
||||
|
||||
|
||||
|
||||
{"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS| METH_KEYWORDS,
|
||||
"Return the visual shape information for one object." },
|
||||
|
||||
@@ -11,6 +11,8 @@ import pybullet as p
|
||||
|
||||
#first try to connect to shared memory (VR), if it fails use local GUI
|
||||
c = p.connect(p.SHARED_MEMORY)
|
||||
if (c<0):
|
||||
c = p.connect(p.GUI)
|
||||
#p.resetSimulation()
|
||||
p.setGravity(0,0,-10)
|
||||
print(c)
|
||||
@@ -51,7 +53,7 @@ def convertSensor(x):
|
||||
b = (v-minV)/float(maxV-minV)
|
||||
return (1.0-b)
|
||||
|
||||
ser = serial.Serial(port='COM3',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
|
||||
ser = serial.Serial(port='COM9',baudrate=115200,parity=serial.PARITY_ODD,stopbits=serial.STOPBITS_TWO,bytesize=serial.SEVENBITS)
|
||||
if (ser.isOpen()):
|
||||
while True:
|
||||
events = p.getVREvents()
|
||||
|
||||
Reference in New Issue
Block a user