From ddf304ca782d6e6b59bd3a1e7cae166e759979fb Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 16 Feb 2018 19:44:33 -0800 Subject: [PATCH] PyBullet: expose internal edge utility, to adjust edge normals to prevent object penetrating along triangle edges of concave triangle meshes (due to local convex-triangle collisions causing opposite contact normals, use the pre-computed edge normal) PyBullet: expose experimental continuous collision detection for maximal coordinate rigid bodies, to prevent tunneling. --- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 12 +++++ examples/SharedMemory/PhysicsClientC_API.cpp | 19 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 5 +- .../PhysicsServerCommandProcessor.cpp | 37 +++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 4 +- examples/SharedMemory/SharedMemoryPublic.h | 2 + .../examples/experimentalCcdSphereRadius.py | 51 +++++++++++++++++++ examples/pybullet/examples/internalEdge.py | 42 +++++++++++++++ examples/pybullet/pybullet.c | 25 +++++++-- 9 files changed, 187 insertions(+), 10 deletions(-) create mode 100644 examples/pybullet/examples/experimentalCcdSphereRadius.py create mode 100644 examples/pybullet/examples/internalEdge.py diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 25bae7c86..1db703604 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -2,6 +2,9 @@ #include "LinearMath/btTransform.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" + + #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h" @@ -502,6 +505,15 @@ void ConvertURDF2BulletInternal( col->setCollisionShape(compoundShape); + if (compoundShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)compoundShape; + if (trimeshShape->getTriangleInfoMap()) + { + col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + } + } + btTransform tr; tr.setIdentity(); tr = linkTransformInWorldSpace; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0ee22969b..2a6009920 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -566,6 +566,14 @@ B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMem return 0; } +B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_allowedCcdPenetration = allowedCcdPenetration; + command->m_updateFlags |= SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION ; + return 0; +} B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { @@ -2308,6 +2316,17 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHan return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius = ccdSweptSphereRadius; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS; + return 0; +} + B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6dea73129..7b0799de8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -125,6 +125,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHand B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping); B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor); +B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); @@ -291,9 +292,7 @@ B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction); B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs); - - - +B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration); B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index a0471ff1d..049399eaf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6,6 +6,8 @@ #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" +#include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h" + #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -1727,6 +1729,7 @@ void logCallback(btDynamicsWorld *world, btScalar timeStep) bool MyContactAddedCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) { + btAdjustInternalEdgeContacts(cp, colObj1Wrap, colObj0Wrap, partId1,index1); return true; } @@ -2253,12 +2256,13 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() isPreTick = true; m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback,this,isPreTick); + gContactAddedCallback = MyContactAddedCallback; #ifdef B3_ENABLE_TINY_AUDIO m_data->m_soundEngine.init(16,true); //we don't use those callbacks (yet), experimental -// gContactAddedCallback = MyContactAddedCallback; + // gContactDestroyedCallback = MyContactDestroyedCallback; // gContactProcessedCallback = MyContactProcessedCallback; // gContactStartedCallback = MyContactStartedCallback; @@ -2411,6 +2415,17 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() for (int j = 0; jm_collisionShapes.size(); j++) { btCollisionShape* shape = m_data->m_collisionShapes[j]; + + + //check for internal edge utility, delete memory + if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) shape; + if (trimesh->getTriangleInfoMap()) + { + delete trimesh->getTriangleInfoMap(); + } + } delete shape; } for (int j=0;jm_meshInterfaces.size();j++) @@ -3823,10 +3838,17 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str meshInterface->addTriangle(v0, v1, v2); } } + { BT_PROFILE("create btBvhTriangleMeshShape"); btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true); m_data->m_collisionShapes.push_back(trimesh); + + if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags & GEOM_CONCAVE_INTERNAL_EDGE) + { + btTriangleInfoMap* triangleInfoMap = new btTriangleInfoMap(); + btGenerateInternalEdgeInfo(trimesh, triangleInfoMap); + } //trimesh->setLocalScaling(collision->m_geometry.m_meshScale); shape = trimesh; if (compound) @@ -6377,6 +6399,13 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc body->m_rigidBody->setMassProps(mass,newLocalInertiaDiagonal); } } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) + { + body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); + //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled + body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius/2.); + } } } @@ -6534,6 +6563,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0); } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration; + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) { m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 99089ce7e..bdbf64f60 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -159,7 +159,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256, CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512, CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024, - + CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048, }; struct ChangeDynamicsInfoArgs @@ -178,6 +178,7 @@ struct ChangeDynamicsInfoArgs double m_contactDamping; double m_localInertiaDiagonal[3]; int m_frictionAnchor; + double m_ccdSweptSphereRadius; }; struct GetDynamicsInfoArgs @@ -438,6 +439,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384, SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768, SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536, + SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072, }; enum EnumLoadSoftBodyUpdateFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 3aebed782..1be0128b9 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -693,6 +693,7 @@ enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes enum eUrdfCollisionFlags { GEOM_FORCE_CONCAVE_TRIMESH=1, + GEOM_CONCAVE_INTERNAL_EDGE=2, }; enum eUrdfVisualFlags @@ -740,6 +741,7 @@ struct b3PhysicsSimulationParameters double m_frictionERP; int m_enableConeFriction; int m_deterministicOverlappingPairs; + double m_allowedCcdPenetration; }; diff --git a/examples/pybullet/examples/experimentalCcdSphereRadius.py b/examples/pybullet/examples/experimentalCcdSphereRadius.py new file mode 100644 index 000000000..eb7eef6c4 --- /dev/null +++ b/examples/pybullet/examples/experimentalCcdSphereRadius.py @@ -0,0 +1,51 @@ +import pybullet as p +import time + +p.connect(p.GUI) +p.setPhysicsEngineParameter(allowedCcdPenetration=0.0) + + +terrain_mass=0 +terrain_visual_shape_id=-1 +terrain_position=[0,0,0] +terrain_orientation=[0,0,0,1] +terrain_collision_shape_id = p.createCollisionShape( + shapeType=p.GEOM_MESH, + fileName="terrain.obj", + flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE, + meshScale=[0.5, 0.5, 0.5]) +p.createMultiBody( + terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id, + terrain_position, terrain_orientation) + + +useMaximalCoordinates = True +sphereRadius = 0.005 +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 (5): + for j in range (5): + for k in range (5): + #if (k&2): + sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*5*sphereRadius,j*5*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates) + #else: + # sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates) + p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0) + p.changeDynamics(sphereUid,-1,ccdSweptSphereRadius=0.002) + + + +p.setGravity(0,0,-10) + +pts = p.getContactPoints() +print("num points=",len(pts)) +print(pts) +while (p.isConnected()): + time.sleep(1./240.) + p.stepSimulation() + \ No newline at end of file diff --git a/examples/pybullet/examples/internalEdge.py b/examples/pybullet/examples/internalEdge.py new file mode 100644 index 000000000..65369e120 --- /dev/null +++ b/examples/pybullet/examples/internalEdge.py @@ -0,0 +1,42 @@ +import pybullet as p +import time + +p.connect(p.GUI) + +if (1): + box_collision_shape_id = p.createCollisionShape( + shapeType=p.GEOM_BOX, + halfExtents=[0.01,0.01,0.055]) + box_mass=0.1 + box_visual_shape_id = -1 + box_position=[0,0.1,1] + box_orientation=[0,0,0,1] + + p.createMultiBody( + box_mass, box_collision_shape_id, box_visual_shape_id, + box_position, box_orientation, useMaximalCoordinates=True) + + +terrain_mass=0 +terrain_visual_shape_id=-1 +terrain_position=[0,0,0] +terrain_orientation=[0,0,0,1] +terrain_collision_shape_id = p.createCollisionShape( + shapeType=p.GEOM_MESH, + fileName="terrain.obj", + flags=p.GEOM_FORCE_CONCAVE_TRIMESH|p.GEOM_CONCAVE_INTERNAL_EDGE, + meshScale=[0.5, 0.5, 0.5]) +p.createMultiBody( + terrain_mass, terrain_collision_shape_id, terrain_visual_shape_id, + terrain_position, terrain_orientation) + + +p.setGravity(0,0,-10) + +pts = p.getContactPoints() +print("num points=",len(pts)) +print(pts) +while (p.isConnected()): + time.sleep(1./240.) + p.stepSimulation() + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 4c5c50413..0fd41f4bd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -875,14 +875,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double angularDamping = -1; double contactStiffness = -1; double contactDamping = -1; + double ccdSweptSphereRadius=-1; int frictionAnchor = -1; PyObject* localInertiaDiagonalObj=0; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOdi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &physicsClientId)) { return NULL; } @@ -942,6 +943,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetFrictionAnchor(command,bodyUniqueId,linkIndex, frictionAnchor); } + if (ccdSweptSphereRadius>=0) + { + b3ChangeDynamicsInfoSetCcdSweptSphereRadius(command,bodyUniqueId,linkIndex, ccdSweptSphereRadius); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -1104,15 +1109,17 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double erp = -1; double contactERP = -1; double frictionERP = -1; + double allowedCcdPenetration = -1; + int enableConeFriction = -1; b3PhysicsClientHandle sm = 0; int deterministicOverlappingPairs = -1; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &physicsClientId)) { return NULL; } @@ -1193,6 +1200,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar b3PhysicsParameterSetDeterministicOverlappingPairs(command,deterministicOverlappingPairs); } + if (allowedCcdPenetration>=0) + { + b3PhysicsParameterSetAllowedCcdPenetration(command,allowedCcdPenetration); + + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -8908,6 +8921,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE); PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH); + PyModule_AddIntConstant(m, "GEOM_CONCAVE_INTERNAL_EDGE", GEOM_CONCAVE_INTERNAL_EDGE); + PyModule_AddIntConstant(m, "STATE_LOG_JOINT_MOTOR_TORQUES", STATE_LOG_JOINT_MOTOR_TORQUES); PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES);