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

PyBullet / BulletRobotics:

Browse Source 
Implement collisionFilterPlugin, use setCollisionFilterPair to enable or disable collision detection between specific pairs of objects.
Also, expose setCollisionFilterGroupMask as PyBullet API and in urdf using the tag <collision group="1" mask="2"/>.
See examples/pybullet/examples/collisionFilter.py for an example.
PyBullet default: Lower the warmstarting factor, for maximal coordinates rigid bodies for more stable simulation.
Add btCollisionWorld::refreshBroadphaseProxy to easier recreate the broadphase proxy without adding/removing objects to the world.
This commit is contained in:
erwincoumans
2018-09-12 19:30:49 -07:00
parent bf3399d0e3
commit 9553892770
20 changed files with 675 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/SharedMemory/plugins/b3PluginAPI.h
View File

@@ -32,6 +32,7 @@ extern "C" {
typedef B3_API_ENTRY int (B3_API_CALL * PFN_TICK)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct UrdfRenderingInterface* (B3_API_CALL * PFN_GET_RENDER_INTERFACE)(struct b3PluginContext* context);
typedef B3_API_ENTRY struct b3PluginCollisionInterface* (B3_API_CALL * PFN_GET_COLLISION_INTERFACE)(struct b3PluginContext* context);
#ifdef __cplusplus

33
examples/SharedMemory/plugins/b3PluginCollisionInterface.h Normal file
View File

@@ -0,0 +1,33 @@
#ifndef B3_PLUGIN_COLLISION_INTERFACE_H
#define B3_PLUGIN_COLLISION_INTERFACE_H
enum b3PluginCollisionFilterModes
{
B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA=0,
B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA
};
struct b3PluginCollisionInterface
{
virtual void setBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB,
bool enableCollision)=0;
virtual void removeBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB)=0;
virtual int getNumRules() const = 0;
virtual void resetAll()=0;
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA,int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB,int collisionFilterMaskB,
int filterMode
)=0;
};
#endif //B3_PLUGIN_COLLISION_INTERFACE_H

189
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
View File

@@ -7,11 +7,148 @@
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
#include "Bullet3Common/b3HashMap.h"
#include "../b3PluginCollisionInterface.h"
struct b3CustomCollisionFilter
{
int m_objectUniqueIdA;
int m_linkIndexA;
int m_objectUniqueIdB;
int m_linkIndexB;
bool m_enableCollision;
B3_FORCE_INLINE unsigned int getHash()const
{
int obA = (m_objectUniqueIdA&0xff);
int obB = ((m_objectUniqueIdB &0xf)<<8);
int linkA = ((m_linkIndexA&0xff)<<16);
int linkB = ((m_linkIndexB&0xff)<<24);
int key = obA+obB+linkA+linkB;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
return key;
}
bool equals(const b3CustomCollisionFilter& other) const
{
return m_objectUniqueIdA == other.m_objectUniqueIdA &&
m_objectUniqueIdB == other.m_objectUniqueIdB&&
m_linkIndexA == other.m_linkIndexA &&
m_linkIndexB == other.m_linkIndexB;
}
};
struct DefaultPluginCollisionInterface : public b3PluginCollisionInterface
{
b3HashMap<b3CustomCollisionFilter, b3CustomCollisionFilter> m_customCollisionFilters;
virtual void setBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB,
bool enableCollision)
{
b3CustomCollisionFilter keyValue;
keyValue.m_objectUniqueIdA = objectUniqueIdA;
keyValue.m_linkIndexA = linkIndexA;
keyValue.m_objectUniqueIdB = objectUniqueIdB;
keyValue.m_linkIndexB = linkIndexB;
keyValue.m_enableCollision = enableCollision;
if (objectUniqueIdA>objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
}
m_customCollisionFilters.insert(keyValue,keyValue);
}
virtual void removeBroadphaseCollisionFilter(
int objectUniqueIdA, int objectUniqueIdB,
int linkIndexA, int linkIndexB)
{
b3CustomCollisionFilter keyValue;
keyValue.m_objectUniqueIdA = objectUniqueIdA;
keyValue.m_linkIndexA = linkIndexA;
keyValue.m_objectUniqueIdB = objectUniqueIdB;
keyValue.m_linkIndexB = linkIndexB;
if (objectUniqueIdA>objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
}
m_customCollisionFilters.remove(keyValue);
}
virtual int getNumRules() const
{
return m_customCollisionFilters.size();
}
virtual void resetAll()
{
m_customCollisionFilters.clear();
}
virtual int needsBroadphaseCollision(int objectUniqueIdA, int linkIndexA,
int collisionFilterGroupA,int collisionFilterMaskA,
int objectUniqueIdB, int linkIndexB,
int collisionFilterGroupB,int collisionFilterMaskB,
int filterMode
)
{
//check and apply any custom rules for those objects/links
b3CustomCollisionFilter keyValue;
keyValue.m_objectUniqueIdA = objectUniqueIdA;
keyValue.m_linkIndexA = linkIndexA;
keyValue.m_objectUniqueIdB = objectUniqueIdB;
keyValue.m_linkIndexB = linkIndexB;
if (objectUniqueIdA>objectUniqueIdB)
{
b3Swap(keyValue.m_objectUniqueIdA,keyValue.m_objectUniqueIdB);
b3Swap(keyValue.m_linkIndexA,keyValue.m_linkIndexB);
}
b3CustomCollisionFilter* filter = m_customCollisionFilters.find(keyValue);
if (filter)
{
return filter->m_enableCollision;
}
//otherwise use the default fallback
if (filterMode==B3_FILTER_GROUPAMASKB_AND_GROUPBMASKA)
{
bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
collides = collides && (collisionFilterGroupB & collisionFilterMaskA);
return collides;
}
if (filterMode==B3_FILTER_GROUPAMASKB_OR_GROUPBMASKA)
{
bool collides = (collisionFilterGroupA & collisionFilterMaskB) != 0;
collides = collides || (collisionFilterGroupB & collisionFilterMaskA);
return collides;
}
return false;
}
};
struct CollisionFilterMyClass
{
int m_testData;
DefaultPluginCollisionInterface m_collisionFilter;
CollisionFilterMyClass()
:m_testData(42)
{
@@ -29,60 +166,16 @@ B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* conte
}
B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context)
{
//apply pd control here, apply forces using the PD gains
return 0;
}
B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context)
B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context)
{
CollisionFilterMyClass* obj = (CollisionFilterMyClass* )context->m_userPointer;
obj->m_testData++;
return 0;
return &obj->m_collisionFilter;
}
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
//set the PD gains
printf("text argument:%s\n",arguments->m_text);
printf("int args: [");
for (int i=0;i<arguments->m_numInts;i++)
{
printf("%d", arguments->m_ints[i]);
if ((i+1)<arguments->m_numInts)
{
printf(",");
}
}
printf("]\nfloat args: [");
for (int i=0;i<arguments->m_numFloats;i++)
{
printf("%f", arguments->m_floats[i]);
if ((i+1)<arguments->m_numFloats)
{
printf(",");
}
}
printf("]\n");
CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer;
b3SharedMemoryStatusHandle statusHandle;
int statusType = -1;
int bodyUniqueId = -1;
b3SharedMemoryCommandHandle command =
b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
}
return bodyUniqueId;
return 0;
}

4
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h
View File

@@ -14,9 +14,7 @@ B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* cont
B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//all the APIs below are optional
B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context);
B3_SHARED_API struct b3PluginCollisionInterface* getCollisionInterface_collisionFilterPlugin(struct b3PluginContext* context);
#ifdef __cplusplus
};