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

Merge pull request #1496 from erwincoumans/master

Browse Source 
warning fix (param name different in header/cpp)
This commit is contained in:
erwincoumans
2018-01-08 13:23:44 -08:00
committed by GitHub
parent 380738308d f39d83580b
commit 1e0d1c2b6d
27 changed files with 2106 additions and 1431 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
Extras/Serialize/BulletFileLoader/autogenerated/bullet.h
View File

@@ -556,8 +556,8 @@ typedef struct bInvalidHandle {
double m_contactBreakingThreshold;
double m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
btCollisionObjectDoubleData *m_body0;
btCollisionObjectDoubleData *m_body1;
};
@@ -570,8 +570,8 @@ typedef struct bInvalidHandle {
btVector3FloatData m_pointCachePositionWorldOnA[4];
btVector3FloatData m_pointCachePositionWorldOnB[4];
btVector3FloatData m_pointCacheNormalWorldOnB[4];
btVector3FloatData m_pointCacheLateralFrictionDir1;
btVector3FloatData m_pointCacheLateralFrictionDir2;
btVector3FloatData m_pointCacheLateralFrictionDir1[4];
btVector3FloatData m_pointCacheLateralFrictionDir2[4];
float m_pointCacheDistance[4];
float m_pointCacheAppliedImpulse[4];
float m_pointCacheCombinedFriction[4];
@@ -601,8 +601,8 @@ typedef struct bInvalidHandle {
float m_contactBreakingThreshold;
float m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
btCollisionObjectFloatData *m_body0;
btCollisionObjectFloatData *m_body1;
};
@@ -1451,7 +1451,7 @@ typedef struct bInvalidHandle {
{
public:
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_parentComToThisPivotOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
@@ -1482,7 +1482,7 @@ typedef struct bInvalidHandle {
{
public:
btQuaternionFloatData m_zeroRotParentToThis;
btVector3FloatData m_parentComToThisComOffset;
btVector3FloatData m_parentComToThisPivotOffset;
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];
@@ -1548,7 +1548,7 @@ typedef struct bInvalidHandle {
{
public:
btCollisionObjectFloatData m_colObjData;
void *m_multiBody;
btMultiBodyFloatData *m_multiBody;
int m_link;
char m_padding[4];
};
@@ -1559,7 +1559,7 @@ typedef struct bInvalidHandle {
{
public:
btCollisionObjectDoubleData m_colObjData;
void *m_multiBody;
btMultiBodyDoubleData *m_multiBody;
int m_link;
char m_padding[4];
};

10
Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp
View File

@@ -194,7 +194,7 @@ template<class T> void convertMultiBody(T* mbd, btMultiBodyWorldImporterInterna
btQuaternion parentRotToThis;
parentRotToThis.deSerialize(mbd->m_links[i].m_zeroRotParentToThis);
btVector3 parentComToThisPivotOffset;
parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisComOffset);
parentComToThisPivotOffset.deSerialize(mbd->m_links[i].m_parentComToThisPivotOffset);
btVector3 thisPivotToThisComOffset;
thisPivotToThisComOffset.deSerialize(mbd->m_links[i].m_thisPivotToThisComOffset);
@@ -333,7 +333,7 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
if (ptr)
{
manifoldData->m_body0 = ptr;
manifoldData->m_body0 = (btCollisionObjectDoubleData*)ptr;
}
}
@@ -341,7 +341,7 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
if (ptr)
{
manifoldData->m_body1 = ptr;
manifoldData->m_body1 = (btCollisionObjectDoubleData*)ptr;
}
}
}
@@ -371,14 +371,14 @@ bool btMultiBodyWorldImporter::convertAllObjects( bParse::btBulletFile* bulletF
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body0);
if (ptr)
{
manifoldData->m_body0 = ptr;
manifoldData->m_body0 = (btCollisionObjectFloatData*)ptr;
}
}
{
void* ptr = bulletFile2->findLibPointer(manifoldData->m_body1);
if (ptr)
{
manifoldData->m_body1 = ptr;
manifoldData->m_body1 = (btCollisionObjectFloatData*)ptr;
}
}
}

4
data/plane_transparent.urdf
View File

@@ -12,10 +12,10 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="plane_transparent.obj" scale="1 1 1"/>
<mesh filename="plane.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 0.4"/>
<color rgba="1 1 1 .9"/>
</material>
</visual>
<collision>

1
examples/CommonInterfaces/CommonRenderInterface.h
View File

@@ -15,6 +15,7 @@ enum
//B3_WIREFRAME_RENDERMODE,
B3_CREATE_SHADOWMAP_RENDERMODE,
B3_USE_SHADOWMAP_RENDERMODE,
B3_USE_SHADOWMAP_RENDERMODE_REFLECTION,
};

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

@@ -49,7 +49,8 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
btHashMap<btHashInt,UrdfMaterialColor> m_linkColors;
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
mutable btAlignedObjectArray<btTriangleMesh*> m_allocatedMeshInterfaces;
btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
LinkVisualShapesConverter* m_customVisualShapesConverter;
bool m_enableTinyRenderer;
@@ -569,6 +570,17 @@ bool findExistingMeshFile(
}
}
int BulletURDFImporter::getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const
{
UrdfCollision* col = m_data->m_bulletCollisionShape2UrdfCollision.find(collisionShape);
if (col)
{
collision = *col;
return 1;
}
return 0;
}
btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix) const
{
BT_PROFILE("convertURDFToCollisionShape");
@@ -671,6 +683,7 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
//create a convex hull for each shape, and store it in a btCompoundShape
shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
return shape;
}
break;
@@ -812,6 +825,10 @@ upAxisMat.setIdentity();
b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
}
if (shape && collision->m_geometry.m_type==URDF_GEOM_MESH)
{
m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision);
}
return shape;
}

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

@@ -72,6 +72,8 @@ public:
class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const;
virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;

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

@@ -75,6 +75,10 @@ public:
//default implementation for backward compatibility
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const = 0;
virtual int getUrdfFromCollisionShape(const class btCollisionShape* collisionShape, struct UrdfCollision& collision) const
{
return 0;
}
virtual int getNumAllocatedCollisionShapes() const { return 0;}
virtual class btCollisionShape* getAllocatedCollisionShape(int /*index*/ ) {return 0;}

34
examples/OpenGLWindow/GLInstancingRenderer.cpp
View File

