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

Merge branch 'master' into master

Browse Source
This commit is contained in:
YunfeiBai
2018-01-08 18:13:03 -08:00
committed by GitHub
parent 0e0d3da2b5 6a74f63604
commit de3b8bf3c9
41 changed files with 2968 additions and 1413 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

2
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -251,7 +251,7 @@ struct BulletMJCFImporterInternalData
const char* angle = root_xml->Attribute("angle");
m_angleUnits = angle ? angle : "degree"; // degrees by default, http://www.mujoco.org/book/modeling.html#compiler
const char* inertiaFromGeom = root_xml->Attribute("inertiafromgeom");
if(inertiaFromGeom[0] == 'f') // false, other values assumed `true`.
if(inertiaFromGeom && inertiaFromGeom[0] == 'f') // false, other values assumed `true`.
{
m_inertiaFromGeom = false;
}

21
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@@ -119,9 +119,10 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
if (gMCFJFileNameArray.size()==0)
{
gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
gMCFJFileNameArray.push_back("MPL/MPL.xml");
gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
gMCFJFileNameArray.push_back("mjcf/ant.xml");
gMCFJFileNameArray.push_back("mjcf/hello_mjcf.xml");
@@ -198,7 +199,7 @@ void ImportMJCFSetup::initPhysics()
//@todo also use the modified collision filter for raycast and other collision related queries
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
//m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
btIDebugDraw::DBG_DrawConstraints
@@ -255,6 +256,8 @@ void ImportMJCFSetup::initPhysics()
s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
@@ -270,10 +273,11 @@ void ImportMJCFSetup::initPhysics()
#endif//TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLinkCollider(i)->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
m_data->m_mb = mb;
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
@@ -363,7 +367,16 @@ void ImportMJCFSetup::stepSimulation(float deltaTime)
{
if (m_data->m_jointMotors[i])
{
m_data->m_jointMotors[i]->setPositionTarget(m_data->m_motorTargetPositions[i]);
btScalar pos = m_data->m_motorTargetPositions[i];
int link = m_data->m_jointMotors[i]->getLinkA();
btScalar lowerLimit = m_data->m_mb->getLink(link).m_jointLowerLimit;
btScalar upperLimit = m_data->m_mb->getLink(link).m_jointUpperLimit;
if (lowerLimit < upperLimit)
{
btClamp(pos, lowerLimit, upperLimit);
}
m_data->m_jointMotors[i]->setPositionTarget(pos);
}
if (m_data->m_generic6DofJointMotors[i])
{

31
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");
@@ -599,8 +611,8 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
case URDF_GEOM_CYLINDER:
{
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
btScalar cylLength = collision->m_geometry.m_capsuleHeight;
btScalar cylHalfLength = 0.5*collision->m_geometry.m_capsuleHeight;
#if 0
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
@@ -620,10 +632,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl
cylZShape->optimizeConvexHull();
//btConvexShape* cylZShape = new btConeShapeZ(cylRadius,cylLength);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
//btVector3 halfExtents(cylRadius,cylRadius,cylLength);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
#else
btVector3 halfExtents(cylRadius,cylRadius, cylHalfLength);
btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
#endif
shape = cylZShape;
break;
@@ -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"

26
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -67,6 +67,32 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
info.m_jointUpperLimit = mb->m_links[link].m_jointUpperLimit;
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_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];
info.m_parentFrame[6] = mb->m_links[link].m_zeroRotParentToThis.m_floats[3];
info.m_jointAxis[0] = 0;
info.m_jointAxis[1] = 0;
info.m_jointAxis[2] = 0;
info.m_parentIndex = mb->m_links[link].m_parentIndex;
if (info.m_jointType == eRevoluteType)
{
info.m_jointAxis[0] = mb->m_links[link].m_jointAxisTop[0].m_floats[0];
info.m_jointAxis[1] = mb->m_links[link].m_jointAxisTop[0].m_floats[1];
info.m_jointAxis[2] = mb->m_links[link].m_jointAxisTop[0].m_floats[2];
}
if (info.m_jointType == ePrismaticType)
{
info.m_jointAxis[0] = mb->m_links[link].m_jointAxisBottom[0].m_floats[0];
info.m_jointAxis[1] = mb->m_links[link].m_jointAxisBottom[0].m_floats[1];
info.m_jointAxis[2] = mb->m_links[link].m_jointAxisBottom[0].m_floats[2];
}
if ((mb->m_links[link].m_jointType == eRevoluteType) ||
(mb->m_links[link].m_jointType == ePrismaticType)) {

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -58,6 +58,8 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) = 0;
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0;
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) = 0;

37
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2154,13 +2154,12 @@ B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, str
if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
return false;
info->m_mass = dynamicsInfo.m_mass;
info->m_localInertialDiagonal[0] = dynamicsInfo.m_localInertialDiagonal[0];
info->m_localInertialDiagonal[1] = dynamicsInfo.m_localInertialDiagonal[1];
info->m_localInertialDiagonal[2] = dynamicsInfo.m_localInertialDiagonal[2];
info->m_lateralFrictionCoeff = dynamicsInfo.m_lateralFrictionCoeff;
return true;
if (info)
{
*info = dynamicsInfo;
return true;
}
return false;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
@@ -3431,6 +3430,30 @@ B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_COLLISION_SHAPE_INFO;
command->m_requestCollisionShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
command->m_requestCollisionShapeDataArguments.m_linkIndex = linkIndex;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedCollisionShapeInformation(collisionShapeInfo);
}
}
//request visual shape information
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA)

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -259,6 +259,11 @@ B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, str
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);

30
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -44,6 +44,8 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
@@ -1253,6 +1255,27 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_COLLISION_SHAPE_INFO_FAILED:
{
b3Warning("getCollisionShapeData failed");
break;
}
case CMD_COLLISION_SHAPE_INFO_COMPLETED:
{
B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Collision Shape Information Request OK\n");
}
int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes;
m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied);
b3CollisionShapeData* shapeData = (b3CollisionShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
for (int i = 0; i < numCollisionShapesCopied; i++)
{
m_data->m_cachedCollisionShapes[i] = shapeData[i];
}
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@@ -1595,6 +1618,13 @@ void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualS
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}
void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
{
collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size();
collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes? &m_data->m_cachedCollisionShapes[0] : 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
if (m_data->m_debugLinesFrom.size()) {

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -68,6 +68,8 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

31
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -54,6 +54,8 @@ struct PhysicsDirectInternalData
btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
@@ -1011,6 +1013,27 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
break;
}
case CMD_COLLISION_SHAPE_INFO_FAILED:
{
b3Warning("getCollisionShapeData failed");
break;
}
case CMD_COLLISION_SHAPE_INFO_COMPLETED:
{
B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Collision Shape Information Request OK\n");
}
int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes;
m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied);
b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i = 0; i < numCollisionShapesCopied; i++)
{
m_data->m_cachedCollisionShapes[i] = shapeData[i];
}
break;
}
case CMD_RESTORE_STATE_FAILED:
{
b3Warning("restoreState failed");
@@ -1242,6 +1265,14 @@ void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformat
visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}
void PhysicsDirect::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
{
collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size();
collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0;
}
void PhysicsDirect::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();

2
examples/SharedMemory/PhysicsDirect.h
View File

@@ -91,6 +91,8 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

6
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -180,6 +180,12 @@ void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInform
return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
}
void PhysicsLoopBack::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
{
return m_data->m_physicsClient->getCachedCollisionShapeInformation(collisionShapesInfo);
}
void PhysicsLoopBack::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
return m_data->m_physicsClient->getCachedVREvents(vrEventsData);

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -72,6 +72,8 @@ public:
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);

