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

PyBullet: add option to cache graphics shapes for URDF files, handy for benchmarks with many duplicate robots

Browse Source 
See https://github.com/erwincoumans/pybullet_robots ANYmal.py for an example.
PyBullet: Expose p.setPhysicsEngineParameter(solverResidualThreshold=1e-2) (b3PhysicsParamSetSolverResidualThreshold), increases solver performance a lot
PyBullet: Expose p.setPhysicsEngineParameter(contactSlop) Set it to zero, to avoid issues with restitution.
PyBullet: Expose isNumpyEnabled, return True is PyBullet was compiled with NUMPY support for 'getCameraImage'.
PyBullet: Expose p.ChangeDynamics(objectUid, linkIndex, contactProcessingThreshold), to avoid issues of speculative/predictive contacts with restitution.
See also http://twvideo01.ubm-us.net/o1/vault/gdc2012/slides/Programming%20Track/Vincent_ROBERT_Track_ADifferentApproach.pdf
This commit is contained in:
Erwin Coumans
2018-05-23 13:26:00 +10:00
parent f5952a73e7
commit 77c332bd88
12 changed files with 189 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -593,6 +593,26 @@ B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHand
return 0;
}
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_solverResidualThreshold = solverResidualThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_contactSlop = contactSlop;
command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_SLOP;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
{
@@ -2354,6 +2374,19 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryComm
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold)
{
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_contactProcessingThreshold = contactProcessingThreshold;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)

6
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -126,6 +126,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan
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 int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
@@ -299,6 +300,11 @@ B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandl
B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs);
B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration);
B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode);
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold);
B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop);

41
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1484,6 +1484,7 @@ struct SaveStateData
btSerializer* m_serializer;
};
struct PhysicsServerCommandProcessorInternalData
{
///handle management
@@ -1581,6 +1582,7 @@ struct PhysicsServerCommandProcessorInternalData
#endif
b3HashMap<b3HashString, char*> m_profileEvents;
b3HashMap<b3HashString, UrdfVisualShapeCache> m_cachedVUrdfisualShapes;
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
:m_pluginManager(proc),
@@ -2618,7 +2620,22 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
u2b.getRootTransformInWorld(rootTrans);
//CUF_RESERVED is a temporary flag, for backward compatibility purposes
flags |= CUF_RESERVED;
ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags);
if (flags & CUF_ENABLE_CACHED_GRAPHICS_SHAPES)
{
{
UrdfVisualShapeCache* tmpPtr = m_data->m_cachedVUrdfisualShapes[fileName];
if (tmpPtr==0)
{
m_data->m_cachedVUrdfisualShapes.insert(fileName, UrdfVisualShapeCache());
}
}
UrdfVisualShapeCache* cachedVisualShapesPtr = m_data->m_cachedVUrdfisualShapes[fileName];
ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags, *cachedVisualShapesPtr);
} else
{
ConvertURDF2Bullet(u2b, creation, rootTrans, m_data->m_dynamicsWorld, useMultiBody, u2b.getPathPrefix(), flags);
}
@@ -6483,6 +6500,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
{
body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
{
body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
@@ -6696,11 +6718,24 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD)
{
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
{
gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP)
{
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
{
m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
@@ -9714,6 +9749,8 @@ void PhysicsServerCommandProcessor::resetSimulation()
{
//clean up all data
m_data->m_cachedVUrdfisualShapes.clear();
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (m_data && m_data->m_dynamicsWorld)
{

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -164,6 +164,7 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512,
CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024,
CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048,
CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD = 4096,
};
struct ChangeDynamicsInfoArgs
@@ -183,6 +184,7 @@ struct ChangeDynamicsInfoArgs
double m_localInertiaDiagonal[3];
int m_frictionAnchor;
double m_ccdSweptSphereRadius;
double m_contactProcessingThreshold;
};
struct GetDynamicsInfoArgs
@@ -450,6 +452,8 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144,
SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 524288,
SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576,
SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152,
SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
};
enum EnumLoadSoftBodyUpdateFlags

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -681,6 +681,7 @@ enum eURDF_Flags
URDF_USE_IMPLICIT_CYLINDER =128,
URDF_GLOBAL_VELOCITIES_MB =256,
MJCF_COLORS_FROM_FILE=512,
URDF_ENABLE_CACHED_GRAPHICS_SHAPES=1024,
};
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
@@ -755,6 +756,8 @@ struct b3PhysicsSimulationParameters
int m_deterministicOverlappingPairs;
double m_allowedCcdPenetration;
int m_jointFeedbackMode;
double m_solverResidualThreshold;
double m_contactSlop;
};