From 12a391e1f99522d6f6ac087223cbddd2d478d4cd Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 16 Jan 2017 18:17:18 -0800 Subject: [PATCH] [pybullet] expose collision filter mode and max constraint force: pybullet.changeUserConstraint(maxForce=) pybullet.setPhysicsEngineParameter(collisionFilterMode=) --- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 6 +++++ .../PhysicsServerCommandProcessor.cpp | 5 ++++ examples/SharedMemory/SharedMemoryCommands.h | 3 +++ examples/pybullet/pybullet.c | 20 ++++++++++++--- 5 files changed, 55 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 251f4ea86..897ea2e24 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -347,6 +347,18 @@ int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHand return 0; } + +int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_collisionFilterMode = filterMode; + command->m_updateFlags |= SIM_PARAM_UPDATE_COLLISION_FILTER_MODE; + return 0; +} + + + int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -1109,6 +1121,19 @@ int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHan return 0; } +int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_MAX_FORCE; + command->m_userConstraintArguments.m_maxAppliedForce = maxAppliedForce; + + return 0; +} + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 54daaa77c..0c465224e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -77,6 +77,8 @@ int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); +int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); @@ -170,6 +172,10 @@ int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); +int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode); + + + int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse); int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f8106916e..235cfc5fa 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3664,6 +3664,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btMatrix3x3 childFrameBasis(childFrameOrn); userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime; + userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp); + } } if (userConstraintPtr->m_rbConstraint) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 0f536bb94..91049ebaf 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -540,6 +540,8 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_CONSTRAINT=4, USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8, USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, + USER_CONSTRAINT_CHANGE_MAX_FORCE=32, + }; struct UserConstraintArgs @@ -552,6 +554,7 @@ struct UserConstraintArgs double m_childFrame[7]; double m_jointAxis[3]; int m_jointType; + double m_maxAppliedForce; int m_userConstraintUniqueId; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 742bfe8d5..57ad9b634 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -542,12 +542,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int useSplitImpulse = -1; double splitImpulsePenetrationThreshold = -1; int numSubSteps = -1; + int collisionFilterMode = -1; + b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","physicsClientId", NULL }; + static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps, + &collisionFilterMode, &physicsClientId)) { return NULL; } @@ -567,6 +570,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); } + if (collisionFilterMode>=0) + { + b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode); + } if (numSubSteps >= 0) { b3PhysicsParamSetNumSubSteps(command, numSubSteps); @@ -3016,7 +3023,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) { - static char *kwlist[] = { "userConstraintUniqueId","jointChildPivot", "jointChildFrameOrientation", "physicsClientId", NULL}; + static char *kwlist[] = { "userConstraintUniqueId","jointChildPivot", "jointChildFrameOrientation","maxForce", "physicsClientId", NULL}; int userConstraintUniqueId=-1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -3027,8 +3034,9 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P PyObject* jointChildFrameOrnObj=0; double jointChildPivot[3]; double jointChildFrameOrn[4]; + double maxForce = -1; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist,&userConstraintUniqueId,&jointChildPivotObj, &jointChildFrameOrnObj,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist,&userConstraintUniqueId,&jointChildPivotObj, &jointChildFrameOrnObj,&maxForce, &physicsClientId)) { return NULL; } @@ -3050,6 +3058,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P { b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); } + if (maxForce>=0) + { + b3InitChangeUserConstraintSetMaxForce(commandHandle,maxForce); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle);