diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index de2b3a8f6..dde3d2ed8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -721,7 +721,8 @@ b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHan } return 0; } -void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) + +int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -735,11 +736,13 @@ void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,d command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius; command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; } } + return -1; } -void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) +int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -755,6 +758,112 @@ void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doub command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1]; command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2]; command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + + +int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[2] = planeNormal[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeConstant = planeConstant; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + strcpy(command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName); + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + return shapeIndex; + } + } + return -1; +} + +void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + if (shapeIndexm_createCollisionShapeArgs.m_numCollisionShapes) + { + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags |= flags; } } } @@ -857,14 +966,14 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass int baseLinkIndex = numLinks; command->m_updateFlags |=MULTI_BODY_HAS_BASE; command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex; - command->m_createMultiBodyArgs.m_baseWorldPosition[0]=basePosition[0]; - command->m_createMultiBodyArgs.m_baseWorldPosition[1]=basePosition[1]; - command->m_createMultiBodyArgs.m_baseWorldPosition[2]=basePosition[2]; + command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+0]=basePosition[0]; + command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+1]=basePosition[1]; + command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+2]=basePosition[2]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[0]=baseOrientation[0]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[1]=baseOrientation[1]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[2]=baseOrientation[2]; - command->m_createMultiBodyArgs.m_baseWorldOrientation[3]=baseOrientation[3]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+0]=baseOrientation[0]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+1]=baseOrientation[1]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+2]=baseOrientation[2]; + command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+3]=baseOrientation[3]; command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = mass; command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = mass; @@ -883,7 +992,7 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique; command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId; - command->m_createMultiBodyArgs.m_linkMasses[command->m_createMultiBodyArgs.m_numLinks] = mass; + command->m_createMultiBodyArgs.m_linkMasses[baseLinkIndex] = mass; command->m_createMultiBodyArgs.m_numLinks++; } return numLinks; @@ -891,6 +1000,69 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass return -2; } +int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, + double linkVisualShapeIndex, + double linkPosition[3], + double linkOrientation[4], + int linkParentIndex, + int linkJointType, + double linkJointAxis[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + if (command->m_type==CMD_CREATE_MULTI_BODY) + { + int numLinks = command->m_createMultiBodyArgs.m_numLinks; + + if (numLinksm_updateFlags |=MULTI_BODY_HAS_BASE; + command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+0]=linkPosition[0]; + command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+1]=linkPosition[1]; + command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+2]=linkPosition[2]; + + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+0]=linkOrientation[0]; + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+1]=linkOrientation[1]; + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+2]=linkOrientation[2]; + command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+3]=linkOrientation[3]; + + command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+0] = linkMass; + command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+1] = linkMass; + command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+2] = linkMass; + + + command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+0] = 0; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+1] = 0; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+2] = 0; + + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+0] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+1] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+2] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+3] = 1; + + command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex]= linkCollisionShapeIndex; + command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] = linkVisualShapeIndex; + + command->m_createMultiBodyArgs.m_linkParentIndices[linkIndex] = linkParentIndex; + command->m_createMultiBodyArgs.m_linkJointTypes[linkIndex] = linkJointType; + command->m_createMultiBodyArgs.m_linkJointAxis[0] = linkJointAxis[0]; + command->m_createMultiBodyArgs.m_linkJointAxis[1] = linkJointAxis[1]; + command->m_createMultiBodyArgs.m_linkJointAxis[2] = linkJointAxis[2]; + + command->m_createMultiBodyArgs.m_linkMasses[linkIndex] = linkMass; + command->m_createMultiBodyArgs.m_numLinks++; + return numLinks; + } + } + + return -1; +} + + + + //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 267691dd3..24a3ddd77 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -326,9 +326,13 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); -void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); -void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]); - +int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); +int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]); +int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height); +int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height); +int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant); +int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]); +void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags); void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]); @@ -340,6 +344,15 @@ int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]); +int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, + double linkVisualShapeIndex, + double linkPosition[3], + double linkOrientation[4], + int linkParentIndex, + int linkJointType, + double linkJointAxis[3]); + + //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f3d0f40b9..ee0412a9b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -11,8 +11,9 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h" - - +#include "../Importers/ImportURDFDemo/UrdfParser.h" +#include "../Utils/b3ResourcePath.h" +#include "Bullet3Common/b3FileUtils.h" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" @@ -1620,6 +1621,14 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const { + for (int i=0;im_customVisualShapesConverter) + { + const UrdfModel& model = m_data->m_urdfParser.getModel(); + UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex); + if (linkPtr) + { + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId); + } + } + } + #endif } virtual void setBodyUniqueId(int bodyId) { @@ -3493,6 +3517,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + btCollisionShape* shape = 0; btCompoundShape* compound = 0; @@ -3547,6 +3572,121 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } break; } + case GEOM_CAPSULE: + { + shape = worldImporter->createCapsuleShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_CYLINDER: + { + shape = worldImporter->createCylinderShapeZ(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius, + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_PLANE: + { + btVector3 planeNormal(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]); + + shape = worldImporter->createPlaneShape(planeNormal,0); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_MESH: + { + btScalar defaultCollisionMargin = 0.001; + + btVector3 meshScale(clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshScale[2]); + + const std::string& urdf_path=""; + + std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName; + + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (b3ResourcePath::findResourcePath(fileName.c_str(), relativeFileName, 1024)) + { + + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + + const std::string& error_message_prefix=""; + std::string out_found_filename; + int out_type; + + bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); + if (foundFile) + { + if (out_type==UrdfGeometry::FILE_OBJ) + { + std::vector shapes; + std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str()); + //create a convex hull for each shape, and store it in a btCompoundShape + + //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); + //static btCollisionShape* createConvexHullFromShapes(std::vector& 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; faddPoint(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: + { + } } } #if 0 @@ -3593,6 +3733,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_sdfRecentLoadedBodies.clear(); + #if 0 + struct UrdfModel + { + std::string m_name; + std::string m_sourceFile; + btTransform m_rootTransformInWorld; + btHashMap m_materials; + btHashMap m_links; + btHashMap m_joints; + + btArray m_rootLinks; + bool m_overrideFixedBase; + + UrdfModel() + + clientCmd.m_createMultiBodyArgs. + + char m_bodyName[1024]; + int m_baseLinkIndex; + + double m_baseWorldPosition[3]; + double m_baseWorldOrientation[4]; + + UrdfModel tmpModel; + tmpModel.m_bodyName = + #endif + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); bool useMultiBody = true; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index cfe3db329..260c6cd7d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -816,10 +816,13 @@ struct b3CreateCollisionShape double m_capsuleFrom[3]; double m_capsuleTo[3]; double m_planeNormal[3]; + double m_planeConstant; int m_meshFileType; - char m_meshFileName[1024]; - double m_meshScale; + char m_meshFileName[VISUAL_SHAPE_MAX_PATH_LEN]; + double m_meshScale[3]; + int m_collisionFlags; + }; #define MAX_COMPOUND_COLLISION_SHAPES 16 @@ -847,17 +850,22 @@ struct b3CreateMultiBodyArgs char m_bodyName[1024]; int m_baseLinkIndex; - double m_baseWorldPosition[3]; - double m_baseWorldOrientation[4]; + double m_linkPositions[3*MAX_CREATE_MULTI_BODY_LINKS]; + double m_linkOrientations[4*MAX_CREATE_MULTI_BODY_LINKS]; int m_numLinks; + int m_linkParents[MAX_CREATE_MULTI_BODY_LINKS]; double m_linkMasses[MAX_CREATE_MULTI_BODY_LINKS]; double m_linkInertias[MAX_CREATE_MULTI_BODY_LINKS*3]; + double m_linkInertialFramePositions[MAX_CREATE_MULTI_BODY_LINKS*3]; double m_linkInertialFrameOrientations[MAX_CREATE_MULTI_BODY_LINKS*4]; - int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkCollisionShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; int m_linkVisualShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkParentIndices[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; + double m_linkJointAxis[3*MAX_CREATE_MULTI_BODY_LINKS]; #if 0 std::string m_name; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b5526bcd5..882db7a24 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -594,6 +594,12 @@ enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes GEOM_UNKNOWN, }; +enum eUrdfCollisionFlags +{ + GEOM_FORCE_CONCAVE_TRIMESH=1, +}; + + #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e75e6c1b7..478c92bd7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -84,6 +84,24 @@ static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) return v; } +static int pybullet_internalGetIntFromSequence(PyObject* seq, int index) +{ + int v = 0; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + v = PyLong_AsLong(item); + } + else + { + item = PyTuple_GET_ITEM(seq, index); + v = PyLong_AsLong(item); + } + return v; +} + // internal function to set a float matrix[16] // used to initialize camera position with // a view and projection matrix in renderImage() @@ -197,6 +215,45 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) return 0; } + +static int pybullet_internalGetVector3FromSequence(PyObject* seq, int index, double vec[3]) +{ + int v = 0; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + pybullet_internalSetVectord(item,vec); + } + else + { + item = PyTuple_GET_ITEM(seq, index); + pybullet_internalSetVectord(item,vec); + } + return v; +} + +static int pybullet_internalGetVector4FromSequence(PyObject* seq, int index, double vec[4]) +{ + int v = 0; + PyObject* item; + + if (PyList_Check(seq)) + { + item = PyList_GET_ITEM(seq, index); + pybullet_internalSetVector4d(item,vec); + } + else + { + item = PyTuple_GET_ITEM(seq, index); + pybullet_internalSetVector4d(item,vec); + } + return v; +} + + + // Step through one timestep of the simulation static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObject* keywds) { @@ -4891,12 +4948,21 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P int physicsClientId = 0; b3PhysicsClientHandle sm = 0; int shapeType=-1; - double radius=-1; + double radius=0.5; + double height = 1; + PyObject* meshScaleObj=0; + double meshScale[3] = {1,1,1}; + PyObject* planeNormalObj=0; + double planeNormal[3] = {0,0,1}; + + char* fileName=0; + int flags = 0; + PyObject* halfExtentsObj=0; - static char* kwlist[] = {"shapeType","radius","halfExtents", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOi", kwlist, - &shapeType, &radius,&halfExtentsObj, &physicsClientId)) + static char* kwlist[] = {"shapeType","radius","halfExtents", "height", "fileName", "meshScale", "planeNormal", "flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOdsOOii", kwlist, + &shapeType, &radius,&halfExtentsObj, &height, &fileName, &meshScaleObj, &planeNormalObj, &flags, &physicsClientId)) { return NULL; } @@ -4910,16 +4976,41 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P { b3SharedMemoryStatusHandle statusHandle; int statusType; + int shapeIndex = -1; + b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm); if (shapeType==GEOM_SPHERE && radius>0) { - b3CreateCollisionShapeAddSphere(commandHandle,radius); + shapeIndex = b3CreateCollisionShapeAddSphere(commandHandle,radius); } if (shapeType==GEOM_BOX && halfExtentsObj) { double halfExtents[3] = {1,1,1}; pybullet_internalSetVectord(halfExtentsObj,halfExtents); - b3CreateCollisionShapeAddBox(commandHandle,halfExtents); + shapeIndex = b3CreateCollisionShapeAddBox(commandHandle,halfExtents); + } + if (shapeType==GEOM_CAPSULE && radius>0 && height>=0) + { + shapeIndex = b3CreateCollisionShapeAddCapsule(commandHandle,radius,height); + } + if (shapeType==GEOM_CYLINDER && radius>0 && height>=0) + { + shapeIndex = b3CreateCollisionShapeAddCylinder(commandHandle,radius,height); + } + if (shapeType==GEOM_MESH && fileName) + { + pybullet_internalSetVectord(meshScaleObj,meshScale); + shapeIndex = b3CreateCollisionShapeAddMesh(commandHandle, fileName,meshScale); + } + if (shapeType==GEOM_PLANE) + { + double planeConstant=0; + pybullet_internalSetVectord(planeNormalObj,planeNormal); + shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant); + } + if (shapeIndex && flags) + { + b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -4993,14 +5084,28 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje int useMaximalCoordinates = 0; PyObject* basePosObj=0; PyObject* baseOrnObj=0; - + + PyObject* linkMassesObj=0; + PyObject* linkCollisionShapeIndicesObj=0; + PyObject* linkVisualShapeIndicesObj=0; + PyObject* linkPositionsObj=0; + PyObject* linkOrientationsObj=0; + PyObject* linkParentIndicesObj=0; + PyObject* linkJointTypesObj=0; + PyObject* linkJointAxisObj=0; + static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation", -// "linkParentIndices", "linkJointTypes","linkMasses","linkCollisionShapeIndices", -"useMaximalCoordinates","physicsClientId", + "linkMasses","linkCollisionShapeIndices", "linkVisualShapeIndices", + "linkPositions", "linkOrientations","linkParentIndices", "linkJointTypes","linkJointAxis", + "useMaximalCoordinates","physicsClientId", + NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, - &basePosObj, &baseOrnObj,&useMaximalCoordinates, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOOOOOOOOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, + &basePosObj, &baseOrnObj, + &linkMassesObj, &linkCollisionShapeIndicesObj, &linkVisualShapeIndicesObj, &linkPositionsObj, &linkOrientationsObj, + &linkParentIndicesObj, &linkJointTypesObj,&linkJointAxisObj, + &useMaximalCoordinates, &physicsClientId)) { return NULL; } @@ -5012,27 +5117,148 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje } { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm); - double basePosition[3]={0,0,0}; - double baseOrientation[4]={0,0,0,1}; - int baseIndex; - pybullet_internalSetVectord(basePosObj,basePosition); - pybullet_internalSetVector4d(baseOrnObj,baseOrientation); - baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); - if (useMaximalCoordinates>0) + + + int numLinkMasses = linkMassesObj?PySequence_Size(linkMassesObj):0; + int numLinkCollisionShapes = linkCollisionShapeIndicesObj?PySequence_Size(linkCollisionShapeIndicesObj):0; + int numLinkVisualShapes = linkVisualShapeIndicesObj?PySequence_Size(linkVisualShapeIndicesObj):0; + int numLinkPositions = linkPositionsObj? PySequence_Size(linkPositionsObj):0; + int numLinkOrientations = linkOrientationsObj? PySequence_Size(linkOrientationsObj):0; + int numLinkParentIndices = linkParentIndicesObj?PySequence_Size(linkParentIndicesObj):0; + int numLinkJointTypes = linkJointTypesObj?PySequence_Size(linkJointTypesObj):0; + int numLinkJoinAxis = linkJointAxisObj? PySequence_Size(linkJointAxisObj):0; + + + PyObject* seqLinkMasses = linkMassesObj?PySequence_Fast(linkMassesObj, "expected a sequence"):0; + PyObject* seqLinkCollisionShapes = linkCollisionShapeIndicesObj?PySequence_Fast(linkCollisionShapeIndicesObj, "expected a sequence"):0; + PyObject* seqLinkVisualShapes = linkVisualShapeIndicesObj?PySequence_Fast(linkVisualShapeIndicesObj, "expected a sequence"):0; + PyObject* seqLinkPositions = linkPositionsObj?PySequence_Fast(linkPositionsObj, "expected a sequence"):0; + PyObject* seqLinkOrientations = linkOrientationsObj?PySequence_Fast(linkOrientationsObj, "expected a sequence"):0; + PyObject* seqLinkParentIndices = linkParentIndicesObj?PySequence_Fast(linkParentIndicesObj, "expected a sequence"):0; + PyObject* seqLinkJointTypes = linkJointTypesObj?PySequence_Fast(linkJointTypesObj, "expected a sequence"):0; + PyObject* seqLinkJoinAxis = linkJointAxisObj?PySequence_Fast(linkJointAxisObj, "expected a sequence"):0; + + if ((numLinkMasses==numLinkCollisionShapes) && + (numLinkMasses==numLinkVisualShapes) && + (numLinkMasses==numLinkPositions) && + (numLinkMasses==numLinkOrientations) && + (numLinkMasses==numLinkParentIndices) && + (numLinkMasses==numLinkJointTypes) && + (numLinkMasses==numLinkJoinAxis)) { - b3CreateMultiBodyUseMaximalCoordinates(commandHandle); - } - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int i; + b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm); + double basePosition[3]={0,0,0}; + double baseOrientation[4]={0,0,0,1}; + int baseIndex; + pybullet_internalSetVectord(basePosObj,basePosition); + pybullet_internalSetVector4d(baseOrnObj,baseOrientation); + baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); + + for (i=0;i0) + { + b3CreateMultiBodyUseMaximalCoordinates(commandHandle); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) + { + int uid = b3GetStatusBodyIndex(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + + } else { - int uid = b3GetStatusBodyIndex(statusHandle); - PyObject* ob = PyLong_FromLong(uid); - return ob; + if (seqLinkMasses) + Py_DECREF(seqLinkMasses); + if (seqLinkCollisionShapes) + Py_DECREF(seqLinkCollisionShapes); + if (seqLinkVisualShapes) + Py_DECREF(seqLinkVisualShapes); + if (seqLinkPositions) + Py_DECREF(seqLinkPositions); + if (seqLinkOrientations) + Py_DECREF(seqLinkOrientations); + if (seqLinkParentIndices) + Py_DECREF(seqLinkParentIndices); + if (seqLinkJointTypes) + Py_DECREF(seqLinkJointTypes); + if (seqLinkJoinAxis) + Py_DECREF(seqLinkJoinAxis); + + PyErr_SetString(SpamError, "All link arrays need to be same size."); + return NULL; } + + + #if 0 + PyObject* seq; + seq = PySequence_Fast(objMat, "expected a sequence"); + if (seq) + { + len = PySequence_Size(objMat); + if (len == 16) + { + for (i = 0; i < len; i++) + { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + #endif + + } PyErr_SetString(SpamError, "createMultiBody failed."); return NULL;