@@ -1527,6 +1527,7 @@ void GLInstancingRenderer::renderScene()
renderSceneInternal(B3_CREATE_SHADOWMAP_RENDERMODE);
//glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
//renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE_REFLECTION);
renderSceneInternal(B3_USE_SHADOWMAP_RENDERMODE);
} else
@@ -1960,8 +1961,15 @@ struct TransparentDistanceSortPredicate
}
};
void GLInstancingRenderer::renderSceneInternal(int renderMode)
void GLInstancingRenderer::renderSceneInternal(int orgRenderMode)
{
int renderMode=orgRenderMode;
bool reflectionPass = false;
if (orgRenderMode==B3_USE_SHADOWMAP_RENDERMODE_REFLECTION)
{
reflectionPass = true;
renderMode = B3_USE_SHADOWMAP_RENDERMODE;
}
if (!useShadowMap)
{
@@ -2385,9 +2393,10 @@ b3Assert(glGetError() ==GL_NO_ERROR);
glUseProgram(useShadowMapInstancingShader);
glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]);
glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
//glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
//glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrix[0]);
//glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]);
//glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]);
glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]);
@@ -2395,7 +2404,22 @@ b3Assert(glGetError() ==GL_NO_ERROR);
float MVP[16];
b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
if (reflectionPass)
{
float tmp[16];
float reflectionMatrix[16] = {1,0,0,0,
0,1,0,0,
0,0,-1,0,
0,0,0,1};
glCullFace(GL_FRONT);
b3Matrix4x4Mul16(m_data->m_viewMatrix,reflectionMatrix,tmp);
b3Matrix4x4Mul16(m_data->m_projectionMatrix,tmp,MVP);
} else
{
b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
glCullFace(GL_BACK);
}
glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]);
//gLightDir.normalize();
glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]);

2
examples/OpenGLWindow/GLInstancingRenderer.h
View File

@@ -52,7 +52,7 @@ public:
virtual void init();
virtual void renderScene();
virtual void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
virtual void renderSceneInternal(int orgRenderMode=B3_DEFAULT_RENDERMODE);
void InitShaders();
void CleanupShaders();

2
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl
View File

@@ -11,8 +11,6 @@ layout (location = 5) in vec4 instance_color;
layout (location = 6) in vec3 instance_scale;
uniform mat4 ModelViewMatrix;
uniform mat4 ProjectionMatrix;
uniform mat4 DepthBiasModelViewProjectionMatrix;
uniform mat4 MVP;
uniform vec3 lightPosIn;

2
examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h
View File

@@ -9,8 +9,6 @@ static const char* useShadowMapInstancingVertexShader= \
"layout (location = 4) in vec3 vertexnormal;\n"
"layout (location = 5) in vec4 instance_color;\n"
"layout (location = 6) in vec3 instance_scale;\n"
"uniform mat4 ModelViewMatrix;\n"
"uniform mat4 ProjectionMatrix;\n"
"uniform mat4 DepthBiasModelViewProjectionMatrix;\n"
"uniform mat4 MVP;\n"
"uniform vec3 lightPosIn;\n"

6
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -68,9 +68,9 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisComOffset.m_floats[0];
info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisComOffset.m_floats[1];
info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisComOffset.m_floats[2];
info.m_parentFrame[0] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[0];
info.m_parentFrame[1] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[1];
info.m_parentFrame[2] = mb->m_links[link].m_parentComToThisPivotOffset.m_floats[2];
info.m_parentFrame[3] = mb->m_links[link].m_zeroRotParentToThis.m_floats[0];
info.m_parentFrame[4] = mb->m_links[link].m_zeroRotParentToThis.m_floats[1];
info.m_parentFrame[5] = mb->m_links[link].m_zeroRotParentToThis.m_floats[2];

4
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3417,7 +3417,7 @@ B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA, int linkIndex)
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -3425,7 +3425,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_COLLISION_SHAPE_INFO;
command->m_requestCollisionShapeDataArguments.m_bodyUniqueId = bodyUniqueIdA;
command->m_requestCollisionShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
command->m_requestCollisionShapeDataArguments.m_linkIndex = linkIndex;
command->m_updateFlags = 0;

