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

add capsule, cylinder, plane, mesh support for pybullet.createCollisionShape

Browse Source 
preparation to add links to pybullet.createMultiBody
This commit is contained in:
Erwin Coumans
2017-06-19 10:14:26 -07:00
parent 5a8f12284a
commit f3c11b6f31
6 changed files with 648 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

192
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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 <MAX_COMPOUND_COLLISION_SHAPES)
{
command->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 <MAX_COMPOUND_COLLISION_SHAPES)
{
command->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 <MAX_COMPOUND_COLLISION_SHAPES)
{
command->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 <MAX_COMPOUND_COLLISION_SHAPES && strlen(fileName)<VISUAL_SHAPE_MAX_PATH_LEN)
{
command->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 (shapeIndex<command->m_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 (numLinks<MAX_CREATE_MULTI_BODY_LINKS)
{
int linkIndex = numLinks;
command->m_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)
{

19
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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);

185
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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<int>& childLinkIndices) const
{
for (int i=0;i<m_createBodyArgs.m_numLinks;i++)
{
if (m_createBodyArgs.m_linkParents[i] == urdfLinkIndex)
{
childLinkIndices.push_back(i);
}
}
}
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
@@ -1637,15 +1646,17 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const
{
int baseLinkIndex = m_createBodyArgs.m_baseLinkIndex;
rootTransformInWorld.setOrigin(btVector3(
m_createBodyArgs.m_baseWorldPosition[0],
m_createBodyArgs.m_baseWorldPosition[1],
m_createBodyArgs.m_baseWorldPosition[2]));
m_createBodyArgs.m_linkPositions[baseLinkIndex*3+0],
m_createBodyArgs.m_linkPositions[baseLinkIndex*3+1],
m_createBodyArgs.m_linkPositions[baseLinkIndex*3+2]));
rootTransformInWorld.setRotation(btQuaternion(
m_createBodyArgs.m_baseWorldOrientation[0],
m_createBodyArgs.m_baseWorldOrientation[1],
m_createBodyArgs.m_baseWorldOrientation[2],
m_createBodyArgs.m_baseWorldOrientation[3]));
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+0],
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+1],
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+2],
m_createBodyArgs.m_linkOrientations[baseLinkIndex*4+3]));
return true;
}
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld)
@@ -1661,6 +1672,19 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
{
#if 0
if (m_data->m_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<tinyobj::shape_t> 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<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:
{
}
}
}
#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<btHashString, UrdfMaterial*> m_materials;
btHashMap<btHashString, UrdfLink*> m_links;
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> 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;

18
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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;

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -594,6 +594,12 @@ enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
GEOM_UNKNOWN,
};
enum eUrdfCollisionFlags
{
GEOM_FORCE_CONCAVE_TRIMESH=1,
};
#endif//SHARED_MEMORY_PUBLIC_H

284
examples/pybullet/pybullet.c
View File

@@ -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;i<numLinkMasses;i++)
{
double linkMass = pybullet_internalGetFloatFromSequence(seqLinkMasses,i);
int linkCollisionShapeIndex = pybullet_internalGetIntFromSequence(seqLinkCollisionShapes,i);
int linkVisualShapeIndex = pybullet_internalGetIntFromSequence(seqLinkVisualShapes,i);
double linkPosition[3];
double linkOrientation[4];
double linkJointAxis[3];
pybullet_internalGetVector3FromSequence(seqLinkPositions,i,linkPosition);
pybullet_internalGetVector4FromSequence(seqLinkOrientations,i,linkOrientation);
pybullet_internalGetVector3FromSequence(seqLinkJoinAxis,i,linkJointAxis);
int linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices,i);
int linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes,i);
b3CreateMultiBodyLink(commandHandle,
linkMass,
linkCollisionShapeIndex,
linkVisualShapeIndex,
linkPosition,
linkOrientation,
linkParentIndex,
linkJointType,
linkJointAxis
);
}
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);
if (useMaximalCoordinates>0)
{
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;