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

PyBullet: expose internal edge utility, to adjust edge normals to prevent object penetrating along triangle edges of concave triangle meshes

Browse Source 
(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.
This commit is contained in:
erwincoumans
2018-02-16 19:44:33 -08:00
parent d35941ebb8
commit ddf304ca78
9 changed files with 187 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -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;

19
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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)

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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);

37
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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; j<m_data->m_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;j<m_data->m_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;

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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;
};

51
examples/pybullet/examples/experimentalCcdSphereRadius.py Normal file
View File

@@ -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()

42
examples/pybullet/examples/internalEdge.py Normal file
View File

@@ -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()

25
examples/pybullet/pybullet.c
View File

@@ -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);