549
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -29,6 +29,7 @@
#include "LinearMath/btTransform.h"
#include "../Importers/ImportMJCFDemo/BulletMJCFImporter.h"
#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
#include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
#include "../Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "LinearMath/btSerializer.h"
@@ -1510,6 +1511,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback;
@@ -2132,11 +2134,25 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
if (colShapeUniqueId>=0)
{
InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
if (handle)
if (handle && handle->m_collisionShape)
{
btTransform childTrans;
childTrans.setIdentity();
compound->addChildShape(localInertiaFrame.inverse()*childTrans,handle->m_collisionShape);
if (handle->m_collisionShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* childCompound = (btCompoundShape*)handle->m_collisionShape;
for (int c = 0; c < childCompound->getNumChildShapes(); c++)
{
btTransform childTrans = childCompound->getChildTransform(c);
btCollisionShape* childShape = childCompound->getChildShape(c);
btTransform tr = localInertiaFrame.inverse()*childTrans;
compound->addChildShape(tr, childShape);
}
}
else
{
btTransform childTrans;
childTrans.setIdentity();
compound->addChildShape(localInertiaFrame.inverse()*childTrans, handle->m_collisionShape);
}
}
}
m_allocatedCollisionShapes.push_back(compound);
@@ -2378,6 +2394,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
}
m_data->m_meshInterfaces.clear();
m_data->m_collisionShapes.clear();
m_data->m_bulletCollisionShape2UrdfCollision.clear();
delete m_data->m_dynamicsWorld;
m_data->m_dynamicsWorld=0;
@@ -2620,6 +2637,25 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
{
btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
m_data->m_collisionShapes.push_back(shape);
UrdfCollision urdfCollision;
if (u2b.getUrdfFromCollisionShape(shape, urdfCollision))
{
m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, urdfCollision);
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*)shape;
for (int c = 0; c < compound->getNumChildShapes(); c++)
{
btCollisionShape* childShape = compound->getChildShape(c);
if (u2b.getUrdfFromCollisionShape(childShape, urdfCollision))
{
m_data->m_bulletCollisionShape2UrdfCollision.insert(childShape, urdfCollision);
}
}
}
}
m_data->m_saveWorldBodyData.push_back(sd);
@@ -3474,7 +3510,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
bool hasStatus = true;
serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED;
btScalar defaultCollisionMargin = 0.001;
btMultiBodyWorldImporter* worldImporter = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
btCollisionShape* shape = 0;
@@ -3486,8 +3523,11 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
{
compound = worldImporter->createCompoundShape();
}
for (int i=0;i<clientCmd.m_createUserShapeArgs.m_numUserShapes;i++)
for (int i = 0; i < clientCmd.m_createUserShapeArgs.m_numUserShapes; i++)
{
GLInstanceGraphicsShape* glmesh = 0;
char pathPrefix[1024] = "";
char relativeFileName[1024] = "";
UrdfCollision urdfColObj;
btTransform childTransform;
@@ -3503,7 +3543,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[2],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_childOrientation[3]
));
if (compound==0)
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
@@ -3516,227 +3556,274 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
switch (clientCmd.m_createUserShapeArgs.m_shapes[i].m_type)
{
case GEOM_SPHERE:
case GEOM_SPHERE:
{
double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createSphereShape(radius);
if (compound)
{
double radius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
shape = worldImporter->createSphereShape(radius);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
urdfColObj.m_geometry.m_sphereRadius = radius;
break;
compound->addChildShape(childTransform, shape);
}
case GEOM_BOX:
urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
urdfColObj.m_geometry.m_sphereRadius = radius;
break;
}
case GEOM_BOX:
{
//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
btVector3 halfExtents(
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
shape = worldImporter->createBoxShape(halfExtents);
if (compound)
{
//double halfExtents[3] = clientCmd.m_createUserShapeArgs.m_shapes[i].m_sphereRadius;
btVector3 halfExtents(
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
shape = worldImporter->createBoxShape(halfExtents);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
break;
compound->addChildShape(childTransform, shape);
}
case GEOM_CAPSULE:
urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
break;
}
case GEOM_CAPSULE:
{
shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
break;
compound->addChildShape(childTransform, shape);
}
case GEOM_CYLINDER:
{
shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
break;
break;
}
case GEOM_CYLINDER:
{
shape = worldImporter->createCylinderShapeZ(clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius,
0.5*clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight);
if (compound)
{
compound->addChildShape(childTransform, shape);
}
case GEOM_PLANE:
{
btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleRadius;
urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createUserShapeArgs.m_shapes[i].m_capsuleHeight;
shape = worldImporter->createPlaneShape(planeNormal,0);
if (compound)
{
compound->addChildShape(childTransform,shape);
}
urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
urdfColObj.m_geometry.m_planeNormal.setValue(
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
break;
break;
}
case GEOM_PLANE:
{
btVector3 planeNormal(clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
shape = worldImporter->createPlaneShape(planeNormal, 0);
if (compound)
{
compound->addChildShape(childTransform, shape);
}
case GEOM_MESH:
{
btScalar defaultCollisionMargin = 0.001;
urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
urdfColObj.m_geometry.m_planeNormal.setValue(
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_planeNormal[2]);
btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
break;
}
case GEOM_MESH:
{
const std::string& urdf_path="";
btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName;
urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
urdfColObj.m_geometry.m_meshFileName = fileName;
urdfColObj.m_geometry.m_meshScale = meshScale;
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
const std::string& urdf_path = "";
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
const std::string& error_message_prefix="";
std::string out_found_filename;
int out_type;
std::string fileName = clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshFileName;
urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
urdfColObj.m_geometry.m_meshFileName = fileName;
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
if (foundFile)
{
urdfColObj.m_geometry.m_meshFileType = out_type;
if (out_type==UrdfGeometry::FILE_OBJ)
{
//create a convex hull for each shape, and store it in a btCompoundShape
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
{
GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
if (!glmesh || glmesh->m_numvertices<=0)
{
b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
delete glmesh;
break;
}
btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i=0; i<glmesh->m_numvertices; i++)
{
convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0]*meshScale[0],
glmesh->m_vertices->at(i).xyzw[1]*meshScale[1],
glmesh->m_vertices->at(i).xyzw[2]*meshScale[2]));
}
BT_PROFILE("convert trimesh");
btTriangleMesh* meshInterface = new btTriangleMesh();
this->m_data->m_meshInterfaces.push_back(meshInterface);
{
BT_PROFILE("convert vertices");
for (int i=0; i<glmesh->m_numIndices/3; i++)
{
const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)];
const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)];
const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)];
meshInterface->addTriangle(v0,v1,v2);
}
}
{
BT_PROFILE("create btBvhTriangleMeshShape");
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
m_data->m_collisionShapes.push_back(trimesh);
//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
shape = trimesh;
if (compound)
{
compound->addChildShape(childTransform,shape);
}
}
delete glmesh;
} else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
B3_PROFILE("createConvexHullFromShapes");
if (compound==0)
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(defaultCollisionMargin);
for (int s = 0; s<(int)shapes.size(); s++)
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(defaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f = 0; f<faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
urdfColObj.m_geometry.m_meshScale = meshScale;
convexHull->addPoint(pt*meshScale,false);
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024))
{
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt*meshScale, false);
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt*meshScale, false);
}
const std::string& error_message_prefix = "";
std::string out_found_filename;
int out_type;
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform,convexHull);
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (foundFile)
{
urdfColObj.m_geometry.m_meshFileType = out_type;
if (out_type == UrdfGeometry::FILE_STL)
{
glmesh = LoadMeshFromSTL(relativeFileName);
}
if (out_type == UrdfGeometry::FILE_OBJ)
{
//create a convex hull for each shape, and store it in a btCompoundShape
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
{
glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
}
else
{
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str());
//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
B3_PROFILE("createConvexHullFromShapes");
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(defaultCollisionMargin);
for (int s = 0; s < (int)shapes.size(); s++)
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(defaultCollisionMargin);
tinyobj::shape_t& shape = shapes[s];
int faceCount = shape.mesh.indices.size();
for (int f = 0; f < faceCount; f += 3)
{
btVector3 pt;
pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
convexHull->addPoint(pt*meshScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
convexHull->addPoint(pt*meshScale, false);
pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
convexHull->addPoint(pt*meshScale, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform, convexHull);
}
}
}
break;
}
default:
{
}
break;
}
default:
{
}
}
if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN)
{
urdfCollisionObjects.push_back(urdfColObj);
}
}
if (glmesh)
{
btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1],
clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[2]);
if (!glmesh || glmesh->m_numvertices <= 0)
{
b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
delete glmesh;
}
else
{
btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i = 0; i < glmesh->m_numvertices; i++)
{
convertedVerts.push_back(btVector3(
glmesh->m_vertices->at(i).xyzw[0] * meshScale[0],
glmesh->m_vertices->at(i).xyzw[1] * meshScale[1],
glmesh->m_vertices->at(i).xyzw[2] * meshScale[2]));
}
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
{
BT_PROFILE("convert trimesh");
btTriangleMesh* meshInterface = new btTriangleMesh();
this->m_data->m_meshInterfaces.push_back(meshInterface);
{
BT_PROFILE("convert vertices");
for (int i = 0; i < glmesh->m_numIndices / 3; i++)
{
const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i * 3)];
const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i * 3 + 1)];
const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i * 3 + 2)];
meshInterface->addTriangle(v0, v1, v2);
}
}
{
BT_PROFILE("create btBvhTriangleMeshShape");
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface, true, true);
m_data->m_collisionShapes.push_back(trimesh);
//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
shape = trimesh;
if (compound)
{
compound->addChildShape(childTransform, shape);
}
}
delete glmesh;
}
else
{
//convex mesh
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
}
compound->setMargin(defaultCollisionMargin);
{
btConvexHullShape* convexHull = worldImporter->createConvexHullShape();
convexHull->setMargin(defaultCollisionMargin);
for (int v = 0; v < convertedVerts.size(); v++)
{
btVector3 pt = convertedVerts[v];
convexHull->addPoint(pt, false);
}
convexHull->recalcLocalAabb();
convexHull->optimizeConvexHull();
compound->addChildShape(childTransform, convexHull);
}
}
}
}
}
if (compound && compound->getNumChildShapes())
{
shape = compound;
@@ -4633,6 +4720,9 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
motor->setVelocityTarget(desiredVelocity,kd);
//todo: instead of clamping, combine the motor and limit
//and combine handling of limit force and motor force.
//clamp position
if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
{
@@ -8047,11 +8137,35 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape
{
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
sprintf( collisionShapeBuffer[0].m_meshAssetFileName, "unknown_file");
collisionShapeBuffer[0].m_dimensions[0] = 1;
collisionShapeBuffer[0].m_dimensions[1] = 1;
collisionShapeBuffer[0].m_dimensions[2] = 1;
UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
if (urdfCol && (urdfCol->m_geometry.m_type == GEOM_MESH))
{
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
numConverted += 1;
}
else
{
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
sprintf(collisionShapeBuffer[0].m_meshAssetFileName, "unknown_file");
collisionShapeBuffer[0].m_dimensions[0] = 1;
collisionShapeBuffer[0].m_dimensions[1] = 1;
collisionShapeBuffer[0].m_dimensions[2] = 1;
numConverted++;
}
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
btCapsuleShapeZ* capsule = (btCapsuleShapeZ*)colShape;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CAPSULE;
collisionShapeBuffer[0].m_dimensions[0] = 2.*capsule->getHalfHeight();
collisionShapeBuffer[0].m_dimensions[1] = capsule->getRadius();
collisionShapeBuffer[0].m_dimensions[2] = 0;
numConverted++;
break;
}
@@ -8088,14 +8202,29 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape
}
case COMPOUND_SHAPE_PROXYTYPE:
{
//recurse, accumulate childTransform
btCompoundShape* compound = (btCompoundShape*)colShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
//it could be a compound mesh from a wavefront OBJ, check it
UrdfCollision* urdfCol = m_data->m_bulletCollisionShape2UrdfCollision.find(colShape);
if (urdfCol && (urdfCol->m_geometry.m_type == GEOM_MESH))
{
btTransform childTrans = transform*compound->getChildTransform(i);
int remain = maxCollisionShapes - numConverted;
int converted = extractCollisionShapes(compound->getChildShape(i), childTrans, &collisionShapeBuffer[numConverted], remain);
numConverted += converted;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_MESH;
collisionShapeBuffer[0].m_dimensions[0] = urdfCol->m_geometry.m_meshScale[0];
collisionShapeBuffer[0].m_dimensions[1] = urdfCol->m_geometry.m_meshScale[1];
collisionShapeBuffer[0].m_dimensions[2] = urdfCol->m_geometry.m_meshScale[2];
strcpy(collisionShapeBuffer[0].m_meshAssetFileName, urdfCol->m_geometry.m_meshFileName.c_str());
numConverted += 1;
}
else
{
//recurse, accumulate childTransform
btCompoundShape* compound = (btCompoundShape*)colShape;
for (int i = 0; i < compound->getNumChildShapes(); i++)
{
btTransform childTrans = transform*compound->getChildTransform(i);
int remain = maxCollisionShapes - numConverted;
int converted = extractCollisionShapes(compound->getChildShape(i), childTrans, &collisionShapeBuffer[numConverted], remain);
numConverted += converted;
}
}
break;
}

2
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -84,7 +84,7 @@ protected:
bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& childTransform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes);
int extractCollisionShapes(const class btCollisionShape* colShape, const class btTransform& transform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes);
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201801010
#define SHARED_MEMORY_MAGIC_NUMBER 201801080
//#define SHARED_MEMORY_MAGIC_NUMBER 201801010
//#define SHARED_MEMORY_MAGIC_NUMBER 201710180
//#define SHARED_MEMORY_MAGIC_NUMBER 201710050
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
@@ -544,7 +545,7 @@ struct b3CollisionShapeData
int m_objectUniqueId;
int m_linkIndex;
int m_collisionGeometryType;//GEOM_BOX, GEOM_SPHERE etc
double m_dimensions[3];//meaning depends on m_visualGeometryType GEOM_BOX: extents, GEOM_SPHERE: radius, GEOM_CAPSULE:
double m_dimensions[3];//meaning depends on m_visualGeometryType GEOM_BOX: extents, GEOM_SPHERE: radius, GEOM_CAPSULE+GEOM_CYLINDER:length, radius, GEOM_MESH: mesh scale
double m_localCollisionFrame[7];//pos[3], orn[4]
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
};

