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

PyBullet createMultiBody(Batch), return all body unique ids

Browse Source 
PyBullet: fix crash: always check for existance m_multibodyWorld in ::render method
This commit is contained in:
Erwin Coumans
2019-09-09 16:41:25 -07:00
parent 5a3c60c709
commit 64ea8e9f27
2 changed files with 664 additions and 719 deletions
Download Patch File Download Diff File Expand all files Collapse all files

148
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -58,7 +58,6 @@
#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
#endif
#ifdef ENABLE_STATIC_GRPC_PLUGIN
#include "plugins/grpcPlugin/grpcPlugin.h"
#endif //ENABLE_STATIC_GRPC_PLUGIN
@@ -67,8 +66,6 @@
#include "plugins/pdControlPlugin/pdControlPlugin.h"
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifdef STATIC_LINK_SPD_PLUGIN
#include "plugins/stablePDPlugin/BulletConversion.h"
#include "plugins/stablePDPlugin/RBDModel.h"
@@ -87,7 +84,6 @@
#include "plugins/tinyRendererPlugin/tinyRendererPlugin.h"
#endif
#ifdef B3_ENABLE_FILEIO_PLUGIN
#include "plugins/fileIOPlugin/fileIOPlugin.h"
#endif //B3_DISABLE_FILEIO_PLUGIN
@@ -2228,7 +2224,6 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const
{
if (m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL)
{
const UrdfMaterialColor* matColPtr = m_linkColors[linkIndex];
@@ -2242,7 +2237,8 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
return true;
}
} else
}
else
{
if (m_createBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] >= 0)
{
@@ -2903,8 +2899,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
|| mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
return canHaveMotor;
}
@@ -2953,8 +2948,6 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
SaveWorldObjectData sd;
sd.m_fileName = fileName;
for (int m = 0; m < u2b.getNumModels(); m++)
{
u2b.activateModel(m);
@@ -3127,7 +3120,6 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
fclose(f);
}
#endif
}
else
{
@@ -3181,8 +3173,6 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
}
{
if (m_data->m_pluginManager.getRenderInterface())
{
int currentOpenGLTextureIndex = 0;
@@ -3432,7 +3422,8 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE, mb);
streamSizeInBytes = ser.getCurrentBufferSize();
}
else if(bodyHandle->m_rigidBody){
else if (bodyHandle->m_rigidBody)
{
btRigidBody* rb = bodyHandle->m_rigidBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
@@ -3456,7 +3447,8 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
streamSizeInBytes = ser.getCurrentBufferSize();
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if(bodyHandle->m_softBody){
else if (bodyHandle->m_softBody)
{
//minimum serialization, registerNameForPointer
btSoftBody* sb = bodyHandle->m_softBody;
btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);
@@ -4255,17 +4247,12 @@ bool PhysicsServerCommandProcessor::processSaveWorldCommand(const struct SharedM
return hasStatus;
}
#define MYLINELENGTH 16 * 32768
static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY_ScalarType type, const char* fileName, int& width, int& height,
btScalar& minHeight,
btScalar& maxHeight)
{
std::string ext;
std::string fn(fileName);
std::string ext_ = fn.substr(fn.size() - 4);
@@ -4274,10 +4261,8 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
ext += char(tolower(*i));
}
if (ext != ".txt")
{
char relativeFileName[1024];
int found = fileIO.findResourcePath(fileName, relativeFileName, 1024);
@@ -4320,10 +4305,8 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
for (int j = 0; j < height; ++j)
{
for (int i = 0; i < width; ++i)
{
float z = double(image[(width - 1 - i) * 3 + width * j * 3]) * (1. / 255.);
btScalar* pf = (btScalar*)p;
*pf = z;
@@ -4397,7 +4380,6 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
pos = nextPos;
}
cols++;
}
width = rows;
height = cols;
@@ -4418,10 +4400,8 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
unsigned char* p = raw;
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
double z = allValues[i + width * j];
//convertFromFloat(p, z, type);
btScalar* pf = (btScalar*)p;
@@ -4444,7 +4424,6 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
maxHeight = z;
}
}
}
}
return raw;
@@ -4452,7 +4431,6 @@ static unsigned char* MyGetRawHeightfieldData(CommonFileIOInterface& fileIO, PHY
}
}
return 0;
}
@@ -4608,7 +4586,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns > 0 &&
clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows > 0)
{
width = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldRows;
height = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numHeightfieldColumns;
float* heightfieldDataSrc = (float*)bufferServerToClient;
@@ -4629,7 +4606,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
}
if (heightfieldData)
{
//replace heightfield data or create new heightfield
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_replaceHeightfieldIndex >= 0)
{
@@ -4654,7 +4630,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
btAlignedObjectArray<int> indices;
int strideInBytes = 9 * sizeof(float);
MyTriangleCollector4 col;
col.m_pVerticesOut = &gfxVertices;
col.m_pIndicesOut = &indices;
@@ -4673,7 +4648,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
terrainShape->clearAccelerator();
terrainShape->buildAccelerator();
btTriangleInfoMap* oldTriangleInfoMap = terrainShape->getTriangleInfoMap();
delete (oldTriangleInfoMap);
terrainShape->setTriangleInfoMap(0);
@@ -4693,7 +4667,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
}
else
{
btScalar gridSpacing = 0.5;
btScalar gridHeightScale = 1. / 256.;
@@ -4782,7 +4755,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
if (clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices)
{
int numVertices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices;
int numIndices = clientCmd.m_createUserShapeArgs.m_shapes[i].m_numIndices;
@@ -4792,7 +4764,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
double* vertexUpload = (double*)data;
int* indexUpload = (int*)(data + numVertices * sizeof(double) * 3);
if (compound == 0)
{
compound = worldImporter->createCompoundShape();
@@ -4851,7 +4822,6 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str
for (int v = 0; v < clientCmd.m_createUserShapeArgs.m_shapes[i].m_numVertices; v++)
{
btVector3 pt(vertexUpload[v * 3 + 0],
vertexUpload[v * 3 + 1],
vertexUpload[v * 3 + 2]);
@@ -5102,7 +5072,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied;
}
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
}
serverStatusOut.m_numDataStreamBytes = 0;
@@ -5197,7 +5166,6 @@ bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct
}
else
{
}
}
else
@@ -5910,7 +5878,6 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
}
}
BatchRayCaster batchRayCaster(m_data->m_threadPool, m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo*)bufferServerToClient, totalRays);
batchRayCaster.castRays(numThreads);
@@ -5996,8 +5963,6 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
b3AlignedObjectArray<int> usedHandles;
m_data->m_bodyHandles.getUsedHandles(usedHandles);
int actualNumBodies = 0;
int* bodyIds = (int*) bufferServerToClient;
for (int i = 0; i < usedHandles.size(); i++)
{
int usedHandle = usedHandles[i];
@@ -6008,19 +5973,18 @@ bool PhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct Shar
if (body && (body->m_multiBody || body->m_rigidBody))
#endif
{
bodyIds[actualNumBodies++] = usedHandle;
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle;
}
}
serverStatusOut.m_sdfLoadedArgs.m_numBodies = actualNumBodies;
int usz = m_data->m_userConstraints.size();
serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = usz;
int* constraintIds = bodyIds + actualNumBodies;
for (int i = 0; i < usz; i++)
{
int key = m_data->m_userConstraints.getKeyAtIndex(i).getUid1();
// int uid = m_data->m_userConstraints.getAtIndex(i)->m_userConstraintData.m_userConstraintUniqueId;
constraintIds[i] = key;
serverStatusOut.m_sdfLoadedArgs.m_userConstraintUniqueIds[i] = key;
}
serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
@@ -6679,7 +6643,6 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
}
cRBDModel* rbdModel = 0;
{
@@ -6688,7 +6651,6 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
if (rbdModel)
{
int num_dof = jointPositionsQ.size();
const Eigen::VectorXd& pose = rbdModel->GetPose();
const Eigen::VectorXd& vel = rbdModel->GetVel();
@@ -6780,8 +6742,6 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
torqueIndex++;
}
}
}
break;
@@ -7758,11 +7718,8 @@ bool PhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct S
const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
//stream info into memory
{
BT_PROFILE("createBodyInfoStream");
int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
}
serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0;
@@ -7773,7 +7730,7 @@ bool PhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct S
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, bodyHandle->m_bodyName.c_str());
}
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
return hasStatus;
}
@@ -7873,9 +7830,6 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
useMultiBody = false;
}
bool ok = 0;
{
BT_PROFILE("processImportedObjects");
@@ -7894,7 +7848,6 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommandSingle(const st
m_data->m_sdfRecentLoadedBodies.clear();
if (bodyUniqueId >= 0)
{
serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
if (bufferSizeInBytes > 0 && serverStatusOut.m_numDataStreamBytes == 0)
{
@@ -8004,7 +7957,6 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0);
btAssert(loadSoftBodyArgs.m_fileName);
btVector3 initialPos(0, 0, 0);
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
{
@@ -8307,21 +8259,17 @@ bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct Sha
{
bool hasStatus = true;
{
if (clientCmd.m_profile.m_type == 0)
{
char** eventNamePtr = m_data->m_profileEvents[clientCmd.m_profile.m_name];
char* eventName = 0;
if (eventNamePtr)
{
eventName = *eventNamePtr;
}
else
{
int len = strlen(clientCmd.m_profile.m_name);
eventName = new char[len + 1];
strcpy(eventName, clientCmd.m_profile.m_name);
@@ -8335,7 +8283,6 @@ bool PhysicsServerCommandProcessor::processProfileTimingCommand(const struct Sha
{
b3LeaveProfileZone();
}
}
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
@@ -8434,7 +8381,8 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
}
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){
else if (body && body->m_softBody)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
@@ -8597,7 +8545,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setCanWakeup(false);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
@@ -8671,7 +8618,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT);
mb->setFixedBase(false);
m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
}
}
else
@@ -8688,7 +8634,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
}
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
{
@@ -8703,7 +8648,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity);
}
}
else
{
@@ -8775,7 +8719,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
}
else
{
btRigidBody* rb = 0;
if (body && body->m_rigidBody)
{
@@ -8791,7 +8734,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB();
rb = childRb;
}
}
}
@@ -8888,8 +8830,6 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
{
rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
@@ -9060,7 +9000,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
serverCmd.m_dynamicsInfo.m_mass = rb->getMass();
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
else if (body && body->m_softBody){
else if (body && body->m_softBody)
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY;
@@ -9125,7 +9066,6 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION)
{
if (clientCmd.m_physSimParamArgs.m_enableConeFriction)
@@ -9855,11 +9795,9 @@ bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(cons
m_data->m_remoteSyncTransformInterval = clientCmd.m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval;
}
return hasStatus;
}
bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -9893,11 +9831,11 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
}
#endif
} else
}
else
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
@@ -9908,7 +9846,6 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ + num_dofs) &&
clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot + num_dofs))
{
btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
@@ -10135,7 +10072,6 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru
}
else
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
@@ -10940,7 +10876,6 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
return hasStatus;
}
bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
bool hasStatus = true;
@@ -11268,11 +11203,9 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
ikHelperPtr = tmpHelper;
}
btAlignedObjectArray<double> startingPositions;
startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
{
int DofIndex = 0;
for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
@@ -11331,7 +11264,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
validEndEffectorLinkIndices = validEndEffectorLinkIndices && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks());
@@ -11425,7 +11357,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++)
{
int endEffectorLinkIndex2 = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne];
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
@@ -11455,7 +11386,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
endEffectorTargetWorldPositions[ne * 3 + 2]));
//diff
currentDiff = btMax(currentDiff, (world_origin - targetPos).length());
}
}
}
@@ -11544,9 +11474,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
@@ -11569,7 +11496,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices == 1)
{
BT_PROFILE("computeIK");
ikHelperPtr->computeIK(&endEffectorTargetWorldPositions[0],
&endEffectorTargetWorldOrientations[0],
@@ -11581,7 +11507,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand2(co
}
else
{
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices > 1)
{
ikHelperPtr->computeIK2(&endEffectorTargetWorldPositions[0],
@@ -11818,7 +11743,8 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
//int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1;
//set serverCmd.m_sendVisualShapeArgs when totalNumVisualShapes is zero
if (totalNumVisualShapes==0) {
if (totalNumVisualShapes == 0)
{
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = 0;
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 0;
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
@@ -11827,7 +11753,8 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
else{
else
{
b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient;
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
@@ -11863,7 +11790,8 @@ bool PhysicsServerCommandProcessor::processRequestVisualShapeInfoCommand(const s
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData) * serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
}
else{
else
{
b3Warning("failed to get shape info");
}
}
@@ -11898,7 +11826,8 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex,
texHandle->m_tinyRendererTextureId);
}
} else
}
else
{
m_data->m_pluginManager.getRenderInterface()->changeShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId,
clientCmd.m_updateVisualShapeDataArguments.m_jointIndex,
@@ -11928,7 +11857,8 @@ bool PhysicsServerCommandProcessor::processUpdateVisualShapeCommand(const struct
if (texHandle)
{
m_data->m_guiHelper->replaceTexture(shapeIndex, texHandle->m_openglTextureId);
} else
}
else
{
m_data->m_guiHelper->replaceTexture(shapeIndex, -1);
}
@@ -12109,7 +12039,8 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
{
imageData = stbi_load_from_memory((const unsigned char*)&buffer[0], buffer.size(), &width, &height, &n, 3);
}
} else
}
else
{
imageData = stbi_load(relativeFileName, &width, &height, &n, 3);
}
@@ -12178,7 +12109,6 @@ bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedM
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_REMOVE_STATE");
@@ -12201,7 +12131,6 @@ bool PhysicsServerCommandProcessor::processRemoveStateCommand(const struct Share
return hasStatus;
}
bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
BT_PROFILE("CMD_RESTORE_STATE");
@@ -12236,7 +12165,6 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
buffer.reserve(1024);
if (fileIO)
{
int fileId = -1;
found = fileIO->findResourcePath(clientCmd.m_fileArguments.m_fileName, fileName, 1024);
if (found)
@@ -12254,7 +12182,8 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
} else
}
else
{
found = true;
}
@@ -12266,7 +12195,8 @@ bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct Shar
if (found && buffer.size())
{
ok = importer->loadFileFromMemory(&buffer[0], buffer.size());
} else
}
else
{
b3Error("Error in restoreState: cannot load file %s\n", clientCmd.m_fileArguments.m_fileName);
}
@@ -12316,7 +12246,8 @@ bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct Shared
{
b3Warning("image filesize mismatch!\n");
buffer.resize(0);
} else
}
else
{
found = true;
}
@@ -12701,7 +12632,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices == 1)
{
hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
} else
}
else
{
hasStatus = processCalculateInverseKinematicsCommand2(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
}
@@ -12817,13 +12749,11 @@ void PhysicsServerCommandProcessor::syncPhysicsToGraphics()
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
}
void PhysicsServerCommandProcessor::syncPhysicsToGraphics2()
{
m_data->m_guiHelper->syncPhysicsToGraphics2(m_data->m_dynamicsWorld);
}
void PhysicsServerCommandProcessor::renderScene(int renderFlags)
{
if (m_data->m_guiHelper)
@@ -12836,6 +12766,8 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
if (m_data->m_dynamicsWorld)
{
if (m_data->m_dynamicsWorld->getSoftBodyArray().size())
{
for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++)
@@ -12849,6 +12781,7 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
m_data->m_guiHelper->drawDebugDrawerLines();
m_data->m_guiHelper->clearLines();
}
}
#endif
#endif
}
@@ -13320,7 +13253,6 @@ void PhysicsServerCommandProcessor::resetSimulation()
m_data->m_pluginManager.addNotification(notification);
syncPhysicsToGraphics2();
}
void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/)

13
examples/pybullet/pybullet.c
View File

@@ -9024,10 +9024,23 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED)
{
int uid = b3GetStatusBodyIndex(statusHandle);
if (numBatchPositions > 0)
{
PyObject* pyResultList = PyTuple_New(numBatchPositions );
for (i = 0; i < numBatchPositions; i++)
{
PyTuple_SetItem(pyResultList, i, PyLong_FromLong(uid - numBatchPositions + i + 1));
}
return pyResultList;
}
else
{
PyObject* ob = PyLong_FromLong(uid);
return ob;
}
}
}
else
{
if (seqLinkMasses)