diff --git a/build_visual_studio_vr_pybullet_double.bat b/build_visual_studio_vr_pybullet_double.bat index 71d676172..6ca0652b9 100644 --- a/build_visual_studio_vr_pybullet_double.bat +++ b/build_visual_studio_vr_pybullet_double.bat @@ -1,8 +1,21 @@ - IF NOT EXIST bin mkdir bin IF NOT EXIST bin\openvr_api.dll copy examples\ThirdPartyLibs\openvr\bin\win32\openvr_api.dll bin +#optionally, hardcode the python path or +#SET myvar=c:\python-3.5.3 + +#find a python version (hopefully just 1) and use this +dir c:\python* /b /ad > tmp1234.txt + +set /p myvar1= < tmp1234.txt +set myvar=c:/%myvar1% +del tmp1234.txt + +#you can also override and hardcode the Python path like this (just remove the # hashmark in next line) +#SET myvar=c:\python-3.5.2 + cd build3 -premake4 --double --enable_openvr --enable_pybullet --python_include_dir="C:/Python-3.5.2/include" --python_lib_dir="C:/Python-3.5.2/libs" --targetdir="../bin" vs2010 +premake4 --double --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010 start vs2010 + diff --git a/data/multibody.bullet b/data/multibody.bullet index 70f15a27e..084ee5a8a 100644 Binary files a/data/multibody.bullet and b/data/multibody.bullet differ diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index e9f09b692..70e717159 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1211,11 +1211,7 @@ void OpenGLExampleBrowser::update(float deltaTime) { m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight()); } - if (gui2) - { - gui2->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight()); - } - + if (sUseOpenGL2) { restoreOpenGLState(); diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 6e8437950..77be9f7da 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -228,7 +228,7 @@ void ImportSDFSetup::initPhysics() //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); + //b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); u2b.getRootTransformInWorld(rootTrans); diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 1f16758a4..dd9159335 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1144,13 +1144,13 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& return false; } -void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int objectIndex) const +void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const { if (m_data->m_customVisualShapesConverter) { const UrdfModel& model = m_data->m_urdfParser.getModel(); - m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, objectIndex); + m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId); } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 127ed25b7..26b5a66e2 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -51,7 +51,7 @@ public: virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; - virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const; + virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const; ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index d2fb76716..3dfe17dd0 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -238,7 +238,7 @@ void ImportUrdfSetup::initPhysics() //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); + //b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix()); diff --git a/examples/MultiBody/MultiDofDemo.cpp b/examples/MultiBody/MultiDofDemo.cpp index 33719a2a8..84403024c 100644 --- a/examples/MultiBody/MultiDofDemo.cpp +++ b/examples/MultiBody/MultiDofDemo.cpp @@ -136,8 +136,8 @@ void MultiDofDemo::initPhysics() bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool multibodyOnly = false; bool canSleep = true; - bool selfCollide = false; - bool multibodyConstraint = true; + bool selfCollide = true; + bool multibodyConstraint = false; btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.05, 0.37, 0.1); @@ -159,6 +159,7 @@ void MultiDofDemo::initPhysics() } // m_dynamicsWorld->setGravity(btVector3(0, -9.81 ,0)); + //m_dynamicsWorld->getSolverInfo().m_numIterations = 100; ////////////////////////////////////////////// if(numLinks > 0) { @@ -247,8 +248,8 @@ void MultiDofDemo::initPhysics() frameInA.setIdentity(); frameInB.setIdentity(); btVector3 jointAxis(1.0,0.0,0.0); - btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis); - //btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB); + //btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis); + btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB); p2p->setMaxAppliedImpulse(2.0); m_dynamicsWorld->addMultiBodyConstraint(p2p); } @@ -306,10 +307,10 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD for(int i = 0; i < numLinks; ++i) { if(!spherical) - pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); else //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); - pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); } pMultiBody->finalizeMultiDof(); diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 3fc0fa16a..f028f537f 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -48,6 +48,8 @@ public: virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0; + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0; + }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 9ff8225e6..813457225 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1227,6 +1227,33 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con } + +//request visual shape information +b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command->m_requestVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueIdA; + command->m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = 0; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} + +void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + cl->getCachedVisualShapeInformation(visualShapeInfo); + } +} + + + b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e1084cd30..2d2a4460c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -93,7 +93,12 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); -void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData); +void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); + +//request visual shape information +b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA); +void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); + b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 95faa3fc7..afb0e1fb8 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -25,6 +25,11 @@ struct MyMotorInfo2 int camVisualizerWidth = 320;//1024/3; int camVisualizerHeight = 240;//768/3; +enum CustomCommands +{ + CMD_CUSTOM_SET_REALTIME_SIMULATION = CMD_MAX_CLIENT_COMMANDS+1, + CMD_CUSTOM_SET_GRAVITY +}; #define MAX_NUM_MOTORS 128 @@ -306,7 +311,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) for (int i = 0; i < numJoints; ++i) { struct b3JointSensorState sensorState; b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState); - b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque); + //b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque); } } break; @@ -410,12 +415,20 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) break; } - case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { + case CMD_CUSTOM_SET_GRAVITY: { b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + + case CMD_CUSTOM_SET_REALTIME_SIMULATION: + { + b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); + b3PhysicsParamSetRealTimeSimulation(commandHandle,1); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + break; + } case CMD_CALCULATE_INVERSE_DYNAMICS: { @@ -458,6 +471,16 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } + case CMD_REQUEST_VISUAL_SHAPE_INFO: + { + if (m_selectedBody >= 0) + { + //request visual shape information + b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + } + break; + } default: { b3Error("Unknown buttonId"); @@ -535,6 +558,8 @@ void PhysicsClientExample::createButtons() createButton("Save World",CMD_SAVE_WORLD, isTrigger); createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger); createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); + createButton("Realtime Sim",CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger); + createButton("Get Visual Shape Info",CMD_REQUEST_VISUAL_SHAPE_INFO, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); if (m_options!=eCLIENTEXAMPLE_SERVER) { @@ -545,7 +570,7 @@ void PhysicsClientExample::createButtons() createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger); createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); - createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); + createButton("Set gravity", CMD_CUSTOM_SET_GRAVITY, isTrigger); createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger); createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger); @@ -584,7 +609,7 @@ void PhysicsClientExample::createButtons() { b3JointInfo info; b3GetJointInfo(m_physicsClientHandle,m_selectedBody,i,&info); - b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); + //b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { @@ -959,7 +984,7 @@ 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); + //b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); } } @@ -968,6 +993,18 @@ void PhysicsClientExample::stepSimulation(float deltaTime) { b3Warning("Cannot get contact information"); } + if (statusType == CMD_VISUAL_SHAPE_INFO_FAILED) + { + b3Warning("Cannot get visual shape information"); + } + + if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + b3VisualShapeInformation shapeInfo; + b3GetVisualShapeInformation(m_physicsClientHandle, &shapeInfo); + b3Printf("Num visual shapes: %d", shapeInfo.m_numVisualShapes); + } + if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) { b3ContactInformation contactPointData; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c9fad2ab1..9dda6aca1 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -40,6 +40,8 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedSegmentationMaskBuffer; btAlignedObjectArray m_cachedContactPoints; + + btAlignedObjectArray m_cachedVisualShapes; btAlignedObjectArray m_bodyIdsRequestInfo; SharedMemoryStatus m_tempBackupServerStatus; @@ -633,7 +635,28 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Calculate Inverse Kinematics Request failed"); break; } + case CMD_VISUAL_SHAPE_INFO_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Visual Shape Information Request OK\n"); + } + int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; + int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); + b3VisualShapeData* shapeData = (b3VisualShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + for (int i = 0; i < numVisualShapesCopied; i++) + { + m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; + } + break; + } + case CMD_VISUAL_SHAPE_INFO_FAILED: + { + b3Warning("Visual Shape Info Request failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); @@ -710,6 +733,21 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } } + if (serverCmd.m_type == CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) + { + command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; + submitClientCommand(command); + return 0; + } + } + + + if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED) { SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; @@ -813,6 +851,13 @@ void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3Contac } +void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); + visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; +} + + const float* PhysicsClientSharedMemory::getDebugLinesFrom() const { if (m_data->m_debugLinesFrom.size()) { return &m_data->m_debugLinesFrom[0].m_x; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 1d0047bff..2dc659646 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -56,6 +56,8 @@ public: virtual void getCachedCameraImage(struct b3CameraImageData* cameraData); virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); }; #endif // BT_PHYSICS_CLIENT_API_H diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 3367b8323..65c1beb13 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -41,9 +41,9 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedSegmentationMask; btAlignedObjectArray m_cachedContactPoints; + + btAlignedObjectArray m_cachedVisualShapes; - - PhysicsServerCommandProcessor* m_commandProcessor; PhysicsDirectInternalData() @@ -196,6 +196,44 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma return m_data->m_hasStatus; } +bool PhysicsDirect::processVisualShapeData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + + if (m_data->m_verboseOutput) + { + b3Printf("Visual Shape Information Request OK\n"); + } + int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; + int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); + b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numVisualShapesCopied; i++) + { + m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; + } + + if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) + { + command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; + } + } + } while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied); + + return m_data->m_hasStatus; +} + + bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand) { SharedMemoryCommand command = orgCommand; @@ -380,6 +418,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman return processContactPointData(command); } + if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) + { + return processVisualShapeData(command); + } + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); m_data->m_hasStatus = hasStatus; if (hasStatus) @@ -576,3 +619,8 @@ void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation } +void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); + visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; +} diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index e312823e2..2252da3c3 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -23,6 +23,8 @@ protected: bool processContactPointData(const struct SharedMemoryCommand& orgCommand); + bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); public: @@ -74,6 +76,8 @@ public: virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + //those 2 APIs are for internal use for visualization virtual bool connect(struct GUIHelperInterface* guiHelper); virtual void renderScene(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index eeb641023..f669dde30 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -140,3 +140,8 @@ void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformati { return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData); } + +void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo); +} \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index baa7939fc..42acca718 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -62,6 +62,7 @@ public: virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); }; #endif //PHYSICS_LOOP_BACK_H diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4c86279ac..613bc0234 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2971,7 +2971,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } - + case CMD_REQUEST_VISUAL_SHAPE_INFO: + { + + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_FAILED; + //retrieve the visual shape information for a specific body + + int totalNumVisualShapes = m_data->m_visualConverter.getNumVisualShapes(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId); + int totalBytesPerVisualShape = sizeof (b3VisualShapeData); + int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; + b3VisualShapeData* visualShapeStoragePtr = (b3VisualShapeData*)bufferServerToClient; + + int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + + m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId, + shapeIndex, + visualShapeStoragePtr); + + + //m_visualConverter + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; + serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; + serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId; + + serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED; + hasStatus = true; + break; + } default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index adfd533f1..5a1141821 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -151,6 +151,21 @@ struct RequestContactDataArgs int m_objectBIndexFilter; }; +struct RequestVisualShapeDataArgs +{ + int m_bodyUniqueId; + int m_startingVisualShapeIndex; +}; + +struct SendVisualShapeDataArgs +{ + int m_bodyUniqueId; + int m_startingVisualShapeIndex; + int m_numVisualShapesCopied; + int m_numRemainingVisualShapes; +}; + + struct SendDebugLinesArgs { @@ -461,6 +476,7 @@ struct SharedMemoryCommand struct CalculateJacobianArgs m_calculateJacobianArguments; struct CreateJointArgs m_createJointArguments; struct RequestContactDataArgs m_requestContactPointArguments; + struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments; struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; }; }; @@ -496,6 +512,7 @@ struct SharedMemoryStatus struct CalculateJacobianResultArgs m_jacobianResultArgs; struct SendContactDataArgs m_sendContactPointArgs; struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs; + struct SendVisualShapeDataArgs m_sendVisualShapeArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7d9d6967b..c668e5fe8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -34,6 +34,7 @@ enum EnumSharedMemoryClientCommand CMD_CREATE_JOINT, CMD_REQUEST_CONTACT_POINT_INFORMATION, CMD_SAVE_WORLD, + CMD_REQUEST_VISUAL_SHAPE_INFO, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -78,8 +79,9 @@ enum EnumSharedMemoryServerStatus CMD_CALCULATE_INVERSE_KINEMATICS_FAILED, CMD_SAVE_WORLD_COMPLETED, CMD_SAVE_WORLD_FAILED, - - //don't go beyond 'CMD_MAX_SERVER_COMMANDS! + CMD_VISUAL_SHAPE_INFO_COMPLETED, + CMD_VISUAL_SHAPE_INFO_FAILED, + //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -188,6 +190,25 @@ struct b3ContactInformation struct b3ContactPointData* m_contactPointData; }; +#define VISUAL_SHAPE_MAX_PATH_LEN 128 + +struct b3VisualShapeData +{ + int m_objectUniqueId; + int m_linkIndex; + int m_visualGeometryType;//box primitive, sphere primitive, triangle mesh + double m_dimensions[3];//meaning depends on m_visualGeometryType + char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN]; + double m_localInertiaFrame[7];//pos[3], orn[4] + //todo: add more data if necessary (material color etc, although material can be in asset file .obj file) +}; + +struct b3VisualShapeInformation +{ + int m_numVisualShapes; + struct b3VisualShapeData* m_visualShapeData; +}; + ///b3LinkState provides extra information such as the Cartesian world coordinates ///center of mass (COM) of the link, relative to the world reference frame. ///Orientation is a quaternion x,y,z,w diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 736ce57fa..4ef4b0efc 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -32,6 +32,7 @@ subject to the following restrictions: #include #include #include "../Importers/ImportURDFDemo/UrdfParser.h" +#include "../SharedMemory/SharedMemoryPublic.h"//for b3VisualShapeData enum MyFileType @@ -60,6 +61,8 @@ struct TinyRendererVisualShapeConverterInternalData { btHashMap m_swRenderInstances; + + btAlignedObjectArray m_visualShapes; int m_upAxis; int m_swWidth; @@ -105,9 +108,14 @@ TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter() -void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) +void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) { + visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type; + visualShapeOut.m_dimensions[0] = 0; + visualShapeOut.m_dimensions[1] = 0; + visualShapeOut.m_dimensions[2] = 0; + visualShapeOut.m_meshAssetFileName[0] = 0; GLInstanceGraphicsShape* glmesh = 0; @@ -119,6 +127,9 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref { btAlignedObjectArray vertices; + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius; + //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); int numSteps = 32; for (int i = 0; im_geometry.m_boxSize[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_boxSize[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_boxSize[2]; + btVector3 extents = visual->m_geometry.m_boxSize; btBoxShape* boxShape = new btBoxShape(extents*0.5f); @@ -151,6 +165,8 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref } case URDF_GEOM_SPHERE: { + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius; + btScalar radius = visual->m_geometry.m_sphereRadius; btSphereShape* sphereShape = new btSphereShape(radius); convexColShape = sphereShape; @@ -200,6 +216,18 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref sprintf(fullPath, "%s%s", urdfPathPrefix, filename); + + visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0]; + visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1]; + visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2]; + + int sl = strlen(fullPath); + if (sl < (VISUAL_SHAPE_MAX_PATH_LEN-1)) + { + memcpy(visualShapeOut.m_meshAssetFileName, fullPath, sl); + visualShapeOut.m_meshAssetFileName[sl] = 0; + } + FILE* f = fopen(fullPath, "rb"); if (f) { @@ -443,7 +471,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref -void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex) +void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId) { @@ -486,11 +514,23 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const btAssert(visualsPtr); TinyRendererObjectArray* visuals = *visualsPtr; - convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures); + b3VisualShapeData visualShape; + visualShape.m_objectUniqueId = bodyUniqueId; + visualShape.m_linkIndex = linkIndex; + visualShape.m_localInertiaFrame[0] = localInertiaFrame.getOrigin()[0]; + visualShape.m_localInertiaFrame[1] = localInertiaFrame.getOrigin()[1]; + visualShape.m_localInertiaFrame[2] = localInertiaFrame.getOrigin()[2]; + visualShape.m_localInertiaFrame[3] = localInertiaFrame.getRotation()[0]; + visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1]; + visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2]; + visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3]; + + convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape); + m_data->m_visualShapes.push_back(visualShape); if (vertices.size() && indices.size()) { - TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, objectIndex); + TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId); unsigned char* textureImage=0; int textureWidth=0; int textureHeight=0; @@ -513,6 +553,64 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const } } +int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId) +{ + int start = -1; + //find first one, then count how many + for (int i = 0; i < m_data->m_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId) + { + start = i; + break; + } + } + int count = 0; + + if (start >= 0) + { + for (int i = start; i < m_data->m_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId) + { + count++; + } + else + { + //storage of each visual shape for a given body unique id assumed to be contiguous + break; + } + } + } + return count; +} + +int TinyRendererVisualShapeConverter::getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData) +{ + int start = -1; + //find first one, then count how many + for (int i = 0; i < m_data->m_visualShapes.size(); i++) + { + if (m_data->m_visualShapes[i].m_objectUniqueId == bodyUniqueId) + { + start = i; + break; + } + } + int count = 0; + + if (start >= 0) + { + if (start + shapeIndex < m_data->m_visualShapes.size()) + { + *shapeData = m_data->m_visualShapes[start + shapeIndex]; + return 1; + } + } + return 0; +} + + void TinyRendererVisualShapeConverter::setUpAxis(int axis) { m_data->m_upAxis = axis; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 0602fe29c..df490716b 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -4,6 +4,9 @@ #include "../Importers/ImportURDFDemo/LinkVisualShapesConverter.h" + + + struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter { @@ -15,6 +18,10 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex); + virtual int getNumVisualShapes(int bodyUniqueId); + + virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData); + void setUpAxis(int axis); void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f3ef08ed4..1af256b47 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1260,6 +1260,107 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) { return 0; } +static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) +{ + int size = PySequence_Size(args); + int objectUniqueId = -1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3VisualShapeInformation visualShapeInfo; + int statusType; + int i; + PyObject* pyResultList = 0; + + if (size == 1) + { + if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) { + PyErr_SetString(SpamError, "Error parsing object unique id"); + return NULL; + } + + commandHandle = b3InitRequestVisualShapeInformation(sm, objectUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + b3GetVisualShapeInformation(sm, &visualShapeInfo); + pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes); + for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++) + { + PyObject* visualShapeObList = PyTuple_New(7); + PyObject* item; + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId); + PyTuple_SetItem(visualShapeObList, 0, item); + + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_linkIndex); + PyTuple_SetItem(visualShapeObList, 1, item); + + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_visualGeometryType); + PyTuple_SetItem(visualShapeObList, 2, item); + + { + PyObject* vec = PyTuple_New(3); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]); + PyTuple_SetItem(vec, 0, item); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]); + PyTuple_SetItem(vec, 1, item); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]); + PyTuple_SetItem(vec, 2, item); + PyTuple_SetItem(visualShapeObList, 3, vec); + } + + item = PyString_FromString(visualShapeInfo.m_visualShapeData[i].m_meshAssetFileName); + PyTuple_SetItem(visualShapeObList, 4, item); + + { + PyObject* vec = PyTuple_New(3); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[0]); + PyTuple_SetItem(vec, 0, item); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[1]); + PyTuple_SetItem(vec, 1, item); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[2]); + PyTuple_SetItem(vec, 2, item); + PyTuple_SetItem(visualShapeObList, 5, vec); + } + + { + PyObject* vec = PyTuple_New(4); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[3]); + PyTuple_SetItem(vec, 0, item); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[4]); + PyTuple_SetItem(vec, 1, item); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[5]); + PyTuple_SetItem(vec, 2, item); + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[6]); + PyTuple_SetItem(vec, 3, item); + PyTuple_SetItem(visualShapeObList, 6, vec); + } + + + + PyTuple_SetItem(pyResultList, i, visualShapeObList); + } + return pyResultList; + } + else + { + PyErr_SetString(SpamError, "Error receiving visual shape info"); + return NULL; + } + } + else + { + PyErr_SetString(SpamError, "getVisualShapeData requires 1 argument (object unique id)"); + return NULL; + } + + Py_INCREF(Py_None); + return Py_None; +} + + + + static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { int size = PySequence_Size(args); int objectUniqueIdA = -1; @@ -1287,7 +1388,6 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args) { commandHandle = b3InitRequestContactPointInformation(sm); b3SetContactFilterBodyA(commandHandle, objectUniqueIdA); b3SetContactFilterBodyB(commandHandle, objectUniqueIdB); - b3SubmitClientCommand(sm, commandHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -2194,6 +2294,9 @@ static PyMethodDef SpamMethods[] = { "object-object collisions. Optional arguments one or two object unique " "ids, that need to be involved in the contact."}, + { "getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, + "Return the visual shape information for one object." }, + {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to " "quaternion [x,y,z,w]"}, diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 0a8b6031d..c366ab3a5 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -67,6 +67,20 @@ void testSharedMemory(b3PhysicsClientHandle sm) numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); ASSERT_EQ(numBodies,1); bodyUniqueId = bodyIndicesOut[0]; + { + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + struct b3VisualShapeInformation vi; + b3GetVisualShapeInformation(sm, &vi); + } + } + } numJoints = b3GetNumJoints(sm,bodyUniqueId); ASSERT_EQ(numJoints,7); @@ -119,6 +133,8 @@ void testSharedMemory(b3PhysicsClientHandle sm) if (bodyIndex>=0) { + + numJoints = b3GetNumJoints(sm,bodyIndex); for (i=0;i