26
examples/pybullet/examples/jointFrictionAndMotor.py Normal file
View File

@@ -0,0 +1,26 @@
import pybullet as p
import time
p.connect(p.GUI)
door=p.loadURDF("door.urdf")
#linear/angular damping for base and all children=0
p.changeDynamics(door,-1,linearDamping=0, angularDamping=0)
for j in range (p.getNumJoints(door)):
p.changeDynamics(door,j,linearDamping=0, angularDamping=0)
print(p.getJointInfo(door,j))
frictionId = p.addUserDebugParameter("jointFriction",0,20,10)
torqueId = p.addUserDebugParameter("joint torque",0,20,5)
while (1):
frictionForce = p.readUserDebugParameter(frictionId)
jointTorque = p.readUserDebugParameter(torqueId)
#set the joint friction
p.setJointMotorControl2(door,1,p.VELOCITY_CONTROL,targetVelocity=0,force=frictionForce)
#apply a joint torque
p.setJointMotorControl2(door,1,p.TORQUE_CONTROL,force=jointTorque)
p.stepSimulation()
time.sleep(0.01)

339
examples/pybullet/examples/urdfEditor.py
View File

@@ -1,9 +1,7 @@
import pybullet as p
import time
p.connect(p.GUI)
door = p.loadURDF("door.urdf")
class UrdfInertial(object):
def __init__(self):
@@ -33,7 +31,8 @@ class UrdfVisual(object):
self.geom_radius = 1
self.geom_extents = [7,8,9]
self.geom_length=[10]
self.geom_meshfile = "meshfile"
self.geom_meshfilename = "meshfile"
self.geom_meshscale=[1,1,1]
self.material_rgba = [1,0,0,1]
self.material_name = ""
@@ -43,11 +42,13 @@ class UrdfCollision(object):
self.origin_xyz = [4,5,6]
self.geom_type = p.GEOM_BOX
self.geom_radius = 1
self.geom_length = 2
self.geom_extents = [7,8,9]
self.geom_meshfile = "meshfile"
self.geom_meshfilename = "meshfile"
self.geom_meshscale = [1,1,1]
class UrdfJoint(object):
def __init__(self):
self.link = UrdfLink()
self.joint_name = "joint_dummy"
self.joint_type = p.JOINT_REVOLUTE
self.joint_lower_limit = 0
@@ -55,6 +56,7 @@ class UrdfJoint(object):
self.parent_name = "parentName"
self.child_name = "childName"
self.joint_origin_xyz = [1,2,3]
self.joint_origin_rpy = [1,2,3]
self.joint_axis_xyz = [1,2,3]
class UrdfEditor(object):
@@ -65,13 +67,14 @@ class UrdfEditor(object):
self.urdfLinks=[]
self.urdfJoints=[]
self.robotName = ""
self.linkNameToIndex={}
self.jointTypeToName={p.JOINT_FIXED:"JOINT_FIXED" ,\
p.JOINT_REVOLUTE:"JOINT_REVOLUTE",\
p.JOINT_PRISMATIC:"JOINT_PRISMATIC" }
#def addLink(...)
#def createMultiBody(self):
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink):
dyn = p.getDynamicsInfo(bodyUid,linkIndex)
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
@@ -79,11 +82,11 @@ class UrdfEditor(object):
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid)
visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1]==linkIndex):
print("visualShape base:",v)
#print("visualShape base:",v)
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
@@ -91,10 +94,16 @@ class UrdfEditor(object):
if (v[2]==p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfVisual.geom_meshfile = v[4].decode("utf-8")
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfVisual.geom_radius=v[3][1]
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
#print("Capsule length=",urdfVisual.geom_length)
#print("Capsule radius=",urdfVisual.geom_radius)
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
@@ -105,9 +114,9 @@ class UrdfEditor(object):
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex=matIndex+1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex)
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
for v in collisionShapes:
print("collisionShape base:",v)
#print("collisionShape base:",v)
urdfCollision = UrdfCollision()
print("geom type=",v[0])
urdfCollision.geom_type = v[2]
@@ -116,61 +125,77 @@ class UrdfEditor(object):
if (v[2]==p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfCollision.geom_meshfile = v[4].decode("utf-8")
#localInertiaFrame*childTrans
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
urdfCollision.geom_meshscale = v[3]
print("p.GEOM_MESH filename=",urdfCollision.geom_meshfilename)
if (v[2]==p.GEOM_CYLINDER):
urdfCollision.geom_radius=v[3][1]
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
print("Capsule length=",urdfCollision.geom_length)
print("Capsule radius=",urdfCollision.geom_radius)
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def initializeFromBulletBody(self, bodyUid):
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink)
baseLink.link_name = p.getBodyInfo(bodyUid)[0].decode("utf-8")
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid)):
jointInfo = p.getJointInfo(bodyUid,j)
for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink)
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
parentIndex = jointInfo[16]
if (parentIndex<0):
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid,parentIndex)
parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dyn = p.getDynamicsInfo(bodyUid,parentIndex)
parentInertiaPos = dyn[3]
parentInertiaOrn = dyn[4]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
jointInfo[14], jointInfo[15])
dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos=jointInfo[14]
parentCom2JointOrn=jointInfo[15]
tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def writeInertial(self,file,urdfInertial, precision=5):
@@ -204,13 +229,17 @@ class UrdfEditor(object):
prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_MESH:
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfile,\
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfilename,\
prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_CYLINDER:
str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_CAPSULE:
str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
file.write(str)
file.write("\t\t\t</geometry>\n")
str = '\t\t\t<material name=\"{}\">\n'.format(urdfVisual.material_name)
@@ -237,14 +266,17 @@ class UrdfEditor(object):
prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_MESH:
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfile,\
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfilename,\
prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_CYLINDER:
str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_CAPSULE:
str = '\t\t\t\t<capsule length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
file.write(str)
file.write("\t\t\t</geometry>\n")
file.write("\t\t</collision>\n")
@@ -278,6 +310,14 @@ class UrdfEditor(object):
file.write(str)
str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
file.write(str)
if urdfJoint.joint_type == p.JOINT_PRISMATIC:
#todo: handle limits
lowerLimit=-0.5
upperLimit=0.5
str='<limit effort="1000.0" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="0.5"/>'.format(lowerLimit,upperLimit,prec=precision)
file.write(str)
file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
@@ -303,23 +343,224 @@ class UrdfEditor(object):
file.write("</robot>\n")
file.close()
#def addLink(...)
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
print("baseMass=",baseMass)
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
for v in base.urdf_collision_shapes:
print("base v.origin_xyz=",v.origin_xyz)
print("base v.origin_rpy=",v.origin_rpy)
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
print("v.origin_rpy=",v.origin_rpy)
orn=p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
print("baseHalfExtentsArray=",baseHalfExtentsArray)
if (len(baseShapeTypeArray)):
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
print("baseCollisionShapeIndex=",baseCollisionShapeIndex)
linkMasses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkMeshScaleArray=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
linkJointTypes=[]
linkJointAxis=[]
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
print("linkMass=",linkMass)
linkCollisionShapeIndex=-1
linkVisualShapeIndex=-1
linkPosition=[0,0,0]
linkOrientation=[0,0,0]
linkInertialFramePosition=[0,0,0]
linkInertialFrameOrientation=[0,0,0]
linkParentIndex=self.linkNameToIndex[joint.parent_name]
print("parentId=",linkParentIndex)
linkJointType=joint.joint_type
print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType] )
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
for v in link.urdf_collision_shapes:
print("link v.origin_xyz=",v.origin_xyz)
print("link v.origin_rpy=",v.origin_rpy)
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
print("linkHalfExtentsArray=",linkHalfExtentsArray)
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
print("\n\n\nlinkMasses=",linkMasses)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex= baseCollisionShapeIndex,
basePosition=basePosition,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=base.urdf_inertial.origin_rpy,
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
#create visual and collision shapes
#for link in self.urdfLinks:
# for v in link.urdf_visual_shapes:
# v.tmp_visual_shape_ids=[]
# shapeType = v[0]
# if shapeType==p.GEOM_BOX:
# p.createVisualShape(physicsClientId=physId)
#for joint in self.urdfJoints:
# self.writeJoint(file,joint)
def __del__(self):
pass
##########################################
org2 = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org<0):
org = p.connect(p.DIRECT)
gui = p.connect(p.GUI)
p.resetSimulation(physicsClientId=org)
#door.urdf, hinge.urdf, duck_vhacd.urdf, r2d2.urdf, quadruped/quadruped.urdf
mb = p.loadURDF("r2d2.urdf", physicsClientId=org)
for i in range(p.getNumJoints(mb,physicsClientId=org)):
p.setJointMotorControl2(mb,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
#for i in range(p.getNumJoints(mb,physicsClientId=org)):
# print("jointInfo(",i,"):",p.getJointInfo(mb,i,physicsClientId=org))
# print("linkState(",i,"):",p.getLinkState(mb,i,physicsClientId=org))
parser = UrdfEditor()
parser.initializeFromBulletBody(door)
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
parser=0
p.setRealTimeSimulation(1)
print("numJoints:",p.getNumJoints(door))
if (1):
print("\ncreatingMultiBody...................n")
print("base name:",p.getBodyInfo(door))
obUid = parser.createMultiBody(physicsClientId=gui)
for i in range(p.getNumJoints(door)):
print("jointInfo(",i,"):",p.getJointInfo(door,i))
print("linkState(",i,"):",p.getLinkState(door,i))
parser2 = UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
while (p.getConnectionInfo()["isConnected"]):
for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0.1,force=1,physicsClientId=gui)
print(p.getJointInfo(obUid,i,physicsClientId=gui))
parser=0
p.setRealTimeSimulation(1,physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
time.sleep(0.01)

251
examples/pybullet/pybullet.c
View File

@@ -66,6 +66,7 @@ b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
return 0;
}
static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index)
{
double v = 0.0;
@@ -153,6 +154,7 @@ static int pybullet_internalSetVector(PyObject* objVec, float vector[3])
if (seq)
{
len = PySequence_Size(objVec);
assert(len == 3);
if (len == 3)
{
for (i = 0; i < len; i++)
@@ -180,6 +182,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3])
if (seq)
{
len = PySequence_Size(obVec);
assert(len == 3);
if (len == 3)
{
for (i = 0; i < len; i++)
@@ -5639,7 +5642,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
if (collisionFrameOrientationObj)
{
pybullet_internalSetVectord(collisionFrameOrientationObj,collisionFrameOrientation);
pybullet_internalSetVector4d(collisionFrameOrientationObj,collisionFrameOrientation);
}
b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition,collisionFrameOrientation);
@@ -5657,6 +5660,233 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
return NULL;
}
static PyObject* pybullet_createCollisionShapeArray(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
PyObject* shapeTypeArray = 0;
PyObject* radiusArray = 0;
PyObject* halfExtentsObjArray = 0;
PyObject* lengthArray = 0;
PyObject* fileNameArray = 0;
PyObject* meshScaleObjArray = 0;
PyObject* planeNormalObjArray = 0;
PyObject* flagsArray = 0;
PyObject* collisionFramePositionObjArray = 0;
PyObject* collisionFrameOrientationObjArray = 0;
static char* kwlist[] = { "shapeTypes", "radii", "halfExtents", "lengths", "fileNames", "meshScales", "planeNormals",
"flags", "collisionFramePositions", "collisionFrameOrientations", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|OOOOOOOOOi", kwlist,
&shapeTypeArray, &radiusArray, &halfExtentsObjArray, &lengthArray, &fileNameArray, &meshScaleObjArray, &planeNormalObjArray, &flagsArray, &collisionFramePositionObjArray, &collisionFrameOrientationObjArray, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm);
{
int numShapeTypes = 0;
int numRadius = 0;
int numHalfExtents = 0;
int numLengths = 0;
int numFileNames = 0;
int numMeshScales = 0;
int numPlaneNormals = 0;
int numFlags = 0;
int numPositions = 0;
int numOrientations = 0;
int s;
PyObject* shapeTypeArraySeq = shapeTypeArray?PySequence_Fast(shapeTypeArray, "expected a sequence of shape types"):0;
PyObject* radiusArraySeq = radiusArray?PySequence_Fast(radiusArray, "expected a sequence of radii"):0;
PyObject* halfExtentsArraySeq = halfExtentsObjArray?PySequence_Fast(halfExtentsObjArray, "expected a sequence of half extents"):0;
PyObject* lengthArraySeq = lengthArray ?PySequence_Fast(lengthArray, "expected a sequence of lengths"):0;
PyObject* fileNameArraySeq = fileNameArray?PySequence_Fast(fileNameArray, "expected a sequence of filename"):0;
PyObject* meshScaleArraySeq = meshScaleObjArray?PySequence_Fast(meshScaleObjArray, "expected a sequence of mesh scale"):0;
PyObject* planeNormalArraySeq = planeNormalObjArray?PySequence_Fast(planeNormalObjArray, "expected a sequence of plane normal"):0;
PyObject* flagsArraySeq = flagsArray?PySequence_Fast(flagsArray, "expected a sequence of flags"):0;
PyObject* positionArraySeq = collisionFramePositionObjArray?PySequence_Fast(collisionFramePositionObjArray, "expected a sequence of collision frame positions"):0;
PyObject* orientationArraySeq = collisionFrameOrientationObjArray?PySequence_Fast(collisionFrameOrientationObjArray, "expected a sequence of collision frame orientations"):0;
if (shapeTypeArraySeq == 0)
{
PyErr_SetString(SpamError, "expected a sequence of shape types");
return NULL;
}
numShapeTypes = shapeTypeArray?PySequence_Size(shapeTypeArray):0;
numRadius = radiusArraySeq?PySequence_Size(radiusArraySeq):0;
numHalfExtents = halfExtentsArraySeq?PySequence_Size(halfExtentsArraySeq):0;
numLengths = lengthArraySeq?PySequence_Size(lengthArraySeq):0;
numFileNames = fileNameArraySeq?PySequence_Size(fileNameArraySeq):0;
numMeshScales = meshScaleArraySeq?PySequence_Size(meshScaleArraySeq):0;
numPlaneNormals = planeNormalArraySeq?PySequence_Size(planeNormalArraySeq):0;
for (s=0;s<numShapeTypes;s++)
{
int shapeType = pybullet_internalGetIntFromSequence(shapeTypeArraySeq, s);
if (shapeType >= GEOM_SPHERE)
{
int shapeIndex = -1;
if (shapeType == GEOM_SPHERE && s <= numRadius)
{
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
if (radius > 0)
{
shapeIndex = b3CreateCollisionShapeAddSphere(commandHandle, radius);
}
}
if (shapeType == GEOM_BOX)
{
PyObject* halfExtentsObj = 0;
double halfExtents[3] = { 1, 1, 1 };
if (halfExtentsArraySeq && s<= numHalfExtents)
{
if (PyList_Check(halfExtentsArraySeq))
{
halfExtentsObj = PyList_GET_ITEM(halfExtentsArraySeq, s);
}
else
{
halfExtentsObj = PyTuple_GET_ITEM(halfExtentsArraySeq, s);
}
}
pybullet_internalSetVectord(halfExtentsObj, halfExtents);
shapeIndex = b3CreateCollisionShapeAddBox(commandHandle, halfExtents);
}
if (shapeType == GEOM_CAPSULE && s<=numRadius)
{
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
if (radius > 0 && height >= 0)
{
shapeIndex = b3CreateCollisionShapeAddCapsule(commandHandle, radius, height);
}
}
if (shapeType == GEOM_CYLINDER && s <= numRadius && s<numLengths)
{
double radius = pybullet_internalGetFloatFromSequence(radiusArraySeq, s);
double height = pybullet_internalGetFloatFromSequence(lengthArraySeq, s);
if (radius > 0 && height >= 0)
{
shapeIndex = b3CreateCollisionShapeAddCylinder(commandHandle, radius, height);
}
}
if (shapeType == GEOM_MESH)
{
double meshScale[3] = { 1, 1, 1 };
PyObject* meshScaleObj = meshScaleArraySeq?PyList_GET_ITEM(meshScaleArraySeq, s):0;
PyObject* fileNameObj = fileNameArraySeq?PyList_GET_ITEM(fileNameArraySeq, s):0;
const char* fileName = 0;
if (fileNameObj)
{
#if PY_MAJOR_VERSION >= 3
PyObject* ob = PyUnicode_AsASCIIString(fileNameObj);
fileName = PyBytes_AS_STRING(ob);
#else
fileName = PyString_AsString(objectsRepresentation);
#endif
}
if (meshScaleObj)
{
pybullet_internalSetVectord(meshScaleObj, meshScale);
}
if (fileName)
{
shapeIndex = b3CreateCollisionShapeAddMesh(commandHandle, fileName, meshScale);
}
}
if (shapeType == GEOM_PLANE)
{
PyObject* planeNormalObj = planeNormalArraySeq?PyList_GET_ITEM(planeNormalArraySeq, s):0;
double planeNormal[3];
double planeConstant = 0;
pybullet_internalSetVectord(planeNormalObj, planeNormal);
shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant);
}
if (flagsArraySeq)
{
int flags = pybullet_internalGetIntFromSequence(flagsArraySeq, s);
b3CreateCollisionSetFlag(commandHandle, shapeIndex, flags);
}
if (positionArraySeq || orientationArraySeq)
{
PyObject* collisionFramePositionObj = positionArraySeq?PyList_GET_ITEM(positionArraySeq, s):0;
PyObject* collisionFrameOrientationObj = orientationArraySeq?PyList_GET_ITEM(orientationArraySeq, s):0;
double collisionFramePosition[3] = { 0, 0, 0 };
double collisionFrameOrientation[4] = { 0, 0, 0, 1 };
if (collisionFramePositionObj)
{
pybullet_internalSetVectord(collisionFramePositionObj, collisionFramePosition);
}
if (collisionFrameOrientationObj)
{
pybullet_internalSetVector4d(collisionFrameOrientationObj, collisionFrameOrientation);
}
if (shapeIndex >= 0)
{
b3CreateCollisionShapeSetChildTransform(commandHandle, shapeIndex, collisionFramePosition, collisionFrameOrientation);
}
}
}
}
if (shapeTypeArraySeq)
Py_DECREF(shapeTypeArraySeq);
if (radiusArraySeq)
Py_DECREF(radiusArraySeq);
if (halfExtentsArraySeq)
Py_DECREF(halfExtentsArraySeq);
if (lengthArraySeq)
Py_DECREF(lengthArraySeq);
if (fileNameArraySeq)
Py_DECREF(fileNameArraySeq);
if (meshScaleArraySeq)
Py_DECREF(meshScaleArraySeq);
if (planeNormalArraySeq)
Py_DECREF(planeNormalArraySeq);
if (flagsArraySeq)
Py_DECREF(flagsArraySeq);
if (positionArraySeq)
Py_DECREF(positionArraySeq);
if (orientationArraySeq)
Py_DECREF(orientationArraySeq);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
{
int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
PyErr_SetString(SpamError, "createCollisionShapeArray failed.");
return NULL;
}
static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
{
@@ -5666,7 +5896,7 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
int shapeType=-1;
double radius=0.5;
double height = 1;
double length = 1;
PyObject* meshScaleObj=0;
double meshScale[3] = {1,1,1};
PyObject* planeNormalObj=0;
@@ -5688,9 +5918,9 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
PyObject* halfExtentsObj=0;
static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
static char* kwlist[] = {"shapeType","radius","halfExtents", "length", "fileName", "meshScale", "planeNormal", "flags", "rgbaColor", "specularColor", "visualFramePosition", "visualFrameOrientation", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOiOOOOi", kwlist,
&shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
&shapeType, &radius,&halfExtentsObj, &length, &fileName, &meshScaleObj, &planeNormalObj, &flags, &rgbaColorObj, &specularColorObj, &visualFramePositionObj, &visualFrameOrientationObj, &physicsClientId))
{
return NULL;
}
@@ -5720,13 +5950,13 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
shapeIndex = b3CreateVisualShapeAddBox(commandHandle,halfExtents);
}
if (shapeType==GEOM_CAPSULE && radius>0 && height>=0)
if (shapeType==GEOM_CAPSULE && radius>0 && length>=0)
{
shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle,radius,height);
shapeIndex = b3CreateVisualShapeAddCapsule(commandHandle,radius,length);
}
if (shapeType==GEOM_CYLINDER && radius>0 && height>=0)
if (shapeType==GEOM_CYLINDER && radius>0 && length>=0)
{
shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle,radius,height);
shapeIndex = b3CreateVisualShapeAddCylinder(commandHandle,radius,length);
}
if (shapeType==GEOM_MESH && fileName)
{
@@ -5767,7 +5997,7 @@ static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyOb
if (visualFrameOrientationObj)
{
pybullet_internalSetVectord(visualFrameOrientationObj,visualFrameOrientation);
pybullet_internalSetVector4d(visualFrameOrientationObj,visualFrameOrientation);
}
b3CreateVisualShapeSetChildTransform(commandHandle, shapeIndex, visualFramePosition,visualFrameOrientation);
@@ -8016,6 +8246,9 @@ static PyMethodDef SpamMethods[] = {
{"createCollisionShape", (PyCFunction)pybullet_createCollisionShape, METH_VARARGS | METH_KEYWORDS,
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},
{ "createCollisionShapeArray", (PyCFunction)pybullet_createCollisionShapeArray, METH_VARARGS | METH_KEYWORDS,
"Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise." },
{"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,
"Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."},

2
setup.py
View File

@@ -443,7 +443,7 @@ print("-----")
setup(
name = 'pybullet',
version='1.7.6',
version='1.7.8',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',

9
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -18,6 +18,11 @@ subject to the following restrictions:
#include "LinearMath/btTransform.h"
#include "LinearMath/btSerializer.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btCollisionObjectData btCollisionObjectDoubleData
#else
#define btCollisionObjectData btCollisionObjectFloatData
#endif
btScalar gContactBreakingThreshold = btScalar(0.02);
ContactDestroyedCallback gContactDestroyedCallback = 0;
@@ -316,8 +321,8 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer;
memset(dataOut, 0, sizeof(btPersistentManifoldData));
dataOut->m_body0 = serializer->getUniquePointer((void*)manifold->getBody0());
dataOut->m_body1 = serializer->getUniquePointer((void*)manifold->getBody1());
dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0());
dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1());
dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold();
dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold();
dataOut->m_numCachedPoints = manifold->getNumContacts();

10
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -24,6 +24,8 @@ class btCollisionObject;
#include "LinearMath/btAlignedAllocator.h"
struct btCollisionResult;
struct btCollisionObjectDoubleData;
struct btCollisionObjectFloatData;
///maximum contact breaking and merging threshold
extern btScalar gContactBreakingThreshold;
@@ -310,8 +312,8 @@ struct btPersistentManifoldDoubleData
double m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
btCollisionObjectDoubleData *m_body0;
btCollisionObjectDoubleData *m_body1;
};
@@ -356,8 +358,8 @@ struct btPersistentManifoldFloatData
float m_contactProcessingThreshold;
int m_padding;
void *m_body0;
void *m_body1;
btCollisionObjectFloatData *m_body0;
btCollisionObjectFloatData *m_body1;
};
#ifdef BT_USE_DOUBLE_PRECISION