717
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"
@@ -1516,6 +1517,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
btHashMap<btHashPtr, UrdfCollision> m_bulletCollisionShape2UrdfCollision;
btAlignedObjectArray<btStridingMeshInterface*> m_meshInterfaces;
MyOverlapFilterCallback* m_broadphaseCollisionFilterCallback;
@@ -2138,11 +2140,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);
@@ -2392,6 +2408,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;
@@ -2634,6 +2651,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);
@@ -3488,7 +3524,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;
@@ -3500,8 +3537,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;
@@ -3517,7 +3557,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();
}
@@ -3530,227 +3570,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;
@@ -4647,6 +4734,14 @@ 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)
{
btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
}
motor->setPositionTarget(desiredPosition,kp);
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
@@ -6296,6 +6391,29 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getBaseInertia()[1];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getBaseInertia()[2];
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction();
serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_rootLocalInertialFrame.getOrigin()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_rootLocalInertialFrame.getOrigin()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_rootLocalInertialFrame.getRotation()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_rootLocalInertialFrame.getRotation()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_rootLocalInertialFrame.getRotation()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_rootLocalInertialFrame.getRotation()[3];
serverCmd.m_dynamicsInfo.m_restitution = mb->getBaseCollider()->getRestitution();
serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getBaseCollider()->getRollingFriction();
serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getBaseCollider()->getSpinningFriction();
if (mb->getBaseCollider()->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
{
serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getBaseCollider()->getContactStiffness();
serverCmd.m_dynamicsInfo.m_contactDamping = mb->getBaseCollider()->getContactDamping();
}
else
{
serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
serverCmd.m_dynamicsInfo.m_contactDamping = -1;
}
}
else
{
@@ -6304,9 +6422,31 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[1] = mb->getLinkInertia(linkIndex)[1];
serverCmd.m_dynamicsInfo.m_localInertialDiagonal[2] = mb->getLinkInertia(linkIndex)[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[0] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[1] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[2] = body->m_linkLocalInertialFrames[linkIndex].getOrigin()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[3] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[0];
serverCmd.m_dynamicsInfo.m_localInertialFrame[4] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[1];
serverCmd.m_dynamicsInfo.m_localInertialFrame[5] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[2];
serverCmd.m_dynamicsInfo.m_localInertialFrame[6] = body->m_linkLocalInertialFrames[linkIndex].getRotation()[3];
if (mb->getLinkCollider(linkIndex))
{
serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction();
serverCmd.m_dynamicsInfo.m_restitution = mb->getLinkCollider(linkIndex)->getRestitution();
serverCmd.m_dynamicsInfo.m_rollingFrictionCoeff = mb->getLinkCollider(linkIndex)->getRollingFriction();
serverCmd.m_dynamicsInfo.m_spinningFrictionCoeff = mb->getLinkCollider(linkIndex)->getSpinningFriction();
if (mb->getLinkCollider(linkIndex)->getCollisionFlags() & btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)
{
serverCmd.m_dynamicsInfo.m_contactStiffness = mb->getLinkCollider(linkIndex)->getContactStiffness();
serverCmd.m_dynamicsInfo.m_contactDamping = mb->getLinkCollider(linkIndex)->getContactDamping();
}
else
{
serverCmd.m_dynamicsInfo.m_contactStiffness = -1;
serverCmd.m_dynamicsInfo.m_contactDamping = -1;
}
}
else
{
@@ -8026,7 +8166,183 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
return hasStatus;
}
// PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
// PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
// PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
// PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH);
// PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
// PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape* colShape, const btTransform& transform, b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes)
{
if (maxCollisionShapes <= 0)
{
b3Warning("No space in buffer");
return 0;
}
int numConverted = 0;
collisionShapeBuffer[0].m_localCollisionFrame[0] = transform.getOrigin()[0];
collisionShapeBuffer[0].m_localCollisionFrame[1] = transform.getOrigin()[1];
collisionShapeBuffer[0].m_localCollisionFrame[2] = transform.getOrigin()[2];
collisionShapeBuffer[0].m_localCollisionFrame[3] = transform.getRotation()[0];
collisionShapeBuffer[0].m_localCollisionFrame[4] = transform.getRotation()[1];
collisionShapeBuffer[0].m_localCollisionFrame[5] = transform.getRotation()[2];
collisionShapeBuffer[0].m_localCollisionFrame[6] = transform.getRotation()[3];
collisionShapeBuffer[0].m_meshAssetFileName[0] = 0;
switch (colShape->getShapeType())
{
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
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;
}
case CYLINDER_SHAPE_PROXYTYPE:
{
btCylinderShapeZ* cyl = (btCylinderShapeZ*)colShape;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CYLINDER;
collisionShapeBuffer[0].m_dimensions[0] = 2.*cyl->getHalfExtentsWithMargin().getZ();
collisionShapeBuffer[0].m_dimensions[1] = cyl->getHalfExtentsWithMargin().getX();
collisionShapeBuffer[0].m_dimensions[2] = 0;
numConverted++;
break;
}
case BOX_SHAPE_PROXYTYPE:
{
btBoxShape* box = (btBoxShape*)colShape;
btVector3 halfExtents = box->getHalfExtentsWithMargin();
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_BOX;
collisionShapeBuffer[0].m_dimensions[0] = 2.*halfExtents[0];
collisionShapeBuffer[0].m_dimensions[1] = 2.*halfExtents[1];
collisionShapeBuffer[0].m_dimensions[2] = 2.*halfExtents[2];
numConverted++;
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
btSphereShape* sphere = (btSphereShape*)colShape;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_SPHERE;
collisionShapeBuffer[0].m_dimensions[0] = sphere->getRadius();
collisionShapeBuffer[0].m_dimensions[1] = sphere->getRadius();
collisionShapeBuffer[0].m_dimensions[2] = sphere->getRadius();
numConverted++;
break;
}
case COMPOUND_SHAPE_PROXYTYPE:
{
//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))
{
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;
}
default:
{
b3Warning("Unexpected collision shape type in PhysicsServerCommandProcessor::extractCollisionShapes");
}
};
return numConverted;
}
bool PhysicsServerCommandProcessor::processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
BT_PROFILE("CMD_REQUEST_COLLISION_SHAPE_INFO");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_FAILED;
int bodyUniqueId = clientCmd.m_requestCollisionShapeDataArguments.m_bodyUniqueId;
int linkIndex = clientCmd.m_requestCollisionShapeDataArguments.m_linkIndex;
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (bodyHandle)
{
if (bodyHandle->m_multiBody)
{
b3CollisionShapeData* collisionShapeStoragePtr = (b3CollisionShapeData*)bufferServerToClient;
int totalBytesPerObject = sizeof(b3CollisionShapeData);
int maxNumColObjects = bufferSizeInBytes / totalBytesPerObject - 1;
btTransform childTrans;
childTrans.setIdentity();
serverCmd.m_sendCollisionShapeArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendCollisionShapeArgs.m_linkIndex = linkIndex;
if (linkIndex == -1)
{
if (bodyHandle->m_multiBody->getBaseCollider())
{
//extract shape info from base collider
int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getBaseCollider()->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
}
}
else
{
if (linkIndex >= 0 && linkIndex < bodyHandle->m_multiBody->getNumLinks() && bodyHandle->m_multiBody->getLinkCollider(linkIndex))
{
int numConvertedCollisionShapes = extractCollisionShapes(bodyHandle->m_multiBody->getLinkCollider(linkIndex)->getCollisionShape(), childTrans, collisionShapeStoragePtr, maxNumColObjects);
serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes = numConvertedCollisionShapes;
serverCmd.m_type = CMD_COLLISION_SHAPE_INFO_COMPLETED;
}
}
}
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -8736,6 +9052,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_COLLISION_SHAPE_INFO:
{
hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_UPDATE_VISUAL_SHAPE:
{
hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);

3
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -74,6 +74,7 @@ protected:
bool processCreateUserConstraintCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestVisualShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestCollisionShapeInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processUpdateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processChangeTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadTextureCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -83,6 +84,8 @@ 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& transform, struct b3CollisionShapeData* collisionShapeBuffer, int maxCollisionShapes);
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,

18
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -2542,14 +2542,14 @@ void PhysicsServerExample::drawUserDebugLines()
}
float colorRGBA[4] = {
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
1.};
(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
(float)1.};
float pos[3] = {m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
float pos[3] = { (float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};
int graphicsIndex = m_multiThreadedHelper->m_userDebugText[i].m_trackingVisualShapeIndex;
if (graphicsIndex>=0)
@@ -2604,7 +2604,9 @@ void PhysicsServerExample::drawUserDebugLines()
offset.setIdentity();
offset.setOrigin(btVector3(0,-float(t)*sz,0));
btTransform result = tr*offset;
float newpos[3] = {result.getOrigin()[0],result.getOrigin()[1],result.getOrigin()[2]};
float newpos[3] = { (float)result.getOrigin()[0],
(float)result.getOrigin()[1],
(float)result.getOrigin()[2]};
m_guiHelper->getAppInterface()->drawText3D(pieces[t].c_str(),
newpos,orientation,colorRGBA,

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -303,6 +303,12 @@ struct RequestVisualShapeDataArgs
int m_startingVisualShapeIndex;
};
struct RequestCollisionShapeDataArgs
{
int m_bodyUniqueId;
int m_linkIndex;
};
enum EnumUpdateVisualShapeData
{
CMD_UPDATE_VISUAL_SHAPE_TEXTURE=1,
@@ -338,7 +344,12 @@ struct SendVisualShapeDataArgs
int m_numRemainingVisualShapes;
};
struct SendCollisionShapeDataArgs
{
int m_bodyUniqueId;
int m_linkIndex;
int m_numCollisionShapes;
};
struct SendDebugLinesArgs
{
@@ -1000,6 +1011,7 @@ struct SharedMemoryCommand
struct b3SearchPathfArgs m_searchPathArgs;
struct b3CustomCommand m_customCommandArgs;
struct b3StateSerializationArguments m_loadStateArguments;
struct RequestCollisionShapeDataArgs m_requestCollisionShapeDataArguments;
};
};
@@ -1076,6 +1088,7 @@ struct SharedMemoryStatus
struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
struct b3StateSerializationArguments m_saveStateResultArgs;
struct b3LoadSoftBodyResultArgs m_loadSoftBodyResultArguments;
struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
};
};

35
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
@@ -80,6 +81,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
CMD_SAVE_STATE,
CMD_RESTORE_STATE,
CMD_REQUEST_COLLISION_SHAPE_INFO,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -185,6 +187,8 @@ enum EnumSharedMemoryServerStatus
CMD_SAVE_STATE_COMPLETED,
CMD_RESTORE_STATE_FAILED,
CMD_RESTORE_STATE_COMPLETED,
CMD_COLLISION_SHAPE_INFO_COMPLETED,
CMD_COLLISION_SHAPE_INFO_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -217,10 +221,6 @@ enum JointType {
eGearType=6
};
enum b3RequestDynamicsInfoFlags
{
eDYNAMICS_INFO_REPORT_INERTIA=1,
};
enum b3JointInfoFlags
{
@@ -247,6 +247,7 @@ struct b3JointInfo
double m_parentFrame[7]; // position and orientation (quaternion)
double m_childFrame[7]; // ^^^
double m_jointAxis[3]; // joint axis in parent local frame
int m_parentIndex;
};
@@ -278,7 +279,14 @@ struct b3DynamicsInfo
{
double m_mass;
double m_localInertialDiagonal[3];
double m_localInertialFrame[7];
double m_lateralFrictionCoeff;
double m_rollingFrictionCoeff;
double m_spinningFrictionCoeff;
double m_restitution;
double m_contactStiffness;
double m_contactDamping;
};
// copied from btMultiBodyLink.h
@@ -531,6 +539,23 @@ struct b3VisualShapeInformation
struct b3VisualShapeData* m_visualShapeData;
};
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+GEOM_CYLINDER:length, radius, GEOM_MESH: mesh scale
double m_localCollisionFrame[7];//pos[3], orn[4]
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
};
struct b3CollisionShapeInformation
{
int m_numCollisionShapes;
struct b3CollisionShapeData* m_collisionShapeData;
};
enum eLinkStateFlags
{
ACTUAL_STATE_COMPUTE_LINKVELOCITY=1,

32
examples/pybullet/examples/humanoid_manual_control.py Normal file
View File

@@ -0,0 +1,32 @@
import pybullet as p
import time
p.connect(p.GUI)
obUids = p.loadMJCF("mjcf/humanoid.xml")
humanoid = obUids[1]
gravId = p.addUserDebugParameter("gravity",-10,10,-10)
jointIds=[]
paramIds=[]
p.setPhysicsEngineParameter(numSolverIterations=10)
p.changeDynamics(humanoid,-1,linearDamping=0, angularDamping=0)
for j in range (p.getNumJoints(humanoid)):
p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
info = p.getJointInfo(humanoid,j)
#print(info)
jointName = info[1]
jointType = info[2]
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
p.setRealTimeSimulation(1)
while(1):
p.setGravity(0,0,p.readUserDebugParameter(gravId))
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(humanoid,jointIds[i],p.POSITION_CONTROL,targetPos, force=5*240.)
time.sleep(0.01)

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)

