diff --git a/CMakeLists.txt b/CMakeLists.txt index b34c7f996..083835f6e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -191,14 +191,15 @@ IF (APPLE) ENDIF() OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON) + +FIND_PACKAGE(PythonLibs) OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF) + IF(BUILD_PYBULLET) IF(WIN32) - FIND_PACKAGE(PythonLibs 3.4 REQUIRED) SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE) ELSE(WIN32) - FIND_PACKAGE(PythonLibs 2.7 REQUIRED) SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE) ENDIF(WIN32) ENDIF(BUILD_PYBULLET) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c6468cb89..ae1a54782 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -519,6 +519,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) return CMD_INVALID_STATUS; } +int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity) +{ + int numBodies = 0; + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + + if (status) + { + switch (status->m_type) + { + case CMD_SDF_LOADING_COMPLETED: + { + int i,maxBodies; + numBodies = status->m_sdfLoadedArgs.m_numBodies; + maxBodies = btMin(bodyIndicesCapacity, numBodies); + for (i=0;im_sdfLoadedArgs.m_bodyUniqueIds[i]; + } + break; + } + } + } + + return numBodies; +} + int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; @@ -558,6 +585,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* jointReactionForces[]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SendActualStateArgs &args = status->m_sendActualStateArgs; + btAssert(status->m_type == CMD_URDF_LOADING_COMPLETED); + if (status->m_type != CMD_URDF_LOADING_COMPLETED) + return false; + if (bodyUniqueId) { *bodyUniqueId = args.m_bodyUniqueId; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b67f078ae..468347ef5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien /// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes. int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); +int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity); + int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index bcd84d321..acce1dfde 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -77,9 +77,9 @@ protected: virtual void resetCamera() { - float dist = 1.1; - float pitch = 50; - float yaw = 35; + float dist = 4; + float pitch = 193; + float yaw = 25; float targetPos[3]={0,0,0.5};//-3,2.8,-2.5}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); @@ -223,9 +223,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { case CMD_LOAD_URDF: { - b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); - //setting the initial position, orientation and other arguments are optional double startPosX = 0; static double startPosY = 0; @@ -234,7 +232,13 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) startPosY += 2.f; // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); - + break; + } + + case CMD_LOAD_SDF: + { + b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: @@ -452,6 +456,7 @@ void PhysicsClientExample::createButtons() m_guiHelper->getParameterInterface()->removeAllParameters(); createButton("Load URDF",CMD_LOAD_URDF, isTrigger); + createButton("Load SDF",CMD_LOAD_SDF, isTrigger); createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger); createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); @@ -649,11 +654,40 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3Warning("Camera image FAILED\n"); } - - if (statusType == CMD_URDF_LOADING_COMPLETED) + if (statusType == CMD_SDF_LOADING_COMPLETED) { - int bodyIndex = b3GetStatusBodyIndex(status); - if (bodyIndex>=0) + //int bodyIndex = b3GetStatusBodyIndex(status); + /*if (bodyIndex>=0) + { + int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); + + for (int i=0;igetParameterInterface()->registerComboBox(comboParams); + } + */ + } + + + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + int bodyIndex = b3GetStatusBodyIndex(status); + if (bodyIndex>=0) { int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); @@ -662,7 +696,6 @@ void PhysicsClientExample::stepSimulation(float deltaTime) b3JointInfo info; b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); - } ComboBoxParams comboParams; comboParams.m_comboboxId = bodyIndex; @@ -676,12 +709,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime) comboParams.m_items=blarray;//{&bla}; m_guiHelper->getParameterInterface()->registerComboBox(comboParams); - - } - } - } } if (b3CanSubmitCommand(m_physicsClientHandle)) @@ -734,13 +763,12 @@ void PhysicsClientExample::stepSimulation(float deltaTime) if (m_numMotors) { enqueueCommand(CMD_SEND_DESIRED_STATE); - enqueueCommand(CMD_STEP_FORWARD_SIMULATION); - if (m_options != eCLIENTEXAMPLE_SERVER) - { - enqueueCommand(CMD_REQUEST_DEBUG_LINES); - } - //enqueueCommand(CMD_REQUEST_ACTUAL_STATE); } + enqueueCommand(CMD_STEP_FORWARD_SIMULATION); + if (m_options != eCLIENTEXAMPLE_SERVER) + { + enqueueCommand(CMD_REQUEST_DEBUG_LINES); + } } } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 728358b93..a4c57574d 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData { SharedMemoryStatus m_lastServerStatus; int m_counter; - bool m_serverLoadUrdfOK; + bool m_isConnected; bool m_waitingForServer; bool m_hasLastServerStatus; @@ -54,7 +54,6 @@ struct PhysicsClientSharedMemoryInternalData { m_counter(0), m_cachedCameraPixelsWidth(0), m_cachedCameraPixelsHeight(0), - m_serverLoadUrdfOK(false), m_isConnected(false), m_waitingForServer(false), m_hasLastServerStatus(false), @@ -204,8 +203,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } break; } + case CMD_SDF_LOADING_COMPLETED: { + + if (m_data->m_verboseOutput) { + b3Printf("Server loading the SDF OK\n"); + } + break; + } case CMD_URDF_LOADING_COMPLETED: { - m_data->m_serverLoadUrdfOK = true; + if (m_data->m_verboseOutput) { b3Printf("Server loading the URDF OK\n"); } @@ -265,7 +271,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { if (m_data->m_verboseOutput) { b3Printf("Server failed loading the URDF...\n"); } - m_data->m_serverLoadUrdfOK = false; + + break; + } + + case CMD_SDF_LOADING_FAILED: { + if (m_data->m_verboseOutput) { + b3Printf("Server failed loading the SDF...\n"); + } + break; } @@ -490,6 +504,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { m_data->m_waitingForServer = true; } + /*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) + { + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + if (numBodies>0) + { + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + //now transfer the information of the individual objects etc. + command.m_type = CMD_REQUEST_SDF_INFO; + command.m_updateFlags = SDF_REQUEST_INFO_BODY; + command.m_sdfRequestInfoArgs.m_infoIndex = 0; + submitClientCommand(command); + return 0; + } + } + */ + if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 89490caa6..98994ea68 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -393,6 +393,7 @@ struct PhysicsServerCommandProcessorInternalData btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; + btAlignedObjectArray m_sdfRecentLoadedBodies; @@ -681,6 +682,128 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) } +bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes) +{ + btAssert(m_data->m_dynamicsWorld); + if (!m_data->m_dynamicsWorld) + { + b3Error("loadSdf: No valid m_dynamicsWorld"); + return false; + } + + m_data->m_sdfRecentLoadedBodies.clear(); + + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); + + bool useFixedBase = false; + bool loadOk = u2b.loadSDF(fileName, useFixedBase); + if (loadOk) + { + for (int i=0;im_collisionShapes.push_back(shape); + if (shape->isCompound()) + { + btCompoundShape* compound = (btCompoundShape*) shape; + for (int childIndex=0;childIndexgetNumChildShapes();childIndex++) + { + m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex)); + } + } + + } + + btTransform rootTrans; + rootTrans.setIdentity(); + if (m_data->m_verboseOutput) + { + b3Printf("loaded %s OK!", fileName); + } + + for (int m =0; mallocHandle(); + + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); + + { + btScalar mass = 0; + bodyHandle->m_rootLocalInertialFrame.setIdentity(); + btVector3 localInertiaDiagonal(0,0,0); + int urdfLinkIndex = u2b.getRootLinkIndex(); + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); + } + + + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user + int rootLinkIndex = u2b.getRootLinkIndex(); + b3Printf("urdf root link index = %d\n",rootLinkIndex); + MyMultiBodyCreator creation(m_data->m_guiHelper); + + u2b.getRootTransformInWorld(rootTrans); + bool useMultiBody = true; + ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); + + + + mb = creation.getBulletMultiBody(); + if (mb) + { + bodyHandle->m_multiBody = mb; + + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); + + createJointMotors(mb); + + //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire + + UrdfLinkNameMapUtil* util2 = new UrdfLinkNameMapUtil; + m_data->m_urdfLinkNameMapper.push_back(util2); + util2->m_mb = mb; + util2->m_memSerializer = 0; + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); + + bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); + for (int i=0;igetNumLinks();i++) + { + //disable serialization of the collision objects + + int urdfLinkIndex = creation.m_mb2urdfLink[i]; + btScalar mass; + btVector3 localInertiaDiagonal(0,0,0); + btTransform localInertialFrame; + u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame); + bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame); + + std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(linkName); + + mb->getLink(i).m_linkName = linkName->c_str(); + + std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); + m_data->m_strings.push_back(jointName); + + mb->getLink(i).m_jointName = jointName->c_str(); + } + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); + m_data->m_strings.push_back(baseName); + mb->setBaseName(baseName->c_str()); + } else + { + b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); + } + + } + } + return loadOk; +} bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes) @@ -731,8 +854,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); - - ///todo(erwincoumans) refactor this memory allocation issue for (int i=0;im_multiBody = mb; + createJointMotors(mb); @@ -805,12 +928,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); - return true; + return true; } else { b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); return false; } + } else { btAssert(0); @@ -1047,6 +1171,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_LOAD_SDF: + { + const SdfArgs& sdfArgs = clientCmd.m_sdfArguments; + if (m_data->m_verboseOutput) + { + b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); + } + + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); + + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); + int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); + for (int i=0;im_sdfRecentLoadedBodies[i]; + } + + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; + hasStatus = true; + break; + } case CMD_LOAD_URDF: { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index dca5f8930..242a41176 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -19,6 +19,10 @@ class PhysicsServerCommandProcessor protected: + + + bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes); + bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index fa67d7673..a4c93180e 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -33,6 +33,7 @@ #define MAX_SDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH #define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM +#define MAX_SDF_BODIES 1024 struct TmpFloat3 { @@ -298,6 +299,28 @@ struct CreateBoxShapeArgs double m_colorRGBA[4]; }; +struct SdfLoadedArgs +{ + int m_numBodies; + int m_bodyUniqueIds[MAX_SDF_BODIES]; + + ///@todo(erwincoumans) load cameras, lights etc + //int m_numCameras; + //int m_numLights; +}; + + +struct SdfRequestInfoArgs +{ + int m_infoIndex; +}; + +enum EnumSdfRequestInfoFlags +{ + SDF_REQUEST_INFO_BODY=1, + //SDF_REQUEST_INFO_CAMERA=2, +}; + struct SharedMemoryCommand { int m_type; @@ -312,6 +335,7 @@ struct SharedMemoryCommand { struct UrdfArgs m_urdfArguments; struct SdfArgs m_sdfArguments; + struct SdfRequestInfoArgs m_sdfRequestInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; @@ -340,6 +364,7 @@ struct SharedMemoryStatus union { struct BulletDataStreamArgs m_dataStreamArguments; + struct SdfLoadedArgs m_sdfLoadedArgs; struct SendActualStateArgs m_sendActualStateArgs; struct SendDebugLinesArgs m_sendDebugLinesArgs; struct SendPixelDataArgs m_sendPixelDataArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2f676e057..a038ecf51 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -19,6 +19,7 @@ enum EnumSharedMemoryClientCommand CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_DEBUG_LINES, + CMD_REQUEST_SDF_INFO, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, CMD_PICK_BODY, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f056b5b57..b735caeb1 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -182,6 +182,61 @@ pybullet_loadURDF(PyObject* self, PyObject* args) return PyLong_FromLong(bodyIndex); } +#define MAX_SDF_BODIES 512 + +static PyObject* +pybullet_loadSDF(PyObject* self, PyObject* args) +{ + const char* sdfFileName=""; + int size= PySequence_Size(args); + int numBodies = 0; + int i; + int bodyIndicesOut[MAX_SDF_BODIES]; + PyObject* pylist=0; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command; + + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (size==1) + { + if (!PyArg_ParseTuple(args, "s", &sdfFileName)) + return NULL; + } + + command = b3LoadSdfCommandInit(sm, sdfFileName); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType!=CMD_SDF_LOADING_COMPLETED) + { + PyErr_SetString(SpamError, "Cannot load SDF file."); + return NULL; + } + + numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES); + if (numBodies > MAX_SDF_BODIES) + { + PyErr_SetString(SpamError, "SDF exceeds body capacity"); + return NULL; + } + + pylist = PyTuple_New(numBodies); + + if (numBodies >0 && numBodies <= MAX_SDF_BODIES) + { + for (i=0;i