2
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -2033,7 +2033,7 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
btAssert(memPtr->m_dofCount<=3);

4
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -718,7 +718,7 @@ private:
struct btMultiBodyLinkDoubleData
{
btQuaternionDoubleData m_zeroRotParentToThis;
btVector3DoubleData m_parentComToThisComOffset;
btVector3DoubleData m_parentComToThisPivotOffset;
btVector3DoubleData m_thisPivotToThisComOffset;
btVector3DoubleData m_jointAxisTop[6];
btVector3DoubleData m_jointAxisBottom[6];
@@ -751,7 +751,7 @@ struct btMultiBodyLinkDoubleData
struct btMultiBodyLinkFloatData
{
btQuaternionFloatData m_zeroRotParentToThis;
btVector3FloatData m_parentComToThisComOffset;
btVector3FloatData m_parentComToThisPivotOffset;
btVector3FloatData m_thisPivotToThisComOffset;
btVector3FloatData m_jointAxisTop[6];
btVector3FloatData m_jointAxisBottom[6];

6
src/BulletDynamics/Featherstone/btMultiBodyLinkCollider.h
View File

@@ -141,7 +141,7 @@ public:
struct btMultiBodyLinkColliderFloatData
{
btCollisionObjectFloatData m_colObjData;
void *m_multiBody;
btMultiBodyFloatData *m_multiBody;
int m_link;
char m_padding[4];
};
@@ -149,7 +149,7 @@ struct btMultiBodyLinkColliderFloatData
struct btMultiBodyLinkColliderDoubleData
{
btCollisionObjectDoubleData m_colObjData;
void *m_multiBody;
btMultiBodyDoubleData *m_multiBody;
int m_link;
char m_padding[4];
};
@@ -165,7 +165,7 @@ SIMD_FORCE_INLINE const char* btMultiBodyLinkCollider::serialize(void* dataBuffe
btCollisionObject::serialize(&dataOut->m_colObjData,serializer);
dataOut->m_link = this->m_link;
dataOut->m_multiBody = serializer->getUniquePointer(m_multiBody);
dataOut->m_multiBody = (btMultiBodyData*)serializer->getUniquePointer(m_multiBody);
// Fill padding with zeros to appease msan.
memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding));

1111
src/LinearMath/btSerializer.cpp
View File

File diff suppressed because it is too large Load Diff

1111
src/LinearMath/btSerializer64.cpp
View File

File diff suppressed because it is too large Load Diff