567
examples/pybullet/examples/urdfEditor.py Normal file
View File

@@ -0,0 +1,567 @@
import pybullet as p
import time
class UrdfInertial(object):
def __init__(self):
self.mass = 1
self.inertia_xxyyzz=[7,8,9]
self.origin_rpy=[1,2,3]
self.origin_xyz=[4,5,6]
class UrdfContact(object):
def __init__(self):
self.lateral_friction = 1
self.rolling_friction = 0
self.spinning_friction = 0
class UrdfLink(object):
def __init__(self):
self.link_name = "dummy"
self.urdf_inertial = UrdfInertial()
self.urdf_visual_shapes=[]
self.urdf_collision_shapes=[]
class UrdfVisual(object):
def __init__(self):
self.origin_rpy = [1,2,3]
self.origin_xyz = [4,5,6]
self.geom_type = p.GEOM_BOX
self.geom_radius = 1
self.geom_extents = [7,8,9]
self.geom_length=[10]
self.geom_meshfilename = "meshfile"
self.geom_meshscale=[1,1,1]
self.material_rgba = [1,0,0,1]
self.material_name = ""
class UrdfCollision(object):
def __init__(self):
self.origin_rpy = [1,2,3]
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_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
self.joint_upper_limit = -1
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):
def __init__(self):
self.initialize()
def initialize(self):
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 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
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1]==linkIndex):
#print("visualShape base:",v)
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
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])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex,matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex=matIndex+1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
for v in collisionShapes:
#print("collisionShape base:",v)
urdfCollision = UrdfCollision()
print("geom type=",v[0])
urdfCollision.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
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_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, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
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,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = 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]
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
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
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):
file.write("\t\t<inertial>\n")
str = '\t\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(\
urdfInertial.origin_rpy[0],urdfInertial.origin_rpy[1],urdfInertial.origin_rpy[2],\
urdfInertial.origin_xyz[0],urdfInertial.origin_xyz[1],urdfInertial.origin_xyz[2], prec=precision)
file.write(str)
str = '\t\t\t<mass value=\"{:.{prec}f}\"/>\n'.format(urdfInertial.mass,prec=precision)
file.write(str)
str = '\t\t\t<inertia ixx=\"{:.{prec}f}\" ixy=\"0\" ixz=\"0\" iyy=\"{:.{prec}f}\" iyz=\"0\" izz=\"{:.{prec}f}\"/>\n'.format(\
urdfInertial.inertia_xxyyzz[0],\
urdfInertial.inertia_xxyyzz[1],\
urdfInertial.inertia_xxyyzz[2],prec=precision)
file.write(str)
file.write("\t\t</inertial>\n")
def writeVisualShape(self,file,urdfVisual, precision=5):
file.write("\t\t<visual>\n")
str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2],
urdfVisual.origin_xyz[0],urdfVisual.origin_xyz[1],urdfVisual.origin_xyz[2], prec=precision)
file.write(str)
file.write("\t\t\t<geometry>\n")
if urdfVisual.geom_type == p.GEOM_BOX:
str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfVisual.geom_extents[0],\
urdfVisual.geom_extents[1],urdfVisual.geom_extents[2], prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_SPHERE:
str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfVisual.geom_radius,\
prec=precision)
file.write(str)
if urdfVisual.geom_type == p.GEOM_MESH:
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)
file.write(str)
str = '\t\t\t\t<color rgba="{:.{prec}f} {:.{prec}f} {:.{prec}f} {:.{prec}f}" />\n'.format(urdfVisual.material_rgba[0],\
urdfVisual.material_rgba[1],urdfVisual.material_rgba[2],urdfVisual.material_rgba[3],prec=precision)
file.write(str)
file.write("\t\t\t</material>\n")
file.write("\t\t</visual>\n")
def writeCollisionShape(self,file,urdfCollision, precision=5):
file.write("\t\t<collision>\n")
str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
urdfCollision.origin_rpy[0],urdfCollision.origin_rpy[1],urdfCollision.origin_rpy[2],
urdfCollision.origin_xyz[0],urdfCollision.origin_xyz[1],urdfCollision.origin_xyz[2], prec=precision)
file.write(str)
file.write("\t\t\t<geometry>\n")
if urdfCollision.geom_type == p.GEOM_BOX:
str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfCollision.geom_extents[0],\
urdfCollision.geom_extents[1],urdfCollision.geom_extents[2], prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_SPHERE:
str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfCollision.geom_radius,\
prec=precision)
file.write(str)
if urdfCollision.geom_type == p.GEOM_MESH:
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")
def writeLink(self, file, urdfLink):
file.write("\t<link name=\"")
file.write(urdfLink.link_name)
file.write("\">\n")
self.writeInertial(file,urdfLink.urdf_inertial)
for v in urdfLink.urdf_visual_shapes:
self.writeVisualShape(file,v)
for c in urdfLink.urdf_collision_shapes:
self.writeCollisionShape(file,c)
file.write("\t</link>\n")
def writeJoint(self, file, urdfJoint, precision=5):
jointTypeStr = "invalid"
if urdfJoint.joint_type == p.JOINT_REVOLUTE:
if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit:
jointTypeStr = "continuous"
else:
jointTypeStr = "revolute"
if urdfJoint.joint_type == p.JOINT_FIXED:
jointTypeStr = "fixed"
if urdfJoint.joint_type == p.JOINT_PRISMATIC:
jointTypeStr = "prismatic"
str = '\t<joint name=\"{}\" type=\"{}\">\n'.format(urdfJoint.joint_name, jointTypeStr)
file.write(str)
str = '\t\t<parent link=\"{}\"/>\n'.format(urdfJoint.parent_name)
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)
file.write(str)
str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
file.write(str)
file.write("\t</joint>\n")
def saveUrdf(self, fileName):
file = open(fileName,"w")
file.write("<?xml version=\"0.0\" ?>\n")
file.write("<robot name=\"")
file.write(self.robotName)
file.write("\">\n")
for link in self.urdfLinks:
self.writeLink(file,link)
for joint in self.urdfJoints:
self.writeJoint(file,joint)
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(mb,physicsClientId=org)
parser.saveUrdf("test.urdf")
if (1):
print("\ncreatingMultiBody...................n")
obUid = parser.createMultiBody(physicsClientId=gui)
parser2 = UrdfEditor()
print("\n###########################################\n")
parser2.initializeFromBulletBody(obUid,physicsClientId=gui)
parser2.saveUrdf("test2.urdf")
for i in range (p.getNumJoints(obUid, physicsClientId=gui)):
p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui)
print(p.getJointInfo(obUid,i,physicsClientId=gui))
parser=0
p.setRealTimeSimulation(1,physicsClientId=gui)
while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]):
p.stepSimulation(physicsClientId=gui)
time.sleep(0.01)

