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

further work on urdfEditor.py, fix some serialization issues

Browse Source
This commit is contained in:
Erwin Coumans
2018-01-08 12:25:56 -08:00
parent a85a4f387b
commit e97b751781
23 changed files with 1926 additions and 1455 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];

546
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;
@@ -8050,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;
}
@@ -8091,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;
}

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

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

@@ -31,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 = ""
@@ -41,9 +42,10 @@ 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()
@@ -54,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):
@@ -83,7 +86,7 @@ class UrdfEditor(object):
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])
@@ -107,7 +116,7 @@ class UrdfEditor(object):
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,12 +125,17 @@ 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
@@ -164,16 +178,24 @@ class UrdfEditor(object):
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dyn = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
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):
@@ -207,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)
@@ -240,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")
@@ -281,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)
@@ -319,24 +356,41 @@ class UrdfEditor(object):
baseMass = base.urdf_inertial.mass
print("baseMass=",baseMass)
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
if (len(base.urdf_collision_shapes)):
v = base.urdf_collision_shapes[0]
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
if shapeType==p.GEOM_BOX:
baseCollisionShapeIndex = p.createCollisionShape(shapeType, \
halfExtents = [0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]],\
collisionFramePosition=v.origin_xyz,\
collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
physicsClientId=physicsClientId)
if shapeType==p.GEOM_SPHERE:
baseCollisionShapeIndex = p.createCollisionShape(shapeType, \
radius=v.geom_radius,\
collisionFramePosition=v.origin_xyz,\
collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
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)
@@ -348,6 +402,7 @@ class UrdfEditor(object):
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkMeshScaleArray=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
@@ -374,38 +429,39 @@ class UrdfEditor(object):
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
if (len(link.urdf_collision_shapes)):
v = link.urdf_collision_shapes[0]
print("v.origin_xyz=",v.origin_xyz)
print("v.origin_rpy=",v.origin_rpy)
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
if shapeType==p.GEOM_BOX:
linkCollisionShapeIndex = p.createCollisionShape(shapeType, \
halfExtents = [0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]],\
collisionFramePosition=v.origin_xyz,\
collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
physicsClientId=physicsClientId)
if shapeType==p.GEOM_SPHERE:
linkCollisionShapeIndex = p.createCollisionShape(shapeType, \
radius=v.geom_radius,\
collisionFramePosition=v.origin_xyz,\
collisionFrameOrientation=p.getQuaternionFromEuler(v.origin_rpy),\
physicsClientId=physicsClientId)
#for v in link.urdf_collision_shapes:
# v.tmp_collision_shape_ids=[]
# shapeType = v.geom_type
# if shapeType==p.GEOM_BOX:
# cid = p.createCollisionShape(shapeType, \
# radius=v.geom_radius,\
# halfExtents = v.geom_extents,\
# physicsClientId=physicsClientId)
# if (cid>=0):
# v.tmp_collision_shape_ids.append(cid)
# else:
# print("Warning: cannot convert collision shape", v)
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)
@@ -455,23 +511,32 @@ class UrdfEditor(object):
##########################################
org2 = p.connect(p.DIRECT)
org = p.connect(p.DIRECT)
org = p.connect(p.SHARED_MEMORY)
if (org<0):
org = p.connect(p.DIRECT)
gui = p.connect(p.GUI)
door = p.loadURDF("hinge.urdf", physicsClientId=org)
for i in range(p.getNumJoints(door,physicsClientId=org)):
p.setJointMotorControl2(door,i,p.VELOCITY_CONTROL,force=0,physicsClientId=org)
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(door,physicsClientId=org))
#print("numJoints:",p.getNumJoints(mb,physicsClientId=org))
print("base name:",p.getBodyInfo(door,physicsClientId=org))
#print("base name:",p.getBodyInfo(mb,physicsClientId=org))
for i in range(p.getNumJoints(door,physicsClientId=org)):
print("jointInfo(",i,"):",p.getJointInfo(door,i,physicsClientId=org))
print("linkState(",i,"):",p.getLinkState(door,i,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,physicsClientId=org)
parser.initializeFromBulletBody(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
@@ -480,13 +545,14 @@ if (1):
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
print("\n###########################################\n")
for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0.1,force=100,physicsClientId=gui)
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0.1,force=1,physicsClientId=gui)
print(p.getJointInfo(obUid,i,physicsClientId=gui))

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."},

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