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

Add preliminary PhysX 4.0 backend for PyBullet

Browse Source 
Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng
Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py)
Fix related to TinyRenderer object transforms not updating when using collision filtering
This commit is contained in:
erwincoumans
2019-01-22 21:08:37 -08:00
parent 80684f44ea
commit ae8e83988b
366 changed files with 131855 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -1353,7 +1353,9 @@ void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex,
if (linkPtr)
{
m_data->m_customVisualShapesConverter->setFlags(m_data->m_flags);
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO);
int uid3 = m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, *linkPtr, &model, colObj->getBroadphaseHandle()->getUid(), bodyUniqueId, m_data->m_fileIO);
colObj->setUserIndex3(uid3);
}
}
}

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

@@ -113,6 +113,7 @@ void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2BulletCac
}
}
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
{
cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
@@ -126,7 +127,7 @@ void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2BulletCachedDat
}
}
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache)
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int flags)
{
//compute the number of links, and compute parent indices array (and possibly other cached data?)
cache.m_totalNumJoints1 = 0;
@@ -143,7 +144,24 @@ void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2BulletCachedDat
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER)!=0;
if (maintainLinkOrder)
{
URDF2BulletCachedData cache2 = cache;
ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
for (int j=0;j<numTotalLinksIncludingBase;j++)
{
cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
}
}else
{
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
}
}
}
@@ -178,12 +196,12 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
btScalar tmpUrdfScaling = 2;
void ConvertURDF2BulletInternal(
btTransform ConvertURDF2BulletInternal(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
URDF2BulletCachedData& cache, int urdfLinkIndex,
const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
bool createMultiBody, const char* pathPrefix,
int flags = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn = 0, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut = 0)
int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive)
{
B3_PROFILE("ConvertURDF2BulletInternal2");
//b3Printf("start converting/extracting data from URDF interface\n");
@@ -681,13 +699,55 @@ void ConvertURDF2BulletInternal(
int numChildren = urdfChildIndices.size();
if (recursive)
{
for (int i = 0; i < numChildren; i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
}
}
return linkTransformInWorldSpace;
}
struct childParentIndex
{
int m_index;
int m_mbIndex;
int m_parentIndex;
int m_parentMBIndex;
};
void GetAllIndices(const URDFImporterInterface& u2b, URDF2BulletCachedData& cache, int urdfLinkIndex, int parentIndex, btAlignedObjectArray<childParentIndex>& allIndices)
{
childParentIndex cp;
cp.m_index = urdfLinkIndex;
int mbIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
cp.m_mbIndex = mbIndex;
cp.m_parentIndex = parentIndex;
int parentMbIndex = parentIndex>=0? cache.getMbIndexFromUrdfIndex(parentIndex) : -1;
cp.m_parentMBIndex = parentMbIndex;
allIndices.push_back(cp);
btAlignedObjectArray<int> urdfChildIndices;
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
int numChildren = urdfChildIndices.size();
for (int i = 0; i < numChildren; i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut);
GetAllIndices(u2b, cache, urdfChildLinkIndex, urdfLinkIndex, allIndices);
}
}
bool MyIntCompareFunc(childParentIndex a, childParentIndex b)
{
return (a.m_index < b.m_index);
}
void ConvertURDF2Bullet(
const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
const btTransform& rootTransformInWorldSpace,
@@ -695,13 +755,49 @@ void ConvertURDF2Bullet(
bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
{
URDF2BulletCachedData cache;
InitURDF2BulletCache(u2b, cache);
InitURDF2BulletCache(u2b, cache, flags);
int urdfLinkIndex = u2b.getRootLinkIndex();
int rootIndex = u2b.getRootLinkIndex();
B3_PROFILE("ConvertURDF2Bullet");
UrdfVisualShapeCache cachedLinkGraphicsShapesOut;
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut);
bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER)==0;
if (recursive)
{
ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
}
else
{
btAlignedObjectArray<btTransform> parentTransforms;
if (urdfLinkIndex >= parentTransforms.size())
{
parentTransforms.resize(urdfLinkIndex + 1);
}
parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
btAlignedObjectArray<childParentIndex> allIndices;
GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
allIndices.quickSort(MyIntCompareFunc);
for (int i = 0; i < allIndices.size(); i++)
{
int urdfLinkIndex = allIndices[i].m_index;
int parentIndex = allIndices[i].m_parentIndex;
btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr , world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
if ((urdfLinkIndex+1) >= parentTransforms.size())
{
parentTransforms.resize(urdfLinkIndex + 1);
}
parentTransforms[urdfLinkIndex] = tr;
}
}
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
{
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;

23
examples/Importers/ImportURDFDemo/URDF2Bullet.h
View File

@@ -13,28 +13,7 @@ class btTransform;
class URDFImporterInterface;
class MultiBodyCreationInterface;
//manually sync with eURDF_Flags in SharedMemoryPublic.h!
enum ConvertURDFFlags
{
CUF_USE_SDF = 1,
// Use inertia values in URDF instead of recomputing them from collision shape.
CUF_USE_URDF_INERTIA = 2,
CUF_USE_MJCF = 4,
CUF_USE_SELF_COLLISION = 8,
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
CUF_RESERVED = 64,
CUF_USE_IMPLICIT_CYLINDER = 128,
CUF_GLOBAL_VELOCITIES_MB = 256,
CUF_MJCF_COLORS_FROM_FILE = 512,
CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
CUF_ENABLE_SLEEPING = 2048,
CUF_INITIALIZE_SAT_FEATURES = 4096,
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
CUF_PARSE_SENSORS = 16384,
CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 64738,
};
struct UrdfVisualShapeCache
{

5
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -81,6 +81,11 @@ public:
return 0;
}
virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const
{
return 0;
}
virtual int getNumAllocatedCollisionShapes() const { return 0; }
virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/) { return 0; }
virtual int getNumModels() const { return 0; }

25
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -2,6 +2,7 @@
#define URDF_JOINT_TYPES_H
#include "LinearMath/btScalar.h"
#include "LinearMath/btVector3.h"
enum UrdfJointTypes
{
@@ -76,4 +77,28 @@ struct UrdfMaterialColor
}
};
//manually sync with eURDF_Flags in SharedMemoryPublic.h!
enum ConvertURDFFlags
{
CUF_USE_SDF = 1,
// Use inertia values in URDF instead of recomputing them from collision shape.
CUF_USE_URDF_INERTIA = 2,
CUF_USE_MJCF = 4,
CUF_USE_SELF_COLLISION = 8,
CUF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
CUF_RESERVED = 64,
CUF_USE_IMPLICIT_CYLINDER = 128,
CUF_GLOBAL_VELOCITIES_MB = 256,
CUF_MJCF_COLORS_FROM_FILE = 512,
CUF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024,
CUF_ENABLE_SLEEPING = 2048,
CUF_INITIALIZE_SAT_FEATURES = 4096,
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
CUF_PARSE_SENSORS = 16384,
CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
CUF_MAINTAIN_LINK_ORDER = 131072,
};
#endif //URDF_JOINT_TYPES_H

2
examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h
View File

@@ -15,7 +15,7 @@ struct UrdfRenderingInterface
virtual ~UrdfRenderingInterface() {}
///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape.
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
///remove a visual shapes, based on the shape unique id (shapeUid)
virtual void removeVisualShape(int collisionObjectUid) = 0;