427
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++)
@@ -913,13 +916,12 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
{
int bodyUniqueId = -1;
int linkIndex = -2;
int flags = 0;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "flags", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex, &flags, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
{
return NULL;
}
@@ -934,11 +936,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
struct b3DynamicsInfo info;
int numFields = 2;
if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
{
numFields++;
}
if (bodyUniqueId < 0)
{
@@ -959,25 +957,41 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
return NULL;
}
if (b3GetDynamicsInfo(status_handle, &info))
{
int numFields = 10;
PyObject* pyDynamicsInfo = PyTuple_New(numFields);
PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass));
PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff));
if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
{
PyObject* pyInertiaDiag = PyTuple_New(3);
PyTuple_SetItem(pyInertiaDiag,0,PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
PyTuple_SetItem(pyInertiaDiag,1,PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
PyTuple_SetItem(pyInertiaDiag,2,PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
PyTuple_SetItem(pyInertiaDiag, 0, PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
PyTuple_SetItem(pyInertiaDiag, 1, PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
PyTuple_SetItem(pyInertiaDiag, 2, PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
PyTuple_SetItem(pyDynamicsInfo, 2, pyInertiaDiag);
}
{
PyObject* pyInertiaPos= PyTuple_New(3);
PyTuple_SetItem(pyInertiaPos, 0, PyFloat_FromDouble(info.m_localInertialFrame[0]));
PyTuple_SetItem(pyInertiaPos, 1, PyFloat_FromDouble(info.m_localInertialFrame[1]));
PyTuple_SetItem(pyInertiaPos, 2, PyFloat_FromDouble(info.m_localInertialFrame[2]));
PyTuple_SetItem(pyDynamicsInfo, 3, pyInertiaPos);
}
{
PyObject* pyInertiaOrn= PyTuple_New(4);
PyTuple_SetItem(pyInertiaOrn, 0, PyFloat_FromDouble(info.m_localInertialFrame[3]));
PyTuple_SetItem(pyInertiaOrn, 1, PyFloat_FromDouble(info.m_localInertialFrame[4]));
PyTuple_SetItem(pyInertiaOrn, 2, PyFloat_FromDouble(info.m_localInertialFrame[5]));
PyTuple_SetItem(pyInertiaOrn, 3, PyFloat_FromDouble(info.m_localInertialFrame[6]));
PyTuple_SetItem(pyDynamicsInfo, 4, pyInertiaOrn);
}
PyTuple_SetItem(pyDynamicsInfo, 5, PyFloat_FromDouble(info.m_restitution));
PyTuple_SetItem(pyDynamicsInfo, 6, PyFloat_FromDouble(info.m_rollingFrictionCoeff));
PyTuple_SetItem(pyDynamicsInfo, 7, PyFloat_FromDouble(info.m_spinningFrictionCoeff));
PyTuple_SetItem(pyDynamicsInfo, 8, PyFloat_FromDouble(info.m_contactDamping));
PyTuple_SetItem(pyDynamicsInfo, 9, PyFloat_FromDouble(info.m_contactStiffness));
return pyDynamicsInfo;
}
}
@@ -3128,7 +3142,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
int bodyUniqueId = -1;
int jointIndex = -1;
int jointInfoSize = 13; // size of struct b3JointInfo
int jointInfoSize = 17; // size of struct b3JointInfo
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL};
@@ -3199,6 +3213,30 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
PyTuple_SetItem(pyListJointInfo, 12,
PyString_FromString("not available"));
}
{
PyObject* axis = PyTuple_New(3);
PyTuple_SetItem(axis, 0, PyFloat_FromDouble(info.m_jointAxis[0]));
PyTuple_SetItem(axis, 1, PyFloat_FromDouble(info.m_jointAxis[1]));
PyTuple_SetItem(axis, 2, PyFloat_FromDouble(info.m_jointAxis[2]));
PyTuple_SetItem(pyListJointInfo, 13, axis);
}
{
PyObject* pos = PyTuple_New(3);
PyTuple_SetItem(pos, 0, PyFloat_FromDouble(info.m_parentFrame[0]));
PyTuple_SetItem(pos, 1, PyFloat_FromDouble(info.m_parentFrame[1]));
PyTuple_SetItem(pos, 2, PyFloat_FromDouble(info.m_parentFrame[2]));
PyTuple_SetItem(pyListJointInfo, 14, pos);
}
{
PyObject* orn = PyTuple_New(4);
PyTuple_SetItem(orn, 0, PyFloat_FromDouble(info.m_parentFrame[3]));
PyTuple_SetItem(orn, 1, PyFloat_FromDouble(info.m_parentFrame[4]));
PyTuple_SetItem(orn, 2, PyFloat_FromDouble(info.m_parentFrame[5]));
PyTuple_SetItem(orn, 3, PyFloat_FromDouble(info.m_parentFrame[6]));
PyTuple_SetItem(pyListJointInfo, 15, orn);
}
PyTuple_SetItem(pyListJointInfo, 16, PyInt_FromLong(info.m_parentIndex));
return pyListJointInfo;
}
@@ -4834,6 +4872,107 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py
return Py_None;
}
static PyObject* pybullet_getCollisionShapeData(PyObject* self, PyObject* args, PyObject* keywds)
{
int objectUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3CollisionShapeInformation collisionShapeInfo;
int statusType;
int i;
int linkIndex;
PyObject* pyResultList = 0;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = { "objectUniqueId", "linkIndex", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &objectUniqueId, &linkIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
commandHandle = b3InitRequestCollisionShapeInformation(sm, objectUniqueId, linkIndex);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED)
{
b3GetCollisionShapeInformation(sm, &collisionShapeInfo);
pyResultList = PyTuple_New(collisionShapeInfo.m_numCollisionShapes);
for (i = 0; i < collisionShapeInfo.m_numCollisionShapes; i++)
{
PyObject* collisionShapeObList = PyTuple_New(7);
PyObject* item;
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_objectUniqueId);
PyTuple_SetItem(collisionShapeObList, 0, item);
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_linkIndex);
PyTuple_SetItem(collisionShapeObList, 1, item);
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_collisionGeometryType);
PyTuple_SetItem(collisionShapeObList, 2, item);
{
PyObject* vec = PyTuple_New(3);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[0]);
PyTuple_SetItem(vec, 0, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[1]);
PyTuple_SetItem(vec, 1, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(collisionShapeObList, 3, vec);
}
item = PyString_FromString(collisionShapeInfo.m_collisionShapeData[i].m_meshAssetFileName);
PyTuple_SetItem(collisionShapeObList, 4, item);
{
PyObject* vec = PyTuple_New(3);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[0]);
PyTuple_SetItem(vec, 0, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[1]);
PyTuple_SetItem(vec, 1, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(collisionShapeObList, 5, vec);
}
{
PyObject* vec = PyTuple_New(4);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[3]);
PyTuple_SetItem(vec, 0, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[4]);
PyTuple_SetItem(vec, 1, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[5]);
PyTuple_SetItem(vec, 2, item);
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[6]);
PyTuple_SetItem(vec, 3, item);
PyTuple_SetItem(collisionShapeObList, 6, vec);
}
PyTuple_SetItem(pyResultList, i, collisionShapeObList);
}
return pyResultList;
}
else
{
PyErr_SetString(SpamError, "Error receiving collision shape info");
return NULL;
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds)
{
int objectUniqueId = -1;
@@ -5564,7 +5703,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);
@@ -5582,6 +5721,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(fileNameObj);
#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)
{
@@ -5591,7 +5957,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;
@@ -5613,9 +5979,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;
}
@@ -5645,13 +6011,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)
{
@@ -5692,7 +6058,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);
@@ -7944,6 +8310,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."},
@@ -8152,6 +8521,9 @@ static PyMethodDef SpamMethods[] = {
{"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS | METH_KEYWORDS,
"Return the visual shape information for one object."},
{ "getCollisionShapeData", (PyCFunction)pybullet_getCollisionShapeData, METH_VARARGS | METH_KEYWORDS,
"Return the collision shape information for one object." },
{"changeVisualShape", (PyCFunction)pybullet_changeVisualShape, METH_VARARGS | METH_KEYWORDS,
"Change part of the visual shape information for one object."},
@@ -8370,8 +8742,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read
PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType); // user read
PyModule_AddIntConstant(m, "DYNAMICS_INFO_REPORT_INERTIA", eDYNAMICS_INFO_REPORT_INERTIA); // report local inertia in 'getDynamicsInfo'
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read
PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);